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

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

——————来源于结束于否定之否定。——————