From f0d8899f763f4d008bc550c4d868953545e6f5b7 Mon Sep 17 00:00:00 2001 From: Felipe Gomes de Melo Date: Fri, 5 Jan 2024 14:07:53 -0300 Subject: [PATCH 1/4] =?UTF-8?q?=E2=99=BB=EF=B8=8F=20Geometry=20msgs=20lamb?= =?UTF-8?q?da=20refactor?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- turtle_tf2_cpp/src/turtle_tf2_broadcaster.cpp | 58 +++++++++---------- turtle_tf2_cpp/src/turtle_tf2_listener.cpp | 4 +- 2 files changed, 30 insertions(+), 32 deletions(-) diff --git a/turtle_tf2_cpp/src/turtle_tf2_broadcaster.cpp b/turtle_tf2_cpp/src/turtle_tf2_broadcaster.cpp index 09272e0..681fc88 100644 --- a/turtle_tf2_cpp/src/turtle_tf2_broadcaster.cpp +++ b/turtle_tf2_cpp/src/turtle_tf2_broadcaster.cpp @@ -42,42 +42,40 @@ class FramePublisher : public rclcpp::Node stream << "/" << turtlename_.c_str() << "/pose"; std::string topic_name = stream.str(); - subscription_ = this->create_subscription( - topic_name, 10, - std::bind(&FramePublisher::handle_turtle_pose, this, std::placeholders::_1)); - } + auto handle_turtle_pose = [this](const std::shared_ptr msg){ + geometry_msgs::msg::TransformStamped t; -private: - void handle_turtle_pose(const std::shared_ptr msg) - { - geometry_msgs::msg::TransformStamped t; + // Read message content and assign it to + // corresponding tf variables + t.header.stamp = this->get_clock()->now(); + t.header.frame_id = "world"; + t.child_frame_id = turtlename_.c_str(); - // Read message content and assign it to - // corresponding tf variables - t.header.stamp = this->get_clock()->now(); - t.header.frame_id = "world"; - t.child_frame_id = turtlename_.c_str(); + // Turtle only exists in 2D, thus we get x and y translation + // coordinates from the message and set the z coordinate to 0 + t.transform.translation.x = msg->x; + t.transform.translation.y = msg->y; + t.transform.translation.z = 0.0; - // Turtle only exists in 2D, thus we get x and y translation - // coordinates from the message and set the z coordinate to 0 - t.transform.translation.x = msg->x; - t.transform.translation.y = msg->y; - t.transform.translation.z = 0.0; + // For the same reason, turtle can only rotate around one axis + // and this why we set rotation in x and y to 0 and obtain + // rotation in z axis from the message + tf2::Quaternion q; + q.setRPY(0, 0, msg->theta); + t.transform.rotation.x = q.x(); + t.transform.rotation.y = q.y(); + t.transform.rotation.z = q.z(); + t.transform.rotation.w = q.w(); - // For the same reason, turtle can only rotate around one axis - // and this why we set rotation in x and y to 0 and obtain - // rotation in z axis from the message - tf2::Quaternion q; - q.setRPY(0, 0, msg->theta); - t.transform.rotation.x = q.x(); - t.transform.rotation.y = q.y(); - t.transform.rotation.z = q.z(); - t.transform.rotation.w = q.w(); - - // Send the transformation - tf_broadcaster_->sendTransform(t); + // Send the transformation + tf_broadcaster_->sendTransform(t); + }; + subscription_ = this->create_subscription( + topic_name, 10, + handle_turtle_pose); } +private: rclcpp::Subscription::SharedPtr subscription_; std::unique_ptr tf_broadcaster_; std::string turtlename_; diff --git a/turtle_tf2_cpp/src/turtle_tf2_listener.cpp b/turtle_tf2_cpp/src/turtle_tf2_listener.cpp index 749f997..32bb1e2 100644 --- a/turtle_tf2_cpp/src/turtle_tf2_listener.cpp +++ b/turtle_tf2_cpp/src/turtle_tf2_listener.cpp @@ -53,7 +53,7 @@ class FrameListener : public rclcpp::Node // Call on_timer function every second timer_ = this->create_wall_timer( - 1s, std::bind(&FrameListener::on_timer, this)); + 1s, [this](){return this->on_timer();}); } private: @@ -104,10 +104,10 @@ class FrameListener : public rclcpp::Node // Initialize request with turtle name and coordinates // Note that x, y and theta are defined as floats in turtlesim/srv/Spawn auto request = std::make_shared(); - request->name = "turtle2"; request->x = 4.0; request->y = 2.0; request->theta = 0.0; + request->name = "turtle2"; // Call request using ServiceResponseFuture = From 3a0b7630be20e2454980f07ca3aa5e9c9bbe9167 Mon Sep 17 00:00:00 2001 From: Felipe Gomes de Melo Date: Mon, 8 Jan 2024 11:05:44 -0300 Subject: [PATCH 2/4] Update turtle_tf2_cpp/src/turtle_tf2_broadcaster.cpp Co-authored-by: Chris Lalancette --- turtle_tf2_cpp/src/turtle_tf2_broadcaster.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/turtle_tf2_cpp/src/turtle_tf2_broadcaster.cpp b/turtle_tf2_cpp/src/turtle_tf2_broadcaster.cpp index 681fc88..a7c6906 100644 --- a/turtle_tf2_cpp/src/turtle_tf2_broadcaster.cpp +++ b/turtle_tf2_cpp/src/turtle_tf2_broadcaster.cpp @@ -42,7 +42,7 @@ class FramePublisher : public rclcpp::Node stream << "/" << turtlename_.c_str() << "/pose"; std::string topic_name = stream.str(); - auto handle_turtle_pose = [this](const std::shared_ptr msg){ + auto handle_turtle_pose = [this](const std::shared_ptr msg) { geometry_msgs::msg::TransformStamped t; // Read message content and assign it to From 7a98f607224523407a5da547254216bb10fdc65e Mon Sep 17 00:00:00 2001 From: Felipe Gomes de Melo Date: Mon, 8 Jan 2024 11:05:49 -0300 Subject: [PATCH 3/4] Update turtle_tf2_cpp/src/turtle_tf2_broadcaster.cpp Co-authored-by: Chris Lalancette --- turtle_tf2_cpp/src/turtle_tf2_broadcaster.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/turtle_tf2_cpp/src/turtle_tf2_broadcaster.cpp b/turtle_tf2_cpp/src/turtle_tf2_broadcaster.cpp index a7c6906..858a5ec 100644 --- a/turtle_tf2_cpp/src/turtle_tf2_broadcaster.cpp +++ b/turtle_tf2_cpp/src/turtle_tf2_broadcaster.cpp @@ -69,7 +69,7 @@ class FramePublisher : public rclcpp::Node // Send the transformation tf_broadcaster_->sendTransform(t); - }; + }; subscription_ = this->create_subscription( topic_name, 10, handle_turtle_pose); From f3de34df4edb816adfa9d62fea4188bc074b8a2e Mon Sep 17 00:00:00 2001 From: Felipe Gomes de Melo Date: Wed, 10 Jan 2024 09:30:31 -0300 Subject: [PATCH 4/4] Update turtle_tf2_cpp/src/turtle_tf2_listener.cpp Co-authored-by: Chris Lalancette --- turtle_tf2_cpp/src/turtle_tf2_listener.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/turtle_tf2_cpp/src/turtle_tf2_listener.cpp b/turtle_tf2_cpp/src/turtle_tf2_listener.cpp index 32bb1e2..c83f5c5 100644 --- a/turtle_tf2_cpp/src/turtle_tf2_listener.cpp +++ b/turtle_tf2_cpp/src/turtle_tf2_listener.cpp @@ -53,7 +53,7 @@ class FrameListener : public rclcpp::Node // Call on_timer function every second timer_ = this->create_wall_timer( - 1s, [this](){return this->on_timer();}); + 1s, [this]() {return this->on_timer();}); } private: