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 =