使用 MoveIt 控制自己的真实机械臂【2】—

上一篇文章中，使用 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 数据作为指令数据发送给真实的机械臂去执行，这将在下一篇文章中描述。

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

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

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

使用 MoveIt 控制自己的真实机械臂【3】——优化轨迹发给真实机械臂执行
上一篇文章中,使用 MoveIt 控制自己的真实机械臂[2]--编写 action server 端代码,已经实现了 action server 端代码,并且拿到了 MoveIt 规划的轨迹数据.当然 ...
使用 MoveIt 控制自己的真实机械臂【4】——了解 MoveIt 的轨迹规划实现机制
上一节中我们提到了一个planning request adapters 的概念,在这一节中我们将对其展开详细了解. planning request adapters 是运动规划中 Motion P ...
ros melodic控制真实机械臂之获取moveit规划插补点
关于该点可查看前辈博客.本文对其中不一致的地方进行记录,但为了查阅方便,该文也记录了完整的操作步骤. 1.demo.launch文件中参数fake_execution的值改为false <arg ...
机器人学习必看系列：如何使用moveit控制真实机械臂？
大家好,我是你们可爱的小鱼.最近关于moveit相关的问题感觉非常多,毕竟机械臂+视觉的应用的确是非常的火爆,小鱼都想直接开课教机械臂运动规划相关的了. 有的同学问小鱼,怎么使用moveit控制真实机 ...
ros通过moveit控制真实机械臂
1.demo.launch文件在生成的demo.launch文件中,参数fake_execution的值改为false <include file="$(find tk7arm_mo ...
ur机械臂 + robotiq gripper + robotiq ft sensor + gazebo + 连接真实机械臂 + 网页控制
ur机械臂 + robotiq gripper + robotiq ft sensor + gazebo + 连接真实机械臂 + 网页控制仓库地址:[ur_ws](https://github.co ...
遨博机械臂——使用官方demo控制真实机械臂
机械臂在rviz中同步文章目录源码安装 1.设置工作目录 2.clone官方代码 3.安装MoveIt和industrial_core包 4.编译使用真实机械臂 1.获取正确的设置 shell ...
ros kinetic-moveit驱动ur3机械臂------控制真实机械臂并且能动
ros kinetic-moveit驱动ur3机械臂------控制真实机械臂并且能动本文工作环境配置: ubuntu16.04.6 ros-kinetic ur3 已验证本教程代码在Ubuntu1 ...
ros melodic控制真实机械臂之openmanipulator参考
目录一.源码下载及编译 1.相关参考网站 2.创建工作空间及源码下载二.对openmanipulator进行仿真前面的一段时间,针对自己的机器人做了模型配置,moveit配置等,但想真正的实现真 ...

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

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

最新文章

热门文章