在rviz中自由的摩擦

rviz是ROS系统中的一个3D图形显示工具。我们在介绍URDF和机器人模型^{1,
2}的时候就用rviz来查看用URDF来描述的机器人模型的。在本文中，我们从官方例程urdf_tutorial中的display.launch出发，研究如何在用户程序中，控制rviz中的机器人运动。

我们延续上一篇文章的配置，假定在工作空间下有一个叫做learning_urdf的package，其中有一个urdf的子目录，里面保存了一个叫做GRobot.urdf的文件。这个urdf文件描述了一个有23个自由度的双足机器人。我们使用urdf_tutorial中的display.launch来查看一下这个模型：

        $ roscd learning_urdf
        $ roslaunch urdf_tutorial display.launch model:=urdf/GRobot.urdf gui:=true

右下角中的joint_state_publisher工具栏中，对应每个自由度都有一个滑块。拖动滑块可以控制对应的关节转动，如果我们把rviz中模拟的对象替换成一台真正的机器人，那么就可以实际控制它运动了。所以我们要先研究一下display.launch都做了一些什么工作，然后实现一个我们自己的控制器。

1. display.launch

这个display.launch文件就在urdf_tutorial下的launch子目录中，其内容如下：

        <launch>
            <arg name="model" />
            <arg name="gui" default="False" />
            <param name="robot_description" textfile="$(arg model)" />
            <param name="use_gui" value="$(arg gui)"/>
            <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
            <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
            <node name="rviz" pkg="rviz" type="rviz" args="-d $(find urdf_tutorial)/urdf.rviz" required="true" />
        </launch>

其中的第2,3行声明了两个参数model和gui，我们在运行该launch文件的时候通过这两个参数来指定机器人模型，控制是否打开工具栏。我们通过model:=urdf/GRobot.urdf来指定模型文件，在第4行中通过textfile将文件内容加载到参数服务器中的"robot_description"字段下，供各有需要的节点使用。在第5行中，把参数gui:=true保存到参数服务器的"use_gui"字段下，它用于指引joint_state_publisher节点是否创建一个GUI窗口。

第6行中运行了一个叫做"joint_state_publisher"的节点，它是一个python的程序。这个程序发布了一个叫做"joint_states"的主题，其数据类型是"sensor_msgs/JointState"。这是一个描述机器人各个关节状态的主题，它被第7行中运行的"robot_state_publisher"订阅。运行如下指令可以看到，如下图2所示的运行时图关系。

        $ rqt_graph

图2. joint_states主题的发布与订阅关系

"robot_state_publisher"就是一个tf广播器，它是一个C++的程序，订阅了"joint_states"主题，计算各个坐标系之间的变换关系，并将之广播出去。运行如下指令，可以得到如下图3所示的GRobot坐标系树型结构。

        $ rosrun rqt_tf_tree rqt_tf_tree

第8行中运行的rviz则是一个tf监听器，它接受tf的变换广播，并更新视图。

图3. GRobot的坐标系

2. 主题"joint_states"

通过对display.launch的分析，我们知道从拖动工具栏中的滑块，到rviz中机器人的模型发生运动，中间经过了如下的几个过程：

节点joint_state_publisher发布一条sensor_msgs/JointState的消息到主题joint_states;
节点robot_state_publisher接收到主题joint_states的消息，根据消息内容更新GRobot各个坐标系状态，并通过tf广播器机制发布;
rviz监听到GRobot坐标系发生变化，更新视图显示。

可以说，这一系列动作都是由主题"joint_states"触发的。我们可以通过rostopic和rosmsg工具查看主题和消息类型的详细内容：

        $ rostopic info /joint_states 
        Type: sensor_msgs/JointState
        
        Publishers: 
         * /joint_state_publisher (http://gyc:35490/)
        
        Subscribers: 
         * /robot_state_publisher (http://gyc:41828/)

        $ rosmsg info sensor_msgs/JointState 
        std_msgs/Header header
          uint32 seq
          time stamp
          string frame_id
        string[] name
        float64[] position
        float64[] velocity
        float64[] effort

可以看到主题的发布者只有joint_state_publisher，订阅者是robot_state_publisher。消息类型是sensor_msgs/JointState，它由header, name, position, velocity和effort五个部分构成。其中header是一个复合的结构，它的stamp字段就是一个时间戳，对于本例而言是重要的。剩下的四个字段都是数组，在C++语言中都是以std::vector的形式实现的，所以我们可以直接roscpp的接口中获取数组长度。 name是关节(joint)的名称;position是关节的位置消息，量纲为弧度(rad)或者米(m);velocity是关节的速度信息，量纲为rad/s或者m/s;effor是关节的力矩或者力信息，量纲为Nm或者N。它们都是一一对应的，也就是说name[i], position[i]描述的是同一个关节的名字和位置信息。为了监听该主题，我们实现了一个joint_state_listener的节点:

        #include <ros/ros.h>
        #include <sensor_msgs/JointState.h>
        #include <iostream>
        
        void callback(const sensor_msgs::JointState::ConstPtr &msg) {
            std::cout << "===========================================================================" << std::endl;
            std::cout << msg->name.size() << "\t" << msg->position.size() << "\t";
            std::cout << msg->velocity.size() << "\t" << msg->effort.size() << std::endl;
            int n = msg->name.size();
            for (int i = 0; i < n; i++) {
                std::cout << msg->name[i] << ":" << msg->position[i] << std::endl;
            }
        }
        
        int main(int argc, char *argv[]) {
            ros::init(argc, argv, "joint_state_listener");
            ros::NodeHandle n;
        
            ros::Subscriber sub = n.subscribe("joint_states", 1000, callback);
            ros::spin();
        }

在该joint_state_listener中，我们订阅了"joint_states"并在接收到消息后的回调函数中把消息的数据内容都打印出来了，运行效果如下：

        $ rosrun learning_urdf joint_state_listener 
        ===========================================================================
        23  23  0   0
        base_to_neck_z:0
        base_to_left_shoulder_y:0
        left_shoulder_y_to_left_shoulder_x:0
        left_shoulder_x_to_left_shoulder_z:0
        left_upper_arm_to_left_elbow_y:0
        left_elbow_y_to_left_elbow_z:0
        base_to_right_shoulder_y:0
        right_shoulder_y_to_right_shoulder_x:0
        right_shoulder_x_to_right_shoulder_z:0
        right_upper_arm_to_right_elbow_y:0
        right_elbow_y_to_right_elbow_z:0
        base_to_left_hip_z:0
        left_hip_z_to_left_hip_y:0
        left_hip_y_to_left_hip_x:0
        left_thigh_to_left_knee_y:0
        left_shank_to_left_ankle_y:0
        left_ankle_y_to_left_ankle_x:0
        base_to_right_hip_z:0
        right_hip_z_to_right_hip_y:0
        right_hip_y_to_right_hip_x:0
        right_thigh_to_right_knee_y:0
        right_shank_to_right_ankle_y:0
        right_ankle_y_to_right_ankle_x:0

在打印的数据中，第一行依次列出了接收消息中name, position, velocity, effort的元素数量，其中name和position有23个，其余为0。这23正好对应着我们机器人的23个自由度，没有速度和力的信息。其余各行中的数据表示对应关节的位置都为0，如果拖动了工具栏我们会看到对应行也会发生变化。

这就说明我们只需要发布一个joint_states的主题，通过sensor_msgs/JointState列出各个关节的位置、速度、力的信息就可以通过ROS的工具控制一台机器人。下面我们写一个joint_state_publisher来控制GRobot在rviz中不停地扭动。

3. joint_state_publisher

joint_state_publisher的完整代码可以在这里下载。首先包含必要的头文件，ros/ros.h提供了访问ROS系统内核的接口， sensor_msgs/JointState.h则是我们要发布的joint_states主题的消息类型描述，urdf/model.h提供了访问urdf模型的接口，iostream则是C++中的标准输入输出流，程序中可能用到的std::cout就是通过该头文件访问的。

        #include <ros/ros.h>
        #include <sensor_msgs/JointState.h>
        #include <urdf/model.h>
        #include <iostream>

在main函数中，我们先完成ros节点的初始化并注册发布"joint_states"的主题。

        int main(int argc, char *argv[]) {
            ros::init(argc, argv, "joint_state_publisher");
            ros::NodeHandle n;
        
            ros::Publisher pub = n.advertise("joint_states", 1000);
            ros::Rate loop_rate(10);

接着，我们从参数服务器中获取GRobot的模型，并创建urdf模型对象。

            std::string urdf_file_name;
            urdf::Model model;
            if (n.getParam("robot_description", urdf_file_name)) {
                std::cout << "urdf_file_name:" << urdf_file_name << std::endl;
                if (!model.initString(urdf_file_name)) {
                    ROS_ERROR("Failed to parse urdf file");
                    return -1;
                }
            }

从urdf模型中把不是固定连接的关节找出来，因为我们在建立GRobot的时候只用了Fixed和Continuous两种类型的Joint，所以这里不再做更细的区分。

            std::vector joints;
            for (auto it = model.joints_.begin(); it != model.joints_.end(); it++) {
                urdf::JointSharedPtr joint = it->second;
                if (urdf::Joint::FIXED != joint->type) {
                    joints.push_back(joint);
                }
            }

在一个while循环中不断的发布消息，这里和官方教程一样不考虑速度和力的因素，只是单纯的控制所有的关节转动相同的角度。

            double position = 0.0;
            while (ros::ok()) {
                sensor_msgs::JointState msg;
                msg.header.stamp = ros::Time::now();
                for (auto it = joints.begin(); it != joints.end(); it++) {
                    msg.name.push_back((*it)->name);
                    msg.position.push_back(position);
                }
                position = (position > 6.28) ? 0 : position + 0.01;
        
                pub.publish(msg);
                ros::spinOnce();
                loop_rate.sleep();
            }
            return 0;
        }

这个程序依赖于urdf和sensor_msgs两个包，所以我们需要修改learning_urdf的package.xml文件，添加相应的依赖项:

        <build_depend>urdf</build_depend>
        <build_depend>sensor_msgs</build_depend>
        <run_depend>urdf</run_depend>
        <run_depend>sensor_msgs</run_depend>

并在CMakeLists.txt中添加编译规则。

        find_package(catkin REQUIRED COMPONENTS roscpp rospy urdf sensor_msgs)
        ...
        add_executable(joint_state_publisher src/joint_state_publisher.cpp)
        target_link_libraries(joint_state_publisher ${catkin_LIBRARIES})

然后回到工作空间根目录下进行编译:

        $ cd ~/catkin_ws/
        $ catkin_make
        $ source ./devel/setup.bash
        $ roscd learning_urdf

我们从urdf_tutorial中把display.launch拷到learning_urdf的launch目录下，做如下修改，使用我们新建的joint_state_publisher替换官方例程的:

        <launch>
            <arg name="model" />
            <param name="robot_description" textfile="$(arg model)" />
            <node name="joint_state_publisher" pkg="learning_urdf" type="joint_state_publisher" />
            <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
            <node name="rviz" pkg="rviz" type="rviz" args="-d $(find urdf_tutorial)/urdf.rviz" required="true" />
        </launch>

然后运行该launch文件，就可以看到GRobot在rviz中的魔鬼身姿，不停地扭动...

        $ roslaunch learning_urdf display.launch model:=urdf/GRobot.urdf

4. 总结

官方例程urdf_tutorial中通过display.launch实现对于任意给定的机器人模型的显示和简单控制。一共涉及到三个节点之间的消息通信:

节点joint_state_publisher发布sensor_msgs/JointState的消息到主题joint_states上;
节点robot_state_publisher则是一个tf广播器，订阅joint_states，并根据消息内容更新GRobot坐标系状态，通过tf广播器发布;
节点rviz则是一个tf监听器，监听到坐标变化后，就控制更新图形显示。

为研究这一过程，本文创建了joint_state_listener和joint_state_publisher两个例程，实现了用户程序对rviz中的机器人模型的控制。