21 : Node(
"servo_tf_node"),
22 last_state_time_(std::chrono::steady_clock::now())
24 state_topic_ = declare_parameter<std::string>(
"state_topic",
"/servo_state");
25 joint_name_ = declare_parameter<std::string>(
"joint_name",
"nodding_joint");
26 parent_frame_ = declare_parameter<std::string>(
"parent_frame",
"servo_link");
27 child_frame_ = declare_parameter<std::string>(
"child_frame",
"laser");
28 lidar_height_ = declare_parameter<double>(
"lidar_height", 0.0325);
29 state_timeout_sec_ = declare_parameter<double>(
"state_timeout_sec", 0.5);
30 if (state_timeout_sec_ <= 0.0) {
31 throw std::invalid_argument(
"state_timeout_sec must be positive");
34 broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*
this);
35 subscription_ = create_subscription<sensor_msgs::msg::JointState>(
36 state_topic_, rclcpp::QoS(10),
37 std::bind(&ServoTfNode::on_state,
this, std::placeholders::_1));
38 watchdog_ = create_wall_timer(
39 std::chrono::milliseconds(500), std::bind(&ServoTfNode::check_state,
this));
41 get_logger(),
"Broadcasting %s -> %s from JointState '%s' (angle in radians)",
42 parent_frame_.c_str(), child_frame_.c_str(), state_topic_.c_str());
46 void on_state(
const sensor_msgs::msg::JointState::ConstSharedPtr state)
48 const auto joint = std::find(state->name.begin(), state->name.end(), joint_name_);
49 if (joint == state->name.end()) {
51 get_logger(), *get_clock(), 2000,
"JointState does not contain joint '%s'",
55 const auto index =
static_cast<size_t>(std::distance(state->name.begin(), joint));
56 if (index >= state->position.size() || !std::isfinite(state->position[index])) {
58 get_logger(), *get_clock(), 2000,
"JointState has no finite position for '%s'",
62 if (state->header.stamp.sec == 0 && state->header.stamp.nanosec == 0) {
64 get_logger(), *get_clock(), 2000,
"Ignoring servo angle with a zero timestamp");
68 const nodding_lidar::Pose pose =
70 geometry_msgs::msg::TransformStamped transform;
71 transform.header = state->header;
72 transform.header.frame_id = parent_frame_;
73 transform.child_frame_id = child_frame_;
74 transform.transform.translation.x = pose.
t.
x;
75 transform.transform.translation.y = pose.
t.
y;
76 transform.transform.translation.z = pose.
t.
z;
77 transform.transform.rotation.x = pose.
qx;
78 transform.transform.rotation.y = pose.
qy;
79 transform.transform.rotation.z = pose.
qz;
80 transform.transform.rotation.w = pose.
qw;
81 broadcaster_->sendTransform(transform);
83 last_state_time_ = std::chrono::steady_clock::now();
89 const auto elapsed = std::chrono::duration<double>(
90 std::chrono::steady_clock::now() - last_state_time_).count();
91 if (!have_state_ || elapsed > state_timeout_sec_) {
93 get_logger(), *get_clock(), 2000,
94 "No valid servo angle received recently; dynamic TF updates have stopped");
98 std::string state_topic_;
99 std::string joint_name_;
100 std::string parent_frame_;
101 std::string child_frame_;
102 double lidar_height_{0.0325};
103 double state_timeout_sec_{0.5};
104 bool have_state_{
false};
105 std::chrono::steady_clock::time_point last_state_time_;
106 rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr subscription_;
107 rclcpp::TimerBase::SharedPtr watchdog_;
108 std::unique_ptr<tf2_ros::TransformBroadcaster> broadcaster_;