@@ -42,42 +42,40 @@ class FramePublisher : public rclcpp::Node
42
42
stream << " /" << turtlename_.c_str () << " /pose" ;
43
43
std::string topic_name = stream.str ();
44
44
45
- subscription_ = this ->create_subscription <turtlesim::msg::Pose>(
46
- topic_name, 10 ,
47
- std::bind (&FramePublisher::handle_turtle_pose, this , std::placeholders::_1));
48
- }
45
+ auto handle_turtle_pose = [this ](const std::shared_ptr<turtlesim::msg::Pose> msg) {
46
+ geometry_msgs::msg::TransformStamped t;
49
47
50
- private:
51
- void handle_turtle_pose (const std::shared_ptr<turtlesim::msg::Pose> msg)
52
- {
53
- geometry_msgs::msg::TransformStamped t;
48
+ // Read message content and assign it to
49
+ // corresponding tf variables
50
+ t.header .stamp = this ->get_clock ()->now ();
51
+ t.header .frame_id = " world" ;
52
+ t.child_frame_id = turtlename_.c_str ();
54
53
55
- // Read message content and assign it to
56
- // corresponding tf variables
57
- t. header . stamp = this -> get_clock ()-> now () ;
58
- t. header . frame_id = " world " ;
59
- t. child_frame_id = turtlename_. c_str () ;
54
+ // Turtle only exists in 2D, thus we get x and y translation
55
+ // coordinates from the message and set the z coordinate to 0
56
+ t. transform . translation . x = msg-> x ;
57
+ t. transform . translation . y = msg-> y ;
58
+ t. transform . translation . z = 0.0 ;
60
59
61
- // Turtle only exists in 2D, thus we get x and y translation
62
- // coordinates from the message and set the z coordinate to 0
63
- t.transform .translation .x = msg->x ;
64
- t.transform .translation .y = msg->y ;
65
- t.transform .translation .z = 0.0 ;
60
+ // For the same reason, turtle can only rotate around one axis
61
+ // and this why we set rotation in x and y to 0 and obtain
62
+ // rotation in z axis from the message
63
+ tf2::Quaternion q;
64
+ q.setRPY (0 , 0 , msg->theta );
65
+ t.transform .rotation .x = q.x ();
66
+ t.transform .rotation .y = q.y ();
67
+ t.transform .rotation .z = q.z ();
68
+ t.transform .rotation .w = q.w ();
66
69
67
- // For the same reason, turtle can only rotate around one axis
68
- // and this why we set rotation in x and y to 0 and obtain
69
- // rotation in z axis from the message
70
- tf2::Quaternion q;
71
- q.setRPY (0 , 0 , msg->theta );
72
- t.transform .rotation .x = q.x ();
73
- t.transform .rotation .y = q.y ();
74
- t.transform .rotation .z = q.z ();
75
- t.transform .rotation .w = q.w ();
76
-
77
- // Send the transformation
78
- tf_broadcaster_->sendTransform (t);
70
+ // Send the transformation
71
+ tf_broadcaster_->sendTransform (t);
72
+ };
73
+ subscription_ = this ->create_subscription <turtlesim::msg::Pose>(
74
+ topic_name, 10 ,
75
+ handle_turtle_pose);
79
76
}
80
77
78
+ private:
81
79
rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr subscription_;
82
80
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
83
81
std::string turtlename_;
0 commit comments