ROS入门(三)——常用功能与总结

ROS入门(三)——常用功能与总结
　　iwehdio的博客园：https://www.cnblogs.com/iwehdio/

1、坐标管理（tf）

• 用于坐标变化，计算各个坐标系之间的关系。

• 运行坐标变换例程：

  $ roscore
  $ sudo apt-get install ros-melodic-turtle-tf
  $ roslaunch turtle_tf turtle_tf_demo.launch
  $ rosrun turtlesim turtle_teleop_key
  $ rosrun tf view_frames
  

  • 最后一句会生成 tf 框架的 pdf 。

• 动态查询二者的坐标关系：

  $ rosrun tf tf_echo turtle1 turtle2
  $ rosrun rviz rviz d rospack find turtle_tf rviz turtle_rviz rviz
  

• 实现坐标广播：

  • 创建功能包：

    $ catkin_create_pkg learning_tf roscpp rospy tf turtlesim
    

  • 实现 TF 广播器：

    • 定义 TF 广播器。

    • 订阅第一只海龟的位置信息。

    • 创建坐标变换值。

    • 发布坐标变换。

    • C++ 实现：

      /**
       * 该例程产生tf数据，并计算、发布turtle2的速度指令
       */
      
      #include <ros/ros.h>
      #include <tf/transform_broadcaster.h>
      #include <turtlesim/Pose.h>
      
      std::string turtle_name;
      
      void poseCallback(const turtlesim::PoseConstPtr& msg)
      {
      	// 创建tf的广播器
      	static tf::TransformBroadcaster br;
      
      	// 初始化tf数据
      	tf::Transform transform;
      	transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );	//平移坐标
      	tf::Quaternion q;
      	q.setRPY(0, 0, msg->theta);							//旋转坐标
      	transform.setRotation(q);
      
      	// 广播world与海龟坐标系之间的tf数据
      	br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
      }
      
      int main(int argc, char** argv)
      {
          // 初始化ROS节点
      	ros::init(argc, argv, "my_tf_broadcaster");
      
      	// 输入参数作为海龟的名字
      	if (argc != 2)
      	{
      		ROS_ERROR("need turtle name as argument"); 
      		return -1;
      	}
      
      	turtle_name = argv[1];
      
      	// 订阅海龟的位姿话题
      	ros::NodeHandle node;
      	ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
      
          // 循环等待回调函数
      	ros::spin();
      
      	return 0;
      };
      

    • Python 实现：

      #!/usr/bin/env python
      # -*- coding: utf-8 -*-
      # 该例程将请求/show_person服务，服务数据类型learning_service::Person
      
      import roslib
      roslib.load_manifest('learning_tf')
      import rospy
      
      import tf
      import turtlesim.msg
      
      def handle_turtle_pose(msg, turtlename):
          br = tf.TransformBroadcaster()
          br.sendTransform((msg.x, msg.y, 0),
                           tf.transformations.quaternion_from_euler(0, 0, msg.theta),
                           rospy.Time.now(),
                           turtlename,
                           "world")
      
      if __name__ == '__main__':
          rospy.init_node('turtle_tf_broadcaster')
          turtlename = rospy.get_param('~turtle')
          rospy.Subscriber('/%s/pose' % turtlename,
                           turtlesim.msg.Pose,
                           handle_turtle_pose,
                           turtlename)
          rospy.spin()
      

  • 实现坐标监听：

    • 定义 TF 监听器。

    • 查找坐标变换。

    • 将运动指令发布给第二只海龟。

    • C++ 实现：

      /**
       * 该例程监听tf数据，并计算、发布turtle2的速度指令
       */
      
      #include <ros/ros.h>
      #include <tf/transform_listener.h>
      #include <geometry_msgs/Twist.h>
      #include <turtlesim/Spawn.h>
      
      int main(int argc, char** argv)
      {
      	// 初始化ROS节点
      	ros::init(argc, argv, "my_tf_listener");
      
          // 创建节点句柄
      	ros::NodeHandle node;
      
      	// 请求产生turtle2
      	ros::service::waitForService("/spawn");
      	ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");
      	turtlesim::Spawn srv;
      	add_turtle.call(srv);
      
      	// 创建发布turtle2速度控制指令的发布者
      	ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 10);
      
      	// 创建tf的监听器
      	tf::TransformListener listener;
      
      	ros::Rate rate(10.0);
      	while (node.ok())
      	{
      		// 获取turtle1与turtle2坐标系之间的tf数据
      		tf::StampedTransform transform;
      		try
      		{
      			listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));
      			listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
      		}
      		catch (tf::TransformException &ex) 
      		{
      			ROS_ERROR("%s",ex.what());
      			ros::Duration(1.0).sleep();
      			continue;
      		}
      
      		// 根据turtle1与turtle2坐标系之间的位置关系，发布turtle2的速度控制指令
      		geometry_msgs::Twist vel_msg;
      		vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),
      				                        transform.getOrigin().x());
      		vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
      				                      pow(transform.getOrigin().y(), 2));
      		turtle_vel.publish(vel_msg);
      
      		rate.sleep();
      	}
      	return 0;
      };
      

    • Python 实现：

      #!/usr/bin/env python
      # -*- coding: utf-8 -*-
      # 该例程将请求/show_person服务，服务数据类型learning_service::Person
      
      import roslib
      roslib.load_manifest('learning_tf')
      import rospy
      import math
      import tf
      import geometry_msgs.msg
      import turtlesim.srv
      
      if __name__ == '__main__':
          rospy.init_node('turtle_tf_listener')
      
          listener = tf.TransformListener()
      
          rospy.wait_for_service('spawn')
          spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
          spawner(4, 2, 0, 'turtle2')
      
          turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist,queue_size=1)
      
          rate = rospy.Rate(10.0)
          while not rospy.is_shutdown():
              try:
                  (trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))
              except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                  continue
      
              angular = 4 * math.atan2(trans[1], trans[0])
              linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
              cmd = geometry_msgs.msg.Twist()
              cmd.linear.x = linear
              cmd.angular.z = angular
              turtle_vel.publish(cmd)
      
              rate.sleep()
      

    • C++ 的编译规则：

      add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)
      target_link_libraries(turtle_tf_broadcaster ${catkin_LIBRARIES})
      add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)
      target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES})
      

    • C++ 命令行：

      $ roscore
      $ rosrun turtlesim turtlesim_node
      $ rosrun learning_tf turtle_tf_broadcaster __name:=turtle1_tf_broadcaster /turtle1
      $ rosrun learning_tf turtle_tf_broadcaster __name:=turtle2_tf_broadcaster /turtle2
      $ rosrun learning_tf turtle_tf_listener
      $ rosrun turtlesim turtle_teleop_key
      

      • 其中最长的两行是通过重映射，复用了TF广播程序并且修改了节点名。

• launch 启动文件：

  • XML 文件格式。加入后也需要先编译工程。
  • 启动 launch 文件：roslaunch 功能包名 launch文件名。会自动运行 roscore 命令。
  • 根标签：<launch>。
  • 启动节点：<node>。
    • pkg：节点所在的功能包名称。
    • type：节点的可执行文件名称。
    • name：节点运行时的名称。
    • output 是否输出日志、respawn 意外关闭是否重启、required 是否必须启动、ns 命名空间、args 输入的参数。
  • 存储 ROS 参数：<param>。
    • name：参数名。
    • value：参数值。
  • 从文件加载参数：<rosparam file=filename command="load" ns="param">。
  • launch 文件内部参数：<arg name="arg-name" default="arg-value">。
    • 被调用时形式为 "$(arg arg-name)"。
  • 重映射：<remap>。
    • from：原命名。
    • to：映射之后的命名。
  • 嵌套：<include>。
    • file：包含其他 launch 文件路径，类似头文件。

• launch 文件实例：

  • 话题的订阅者和发布者。

    <launch>
        <node pkg="learning_topic" type="person_subscriber" name="talker" output="screen" />
        <node pkg="learning_topic" type="person_publisher" name="listener" output="screen" /> 
    </launch>
    

  • 小海龟参数配置：

    <launch>
    	<param name="/turtle_number"   value="2"/>
        <node pkg="turtlesim" type="turtlesim_node" name="turtlesim_node">
    		<param name="turtle_name1"   value="Tom"/>
    		<param name="turtle_name2"   value="Jerry"/>
    		<rosparam file="$(find learning_launch)/config/param.yaml" command="load"/>
    	</node>
        <node pkg="turtlesim" type="turtle_teleop_key" name="turtle_teleop_key" output="screen"/></launch>
    

    • param 在 node 中时，命名会变成 node节点名 / param名称。都是 name 属性。
    • rosparam 中的 $(find learning_launch) 表示查找这个功能包并返回绝对路径。
    • 如果是从别的文件中 include 中，可以用 名称： 加命名空间。也会在参数名前加命名空间名。

  • 使用 tf 小海龟追逐（C++）：

    <launch>
        <!-- Turtlesim Node-->
        <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
        <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
        <node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle1" name="turtle1_tf_broadcaster" />
        <node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle2" name="turtle2_tf_broadcaster" />
        <node pkg="learning_tf" type="turtle_tf_listener" name="listener" />
    </launch>
    

  • 引入其他文件并重映射：

    • remap：

      <launch>
      	<include file="$(find learning_launch)/launch/simple.launch" />
          <node pkg="turtlesim" type="turtlesim_node" name="turtlesim_node">
      		<remap from="/turtle1/cmd_vel" to="/cmd_vel"/>
      	</node>
      </launch>
      

    • simple：

      <launch>
          <node pkg="learning_topic" type="person_subscriber" name="talker" output="screen" />
          <node pkg="learning_topic" type="person_publisher" name="listener" output="screen" /> 
      </launch>
      

2、可视化工具

• Qt 工具箱：
  • 日志输出工具：rqt_console。
  • 计算图可视化工具： rqt_graph。
  • 数据绘图工具： rqt_plot。
  • 图像渲染工具：rqt _image_view。
  • 所有工具集成：rqt。
• Rviz：三维可视化工具。
  • 通过 xml 对可视化属性进行配置。
  • 启动 rviz：先启动 roscore，再输入rosrun rviz rviz。
• Gazebo：三维物理仿真平台。
  • 相比 Rviz，可以创建仿真数据，有环境模型库。
  • 启动 Gazebo：roslaunch gazebo_ros 环境配置文件。
• 其他工具包：
  • 机器人控制算法：ros_control。
  • 即时定位与地图建模：gamapping / hector。
  • 机械臂：moveit。

3、ROS编程流程总结（python）

1、话题

• 在 Python 中 ROS 节点初始化中的节点名、函数名要一致。

• 需要先import rospy。

• 发布话题的流程：

  1. ROS节点初始化：rospy.init_node('发布者名', anonymous=True)。

  2. 创建发布者：rospy.Publisher('话题名', 消息类型, queue_size=10)。

     • 返回一个发布者对象。
     • 消息类型如 Twist 在 geometry_msgs.msg 包下。

  3. 循环终止条件：while not rospy.is_shutdown()，即ROS系统开启。

  4. 初始化消息类型：

     • 以 Twist 为例，创建消息对象：

       vel_msg = Twist()		# 首先构造方法创建对象
       vel_msg.linear.x = 0.5		# 修改成员变量
       vel_msg.angular.z = 0.2
       

  5. 发布者发布消息：发布者对象.publish(消息对象)。

  6. 按一定间隔发布消息：

     #设置循环的频率
     rate = rospy.Rate(一秒发送的次数) 
     # 按照循环频率延时
     rate.sleep()
     

  7. 在主函数中抛出异常：rospy.ROSInterruptException。

  8. 打印日志信息：rospy.loginfo()。

• 订阅话题的流程：

  1. ROS节点初始化：rospy.init_node('订阅者名', anonymous=True)。
  2. 创建订阅者：rospy.Subscriber('话题名', 消息类型, 回调函数)。
     • 订阅者接收到话题后会执行回调函数，回调函数传入的是订阅得到的消息：def xxxCallback(msg)。
  3. 循环等待回调函数：rospy.spin()。在这之前阻塞。

2、服务

• 同样的，在 Python 中 ROS 节点初始化中的节点名、函数名要一致。

• 客户端请求服务的流程：

  1. ROS节点初始化：rospy.init_node('客户端名')。
  2. 等待发现服务： rospy.wait_for_service('服务名')。
     • 发现服务后的操作都在try..except...中完成。
     • 抛出rospy.ServiceException异常。
  3. 创建一个客户端进行连接：rospy.ServiceProxy('服务名', 消息类型)。
     • 返回一个客户端对象。
  4. 输入请求参数：客户端对象(请求数据)。
     • 返回一个服务响应response。
  5. 请求服务调用：return response。

• 服务器响应服务的流程：

  1. 导入依赖：

     import rospy
     import thread,time
     from geometry_msgs.msg import Twist		# 话题发布消息类型
     from std_srvs.srv import Trigger, TriggerResponse
     						# 请求消息类型、响应消息类型
     

  2. ROS节点初始化：rospy.init_node('服务器名')。

  3. 创建一个服务，注册回调函数：rospy.Service('服务名', 请求消息类型, 回调函数)。

  4. 循环等待回调函数，通过启动多线程：

     thread.start_new_thread(command_thread, ())
     rospy.spin()
     

     • 在函数外，全局声明一个话题发布者：rospy.Publisher('话题名', 消息类型, queue_size=10)。

     • 在线程中判断是否有请求，有请求后发布话题。

       def command_thread():	
       	while True:
       		if pubCommand:
       			vel_msg = Twist()
       			vel_msg.linear.x = 0.5
       			vel_msg.angular.z = 0.2
       			turtle_vel_pub.publish(vel_msg)
       		time.sleep(0.1)
       

  5. 回调函数中，每次收到服务请求后，将请求标志位置反。

     def commandCallback(req):
     	global pubCommand
     	pubCommand = bool(1-pubCommand)
     	# 显示请求数据
     	rospy.loginfo("Publish turtle velocity command![%d]", pubCommand)
     	# 反馈数据（响应消息类型）
     	return TriggerResponse(1, "Change turtle command state!")
     

3、操作全局参数

• 操作全局参数流程：
  1. ROS节点初始化：rospy.init_node('服务器名')。
  2. 读取全局参数：rospy.get_param('参数名')。
  3. 设置全局参数：rospy.set_param('参数名', 参数值)。

参考：古月ROS入门21讲：https://www.bilibili.com/video/BV1zt411G7Vn?p=21

iwehdio的博客园：https://www.cnblogs.com/iwehdio/