[人工智能]第五章--ROS常用组件学习笔记

第五章–ROS常用组件学习笔记

学习来源：

• 官方文档：http://wiki.ros.org/
• Introduction · Autolabor-ROS机器人入门课程《ROS理论与实践》零基础教程

本文仅作学习笔记和回顾使用

学习目标

• 了解坐标变换的原理、应用场景
• 编写复用性强的代码备用
• 学习适用rosbag、rqt等工具箱以便调试

正文

• TF坐标变换，实现不同类型的坐标系之间的转换；
• rosbag 用于录制ROS节点的执行过程并可以重放该过程；
• rqt 工具箱，集成了多款图形化的调试工具。

坐标变换应用场景

• 车载雷达定位

  车载雷达定位的信息需要转化为相对于机器人系统的定位信息

  

• 机械臂定位

  各种传感器定位信息转换为机械臂中心的定位信息

• slam建图

  涉及刚体运动变换

tf坐标常用包及信息

• tf常用包：

  • tf2：坐标变换的常用信息
  • tf2_ros：常用API
  • tf2_geometrt_msgs：ros信息转换成tf2信息

• 坐标信息：geometry_msgs下的两种类型

  • TransformStamped：坐标系的转换信息

  std_msgs/Header header                     #头信息
    uint32 seq                                #|-- 序列号
    time stamp                                #|-- 时间戳
    string frame_id                            #|-- 坐标 ID
  string child_frame_id                    #子坐标系的 id
  geometry_msgs/Transform transform        #坐标信息
    geometry_msgs/Vector3 translation        #偏移量
      float64 x                                #|-- X 方向的偏移量
      float64 y                                #|-- Y 方向的偏移量
      float64 z                                #|-- Z 方向上的偏移量
    geometry_msgs/Quaternion rotation        #四元数
      float64 x                                
      float64 y                                
      float64 z                                
      float64 w
  
  

  • PointStamped:坐标点的信息

  std_msgs/Header header                    #头
    uint32 seq                                #|-- 序号
    time stamp                                #|-- 时间戳
    string frame_id                            #|-- 所属坐标系的 id
  geometry_msgs/Point point                #点坐标
    float64 x                                    #|-- x y z 坐标
    float64 y
    float64 z
  

静态坐标变换

定义

两个坐标系之间的相对位置是固定的

学习背景

• 雷达和小车的坐标系上的点相互转换

• 拓展应用：各种传感器的坐标点的转换，以便路径规划

编码实现

• 分析

  • node1:雷达和小车坐标系相对关系TransformStamped->发布方发布

  • node2:订阅方订阅相对关系，传入坐标点信息PointStamped，输出结果

• 编码

  发布方

  /* 
      静态坐标变换发布方:
          发布关于 laser 坐标系的位置信息 
  
      实现流程:
          1.包含头文件
          2.初始化 ROS 节点
          3.创建静态坐标转换广播器
          4.创建坐标系信息
          5.广播器发布坐标系信息
          6.spin()
  */
  
  
  // 1.包含头文件
  #include "ros/ros.h"
  #include "tf2_ros/static_transform_broadcaster.h"
  #include "geometry_msgs/TransformStamped.h"
  #include "tf2/LinearMath/Quaternion.h"
  
  int main(int argc, char *argv[])
  {
      setlocale(LC_ALL,"");
      // 2.初始化 ROS 节点
      ros::init(argc,argv,"static_brocast");
      // 3.创建静态坐标转换广播器
      tf2_ros::StaticTransformBroadcaster broadcaster;
      // 4.创建坐标系信息
      geometry_msgs::TransformStamped ts;
      //----设置头信息
      ts.header.seq = 100;
      ts.header.stamp = ros::Time::now();
      ts.header.frame_id = "base_link";
      //----设置子级坐标系
      ts.child_frame_id = "laser";
      //----设置子级相对于父级的偏移量
      ts.transform.translation.x = 0.2;
      ts.transform.translation.y = 0.0;
      ts.transform.translation.z = 0.5;
      //----设置四元数:将 欧拉角数据转换成四元数
      tf2::Quaternion qtn;
      qtn.setRPY(0,0,0);
      ts.transform.rotation.x = qtn.getX();
      ts.transform.rotation.y = qtn.getY();
      ts.transform.rotation.z = qtn.getZ();
      ts.transform.rotation.w = qtn.getW();
      // 5.广播器发布坐标系信息
      broadcaster.sendTransform(ts);
      ros::spin();
      return 0;
  }
  

订阅方

  /*  
      订阅坐标系信息，生成一个相对于 子级坐标系的坐标点数据，转换成父级坐标系中的坐标点
  
      实现流程:
          1.包含头文件
          2.初始化 ROS 节点
          3.创建 TF 订阅节点
          4.生成一个坐标点(相对于子级坐标系)
          5.转换坐标点(相对于父级坐标系)
          6.spin()
  */
  //1.包含头文件
  #include "ros/ros.h"
  #include "tf2_ros/transform_listener.h"
  #include "tf2_ros/buffer.h"
  #include "geometry_msgs/PointStamped.h"
  #include "tf2_geometry_msgs/tf2_geometry_msgs.h" //注意: 调用 transform 必须包含该头文件
  
  int main(int argc, char *argv[])
  {
      setlocale(LC_ALL,"");
      // 2.初始化 ROS 节点
      ros::init(argc,argv,"tf_sub");
      ros::NodeHandle nh;
      // 3.创建 TF 订阅节点
      tf2_ros::Buffer buffer;
      tf2_ros::TransformListener listener(buffer);
  
      ros::Rate r(1);
      while (ros::ok())
      {
      // 4.生成一个坐标点(相对于子级坐标系)
          geometry_msgs::PointStamped point_laser;
          point_laser.header.frame_id = "laser";
          point_laser.header.stamp = ros::Time::now();
          point_laser.point.x = 1;
          point_laser.point.y = 2;
          point_laser.point.z = 7.3;
      // 5.转换坐标点(相对于父级坐标系)
          //新建一个坐标点，用于接收转换结果  
          //--------------使用 try 语句或休眠，否则可能由于缓存接收延迟而导致坐标转换失败------------------------
          try
          {
              geometry_msgs::PointStamped point_base;
              point_base = buffer.transform(point_laser,"base_link");
              ROS_INFO("转换后的数据:(%.2f,%.2f,%.2f),参考的坐标系是:",point_base.point.x,point_base.point.y,point_base.point.z,point_base.header.frame_id.c_str());
  
          }
          catch(const std::exception& e)
          {
              // std::cerr << e.what() << '\n';
              ROS_INFO("程序异常.....");
          }
  
  
          r.sleep();  
          ros::spinOnce();
      }
  
  
      return 0;
  }

动态坐标变换

定义

两个坐标系之间的相对位置是变化的

学习背景

• 世界坐标系和乌龟坐标系上的点的转换

• 拓展应用：各种传感器的坐标点的转换，以便路径规划

编码实现

• 分析

  • node1:TF广播

    • 普通订阅方：/turtle1/pose话题动态订阅乌龟在世界坐标系上的位置信息
    • TF广播发布方：设置源坐标系、子坐标系、相对信息、四元数转换

  • node2:

    • TF订阅方：使用transform API实现坐标转换

• 编码

  发布方

  /*  
      动态的坐标系相对姿态发布(一个坐标系相对于另一个坐标系的相对姿态是不断变动的)
  
      需求: 启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘
      控制乌龟运动，将两个坐标系的相对位置动态发布
  
      实现分析:
          1.乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点
          2.订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度
          3.将 pose 信息转换成 坐标系相对信息并发布
  
      实现流程:
          1.包含头文件
          2.初始化 ROS 节点
          3.创建 ROS 句柄
          4.创建订阅对象
          5.回调函数处理订阅到的数据(实现TF广播)
              5-1.创建 TF 广播器
              5-2.创建 广播的数据(通过 pose 设置)
              5-3.广播器发布数据
          6.spin
  */
  // 1.包含头文件
  #include "ros/ros.h"
  #include "turtlesim/Pose.h"
  #include "tf2_ros/transform_broadcaster.h"
  #include "geometry_msgs/TransformStamped.h"
  #include "tf2/LinearMath/Quaternion.h"
  
  void doPose(const turtlesim::Pose::ConstPtr& pose){
      //  5-1.创建 TF 广播器
      static tf2_ros::TransformBroadcaster broadcaster;
      //  5-2.创建 广播的数据(通过 pose 设置)
      geometry_msgs::TransformStamped tfs;
      //  |----头设置
      tfs.header.frame_id = "world";
      tfs.header.stamp = ros::Time::now();
  
      //  |----坐标系 ID
      tfs.child_frame_id = "turtle1";
  
      //  |----坐标系相对信息设置
      tfs.transform.translation.x = pose->x;
      tfs.transform.translation.y = pose->y;
      tfs.transform.translation.z = 0.0; // 二维实现，pose 中没有z，z 是 0
      //  |--------- 四元数设置
      tf2::Quaternion qtn;
      qtn.setRPY(0,0,pose->theta);
      tfs.transform.rotation.x = qtn.getX();
      tfs.transform.rotation.y = qtn.getY();
      tfs.transform.rotation.z = qtn.getZ();
      tfs.transform.rotation.w = qtn.getW();
  
  
      //  5-3.广播器发布数据
      broadcaster.sendTransform(tfs);
  }
  
  int main(int argc, char *argv[])
  {
      setlocale(LC_ALL,"");
      // 2.初始化 ROS 节点
      ros::init(argc,argv,"dynamic_tf_pub");
      // 3.创建 ROS 句柄
      ros::NodeHandle nh;
      // 4.创建订阅对象
      ros::Subscriber sub = nh.subscribe<turtlesim::Pose>("/turtle1/pose",1000,doPose);
      //     5.回调函数处理订阅到的数据(实现TF广播)
      //        
      // 6.spin
      ros::spin();
      return 0;
  }
  

订阅方

  //1.包含头文件
  #include "ros/ros.h"
  #include "tf2_ros/transform_listener.h"
  #include "tf2_ros/buffer.h"
  #include "geometry_msgs/PointStamped.h"
  #include "tf2_geometry_msgs/tf2_geometry_msgs.h" //注意: 调用 transform 必须包含该头文件
  
  int main(int argc, char *argv[])
  {
      setlocale(LC_ALL,"");
      // 2.初始化 ROS 节点
      ros::init(argc,argv,"dynamic_tf_sub");
      ros::NodeHandle nh;
      // 3.创建 TF 订阅节点
      tf2_ros::Buffer buffer;
      tf2_ros::TransformListener listener(buffer);
  
      ros::Rate r(1);
      while (ros::ok())
      {
      // 4.生成一个坐标点(相对于子级坐标系)
          geometry_msgs::PointStamped point_laser;
          point_laser.header.frame_id = "turtle1";
          point_laser.header.stamp = ros::Time();
          point_laser.point.x = 1;
          point_laser.point.y = 1;
          point_laser.point.z = 0;
      // 5.转换坐标点(相对于父级坐标系)
          //新建一个坐标点，用于接收转换结果  
          //--------------使用 try 语句或休眠，否则可能由于缓存接收延迟而导致坐标转换失败------------------------
          try
          {
              geometry_msgs::PointStamped point_base;
              point_base = buffer.transform(point_laser,"world");
              ROS_INFO("坐标点相对于 world 的坐标为:(%.2f,%.2f,%.2f)",point_base.point.x,point_base.point.y,point_base.point.z);
  
          }
          catch(const std::exception& e)
          {
              // std::cerr << e.what() << '\n';
              ROS_INFO("程序异常:%s",e.what());
          }
  
  
          r.sleep();  
          ros::spinOnce();
      }
  
  
      return 0;
  }

多坐标变换

定义

多个坐标系的相互转换

学习背景

• 父级坐标系统 world,下有两子级系统 son1，son2，son1 相对于 world，以及 son2 相对于 world 的关系是已知的，求 son1原点在 son2中的坐标，又已知在 son1中一点的坐标，要求求出该点在 son2 中的坐标

• 拓展应用：各种传感器的坐标点的转换，以便路径规划

编码实现

• 分析

  • node1:发布两个坐标系相对于世界坐标系的坐标转换信息

  • node2:订阅发布的坐标信息，tf2 API transform 实现两个坐标系的转换，然后实现两个坐标系中的点的转换

• 编码

  <launch>
      <node pkg="tf2_ros" type="static_transform_publisher" name="son1" args="0.2 0.8 0.3 0 0 0 /world /son1" output="screen" />
      <node pkg="tf2_ros" type="static_transform_publisher" name="son2" args="0.5 0 0 0 0 0 /world /son2" output="screen" />
  </launch>
  

TF坐标变化实操：乌龟跟随

学习背景

产生两只乌龟，中间的乌龟(A) 和 左下乌龟(B), B 会自动运行至A的位置，并且键盘控制时，只是控制 A 的运动，但是 B 可以跟随 A 运行

编码实现

• 分析

  • node1：服务客户端利用"/spawn"话题生成第二只乌龟，不受键盘控制

  • node2：订阅 ”/乌龟名称/pose“ 话题，回调函数里发布方 动态传参args配合launch文件解析乌龟命令空间，TF广播发布两只乌龟的位姿信息

  • node3：订阅方订阅 ”/turtle2/cmd_vel“话题信息，通过 lookupTransform 获取两只乌龟的相对坐标信息，速度转换

• 编码

  <!--
      tf2 实现小乌龟跟随案例
  -->
  <launch>
      <!-- 启动乌龟节点与键盘控制节点 -->
      <node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen" />
      <node pkg="turtlesim" type="turtle_teleop_key" name="key_control" output="screen"/>
      <!-- 启动创建第二只乌龟的节点 -->
      <node pkg="demo_tf2_test" type="Test01_Create_Turtle2" name="turtle2" output="screen" />
      <!-- 启动两个坐标发布节点 -->
      <node pkg="demo_tf2_test" type="Test02_TF2_Caster" name="caster1" output="screen" args="turtle1" />
      <node pkg="demo_tf2_test" type="Test02_TF2_Caster" name="caster2" output="screen" args="turtle2" />
      <!-- 启动坐标转换节点 -->
      <node pkg="demo_tf2_test" type="Test03_TF2_Listener" name="listener" output="screen" />
  </launch>
  

• 客户端

/* 
    创建第二只小乌龟
 */
#include "ros/ros.h"
#include "turtlesim/Spawn.h"

int main(int argc, char *argv[])
{

    setlocale(LC_ALL,"");

    //执行初始化
    ros::init(argc,argv,"create_turtle");
    //创建节点
    ros::NodeHandle nh;
    //创建服务客户端
    ros::ServiceClient client = nh.serviceClient<turtlesim::Spawn>("/spawn");

    ros::service::waitForService("/spawn");
    turtlesim::Spawn spawn;
    spawn.request.name = "turtle2";
    spawn.request.x = 1.0;
    spawn.request.y = 2.0;
    spawn.request.theta = 3.12415926;
    bool flag = client.call(spawn);
    if (flag)
    {
        ROS_INFO("乌龟%s创建成功!",spawn.response.name.c_str());
    }
    else
    {
        ROS_INFO("乌龟2创建失败!");
    }

    ros::spin();

    return 0;
}

• 发布方

/*  
    该文件实现:需要订阅 turtle1 和 turtle2 的 pose，然后广播相对 world 的坐标系信息

    注意: 订阅的两只 turtle,除了命名空间(turtle1 和 turtle2)不同外,
          其他的话题名称和实现逻辑都是一样的，
          所以我们可以将所需的命名空间通过 args 动态传入

    实现流程:
        1.包含头文件
        2.初始化 ros 节点
        3.解析传入的命名空间
        4.创建 ros 句柄
        5.创建订阅对象
        6.回调函数处理订阅的 pose 信息
            6-1.创建 TF 广播器
            6-2.将 pose 信息转换成 TransFormStamped
            6-3.发布
        7.spin

*/
//1.包含头文件
#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2/LinearMath/Quaternion.h"
#include "geometry_msgs/TransformStamped.h"
//保存乌龟名称
std::string turtle_name;


void doPose(const turtlesim::Pose::ConstPtr& pose){
    //  6-1.创建 TF 广播器 ---------------------------------------- 注意 static
    static tf2_ros::TransformBroadcaster broadcaster;
    //  6-2.将 pose 信息转换成 TransFormStamped
    geometry_msgs::TransformStamped tfs;
    tfs.header.frame_id = "world";
    tfs.header.stamp = ros::Time::now();
    tfs.child_frame_id = turtle_name;
    tfs.transform.translation.x = pose->x;
    tfs.transform.translation.y = pose->y;
    tfs.transform.translation.z = 0.0;
    tf2::Quaternion qtn;
    qtn.setRPY(0,0,pose->theta);
    tfs.transform.rotation.x = qtn.getX();
    tfs.transform.rotation.y = qtn.getY();
    tfs.transform.rotation.z = qtn.getZ();
    tfs.transform.rotation.w = qtn.getW();
    //  6-3.发布
    broadcaster.sendTransform(tfs);

} 

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ros 节点
    ros::init(argc,argv,"pub_tf");
    // 3.解析传入的命名空间
    if (argc != 2)
    {
        ROS_ERROR("请传入正确的参数");
    } else {
        turtle_name = argv[1];
        ROS_INFO("乌龟 %s 坐标发送启动",turtle_name.c_str());
    }

    // 4.创建 ros 句柄
    ros::NodeHandle nh;
    // 5.创建订阅对象
    ros::Subscriber sub = nh.subscribe<turtlesim::Pose>(turtle_name + "/pose",1000,doPose);
    //     6.回调函数处理订阅的 pose 信息
    //         6-1.创建 TF 广播器
    //         6-2.将 pose 信息转换成 TransFormStamped
    //         6-3.发布
    // 7.spin
    ros::spin();
    return 0;
}

• 订阅方

/*  
    订阅 turtle1 和 turtle2 的 TF 广播信息，查找并转换时间最近的 TF 信息
    将 turtle1 转换成相对 turtle2 的坐标，在计算线速度和角速度并发布

    实现流程:
        1.包含头文件
        2.初始化 ros 节点
        3.创建 ros 句柄
        4.创建 TF 订阅对象
        5.处理订阅到的 TF
        6.spin

*/
//1.包含头文件
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/Twist.h"

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ros 节点
    ros::init(argc,argv,"sub_TF");
    // 3.创建 ros 句柄
    ros::NodeHandle nh;
    // 4.创建 TF 订阅对象
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener listener(buffer);
    // 5.处理订阅到的 TF

    // 需要创建发布 /turtle2/cmd_vel 的 publisher 对象

    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel",1000);

    ros::Rate rate(10);
    while (ros::ok())
    {
        try
        {
            //5-1.先获取 turtle1 相对 turtle2 的坐标信息
            geometry_msgs::TransformStamped tfs = buffer.lookupTransform("turtle2","turtle1",ros::Time(0));

            //5-2.根据坐标信息生成速度信息 -- geometry_msgs/Twist.h
            geometry_msgs::Twist twist;
            twist.linear.x = 0.5 * sqrt(pow(tfs.transform.translation.x,2) + pow(tfs.transform.translation.y,2));
            twist.angular.z = 4 * atan2(tfs.transform.translation.y,tfs.transform.translation.x);

            //5-3.发布速度信息 -- 需要提前创建 publish 对象
            pub.publish(twist);
        }
        catch(const std::exception& e)
        {
            // std::cerr << e.what() << '\n';
            ROS_INFO("错误提示:%s",e.what());
        }



        rate.sleep();
        // 6.spin
        ros::spinOnce();
    }

    return 0;
}

rosbag

官方文档：rosbag - ROS Wiki

概念

是用于录制和回放 ROS 主题的一个工具集。

作用

实现了数据的复用，方便调试、测试。

本质

rosbag本质也是ros的节点，当录制时，rosbag是一个订阅节点，可以订阅话题消息并将订阅到的数据写入磁盘文件；当重放时，rosbag是一个发布节点，可以读取磁盘文件，发布文件中的话题消息。

使用方法

• 开始录制

rosbag record -a -O 目标文件
Copy

操作小乌龟一段时间，结束录制使用 ctrl + c，在创建的目录中会生成bag文件。

• 查看文件

rosbag info 文件名
Copy

• 回放文件

rosbag play 文件名
Copy

重启乌龟节点，会发现，乌龟按照录制时的轨迹运动。

• 也可以使用编码实现，具备灵活性

rqt工具箱

概念

ROS基于 QT 框架，针对机器人开发提供了一系列可视化的工具，这些工具的集合就是rqt

作用

可以方便的实现 ROS 可视化调试，并且在同一窗口中打开多个部件，提高开发效率，优化用户体验。

组成

rqt 工具箱组成有三大部分

• rqt——核心实现，开发人员无需关注
• rqt_common_plugins——rqt 中常用的工具套件
• rqt_robot_plugins——运行中和机器人交互的插件(比如: rviz)

重要的图形化插件

• rqt_graph：可视化显示计算图
• rqt_console：用于显示和过滤日志的图形化插件
• rqt_plot ：可以以 2D 绘图的方式绘制发布在 topic 上的数据
• rqt_bag：录制和重放 bag 文件的图形化插件

总结

• 坐标变换√

• rosbag调试√

• rqt的图形化调试√

  后续进入机器人系统仿真，实现复杂的功能，继续冲！！！！

rosbag record -a -O 目标文件
Copy

操作小乌龟一段时间，结束录制使用 ctrl + c，在创建的目录中会生成bag文件。

• 查看文件

rosbag info 文件名
Copy

• 回放文件

rosbag play 文件名
Copy

重启乌龟节点，会发现，乌龟按照录制时的轨迹运动。

• 也可以使用编码实现，具备灵活性

rqt工具箱

概念

ROS基于 QT 框架，针对机器人开发提供了一系列可视化的工具，这些工具的集合就是rqt

作用

可以方便的实现 ROS 可视化调试，并且在同一窗口中打开多个部件，提高开发效率，优化用户体验。

组成

rqt 工具箱组成有三大部分

• rqt——核心实现，开发人员无需关注
• rqt_common_plugins——rqt 中常用的工具套件
• rqt_robot_plugins——运行中和机器人交互的插件(比如: rviz)

重要的图形化插件

• rqt_graph：可视化显示计算图
• rqt_console：用于显示和过滤日志的图形化插件
• rqt_plot ：可以以 2D 绘图的方式绘制发布在 topic 上的数据
• rqt_bag：录制和重放 bag 文件的图形化插件

总结

• 坐标变换√

• rosbag调试√

• rqt的图形化调试√

  后续进入机器人系统仿真，实现复杂的功能，继续冲！！！！