前边我们学习了如何在ROS2中创建一个tf2的静态广播器,机器人系统中还有很多位姿关系会变化的坐标系,这就需要我们在代码中动态广播坐标系的变换,本篇我们就来学习如何使用C++编写一个动态的广播器。
一、编写广播器节点
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <rclcpp/rclcpp.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <turtlesim/msg/pose.hpp>
#include <memory>
#include <string>
using std::placeholders::_1;
class FramePublisher : public rclcpp::Node
{
public:
FramePublisher()
: Node("turtle_tf2_frame_publisher")
{
// Declare and acquire `turtlename` parameter
this->declare_parameter<std::string>("turtlename", "turtle");
this->get_parameter("turtlename", turtlename_);
// Initialize the transform broadcaster
tf_broadcaster_ =
std::make_unique<tf2_ros::TransformBroadcaster>(*this);
// Subscribe to a turtle{1}{2}/pose topic and call handle_turtle_pose
// callback function on each message
std::ostringstream stream;
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, _1));
}
private:
void handle_turtle_pose(const std::shared_ptr<turtlesim::msg::Pose> msg)
{
rclcpp::Time now = this->get_clock()->now();
geometry_msgs::msg::TransformStamped t;
// Read message content and assign it to
// corresponding tf variables
t.header.stamp = 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;
// 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);
}
rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr subscription_;
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
std::string turtlename_;
};
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<FramePublisher>());
rclcpp::shutdown();
return 0;
}
1.1 代码解析
this->declare_parameter<std::string>("turtlename", "turtle");
this->get_parameter("turtlename", turtlename_);
subscription_ = this->create_subscription<turtlesim::msg::Pose>(
topic_name, 10,
std::bind(&FramePublisher::handle_turtle_pose, this, _1));
rclcpp::Time now = this->get_clock()->now();
geometry_msgs::msg::TransformStamped t;
// Read message content and assign it to
// corresponding tf variables
t.header.stamp = 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;
// 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);
1.2 添加编译选项
add_executable(turtle_tf2_broadcaster src/turtle_tf2_broadcaster.cpp)
ament_target_dependencies(
turtle_tf2_broadcaster
geometry_msgs
rclcpp
tf2
tf2_ros
turtlesim
)
install(TARGETS
turtle_tf2_broadcaster
DESTINATION lib/${PROJECT_NAME})
二、编写启动文件
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package='turtlesim',
executable='turtlesim_node',
name='sim'
),
Node(
package='learning_tf2_cpp',
executable='turtle_tf2_broadcaster',
name='broadcaster1',
parameters=[
{'turtlename': 'turtle1'}
]
),
])
2.1 添加依赖项
<exec_depend>launch</exec_depend>
<exec_depend>launch_ros</exec_depend>
install(DIRECTORY
launch
DESTINATION share/${PROJECT_NAME}
)
rosdep install -i --from-path src --rosdistro foxy -y
colcon build --packages-select learning_tf2_cpp
. install/setup.bash
ros2 launch learning_tf2_cpp turtle_tf2_demo.launch.py
ros2 run turtlesim turtle_teleop_key

ros2 run tf2_ros tf2_echo world turtle1

评论(0)
您还未登录,请登录后发表或查看评论