MoveIt 控制自己的真实机械臂【2】——编写 action server 端代码

完成了 MoveIt 这边 action client 的基本配置，MoveIt 理论上可以将规划好的 trajectory 以 action 的形式发布出来了，浅浅尝试一下，在 terminal 中运行 roslaunch xmate7_moveit_config_new demo.launch

报错提示他在等待 xmate_arm_controller/follow_joint_trajectory 这个 action sever 的到来，显然，他等的好辛苦，却还是没有等来所期待的人，最终遗憾地告诉大家，以 xmate_arm_controller/follow_joint_trajectory 为 action 名称的 action client 端没有被连接。

此时，rostopic list 一下：

hjs@hjs:~/new_xmate7pro_ws$ rostopic list
/attached_collision_object
/collision_object
/execute_trajectory/cancel
/execute_trajectory/feedback
/execute_trajectory/goal
/execute_trajectory/result
/execute_trajectory/status
/head_mount_kinect/depth_registered/points
/joint_states
/move_group/cancel
/move_group/display_contacts
/move_group/display_cost_sources
/move_group/display_grasp_markers
/move_group/display_planned_path
/move_group/feedback
/move_group/filtered_cloud
/move_group/goal
/move_group/monitored_planning_scene
/move_group/motion_plan_request
/move_group/ompl/parameter_descriptions
/move_group/ompl/parameter_updates
/move_group/plan_execution/parameter_descriptions
/move_group/plan_execution/parameter_updates
/move_group/planning_scene_monitor/parameter_descriptions
/move_group/planning_scene_monitor/parameter_updates
/move_group/result
/move_group/sense_for_plan/parameter_descriptions
/move_group/sense_for_plan/parameter_updates
/move_group/status
/move_group/trajectory_execution/parameter_descriptions
/move_group/trajectory_execution/parameter_updates
/pickup/cancel
/pickup/feedback
/pickup/goal
/pickup/result
/pickup/status
/place/cancel
/place/feedback
/place/goal
/place/result
/place/status
/planning_scene
/planning_scene_world
/real_controller_joint_states
/recognized_object_array
/rosout
/rosout_agg
/rviz_hjs_12716_2381460729014530723/motionplanning_planning_scene_monitor/parameter_descriptions
/rviz_hjs_12716_2381460729014530723/motionplanning_planning_scene_monitor/parameter_updates
/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/feedback
/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update
/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update_full
/tf
/tf_static
/trajectory_execution_event

发现根本没有 xmate_arm_controller/follow_joint_trajectory 这个名称的 action 发布出来。
想想也能理解，MoveIt 端的 action client 苦苦等了好久的另一半(action server)，action server 却终究没有出现和给出回应，那么action client自然也就，没有必要再将自己的爱意(xmate_arm_controller/follow_joint_trajectory) 表达出来了。

言归正传，之前在 Gazebo 中虚拟仿真的时候，是 Gazebo 的 ros_control 插件充当了 action server 的角色，让 MoveIt 端的 client 和 Gazebo 端的 server 建立了 action 通信连接；那么现在，针对真实机械臂，没有人再充当 action server 的角色了，自然也就没有 follow_joint_trajectory 发布出来，所以下一步要做的就是自己编写一个 Movet 端的 action client 所倾慕的 action server ，并为它们牵线(配置接口)，介绍它们认识(建立连接)，进而实现 follow_joint_trajectory 的发布和接收。

关于 ROS action 相关知识的学习自行补上，我编写的 action server 节点如下：

# include <ros/ros.h>
# include <actionlib/server/simple_action_server.h>
# include <control_msgs/FollowJointTrajectoryAction.h>
# include <std_msgs/Float32MultiArray.h>
# include <iostream>
# include <moveit_msgs/RobotTrajectory.h>using namespace std;
// 重命名类型为 Server
typedef actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> Server;// 用于存储 moveit 发送出来的轨迹数据
moveit_msgs::RobotTrajectory moveit_tra;void execute_callback(const control_msgs::FollowJointTrajectoryGoalConstPtr& goalPtr, Server* moveit_server)
{// 1、解析提交的目标值int n_joints = goalPtr->trajectory.joint_names.size();int n_tra_Points = goalPtr->trajectory.points.size();moveit_tra.joint_trajectory.header.frame_id = goalPtr->trajectory.header.frame_id;moveit_tra.joint_trajectory.joint_names = goalPtr->trajectory.joint_names;moveit_tra.joint_trajectory.points.resize(n_tra_Points);for(int i=0; i<n_tra_Points; i++) // 遍历每组路点{moveit_tra.joint_trajectory.points[i].positions.resize(n_joints);moveit_tra.joint_trajectory.points[i].velocities.resize(n_joints);moveit_tra.joint_trajectory.points[i].accelerations.resize(n_joints);moveit_tra.joint_trajectory.points[i].time_from_start = goalPtr->trajectory.points[i].time_from_start;for(int j=0;j<n_joints; j++) // 遍历每组路点中的每个关节数据{moveit_tra.joint_trajectory.points[i].positions[j] = goalPtr->trajectory.points[i].positions[j];moveit_tra.joint_trajectory.points[i].velocities[j] = goalPtr->trajectory.points[i].velocities[j];moveit_tra.joint_trajectory.points[i].accelerations[j] = goalPtr->trajectory.points[i].accelerations[j];}}cout << "The trajectory data is:" << "********************************************" << endl;cout << moveit_tra;cout << "********************************************" << "The trajectory data is finished printing." << endl;ROS_INFO("The number of joints is %d.",n_joints);ROS_INFO("The waypoints number of the trajectory is %d.",n_tra_Points);ROS_INFO("Receive trajectory successfully");moveit_server->setSucceeded();
}int main(int argc, char *argv[])
{ros::init(argc,argv,"moveit_action_server");ros::NodeHandle nh;// 创建 action 对象(NodeHandle，话题名称，回调函数解析传入的目标值，服务器是否自启动)Server moveit_server(nh,"xmate_arm_controller/follow_joint_trajectory", boost::bind(&execute_callback, _1, &moveit_server), false);// 手动启动服务器moveit_server.start();ros::spin();return 0;
}

该节点实现的功能就是：

启动了一个 xmate_arm_controller/follow_joint_trajectory 名称的 action server，而正是这个名称的 action，才是 MoveIt 端的 action client 所喜欢的，具体原因见上一篇博客5、创建 ros_controllers.yaml 文件；
在启动的 action server 回调函数中解析了 MoveIt 规划的轨迹数据，并把该轨迹数据存储到了 moveit_msgs::RobotTrajectory 类型的变量 moveit_tra 中。
验证：

terminal 1:roscore;
terminal 2: 启动 action server 节点，rosrun xmate7_demo moveit_action_server；
此时 rostopic list 结果：

terminal 3: 启动 action client 节点，roslaunch xmate7_moveit_config_new demo.launch；
此时，终端不会报错，而且提示
[ INFO] [1658643437.543320231]: Added FollowJointTrajectory controller for xmate_arm_controller
将 Rviz 中的拖动球拖动，点击 Plan & Excute 会发现 terminal 2会按照我所写代码打印出 MoveIt 的轨迹数据。

 

可见，MoveIt 规划的轨迹，让机械臂从 home 姿态运动到目标姿态生成了 33 个路点，对应每个路点的位置、速度、加速度以及时间戳也都解析到了 moveit_tra 这个变量中了，以上验证表明我们自己实现的 action server 代码实现了和 MoveIt 端 action client 的 connect，并且拿到了 MoveIt 规划出来的轨迹数据。顺其自然，为了实现用 MoveIt 控制自己的真实机械臂的宏图大业，当然下一步就是将我们拿到的 trajectory 数据作为指令数据发送给真实的机械臂去执行，这将在下一篇文章中描述。

另外，上述示例 action server 代码实现方式比较简单，还可以将 action server 封装成类的形式，在类中编写回调函数，丰富 action 机制的使用接口，在类中初始化 action 对象可参照action 官网