ROS实践笔记14

ROS常用组件

在ROS中内置一些比较实用的工具，通过这些工具可以方便快捷的实现某个功能或调试程序，从而提高开发效率。

1. TF坐标变换，实现不同类型的坐标系之间的转换：

   注意： ROS使用右手坐标系

   tf2常用的功能包有：

   • tf2_geometry_msgs:可以将ROS消息转换成tf2消息。
   • tf2: 封装了坐标变换的常用消息。
   • tf2_ros:为tf2提供了roscpp和rospy绑定，封装了坐标变换常用的API。

   1. 坐标msg消息

      订阅发布模型中数据载体 msg 是一个重要实现，首先需要了解一下，在坐标转换实现中常用的 msg:geometry_msgs/TransformStamped和geometry_msgs/PointStamped

      前者用于传输坐标系相关位置信息，后者用于传输某个坐标系内坐标点的信息。在坐标变换中，频繁的需要使用到坐标系的相对关系以及坐标点信息。

      1. geometry_msgs/TransformStamped

         打开终端输入：

         rosmsg info geometry_msgs/TransformStamped
         

         得到：

         std_msgs/Header header
         uint32 seq      //序列化号
         time stamp      //时间戳
         string frame_id     //主体坐标系名称
         string child_frame_id   //相对的坐标系
         geometry_msgs/Transform transform
         geometry_msgs/Vector3 translation
             float64 x     //相对坐标系在x，y，z上的偏移量
             float64 y      //对于主体坐标系而言 
             float64 z
         geometry_msgs/Quaternion rotation
             float64 x     //在相对方向角度的偏移
             float64 y       //及偏移，巡航，俯仰角
             float64 z
             float64 w
         

      2. geometry_msgs/PointStamped
         打开终端输入：

         rosmsg info geometry_msgs/PointStamped
         

         得到：

         std_msgs/Header header
         uint32 seq
         time stamp          //时间戳
         string frame_id     //属于那个坐标系
         geometry_msgs/Point point
         float64 x
         float64 y
         float64 z
         
         

   2. 静态坐标变换

      1. 创建功能包
         需要的依赖 ：

         tf2 tf2_ros tf2_geometry_msgs roscpp rospy  std_msgs geometry_msgs
         

      2. 编码实现

         发布方

         c++

         //实现静态坐标变换的发布方
         //发布两个坐标系的相对关系
         # 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,"");
         
             ros::init(argc,argv,"static_pub_c");
             ros::NodeHandle nh;
             tf2_ros::StaticTransformBroadcaster pub;
             geometry_msgs::TransformStamped tfs;
         
             tfs.header.stamp = ros::Time::now();
             tfs.header.frame_id = "car_link";
             tfs.child_frame_id = "radar_link";
             tfs.transform.translation.x = 0.2;
             tfs.transform.translation.y = 0.0;
             tfs.transform.translation.z = 0.5;
             tf2::Quaternion qtn; //创建四元数
             //向该对象设置哦啦较，这个对象可以将哦啦较转换为四元数
             qtn.setRPY(0,0,0); //欧拉角的单位是弧度
             tfs.transform.rotation.x = qtn.getX();
             tfs.transform.rotation.y = qtn.getY();
             tfs.transform.rotation.z = qtn.getZ();
             tfs.transform.rotation.w = qtn.getW();
         
             pub.sendTransform(tfs);
             ros::spin();
             return 0;
         }
         

         python

         #! /usr/bin/env python
         
         import rospy
         import tf2_ros
         import tf.transformations
         from geometry_msgs.msg import TransformStamped
         
         
         if __name__ == "__main__":
             rospy.init_node("static_pub_p")
             pub = tf2_ros.StaticTransformBroadcaster()
             ts = TransformStamped()
             
             ts.header.stamp = rospy.Time.now()
             ts.header.frame_id = "car_link"
             ts.child_frame_id = "radar_link"
             ts.transform.translation.x = 2.0
             ts.transform.translation.y = 0.0
             ts.transform.translation.z = 0.5
         
             qtn = tf.transformations.quaternion_from_euler(0,0,0)
         
             ts.transform.rotation.x = qtn[0]
             ts.transform.rotation.y = qtn[1]
             ts.transform.rotation.z = qtn[2]
             ts.transform.rotation.w = qtn[3]
             
             pub.sendTransform(ts)
             
             rospy.spin()
             
         

         订阅方

         c++

         # 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"
         
         
         int main(int argc, char *argv[])
         {
             setlocale(LC_ALL,"");
             ros::init(argc,argv,"static_sub_c");
             ros::NodeHandle nh;
             tf2_ros::Buffer buffer;
             tf2_ros::TransformListener listener(buffer);
             geometry_msgs::PointStamped p;
             p.header.frame_id = "radar_link";
             p.header.stamp = ros::Time::now();
             p.point.x = 2.0;
             p.point.y = 3.0;
             p.point.z = 5.0;
             ros::Rate rate(1);
             while(ros::ok())
             {
                 geometry_msgs::PointStamped np;
                 try
                 {
                     np = buffer.transform(p,"car_link");
                     ROS_INFO("转换后的坐标值:(%.2f %.2f %.2f)",np.point.x,np.point.y,np.point.z);
                     ROS_INFO("相对的坐标系是:%s",np.header.frame_id.c_str());
                 }
                 catch(const std::exception& e)
                 {
                     std::cerr << e.what() << '\n';
                 }
                 rate.sleep();
                 ros::spinOnce();
             }
             return 0;
         }
         

         python

         #! /usr/bin/env python
         
         import rospy
         import tf2_ros
         from tf2_geometry_msgs import tf2_geometry_msgs
         
         
         
         if __name__=="__main__":
             rospy.init_node("static_sub_p")
             # 创建缓存对象
             buffer = tf2_ros.Buffer()
             # 将缓存对象导入订阅对象
             sub = tf2_ros.TransformListener(buffer)
             p = tf2_geometry_msgs.PointStamped()
             p.header.stamp = rospy.Time.now()
             p.header.frame_id = "radar_link"
             p.point.x = 2.0
             p.point.y = 3.0
             p.point.z = 5.0
             rate = rospy.Rate(1)
             while not rospy.is_shutdown():
                 try:
                     np=buffer.transform(p,"car_link")
                     rospy.loginfo("转换后的坐标为(%.2f,%.2f,%.2f)",np.point.x,np.point.y,np.point.z)
                     rospy.loginfo("对应的坐标系为:%s",np.header.frame_id)
                 except Exception as e:
                     rospy.logerr("未找到坐标")
                 rate.sleep() 
         

      3. 通过命令行命令可以直观看到坐标系相对位置

         rviz
         

   3. 动态坐标变换

      启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘控制乌龟运动，将两个坐标系的相对位置动态发布。

      功能包依赖：

      tf2 tf2_ros tf2_geometry_msgs roscpp rospy std_msgs geometry_msgs turtlesim
      

      发布方：

      c++

      # 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)
      {
          static tf2_ros::TransformBroadcaster pub;
          geometry_msgs::TransformStamped ts;
          ts.header.frame_id = "world";
          ts.header.stamp = ros::Time::now();
          ts.child_frame_id = "turtle";
          ts.transform.translation.x = pose->x;
          ts.transform.translation.y = pose->y;
          ts.transform.translation.z = 0.0;
          /*
          由于乌龟不能沿x轴y轴转动，所以可以直接通过乌龟位姿得到欧拉角
          0 0 theta
          */
      tf2::Quaternion qtn;
      qtn.setRPY(0,0,pose->theta);
      ts.transform.rotation.x=qtn.getX();
      ts.transform.rotation.y=qtn.getY();
      ts.transform.rotation.z=qtn.getZ();
      ts.transform.rotation.w=qtn.getW();
      pub.sendTransform(ts);
      }
      
      int main(int argc, char *argv[])
      {
          ros::init(argc,argv,"dynamic_pub_c");
          ros::NodeHandle nh;
          ros::Rate rate(10);
          ros::Subscriber sub=nh.subscribe("/turtle1/pose",100,DoPose);
          rate.sleep();
          ros::spin();
          return 0;
      }
      
      

      python

      #! /usr/bin/env python
      
      import rospy
      import tf2_ros
      import tf.transformations
      from turtlesim.msg import Pose
      from geometry_msgs.msg import TransformStamped
      
      def DoPose(pose):
          pub = tf2_ros.TransformBroadcaster()
          ts = TransformStamped()
          
          ts.header.stamp = rospy.Time.now()
          ts.header.frame_id = "world"
          ts.child_frame_id = "turtle"
          ts.transform.translation.x = pose.x
          ts.transform.translation.y = pose.y
          ts.transform.translation.z = 0
          
          qtn = tf.transformations.quaternion_from_euler(0,0,pose.theta)
          
          ts.transform.rotation.x = qtn[0]
          ts.transform.rotation.y = qtn[1]
          ts.transform.rotation.z = qtn[2]
          ts.transform.rotation.w = qtn[3]
      
          pub.sendTransform(ts)
          
      if __name__ == "__main__" :
          rospy.init_node("dynamic_pub_p")
          rate = rospy.Rate(10)
          sub = rospy.Subscriber("/turtle1/pose",Pose,DoPose,queue_size = 100)
          rate.sleep()
          rospy.spin()
          
      

      订阅方：

      c++

      # 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"
      
      
      int main(int argc, char *argv[])
      {
          setlocale(LC_ALL,"");
          ros::init(argc,argv,"dynamic_sub_c");
          ros::NodeHandle nh;
          tf2_ros::Buffer buffer;
          tf2_ros::TransformListener listener(buffer);
          geometry_msgs::PointStamped p;
          p.header.frame_id = "turtle";
          //防止时间不同，发生异常，需要时间初始化为0
          p.header.stamp = ros::Time(0.0);
          p.point.x = 0.0;
          p.point.y = 0.0;
          p.point.z = 0.0;
          ros::Rate rate(1);
          while(ros::ok())
          {
              geometry_msgs::PointStamped np;
              try
              {
                  np = buffer.transform(p,"world");
                  ROS_INFO("转换后的坐标值:(%.2f %.2f %.2f)",np.point.x,np.point.y,np.point.z);
                  ROS_INFO("相对的坐标系是:%s",np.header.frame_id.c_str());
              }
              catch(const std::exception& e)
              {
                  std::cerr << e.what() << '\n';
              }
              rate.sleep();
              ros::spinOnce();
          }
          return 0;
      }
      
      

      python

      #! /usr/bin/env python
      
      import rospy
      import tf2_ros
      from tf2_geometry_msgs import tf2_geometry_msgs
      
      
      
      if __name__=="__main__":
          rospy.init_node("dynamic_sub_p")
          # 创建缓存对象
          buffer = tf2_ros.Buffer()
          # 将缓存对象导入订阅对象
          sub = tf2_ros.TransformListener(buffer)
          p = tf2_geometry_msgs.PointStamped()
          p.header.stamp = rospy.Time()
          p.header.frame_id = "turtle"
          p.point.x = 0.0
          p.point.y = 0.0
          p.point.z = 0.0
          rate = rospy.Rate(10)
          while not rospy.is_shutdown():
              try:
                  np=buffer.transform(p,"world")
                  rospy.loginfo("转换后的坐标为(%.2f,%.2f,%.2f)",np.point.x,np.point.y,np.point.z)
                  rospy.loginfo("对应的坐标系为:%s",np.header.frame_id)
              except Exception as e:
                  rospy.logerr("未找到坐标")
              rate.sleep() 
      

   4. 多坐标变换

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

      1. 通过launch发布坐标关系

      <launch>
          <node pkg = "tf2_ros" type = "static_transform_publisher" name = "son1" args = "5 0 0 0 0 0 /world /son1" output = "screen"/>
          <node pkg = "tf2_ros" type = "static_transform_publisher" name = "son2" args = "0 0 7 0 0 0 /world /son2" output = "screen"/>
      </launch>
      

      2. 订阅方编写

         c++

         # 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"
         # include"geometry_msgs/TransformStamped.h"
         
         
         int main(int argc, char *argv[])
         {
             setlocale(LC_ALL,"");
             ros::init(argc,argv,"many_sub_c");
             ros::NodeHandle nh;
             tf2_ros::Buffer buffer;
             tf2_ros::TransformListener listener(buffer);
             geometry_msgs::PointStamped p;
             p.header.frame_id = "son1";
             p.header.stamp = ros::Time::now();
             p.point.x = 1.0;
             p.point.y = 2.0;
             p.point.z = 5.0; 
             ros::Rate rate(1);
             while(ros::ok())
             {
                 geometry_msgs::PointStamped np;
                 try
                 {
                     //坐标系转换
                     //time(0)取发布间隔时间最短的两个坐标方位
                     geometry_msgs::TransformStamped tfs = buffer.lookupTransform("son2","son1",ros::Time(0));
                     ROS_INFO("主体坐标系:%s",tfs.header.frame_id.c_str());
                     ROS_INFO("相对坐标系:%s",tfs.child_frame_id.c_str());
                     ROS_INFO("偏移量:(%.2f,%.2f,%.2f)",tfs.transform.translation.x,tfs.transform.translation.y,tfs.transform.translation.z);
                     
                     //坐标点转换
                     np = buffer.transform(p,"son2");
                     ROS_INFO("son1坐标点在son2中的值(%.2f,%.2f,%.2f)",np.point.x,np.point.y,np.point.z);
                 }
                 catch(const std::exception& e)
                 {
                     std::cerr << e.what() << '\n';
                 }
                 rate.sleep();
                 
             }
             return 0;
         }
         

         python

         #! /usr/bin/env python
         
         import rospy
         import tf2_ros
         from tf2_geometry_msgs import tf2_geometry_msgs
         from geometry_msgs.msg import TransformStamped
         
         
         if __name__=="__main__":
             rospy.init_node("many_sub_p")
             # 创建缓存对象
             buffer = tf2_ros.Buffer()
             # 将缓存对象导入订阅对象
             sub = tf2_ros.TransformListener(buffer)
             p = tf2_geometry_msgs.PointStamped()
             p.header.stamp = rospy.Time.now()
             p.header.frame_id = "son1"
             p.point.x = 2.0
             p.point.y = 3.0
             p.point.z = 5.0
             rate = rospy.Rate(1)
             while not rospy.is_shutdown():
                 try:
                     # 坐标系转换
                     tfs=buffer.lookup_transform("son2","son1",rospy.Time(0))
                     rospy.loginfo("主体坐标系:%s",tfs.header.frame_id)
                     rospy.loginfo("相对坐标系:%s",tfs.child_frame_id)
                     rospy.loginfo("偏移量:(%.2f,%.2f,%.2f)",tfs.transform.translation.x,tfs.transform.translation.y,tfs.transform.translation.z)
                     
                     # 坐标点转换
                     np=buffer.transform(p,"son2")
                     rospy.loginfo("转换后的坐标为(%.2f,%.2f,%.2f)",np.point.x,np.point.y,np.point.z)
                     rospy.loginfo("对应的坐标系为:%s",np.header.frame_id)
                 except Exception as e:
                     rospy.logerr("未找到坐标")
                 rate.sleep()
         

   5. 坐标系关系查看

      在机器人系统中，涉及的坐标系有多个，为了方便查看，ros 提供了专门的工具，可以用于生成显示坐标系关系的 pdf 文件，该文件包含树形结构的坐标系图谱。

      1. 准备
         首先调用rospack find tf2_tools查看是否包含该功能包，如果没有，请使用如下命令安装:

         sudo apt install ros-noetic-tf2-tools
         

      2. 使用

         1. 生成 pdf 文件
            启动坐标系广播程序之后，运行如下命令:

            rosrun tf2_tools view_frames.py
            

         2. 查看文件

            可以直接进入目录打开文件，或者调用命令查看文件:evince frames.pdf

   6. 通过命令行发布坐标系关系

      语法：rosrun tf2_ros static_transform_publisher x偏移量 y偏移量 z偏移量 z偏航角度 y俯仰角度 x翻滚角度 父级坐标系 子级坐标系

      rosrun tf2_ros static_transform_publisher 0.2 0 0.5 0 0 0 /baselink /laser
      

2. rosbag 用于录制ROS节点的执行过程并可以回放该过程：

   录制：把数据储存进磁盘文件

   回放：从磁盘文件中读取数据

   1. 命令行使用

      录制

      rosbag record -a -o turtle.bag
      

      查看

      rosbag info turtle_2022-02-20-16-26-26.bag 
      

      回放

      rosbag play turtle_2022-02-20-16-26-26.bag 
      

   2. 编码使用

      录制

      c++

      # include"ros/ros.h"
      # include"rosbag/bag.h"
      # include"std_msgs/String.h"
      
      std_msgs::String msg;
      
      
      void DoMsg(const std_msgs::String::ConstPtr &Msg)
      {
          msg.data = Msg->data;
          return ;
      }
      
      int main(int argc, char *argv[])
      {
          setlocale(LC_ALL,"");
          ros::init(argc,argv,"writing_c");
          ros::NodeHandle nh;
          rosbag::Bag bag;
          //打开文件流
          //Append 追加   Read 读    Write 覆盖
          bag.open("turtle.bag",rosbag::BagMode::Write);
          ros::Subscriber sub = nh.subscribe("topic",10,DoMsg);
          bag.write("/topic",ros::Time::now(),msg);
          bag.close();
          
          ros::spinOnce();
          return 0;
      }
      

      python

      #! /usr/bin/env python
      
      
      import rospy
      import rosbag
      from std_msgs.msg import String
      
      
      if __name__ == "__main__":
          rospy.init_node("writing_p")
          bag = rosbag.Bag("turtle2.bag","w")
          msg = String()
          msg.data = "hello"
          
          bag.write("topic",msg)
          
          bag.close()
      

      回放

      c++

      # include"ros/ros.h"
      # include"rosbag/bag.h"
      # include"rosbag/view.h"
      # include"std_msgs/String.h"
      
      int main(int argc, char *argv[])
      {
          setlocale(LC_ALL,"");
          ros::init(argc,argv,"read_c");
          ros::NodeHandle nh;
          rosbag::Bag bag;
          bag.open("turtle.bag",rosbag::BagMode::Read);
      
          for(auto &&p : rosbag::View(bag))
          {
              //View(bag) 消息的集合
              //解析
              std::string str = p.getTopic();
              ros::Time time = p.getTime();
              auto m = p.instantiate<std_msgs::String>();
              ROS_INFO("解析的内容,话题:%s,时间戳:%.2f,内容:%s",str.c_str(),time.toSec(),m->data.c_str());
          }
          bag.close();
          return 0;
      }
      

      python

      #! /usr/bin/env python
      
      
      import rospy
      import rosbag
      from std_msgs.msg import String
      
      
      
      
      if __name__ == "__main__":
          
          rospy.init_node("read_p")
          bag = rosbag.Bag("turtle2.bag","r")
          
          mags = bag.read_messages("topic")
          
          for topic,msg,time in mags:
              rospy.loginfo("话题 %s 消息 %s 时间 %s",topic,msg.data,time)
          
          bag.close()
      

3. rqt 工具箱，集成了多款图形化的调试工具：

   之前，在 ROS 中使用了一些实用的工具,比如: ros_bag 用于录制与回放、tf2_tools 可以生成 TF 树 ..... 这些工具大大提高了开发的便利性，但是也存在一些问题: 这些工具的启动和使用过程中涉及到一些命令操作，应用起来不够方便，在ROS中，提供了rqt工具箱，在调用工具时以图形化操作代替了命令操作，应用更便利，提高了操作效率，优化了用户体验。

   1. rqt安装启动与基本使用

      一般只要你安装的是desktop-full版本就会自带工具箱

      如果需要安装可以以如下方式安装

      $ sudo apt-get install ros-noetic-rqt
      $ sudo apt-get install ros-noetic-rqt-common-plugins
      

      启动

      两个方式：

      rqt
      rosrun rqt_gui rqt_gui
      

      基本使用

      启动 rqt 之后，可以通过 plugins 添加所需的插件

   2. rqt常用插件:rqt_graph

      rqt_graph可视化显示计算图

      启动:可以在 rqt 的 plugins 中添加，或者使用rqt_graph启动

      圆形是节点，方形是话题

   3. rqt常用插件:rqt_console

      rqt_console 是 ROS 中用于显示和过滤日志的图形化插件

      可以在 rqt 的 plugins 中添加，或者使用rqt_console启动

   4. rqt常用插件:rqt_plot

      图形绘制插件，可以以 2D 绘图的方式绘制发布在 topic 上的数据

      可以在 rqt 的 plugins 中添加，或者使用rqt_plot启动

   5. rqt常用插件:rqt_bag

      录制和重放 bag 文件的图形化插件

      可以在 rqt 的 plugins 中添加，或者使用rqt_bag启动