devbox
我家自动化
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

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

作者：我家自动化 | 2024-06-11 15:43:45

赞

踩

moveit控制源码

上一篇文章中， 使用 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

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56

会发现根本没有 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;
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63

该节点实现的功能就是：

  • 启动了一个 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 官网。

参考文章：
CSDN_合工大机器人实验室：通过ROS控制真实机械臂(5)—Moveit!真实机械臂(move_group和demo.launch修改)
CSDN_爱学习的草莓熊：使用moveit！控制真实机械臂（5）——编写真实机械臂节点中的action服务程序

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/我家自动化/article/detail/703791
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号