前提条件，已经根据URDF文件生成了Moveit功能包，并可以正常使用Rviz仿真。
一、修改Moveit生成的文件
1、./launch/demo.launch
<!-- Choose controller manager: fake, simple, or ros_control -->
<arg name="moveit_controller_manager" default="fake" />找到这一句，把fake改成simple。
感觉这一步随便改也行吧，但是simple和ros_control有预留好的接口，比较方便。如果喜欢自己起名记得之后仔细检查每个文件，都把名字改过来。
<group if="$(eval arg('moveit_controller_manager') == 'fake')">
<!-- We do not have a real robot connected, so publish fake joint states via a joint_state_publisher
MoveIt's fake controller's joint states are considered via the 'source_list' parameter -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" unless="$(arg use_gui)">
<rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
</node>
<!-- If desired, a GUI version is available allowing to move the simulated robot around manually
This corresponds to moving around the real robot without the use of MoveIt. -->
<node name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" if="$(arg use_gui)">
<rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
</node>
<!-- Given the published joint states, publish tf for the robot links -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
</group>这一段是留给Rviz模拟的，虚拟的机械臂会发布关节值和机器人状态，在Rviz里面显示。
上面一步已经把fake改成simple，这一段应该不会运行，保险起见还是注释掉吧，然后在下面加一行代码，发布机器人状态，关节值让真实的机械臂发布。
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />二、./config/simple_moveit_controllers.yaml
为什么要改这个文件呢?因为把控制器从fake改成simple后，demo.launch包含的的文件就从fake_moveit_controller_manager.launch.xml变为simple_moveit_controller_manager.launch.xml了，有兴趣可以看一眼前者里面是什么，后者如下。
<launch>
<!-- Define the MoveIt controller manager plugin to use for trajectory execution -->
<param name="moveit_controller_manager" value="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
<!-- Load controller list to the parameter server -->
<rosparam file="$(find manipulator_moveit)/config/simple_moveit_controllers.yaml" />
<rosparam file="$(find manipulator_moveit)/config/ros_controllers.yaml" />
</launch>这里面就显示控制器参数文件是simple_moveit_controllers.yaml和ros_controllers.yaml，暂时不知道为什么包含了两个，总之改前面一个吧，这个文件也预留好了（上面要是自己起的名字，这种文件就要自己创建一个了，可能还有其它的bug，没试过）。
进去之后发现应该是空的，改成下面这样就行。
controller_list:
- name: Manipulator_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: True
joints:
- Joint_1
- Joint_2
- Joint_3
- Joint_4
- Joint_5
- Joint_6有几个注意事项：
（1）name:应该可以随便写的，我暂时没遇到什么问题。
（2）action_ns:这个不能改，涉及到action通信话题，具体原因可以去看看别的文。
（3）type：暂时用FollowJointTrajectory，有其他种类，用到再学。
（4）joints：关节名字别打错了，可以去fake_moveit_controller_manager.yaml里面抄。
三、action服务器
改完之后，要自己编一个action服务器代码，实现的功能包括接受Moveit的轨迹规划路点，实时发布真实机械臂的关节位姿，把路点发布给真实机械臂执行。
由于我目前的机械臂仅支持单节点上、下位机通信，所以我把这三个功能集成到同一个节点了（因为这三个功能都要和真实机械臂通信才能实现），如果真实机械臂支持多节点通信，可以分不同节点实现。
服务器代码如下，这个由于要包含不同的库函数，借鉴一下思路就行了，硬抄没有用。
# 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>
#include "std_msgs/String.h"
#include "Eigen/Dense"
#include "Eigen/Core"
#include "Eigen/Geometry"
#include "Eigen/StdVector"
#include "robot_msgs/Move.h"
#include "std_srvs/Empty.h"
#include "std_srvs/SetBool.h"
#include "robot_msgs/RobotMsg.h"
#include "robot_msgs/SetUserFrame.h"
#include "robot_msgs/SetTcp.h"
#include "robot_msgs/SetLoad.h"
#include "robot_msgs/ServoL.h"
#include "robot_msgs/ClearErr.h"
#include "robot_msgs/SetCollision.h"
#include "robot_msgs/SetAxis.h"
#include "sensor_msgs/JointState.h"
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/TwistStamped.h"
#include <pthread.h>
#include <unistd.h>
#include <sstream>
#include <vector>
#include "libs/robot.h"
#include "libs/conversion.h"
#include "time.h"
#include <map>
#include <string>
using namespace std;
JAKAZuRobot robot;
// 重命名类型为 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();
JointValue* joint_pos1 = new JointValue[n_tra_Points];
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];
}
joint_pos1[i] =
{
moveit_tra.joint_trajectory.points[i].positions[0],
moveit_tra.joint_trajectory.points[i].positions[1],
moveit_tra.joint_trajectory.points[i].positions[2],
moveit_tra.joint_trajectory.points[i].positions[3],
moveit_tra.joint_trajectory.points[i].positions[4],
moveit_tra.joint_trajectory.points[i].positions[5],
};
// robot.joint_move(&joint_pos1, ABS, FALSE, 2,20,5,NULL);
}
std::cout << "The trajectory data is:" << "********************************************" << endl;
std::cout << moveit_tra;
std::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");
BOOL in_pos;
for (int k = 0; k < n_tra_Points; k++)
{
robot.joint_move(&joint_pos1[k], ABS, FALSE,20,5,1,NULL);
}
// while (1)
// {
// robot.is_in_pos(&in_pos);
// if (in_pos == TRUE)
// {
// PoseStop.publish(in_pos);
// break;
// }
// }
delete [] joint_pos1;
moveit_server->setSucceeded();
}
void joint_states_callback(ros::Publisher joint_states_pub)
{
// clock_t t1 = clock();
sensor_msgs::JointState joint_states;
JointValue joint_pose;
// joint_states.position.clear(); // clear position vector
robot.get_joint_position(&joint_pose); // get current joint pose
//
// joint_pose.jVal[0] = 0;
// joint_pose.jVal[1] = 1;
// joint_pose.jVal[2] = 2;
// joint_pose.jVal[3] = 3;
// joint_pose.jVal[4] = 4;
// joint_pose.jVal[5] = 5;
//
for(int i = 0; i < 6; i++)
{
joint_states.position.push_back(joint_pose.jVal[i]); // write data into standard ros msg
int j = i+1;
joint_states.name.push_back("joint_"+std::to_string(j));
joint_states.header.stamp = ros::Time::now();
}
joint_states_pub.publish(joint_states); // publish data
// cout << (clock() - t1) * 1.0 / CLOCKS_PER_SEC * 1000 << endl;
}
int main(int argc, char *argv[])
{
ros::init(argc,argv,"moveit_action_server");
ros::AsyncSpinner spinner(2);
spinner.start();
std::cout << robot.login_in("10.5.5.100") << std::endl;
robot.power_on();
robot.enable_robot();
ros::NodeHandle nh;
ros::Publisher joint_states_pub = nh.advertise<sensor_msgs::JointState>("/joint_states", 10);
ros::Publisher PoseStop = nh.advertise<std_msgs::String>("/PoseStop",10);
// 创建 action 对象(NodeHandle，话题名称，回调函数解析传入的目标值，服务器是否自启动)
Server moveit_server(nh,"jaka_controller/follow_joint_trajectory", boost::bind(&execute_callback, _1, &moveit_server), false);
// 手动启动服务器
moveit_server.start();
// ros::Rate rate(5);
while (ros::ok())
{
joint_states_callback(joint_states_pub);
ros::spinOnce();
// rate.sleep();
}
ros::spin();
return 0;
}接下来要编写连续规划和运动的程序了，毕竟用Rviz只能单次运行，要实现多段连续运动。
2万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
