ROS中Moveit生成的轨迹如何作用于实际的机器人(三)

joint_state_publisher

前面两篇文档已经分析了moveit使用Action方式与实际的关节控制器进行通信并控制关节电机旋转，现在我们再来分析一下关节状态的返回。仍然以moveit提供的panda_moveit_config包为例。查看demo.launch文件，可以看到里面有如下内容：

<arg name="use_gui" default="false" />
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
	<param name="/use_gui" value="$(arg use_gui)"/>
	<rosparam param="/source_list">[/move_group/fake_controller_joint_states]rosparam>
node>

上面表示启动ros自带的joint_state_publisher节点。先不看其里面的参数是什么意思。我们先来介绍一下joint_state_publisher包的作用。

joint_state_publisher包通过发布sensor_msgs/JointState消息来告知其它节点机器人关机的状态,对应的topic名称为joint_states。该包通过读取参数服务器上的/robot_description来获得机器人的所有非固定关节。它也可以联合robot_state_publisher来发布所有关节状态的变换（TF格式），这个就不在这里论述了。

那么可能需要思考，joint_state_publisher的数据哪里来呢？主要有四个可能的来源：

1. 从GUI直接输入的值。当use_gui参数为true时，joint_state_publisher自身就会启动一个有着关节滑块的小窗口。通过滑动滑块就可以作为关节的状态输出了。我觉得这应该仅仅限于仿真使用。不过也有人通过这个滑块反过来控制硬件，也就是说订阅joint_state_publisher的joint_states来当中运动的指令，这种方法不值得提倡。
2. 设置source_list参数，将其值设定为某个topic的名称，通过订阅这个topic来获得关节的状态信息。
3. 依赖的关节。这个对机械臂来说用不上，暂不讨论。
4. 默认值，也就是零。一般也不会用这个。

好吧，现在就应该很清楚demo.launch文件中joint_state_publisher的参数是什么意思了吧。其/use_gui参数的值为false，表示不从关节滑块GUI获得关节状态。从参数服务器获得"/source_list"参数的值，如果没有则使用方括号中的内容。这里是从/move_group/fake_controller_joint_states主题来获取关节状态的，注意并不是一定是move_group发布。

再就是要问：move_group节点发布的fake_controller_joint_states主题到底包含怎样的关节状态信息呢。在panda_moveit_config的launch和config目录中都没有找到关于fake_controller_joint_states的设定。但是在启动demo.launch后，确实可以通过rostopic命令看到fake_controller_joint_states主题。为什么这样，通过下面的实验来进一步分析。

实验一

然后，我们进行一个简单的实验，将demo.launch中的fake_execution设为true。然后在四个终端中依次执行：

#Terminal 1
$ roslaunch panda_moveit_config demo.launch
#Terminal 2
$ roslaunch moveit_tutorials move_group_interface_tutorial.launch后
#Terminal 3
$ rostopic echo /move_group/fake_controller_joint_states
#Terminal 4
$ rostopic echo /joint_states

可以看到，fake_controller_joint_states（由move_group发布）没有输出，而joint_states（由joint_state_publisher发布）一直输出，但其中的关节值是恒定不变的。当在RvizVisualToolsGui中点击第一次Next后，rviz中的机器人开始显示轨迹规划运动，但fake_controller_joint_states仍没有输出，joint_states内容仍然恒定。这说明仅仅只是轨迹规划并不会影响实际的关节的值，在rviz中的关节运动仅仅只是move_group通过插件直接控制rviz的结果。

当第二次按"Next"的时候，记得之前我们已经将move_group_interface_tutorial代码中的"move_group.move()"行去掉了注释，也就是说让机器人（不管是虚拟的还是真实的）做实际的轨迹运动。这时可以看到fake_controller_joint_states根据运动不断地输出关节运动信息，而joint_states也是随着做同样的变化的。同时可以看到rviz中关节也完成了真正的运动。

通过分析使用的fake_controller_manager代码可以发现其会发布fake_controller_joint_states主题，然后虚拟地发布关节状态信息。

这里有一个疑问，当不使用fake_controller_manager时也会存在fake_controller_joint_states主题，应该是moveit_group也会发布，难道不冲突？还是后面的发布会自动变为订阅？ 带着这个疑问，将demo.launch中的fake_controller_joint_states改为了test_controller_joint_states，再次启动demo.launch，果然查看到的topic也变为了test_controller_joint_states。也就是说joint_state_publisher需要的来源topic有则订阅，没有则创建之，只要有其它node在它上面发布消息就可以了。

实验二

再将fake_execution设为false，然后连上当作硬件控制器的树莓派。然后依次再终端中执行：

### Raspberry Terminal
$ ./ros_motor_control
### PC Terminal 1
$ rosrun trajectory_test trajectory_test_node
### PC Terminal 2
$ roslaunch panda_moveit_config demo.launch
### PC Terminal 3
$ roslaunch moveit_tutorials move_group_interface_tutorial.launch
### PC Terminal 4
$ rostopic echo /move_group/fake_controller_joint_states
### PC Terminal 5
$ rostopic echo /joint_states

当第二次点击RvizVisualToolsGui中的"Next"按钮来触发"move_group.move()"语句时，发现fake_controller_joint_states（也可改为任意想要的名称
）没有输出，joint_states的内容也不变，同时rviz的关节也并不会产生运动。
现在我们分析下原因。在move_group中会使用simple_controller_manager来与实际硬件通信，但其代码中并没有与joint_states相关的任何实现。因此fake_controller_joint_states主题上也不会有任何消息。如果是这样，那么我们可以编写一点代码在fake_controller_joint_states主题上发布关节状态信息，看是否可让joint_state_publisher节点读到关节信息。这在下一节中进行论述。

实验三–真实的关节状态返回

为了验证上一节的判断，对原树莓派以及PC端的ActionServer节点上的代码进行一些扩充。

对于PC端的ActionServer节点，初始化时就创建名为/move_group/fake_controller_joint_states的topic的publisher，然后通过asio_tcp一直处于侦听来自树莓派的关节状态数据。当有来自树莓派的状态数据时，则转换为sensor_msgs::JointState消息发布到/move_group/fake_controller_joint_states上。

对于树莓派，在generate_pulses()函数中，每发送完一段脉冲（也就是说关节走到了指定的轨迹点）时，向PC端的ActionServer节点发送关节状态数据。

按照实验二的步骤再次运行，可以监测到/move_group/fake_controller_joint_states上随着脉冲的产生（意味着电机的转动）会出现变化关节位置状态信息。同时joint_state_publisher节点也是在一直监测/move_group/fake_controller_joint_states，所以也会随即将这些信息发布到/joint_states主题上。这时rviz监测到了/joint_states主题上的变化，使得机器人关节按照反馈回来位置信息进行实际的运动。

下面是整个ROS各节点之间以及与树莓派之间数据通信的框图