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

黄小白的进阶之路

已于 2022-07-27 22:02:34 修改

阅读量4.5k

点赞数 14

分类专栏： moveit 文章标签： ubuntu MoveIt ROS Action

于 2022-07-24 14:32:32 首次发布

本文链接：https://blog.csdn.net/huangjunsheng123/article/details/125958436

版权

moveit 专栏收录该内容

17 篇文章 82 订阅

订阅专栏

上一篇文章中，使用 MoveIt 控制自己的真实机械臂【1】——配置 action client 端，已经完成了 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 数据作为指令数据发送给真实的机械臂去执行，这将在下一篇文章中描述。