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

ops/2024/12/23 1:16:57/
  • 注意 : 本篇文章接上节 (点击此处跳转到上节)

编写接收程序

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

http://www.ppmy.cn/ops/85834.html

相关文章

【科研】# Taylor Francis 论文 LaTeX template模版 及 Word模版

【科研】# Taylor Francis 论文 LaTeX template模版 及 Word模版

【科研写论文】系列 文章目录 【科研写论文】系列前言一、Word 模板&#xff08;附下载网址&#xff09;&#xff1a;二、LaTeX 版本方法1&#xff1a;直接网页端打开&#xff08;附网址&#xff09;方法2&#xff1a;直接下载到本地电脑上编辑下载地址说明及注意事项 前言 给…
阅读更多...
SAPUI5基础知识20 - 对话框和碎片(Dialogs and Fragments)

SAPUI5基础知识20 - 对话框和碎片(Dialogs and Fragments)

1. 背景 在 SAPUI5 中&#xff0c;Fragments 是一种轻量级的 UI 组件&#xff0c;类似于视图&#xff08;Views&#xff09;&#xff0c;但它们没有自己的控制器&#xff08;Controller&#xff09;。Fragments 通常用于定义可以在多个视图中重用的 UI 片段&#xff0c;从而提…
阅读更多...
CSS 实现文本省略显示【单行，2行，多行】

CSS 实现文本省略显示【单行，2行，多行】

在Web开发中&#xff0c;长文本内容可能会超出其容器的范围。为了保持良好的用户体验&#xff0c;我们通常会将超出的文本部分以省略号显示。本文将详细介绍如何在CSS中限制文本显示一行、两行或多行&#xff0c;超过部分显示省略号。 目录 前言单行文本省略多行文本省略 两行…
阅读更多...
技术成神之路：设计模式（九）备忘录模式

技术成神之路：设计模式（九）备忘录模式

介绍 备忘录模式&#xff08;Memento Pattern&#xff09;是一种行为设计模式&#xff0c;它允许在不破坏封装性的前提下捕获和恢复对象的内部状态。通过备忘录模式&#xff0c;可以在程序运行过程中保存和恢复对象的某个状态&#xff0c;从而实现“撤销”等功能。 1.定义 备忘…
阅读更多...
【Redis】Redis的概念 | 特性 | 应用场景 | 安装 | 客户端

【Redis】Redis的概念 | 特性 | 应用场景 | 安装 | 客户端

文章目录 Redis一、认识Redis二、Redis的特性三、Redis的应用场景四、安装Redis五、Redis的客户端 Redis 一、认识Redis Redis是一个开源的&#xff0c;在内存中存储数据。在分布式系统中更有优势。如果是单机环境下&#xff0c;直接通过变量存储数据比用Redis更有优势。在分布…
阅读更多...
常见的文心一言的指令

常见的文心一言的指令

文心一言&#xff0c;作为百度研发的预训练语言模型“ERNIE 3.0”的一项功能&#xff0c;能够与人对话互动&#xff0c;回答问题&#xff0c;协助创作&#xff0c;高效便捷地帮助人们获取信息、知识和灵感。以下是一些常见的文心一言指令类型及其具体示例&#xff1a; 1. 查询…
阅读更多...
政安晨【零基础玩转各类开源AI项目】基于Ubuntu系统部署LivePortrait ：通过缝合和重定向控制实现高效的肖像动画制作

政安晨【零基础玩转各类开源AI项目】基于Ubuntu系统部署LivePortrait ：通过缝合和重定向控制实现高效的肖像动画制作

目录 项目论文介绍 论文中实际开展的工作 非扩散性的肖像动画 基于扩散的肖像动画 方法论 基于Ubuntu的部署实践开始 1. 克隆代码并准备环境 2. 下载预训练权重 3. 推理 快速上手 驱动视频自动裁剪 运动模板制作 4. Gradio 界面 5. 推理速度评估 社区资源 政安…
阅读更多...
扩展-通过idea 中的Frames 来查看当前栈帧

扩展-通过idea 中的Frames 来查看当前栈帧

扩展-通过idea 中的Frames 来查看当前栈帧&#xff1a; 假如有如下一个类&#xff0c;我们在idea 中可以通过后续的操作查看出栈入栈的情况 public class Test {public static void main(String[]args) {A();}public static void A() {System.out.println("A执行了..&qu…
阅读更多...
最新文章

Copyright @ 2022~2023