@@ -42,42 +42,40 @@ class FramePublisher : public rclcpp::Node
4242 stream << " /" << turtlename_.c_str () << " /pose" ;
4343 std::string topic_name = stream.str ();
4444
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;
4947
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 ();
5453
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 ;
6059
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 ();
6669
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);
7976 }
8077
78+ private:
8179 rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr subscription_;
8280 std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
8381 std::string turtlename_;
0 commit comments