Skip to content

Commit

Permalink
♻️ Geometry msgs lambda refactor
Browse files Browse the repository at this point in the history
  • Loading branch information
FelipeGdM committed Jan 5, 2024
1 parent c2667c2 commit f0d8899
Show file tree
Hide file tree
Showing 2 changed files with 30 additions and 32 deletions.
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

0 comments on commit f0d8899

Please sign in to comment.