Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
58 changes: 28 additions & 30 deletions turtle_tf2_cpp/src/turtle_tf2_broadcaster.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -42,42 +42,40 @@ class FramePublisher : public rclcpp::Node
stream << "/" << turtlename_.c_str() << "/pose";
std::string topic_name = stream.str();

subscription_ = this->create_subscription<turtlesim::msg::Pose>(
topic_name, 10,
std::bind(&FramePublisher::handle_turtle_pose, this, std::placeholders::_1));
}
auto handle_turtle_pose = [this](const std::shared_ptr<turtlesim::msg::Pose> msg) {
geometry_msgs::msg::TransformStamped t;

private:
void handle_turtle_pose(const std::shared_ptr<turtlesim::msg::Pose> 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<turtlesim::msg::Pose>(
topic_name, 10,
handle_turtle_pose);
}

private:
rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr subscription_;
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
std::string turtlename_;
Expand Down
4 changes: 2 additions & 2 deletions turtle_tf2_cpp/src/turtle_tf2_listener.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down Expand Up @@ -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<turtlesim::srv::Spawn::Request>();
request->name = "turtle2";
request->x = 4.0;
request->y = 2.0;
request->theta = 0.0;
request->name = "turtle2";

// Call request
using ServiceResponseFuture =
Expand Down