ros melodic控制真实机械臂之获取moveit规划插补点

        关于该点可查看前辈博客。本文对其中不一致的地方进行记录，但为了查阅方便，该文也记录了完整的操作步骤。

1、demo.launch文件中参数fake_execution的值改为false

  <arg name="fake_execution" value="false"/>

2、修改moveit_controller_manager参数

       根据前辈博客中描述为，move_group.launch文件中，moveit_controller_manager在选择参数值时，“unless”前面那个velue值要修改，写一个自己机器人名称作为前缀。但如果已经是自己的机器人名称，可以不用修改。

  <arg name="moveit_controller_manager" value="csrobot" unless="$(arg fake_execution)"/>

3、修改csrobot_moveit_controller_manager.launch.xml文件

       根据前辈博客中描述，修改如下。

<launch>
 
    <!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
    <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
    <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
 
    <arg name="use_controller_manager" default="true" />
    <param name="use_controller_manager" value="$(arg use_controller_manager)" />
 
    <!-- load controller_list -->
    <rosparam file="$(find csrobot_moveit_config)/config/controllers.yaml"/>
 
</launch>

但需要补充的是，在该版本中运行demo.launch文件会报如下错误。

RLException: unused args [execution_type] for include of [/home/lee/CSRobot/src/csrobot_moveit_config/launch/csrobot_moveit_controller_manager.launch.xml]
The traceback for the exception was written to the log file

解决该错误可参考连接。需要在csrobot_moveit_controller_manager.launch.xml中声明excution_type。即csrobot_moveit_controller_manager.launch.xml内容如下。

<launch>

  <!-- loads moveit_controller_manager on the parameter server which is taken as argument
    if no argument is passed, moveit_simple_controller_manager will be set -->
  <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
  <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
  
  <arg name="execution_type" />
  <arg name="use_controller_manager" default="true" />
  <param name="use_controller_manager" value="$(arg use_controller_manager)" />
  
  <!-- loads ros_controllers to the param server -->
  <rosparam file="$(find csrobot_moveit_config)/config/ros_controllers.yaml"/>
</launch>

4、创建controllers.yaml配置文件      

        该版本中已经生成由ros_controllers.yaml文件，csrobot_moveit_controller_manager.launch.xml文件中也是调用的这个文件，这里不用重新生成。

 5、解决真实机械臂与joint_states_publisher消息冲突的问题

        根据前辈博文中描述，当控制真实机械臂时，moveit的配置文件中，demo.launch文件中关于joint_states_publisher的一段代码需要注释掉，因为该程序也在发布机械臂关节状态，而且是一个模拟的状态，与真实机械臂状态是不相符的，若不注释掉该代码，rviz下显示的机械臂状态总是在不停的跳变,将原本的注释，使用真实的传感器发布的/joint_states来控制。

  <!-- We do not have a robot connected, so publish fake joint states -->
  <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 -->
    <rosparam param="/source_list">[/joint_states]</rosparam >
  </node>

6、解决真实机械臂轨迹执行时间超时的问题

        根据前辈博文中描述，moveit配置文件中有几个参数还需要添加上，否则后续真实机械臂执行轨迹时会报超时的错误。需要在move_group.launch文件中增加几个参数，来延长允许执行轨迹的时间。 找到 所对应的标签组，在其中加入以下参数。

<param name="trajectory_execution/allowed_execution_duration_scaling" value="6"/>      //允许轨迹执行时间的一个放大倍数，可以根据实际情况自行修改
    <param name="trajectory_execution/allowed_goal_duration_margin" value="0.5"/>            //超时的一个百分比范围
    或者加入下面的参数，直接关掉监视轨迹执行状况的一个monitoring
    <param name="trajectory_execution/execution_duration_monitoring" value="false"/>

7、创建一个新的功能包

catkin_create_pkg trajectory_server roscpp std_msgs actionlin actionlib_msgs

然后在该功能包src文件夹下写入cpp文件，内容如前辈博文中所示。

#include <ros/ros.h>
#include <iostream>
#include <actionlib/server/simple_action_server.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <std_msgs/Float32MultiArray.h>
 
#include <sstream>
#include <stdlib.h>
#include <sys/types.h>
#include <stdio.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <string.h>
#include <signal.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <errno.h>
#include <pthread.h>
 
using namespace std;
 
typedef actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> Server;
 
std::basic_string<char> joint1;
std::basic_string<char> joint2;
std::basic_string<char> joint3;
std::basic_string<char> joint4;
std::basic_string<char> joint5;
std::basic_string<char> joint6;
 
/* 目标位置 */
double cp1;
double cp2;
double cp3;
double cp4;
double cp5;
double cp6;
 
/* 目标位置 */
double tp1;
double tp2;
double tp3;
double tp4;
double tp5;
double tp6;
 
/* 收到action的goal后调用的回调函数 */
void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server* as)
{
	//ros::Rate rate(1);
 
    joint1 = goal->trajectory.joint_names[0];
    joint2 = goal->trajectory.joint_names[1];
    joint3 = goal->trajectory.joint_names[2];
    joint4 = goal->trajectory.joint_names[3];
    joint5 = goal->trajectory.joint_names[4];
    joint6 = goal->trajectory.joint_names[5];
 
    cp1 = goal->trajectory.points[0].positions[0];
    cp2 = goal->trajectory.points[0].positions[1];
    cp3 = goal->trajectory.points[0].positions[2];
    cp4 = goal->trajectory.points[0].positions[3];
    cp5 = goal->trajectory.points[0].positions[4];
    cp6 = goal->trajectory.points[0].positions[5];
 
    tp1 = goal->trajectory.points[1].positions[0];
    tp2 = goal->trajectory.points[1].positions[1];
    tp3 = goal->trajectory.points[1].positions[2];
    tp4 = goal->trajectory.points[1].positions[3];
    tp5 = goal->trajectory.points[1].positions[4];
    tp6 = goal->trajectory.points[1].positions[5];
 
	// 并且按照1hz的频率发布进度feedback
	// control_msgs::FollowJointTrajectoryFeedback feedback;
    //feedback = NULL;
    //as->publishFeedback(feedback);
 
    // 当action完成后，向客户端返回结果
    //printf("goal=[%f,%f,%f,%f,%f,%f]\n",tp1,tp2,tp3,tp4,tp5,tp6);
    ROS_INFO("Recieve action successful!");
    as->setSucceeded();
}
 
 
/* 主函数主要用于动作订阅和套接字通信 */
int main(int argc, char** argv)
{
	ros::init(argc, argv, "trajectory_server");
	ros::NodeHandle nh;
	// 定义一个服务器
	Server server(nh, "csrobot/follow_joint_trajectory", boost::bind(&execute, _1, &server), false);
	// 服务器开始运行
	server.start();
	ros::spin();
 
}

8、编译功能包

        需要注意的是，用solidworks生成urdf文件时，不要将关节1命名为j1，否则会出现以下编译时会出现以下错误。

/home/lee/ros/urdf_test/src/action_server/src/main.cpp:24:25: error: ‘std::__cxx11::basic_string<char> j1’ redeclared as different kind of symbol
 std::basic_string<char> j1;
......
/usr/include/x86_64-linux-gnu/bits/mathcalls.h:218:1: note: previous declaration ‘double j1(double)’
 __MATHCALL (j1,, (_Mdouble_));

 9、查看action内容

        这里可查看该博文，注意运行顺序。然后在rviz中拖动末端，然后点击plan&Excute就可以在第三个窗口中显示插补数据，如下图。

参考：

1、https://blog.csdn.net/qq_34935373/article/details/95916111

2、https://blog.csdn.net/qq_45398213/article/details/109631149

版权声明：本文为qq_35813161原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接和本声明。