ROS2教程(10) - 编写接收程序、添加frame

注意 : 本篇文章接上节 (点击此处跳转到上节)

编写接收程序

cpp

<the_work_ws>/src/learning_tf2_cpp/src/turtle_tf2_listener.cpp

#include <chrono>
#include <functional>
#include <memory>
#include <string>#include "geometry_msgs/msg/transform_stamped.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2/exceptions.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "turtlesim/srv/spawn.hpp"using namespace std::chrono_literals;class FrameListener : public rclcpp::Node
{
public:FrameListener(): Node("turtle_tf2_frame_listener"),turtle_spawning_service_ready_(false),turtle_spawned_(false){// 声明并获取参数target_frametarget_frame_ = this->declare_parameter<std::string>("target_frame", "turtle1");tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());// 这里我们创建一个对象。 一旦创建了侦听器，它就开始通过网络接收tf2转换，并将它们缓冲至多10秒。tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);// 创建一个客户端 来生成乌龟spawner_ = this->create_client<turtlesim::srv::Spawn>("spawn");// 创建 turtle2 速度发布器publisher_ = this->create_publisher<geometry_msgs::msg::Twist>("turtle2/cmd_vel", 1);// 每秒调用一次定时器timer_ = this->create_wall_timer(1s, [this]() {return this->on_timer();});}private:void on_timer(){// 将frame names 储存，以便计算变换std::string fromFrameRel = target_frame_.c_str();std::string toFrameRel = "turtle2";if (turtle_spawning_service_ready_) {if (turtle_spawned_) {geometry_msgs::msg::TransformStamped t;// 查找 target_frame 和 turtle2 frames 之间的转换，并发送指令使 turtle2 到达目标范围try {// 向侦听器查询特定的转换// 参数：Target frame(目标frame)、Source frame(源frame)、The time we want to transform(想要变换的时间)t = tf_buffer_->lookupTransform(toFrameRel, fromFrameRel, tf2::TimePointZero);} catch (const tf2::TransformException & ex) {RCLCPP_INFO(this->get_logger(), "Could not transform %s to %s: %s",toFrameRel.c_str(), fromFrameRel.c_str(), ex.what());return;}geometry_msgs::msg::Twist msg;static const double scaleRotationRate = 1.0;msg.angular.z = scaleRotationRate * atan2(t.transform.translation.y,t.transform.translation.x);static const double scaleForwardSpeed = 0.5;msg.linear.x = scaleForwardSpeed * sqrt(pow(t.transform.translation.x, 2) +pow(t.transform.translation.y, 2));publisher_->publish(msg);} else {RCLCPP_INFO(this->get_logger(), "Successfully spawned");turtle_spawned_ = true;}} else {// 检查服务器是否准备就绪if (spawner_->service_is_ready()) {// 初始化// 请注意 x, y 和 在 turtlesim/srv/Spawn 中是 float 类型auto request = std::make_shared<turtlesim::srv::Spawn::Request>();request->x = 4.0;request->y = 2.0;request->theta = 0.0;request->name = "turtle2";// Call request(请求)using ServiceResponseFuture =rclcpp::Client<turtlesim::srv::Spawn>::SharedFuture;auto response_received_callback = [this](ServiceResponseFuture future) {auto result = future.get();if (strcmp(result->name.c_str(), "turtle2") == 0) {turtle_spawning_service_ready_ = true;} else {RCLCPP_ERROR(this->get_logger(), "Service callback result mismatch");}};auto result = spawner_->async_send_request(request, response_received_callback);} else {RCLCPP_INFO(this->get_logger(), "Service is not ready");}}}// 是否有生成海龟的服务的bool值bool turtle_spawning_service_ready_;// 如果生成海龟成功bool turtle_spawned_;rclcpp::Client<turtlesim::srv::Spawn>::SharedPtr spawner_{nullptr};rclcpp::TimerBase::SharedPtr timer_{nullptr};rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_{nullptr};std::shared_ptr<tf2_ros::TransformListener> tf_listener_{nullptr};std::unique_ptr<tf2_ros::Buffer> tf_buffer_;std::string target_frame_;
};int main(int argc, char * argv[])
{rclcpp::init(argc, argv);rclcpp::spin(std::make_shared<FrameListener>());rclcpp::shutdown();return 0;
}

CMakeLists.txt

<the_work_ws>/src/learning_tf2_cpp/CMakeLists.txt

# add
add_executable(turtle_tf2_listener src/turtle_tf2_listener.cpp)
ament_target_dependencies(turtle_tf2_listenergeometry_msgsrclcpptf2tf2_rosturtlesim
)
install(TARGETSturtle_tf2_listenerDESTINATION lib/${PROJECT_NAME})

launch

<the_work_ws>/src/learning_tf2_cpp/launch/turtle_tf2_demo.launch.py

# 更新为以下内容
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfigurationfrom launch_ros.actions import Nodedef 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'}]),DeclareLaunchArgument('target_frame', default_value='turtle1',description='Target frame name.'),Node(package='learning_tf2_cpp',executable='turtle_tf2_broadcaster',name='broadcaster2',parameters=[{'turtlename': 'turtle2'}]),Node(package='learning_tf2_cpp',executable='turtle_tf2_listener',name='listener',parameters=[{'target_frame': LaunchConfiguration('target_frame')}]),])

run

# 终端1
rosdep install -i --from-path src --rosdistro humble -y
colcon build --packages-select learning_tf2_cpp
. install/setup.bash
ros2 launch learning_tf2_cpp turtle_tf2_demo.launch.py # 我们看到有两只海龟# 终端2ros2 run turtlesim turtle_teleop_key # 控制海龟1运动，发现海龟2跟随运动

添加frame

在之前的教程中，我们通过编写tf2广播器和tf2侦听器来重新创建海龟演示。本教程将教你如何向转换树添加额外的固定和动态frame。事实上，在tf2中添加一个frame与创建tf2广播器非常相似，但是这个示例将向您展示tf2的一些附加功能。

静态frmae广播器

我们将编写固定frame广播程序，基于前面的海龟跟随示例，我们将添加一个坐标系carrot1，它是turtle1的子坐标系，并将作为第二只海龟的目标。

cpp

<the_work_ws>/src/learning_tf2_cpp/src/fixed_frame_tf2_broadcaster.cpp

#include <chrono>
#include <functional>
#include <memory>#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_broadcaster.h"using namespace std::chrono_literals;class FixedFrameBroadcaster : public rclcpp::Node
{
public:FixedFrameBroadcaster(): Node("fixed_frame_tf2_broadcaster"){tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);timer_ = this->create_wall_timer(100ms, std::bind(&FixedFrameBroadcaster::broadcast_timer_callback, this));}private:void broadcast_timer_callback(){// 坐标系转换geometry_msgs::msg::TransformStamped t;t.header.stamp = this->get_clock()->now();t.header.frame_id = "turtle1";t.child_frame_id = "carrot1";t.transform.translation.x = 0.0;t.transform.translation.y = 2.0;t.transform.translation.z = 0.0;t.transform.rotation.x = 0.0;t.transform.rotation.y = 0.0;t.transform.rotation.z = 0.0;t.transform.rotation.w = 1.0;tf_broadcaster_->sendTransform(t);}rclcpp::TimerBase::SharedPtr timer_;std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
};int main(int argc, char * argv[])
{rclcpp::init(argc, argv);rclcpp::spin(std::make_shared<FixedFrameBroadcaster>());rclcpp::shutdown();return 0;
}

CMakeLists.txt

<the_work_ws>/src/learning_tf2_cpp/CMakeLists.txt

# add
add_executable(fixed_frame_tf2_broadcaster src/fixed_frame_tf2_broadcaster.cpp)
ament_target_dependencies(fixed_frame_tf2_broadcastergeometry_msgsrclcpptf2_ros
)
install(TARGETSfixed_frame_tf2_broadcasterDESTINATION lib/${PROJECT_NAME})

launch

<the_work_ws>/src/learning_tf2_cpp/launch/turtle_tf2_fixed_frame_demo.launch.py

import osfrom ament_index_python.packages import get_package_share_directoryfrom launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSourcefrom launch_ros.actions import Nodedef generate_launch_description():demo_nodes = IncludeLaunchDescription(PythonLaunchDescriptionSource([os.path.join(get_package_share_directory('learning_tf2_cpp'), 'launch'),'/turtle_tf2_demo.launch.py']),)return LaunchDescription([demo_nodes,Node(package='learning_tf2_cpp',executable='fixed_frame_tf2_broadcaster',name='fixed_broadcaster',),])

run

# 终端1 
rosdep install -i --from-path src --rosdistro humble -y
colcon build --packages-select learning_tf2_cpp
. install/setup.bash
ros2 launch learning_tf2_cpp turtle_tf2_fixed_frame_demo.launch.py

如果您驾驶第一只海龟，您应该注意到，尽管我们添加了一个新frame，但其行为与上一教程相比并没有改变。这是因为添加一个额外的frame不会影响其他帧，我们的监听器仍然使用先前定义的frame。

因此，如果我们想让第二只乌龟跟随carrot而不是第一只乌龟，我们需要改变target_frame的值。这可以通过两种方式实现。一种方法是直接从控制台将参数传递给启动文件:

ros2 launch learning_tf2_cpp turtle_tf2_fixed_frame_demo.launch.py target_frame:=carrot1

第二种方法是更新启动文件turtle_tf2_fixed_frame_demo.launch.py：

def generate_launch_description():demo_nodes = IncludeLaunchDescription(...,launch_arguments={'target_frame': 'carrot1'}.items(),)

现在重新构建包，重新启动，您将看到第二只乌龟跟随carrot而不是第一只乌龟!

动态frame广播器

cpp

<the_work_ws>/src/learning_tf2_cpp/src/dynamic_frame_tf2_broadcaster.cpp

#include <chrono>
#include <functional>
#include <memory>#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_broadcaster.h"using namespace std::chrono_literals;const double PI = 3.141592653589793238463;class DynamicFrameBroadcaster : public rclcpp::Node
{
public:DynamicFrameBroadcaster(): Node("dynamic_frame_tf2_broadcaster"){tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);timer_ = this->create_wall_timer(100ms, std::bind(&DynamicFrameBroadcaster::broadcast_timer_callback, this));}private:void broadcast_timer_callback(){rclcpp::Time now = this->get_clock()->now();double x = now.seconds() * PI;// 设置不断变换的偏移量geometry_msgs::msg::TransformStamped t;t.header.stamp = now;t.header.frame_id = "turtle1";t.child_frame_id = "carrot1";t.transform.translation.x = 10 * sin(x);t.transform.translation.y = 10 * cos(x);t.transform.translation.z = 0.0;t.transform.rotation.x = 0.0;t.transform.rotation.y = 0.0;t.transform.rotation.z = 0.0;t.transform.rotation.w = 1.0;tf_broadcaster_->sendTransform(t);}rclcpp::TimerBase::SharedPtr timer_;std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
};int main(int argc, char * argv[])
{rclcpp::init(argc, argv);rclcpp::spin(std::make_shared<DynamicFrameBroadcaster>());rclcpp::shutdown();return 0;
}

CMakeLists.txt

<the_work_ws>/src/learning_tf2_cpp/CMakeLists.txt

# add
add_executable(dynamic_frame_tf2_broadcaster src/dynamic_frame_tf2_broadcaster.cpp)
ament_target_dependencies(dynamic_frame_tf2_broadcastergeometry_msgsrclcpptf2_ros
)
install(TARGETSdynamic_frame_tf2_broadcasterDESTINATION lib/${PROJECT_NAME})

launch

<the_work_ws>/src/learning_tf2_cpp/launch/turtle_tf2_dynamic_frame_demo.launch.py

import osfrom ament_index_python.packages import get_package_share_directoryfrom launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSourcefrom launch_ros.actions import Nodedef generate_launch_description():demo_nodes = IncludeLaunchDescription(PythonLaunchDescriptionSource([os.path.join(get_package_share_directory('learning_tf2_cpp'), 'launch'),'/turtle_tf2_demo.launch.py']),launch_arguments={'target_frame': 'carrot1'}.items(),)return LaunchDescription([demo_nodes,Node(package='learning_tf2_cpp',executable='dynamic_frame_tf2_broadcaster',name='dynamic_broadcaster',),])

run

rosdep install -i --from-path src --rosdistro humble -y
colcon build --packages-select learning_tf2_cpp
. install/setup.bash
ros2 launch learning_tf2_cpp turtle_tf2_dynamic_frame_demo.launch.py