（持续更新....）
1.Moveit端的FollowJointTrajectoryAction
该Action文件整体如下：Action的重点在它的goal部分，其他部分的分析直接看官方介绍：control_msgs//FollowJointTrajectory。
具体形式如下：
#Goal
trajectory_msgs/JointTrajectory trajectory
#轨迹公差，轨迹执行过程中实际关节角超出公差范围则轨迹目标将被中止
JointTolerance[] path_tolerance
#目标公差，机械臂执行完轨迹之后关节位置必须在目标值的tolerance之内
JointTolerance[] goal_tolerance
#结束时间tolerance
duration goal_time_tolerance
---
int32 error_code
int32 SUCCESSFUL = 0
int32 INVALID_GOAL = -1
int32 INVALID_JOINTS = -2
int32 OLD_HEADER_TIMESTAMP = -3
int32 PATH_TOLERANCE_VIOLATED = -4
int32 GOAL_TOLERANCE_VIOLATED = -5
---
Header header
string[] joint_names
trajectory_msgs/JointTrajectoryPoint desired
trajectory_msgs/JointTrajectoryPoint actual
trajectory_msgs/JointTrajectoryPoint error其中control_msgs/JointTolerance.msg如下：
string name
float64 position
float64 velocity
float64 acceleration 其中trajectory_msgs/msg/JointTrajectory.msg如下：trajectory_msgs
std_msgs/Header header
string[] joint_names
JointTrajectoryPoint[] points其中trajectory_msgs/msg/JointTrajectoryPoint.msg如下：
float64[] positions
float64[] velocities
float64[] accelerations
float64[] effort
builtin_interfaces/Duration time_from_start2.使用moveit提供的数据类型接收FollowJointTrajectoryGoal的轨迹数据
头文件是：# include <moveit_msgs/RobotTrajectory.h>
其中moveit_msgs/RobotTrajectory.msg具体形式如下：
trajectory_msgs/JointTrajectory joint_trajectory
trajectory_msgs/MultiDOFJointTrajectory multi_dof_joint_trajectory直接使用官方的数据类型比自己创建一堆vector数据类型来存储要好一些，接收轨迹数据之后可以用于之后的三次样条插值等处理。
3. moveit server端的编写
moveit server端是通过创建“<goup_name>_controller/follow_joint_trajectory”话题的server端来接收moveit 在规划完路径和解算完运动学逆解之后发布出来的一系列路点数据，并用于后续处理。
.cpp文件编写：
#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
//创建server时需要用到该头文件的SimpleActionServer这个类
#include <control_msgs/FollowJointTrajectoryAction.h>
//moveit client发布出来的action对应的数据类型
#include <moveit_msgs/RobotTrajectory.h>
//使用官方提供的trajectory数据类型来接收路点数据
using namespace std;
// typedef 是重命名的意思
typedef actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> Server;
//存储 moveit 发送出来的轨迹数据，可用于后续处理
moveit_msgs::RobotTrajectory moveit_tra;
void execute_callback(const control_msgs::FollowJointTrajectoryGoalConstPtr &goalPtr, Server *moveit_server) //
{
// 解析获取的目标值（多少个关节和多少个路点）
int jointsNum = goalPtr->trajectory.joint_names.size();
int PointsNum = 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(PointsNum);
for (int i = 0; i < PointsNum; i++)
{
moveit_tra.joint_trajectory.points[i].positions.resize(jointsNum);
moveit_tra.joint_trajectory.points[i].velocities.resize(jointsNum);
moveit_tra.joint_trajectory.points[i].accelerations.resize(jointsNum);
moveit_tra.joint_trajectory.points[i].time_from_start = goalPtr->trajectory.points[i].time_from_start;
for (int j = 0; j < jointsNum; 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];
}
}
ROS_INFO("The number of joints is %d.", jointsNum);
ROS_INFO("The waypoints number of the trajectory is %d.",PointsNum);
ROS_INFO("Receive trajectory successfully");
moveit_server->setSucceeded();
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "moveit_action_server");
ros::NodeHandle nh;
// 创建 SimpleActionServer 对象(其实就是action了)并直接初始化(NodeHandle，话题名称，回调函数解析传入的目标值，服务器是否自启动)
// 话题名称一般为“<goup_name>_controller/follow_joint_trajectory”
Server moveit_server(nh, "arms_controller/follow_joint_trajectory", boost::bind(&execute_callback, _1, &moveit_server), false);
moveit_server.start();
ros::spin();
return 0;
}CMakeLists.txt编译文件编写：
cmake_minimum_required(VERSION 3.0.2)
project(beginner_tutorials)
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
message_generation
actionlib_msgs
actionlib
control_msgs
moveit_msgs
)
## System dependencies are found with CMake's conventions
find_package(Boost REQUIRED COMPONENTS system)
## Generate messages in the 'msg' folder
add_message_files(
FILES
# Message1.msg
# Message2.msg
)
## Generate services in the 'srv' folder
add_service_files(
FILES
# Service1.srv
# Service2.srv
)
## Generate actions in the 'action' folder
add_action_files(
DIRECTORY action
FILES
# Action1.action
# Action2.action
)
## Generate added messages and services with any dependencies listed here
generate_messages(
DEPENDENCIES
std_msgs
actionlib_msgs
control_msgs
moveit_msgs
)
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES beginner_tutorials
# CATKIN_DEPENDS roscpp rospy std_msgs
CATKIN_DEPENDS message_runtime actionlib_msgs actionlib control_msgs moveit_msgs
# DEPENDS system_lib
)
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
${catkin_INCLUDE_DIRS}
${Boost_INCLUDE_DIRS}
)
add_executable(moveit_action_server src/moveit_server.cpp)
add_dependencies(moveit_action_server ${beginner_tutorials_EXPORTED_TARGETS})
target_link_libraries(moveit_action_server ${catkin_LIBRARIES})
package.xml依赖文件编写：
<?xml version="1.0"?>
<package format="2">
<name>beginner_tutorials</name>
<version>0.0.0</version>
<description>The beginner_tutorials package</description>
<maintainer email="robot@todo.todo">robot</maintainer>
<license>TODO</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>actionlib</build_depend>
<build_depend>actionlib_msgs</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>actionlib</build_export_depend>
<build_export_depend>actionlib_msgs</build_export_depend>
<exec_depend>actionlib</exec_depend>
<exec_depend>actionlib_msgs</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<build_export_depend>control_msgs</build_export_depend>
<exec_depend>control_msgs</exec_depend>
<build_depend>control_msgs</build_depend>
<build_export_depend>moveit_msgs</build_export_depend>
<exec_depend>moveit_msgs</exec_depend>
<build_depend>moveit_msgs</build_depend>
<!-- The customs msg file or srv file need these dependens to turned into -->
<!-- source code for C++,Python,and other languages -->
<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>
<exec_depend>message_generation</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
4.moveit client端的文件配置修改
嗐，这部分遇到的问题还真多啊，终于也是完成了。
整体思想是用Moveit Assistant配置助手通过已有的urdf文件来生成机器人的SRDF文件（包含机器人的规划组、预定义位姿、虚拟关节、碰撞矩阵、末端执行器等参数）和各种逆运动学求解器、运动规划器以及碰撞检测等文件和move_group节点(moveit解算的核心节点)以及初始的各种launch启动文件等等，然后配置结果一般保存在以robot_name_moveit_config命名的功能包里。
1）urdf这一步（这里默认已经有通过STL的mesh文件生成的urdf文件了，之后再更新怎么创建自己的urdf吧）
首先应该封装在一个功能包内（命名一般为robot_name_description），注意这里也是要创建一个功能包来存放的哦，里面创建子文件夹urdf来存放robot_name_description.urdf文件，创建子文件夹meshes来存放机械臂各link的.STL文件，还可以创建一个config文件夹来存放第一次打开rviz界面进行配置之后保存的.rviz文件；
其中package.xml需要增加几个依赖：
<?xml version="1.0"?>
<package format="2">
<name>rm_description</name>
<version>0.0.0</version>
<description>The rm_description package</description>
<maintainer email="robot@todo.todo">robot</maintainer>
<license>BSD</license>
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<depend>roslaunch</depend>
<depend>robot_state_publisher</depend>
<depend>joint_state_publisher</depend>
<depend>rviz</depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>CMakeList.txt编译文件这里就不用改了。
2）moveit_assistan配置这一步
指令：roslaunch moveit_setup_assistant setup_assistant.launch，然后根据官方的教程moveit可视化配置进行配置就行了（也可以结合《ROS机械臂开发与实践》这本书来看，也很详细）。配置完之后就可以在工作空间下创建一个文件夹（一般命名为：robot_name_moveit_config）来保存自动生成的配置文件，这里创建 一个普通的文件夹就行，不需要创建功能包，因为moveit生成的配置文件里面已经包含package.xml和CMakeLists.txt文件了。
一般生成的配置文件里面有config和launch两个文件夹，
config文件夹：
其中config文件里有个.srdf文件，包含机器人的规划组、预定义位姿、虚拟关节、碰撞矩阵、末端执行器等参数；
还有各种yaml文件，其在包括ompl_planning.yaml运动规划器文件，为规划组planning_group_name(rml_arm)使用RRT避障算法；
rml_arm:
default_planner_config: RRTkinematics.yaml逆运动学求解器文件，定义了规划组的名称、运动求解器的选择和各自参数调配：
rml_arm:
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.005
goal_joint_tolerance: 0.0001
goal_position_tolerance: 0.0001
goal_orientation_tolerance: 0.001joint_limits.yaml关节参数限制文件，在urdf文件中已经通过<limit>属性指定各个joint的位置、速度限制了，joint_limits.yaml文件允许根据需要覆盖或增强URDF文件中指定的关节动力学属性，来设置这些关节限制。
joint_limits:
joint1:
has_velocity_limits: true
max_velocity: 1
has_acceleration_limits: false
max_acceleration: 0fake_controllers.yaml虚拟控制器文件，该虚拟控制器使得Rviz中的机械臂可以执行规划出的轨迹并反馈实时关节状态，在没有真实机械臂或gazebo仿真时进行演示测试；
config中其他的暂且没用上，上面提到的这几个配置文件一般也在assistant那里配置好了，就不用动了。
launch文件夹：
这个文件夹里面提供了一个比较重要的demo.launch文件，就是用来调用虚拟控制器，在rviz中演示用的。还有其他的move_group.launch、trajectory_execution.launch.xml文件这些在后面流程这里再具体来看。
-------------------------------------------------------------------------------------------------------------------
------------------------------------------------------------------------------------------------------------------
具体的各个包意义这里就不全部罗列了，接下来直接看在控制真实机械臂的时候应该怎么修改各个文件吧。首先参考这篇文章：使用Moveit控制真实机械臂-moveit client端配置。（下文统称为参考链接）不过这里面有些地方需要修改，要不然运行会出错。
🕐最好不要直接在demo.launch里面改，这个是在rviz演示用，留着吧，重新创建一个.launch文件，把demo那个复制过来，可以命名为demo_realRobot.launch，也可以命名为robot_name_planning_execution.launch文件，都可以。在参考链接中，需要修改这个地方，
<arg name="fake_execution" value="false"/>但是我们的配置文件中没有这个参数，只有下面“fake_execution_type”这个参数，
<arg name="fake_execution_type" default="interpolate" />所以我们需要在下面这个地方里面加那一行：
<!-- 启动Moveit!的核心节点启动文件 -->
<!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
<include file="$(find rml_63_moveit_config)/launch/move_group.launch">
<arg name="allow_trajectory_execution" value="true"/>
<arg name="moveit_controller_manager" value="$(arg moveit_controller_manager)" />
<!-- 这里这里👇 -->
<arg name="fake_execution" value="false"/>
<arg name="fake_execution_type" value="$(arg fake_execution_type)"/>
<arg name="info" value="true"/>
<arg name="debug" value="$(arg debug)"/>
<arg name="pipeline" value="$(arg pipeline)"/>
<arg name="load_robot_description" value="$(arg load_robot_description)"/>
</include>可以看到这里这个节点里面还有一行：（原demo里面的moveit_controller_manager的default是fake，参考链接里面也没改）
<arg name="moveit_controller_manager" value="$(arg moveit_controller_manager)" />因此需要把这个文件的那个default参数也修改一下：其中rml_63_description与生成的那个srdf文件名保持一致；（也就是自己机器人的名称，我urdf文件名是rml_63_description.urdf）。
<arg name="moveit_controller_manager" default="rml_63_description" />
🕑第二个点是，参考链接把“joint_state_publisher”那一整段都注释掉了，导致在后面运行时报错提示如下：大致意思就是无法获取joint state。
这里的解决办法呢，就是注释掉发布虚拟joint_state那段之后，要自己补上一个joint_state来发布关节状态，要不然rviz那里总是报错无法获取关节状态，同时还要保存robot_state_publisher那个节点，用于机器人各link的tf信息发布：
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<node pkg="topic_tools" type="relay" name="relay_joint_info" args="joint_states_desired joint_states" output="screen"/>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
这个文件基本上就这个两个注意事项了，完整文件如下：可以用rostopic echo /joint_states 来看看joint的关节信息是否发布哦。
<launch>
<!-- 设置启用数据、数据库路径、调试模式、fake虚拟控制器类型等参数 -->
<!-- specify the planning pipeline -->
<arg name="pipeline" default="ompl" />
<!-- By default, we do not start a database (it can be large) -->
<arg name="db" default="false" />
<!-- Allow user to specify database location -->
<arg name="db_path" default="$(find rml_63_moveit_config)/default_warehouse_mongo_db" />
<!-- By default, we are not in debug mode -->
<arg name="debug" default="false" />
<!-- By default, we will load or override the robot_description -->
<arg name="load_robot_description" default="true"/>
<!-- 这里这里👇 -->
<!-- Choose controller manager: fake, simple, or ros_control -->
<arg name="moveit_controller_manager" default="rml_63_description" />
<!-- Set execution mode for fake execution controllers -->
<arg name="fake_execution_type" default="interpolate" />
<!-- By default, hide joint_state_publisher's GUI in 'fake' controller_manager mode -->
<arg name="use_gui" default="false" />
<arg name="use_rviz" default="true" />
<!-- 这里这里👇 -->
<!-- If needed, broadcast static tf for robot root -->
<!-- <node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 world base_link" /> -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<node pkg="topic_tools" type="relay" name="relay_joint_info" args="joint_states_desired joint_states" output="screen"/>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
<!-- 这里这里👇 -->
<!-- 启动Moveit!的核心节点启动文件 -->
<!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
<include file="$(find rml_63_moveit_config)/launch/move_group.launch">
<arg name="allow_trajectory_execution" value="true"/>
<arg name="moveit_controller_manager" value="$(arg moveit_controller_manager)" />
<arg name="fake_execution" value="false"/>
<arg name="fake_execution_type" value="$(arg fake_execution_type)"/>
<arg name="info" value="true"/>
<arg name="debug" value="$(arg debug)"/>
<arg name="pipeline" value="$(arg pipeline)"/>
<arg name="load_robot_description" value="$(arg load_robot_description)"/>
</include>
<!-- 启动rviz -->
<!-- Run Rviz and load the default config to see the state of the move_group node -->
<include file="$(find rml_63_moveit_config)/launch/moveit_rviz.launch" if="$(arg use_rviz)">
<arg name="rviz_config" value="$(find rml_63_moveit_config)/launch/moveit.rviz"/>
<arg name="debug" value="$(arg debug)"/>
</include>
<!-- If database loading was enabled, start mongodb as well -->
<include file="$(find rml_63_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)">
<arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
</include>
</launch>
------------------------------------------------------------------------------------------------------------------
然后根据demo_realRobot.launch文件，它启动了move_group.launch这个文件，现在我们进入这个文件接着修改。
参考链接这里没啥问题，就是没提醒在上面添加两个参数：
<arg name="moveit_controller_manager" default="rml_63_description" />
<arg name="fake_execution" default="false"/>其他的按他修改就行，整体如下：
<launch>
<!-- 启动Moveit!核心节点move_group，该文件会加载路径规划、轨迹执行、3D传感器等配置参数传递给move_group节点并启动节点 -->
<!-- GDB Debug Option -->
<arg name="debug" default="false" />
<arg unless="$(arg debug)" name="launch_prefix" value="" />
<arg if="$(arg debug)" name="launch_prefix"
value="gdb -x $(dirname)/gdb_settings.gdb --ex run --args" />
<!-- Verbose Mode Option -->
<arg name="info" default="$(arg debug)" />
<arg unless="$(arg info)" name="command_args" value="" />
<arg if="$(arg info)" name="command_args" value="--debug" />
<!-- move_group settings -->
<arg name="pipeline" default="ompl" />
<arg name="allow_trajectory_execution" default="true"/> <!-- 运行执行轨迹 -->
<!-- 这里这里👇 -->
<arg name="moveit_controller_manager" default="rml_63_description" />
<arg name="fake_execution" default="false"/>
<arg name="fake_execution_type" default="interpolate"/>
<arg name="max_safe_path_cost" default="1"/>
<arg name="publish_monitored_planning_scene" default="true"/>
<arg name="capabilities" default=""/>
<arg name="disable_capabilities" default=""/>
<!-- load these non-default MoveGroup capabilities (space seperated) -->
<!--
<arg name="capabilities" value="
a_package/AwsomeMotionPlanningCapability
another_package/GraspPlanningPipeline
" />
-->
<!-- inhibit these default MoveGroup capabilities (space seperated) -->
<!--
<arg name="disable_capabilities" value="
move_group/MoveGroupKinematicsService
move_group/ClearOctomapService
" />
-->
<arg name="load_robot_description" default="true" />
<!-- load URDF, SRDF, 求解器 and joint_limits configuration -->
<include file="$(dirname)/planning_context.launch">
<arg name="load_robot_description" value="$(arg load_robot_description)" />
</include>
<!-- Planning Pipelines -->
<group ns="move_group/planning_pipelines">
<!-- OMPL -->
<include file="$(dirname)/planning_pipeline.launch.xml">
<arg name="pipeline" value="ompl" />
</include>
<!-- CHOMP -->
<include file="$(dirname)/planning_pipeline.launch.xml">
<arg name="pipeline" value="chomp" />
</include>
<!-- Pilz Industrial Motion -->
<include file="$(dirname)/planning_pipeline.launch.xml">
<arg name="pipeline" value="pilz_industrial_motion_planner" />
</include>
<!-- Support custom planning pipeline -->
<include if="$(eval arg('pipeline') not in ['ompl', 'chomp', 'pilz_industrial_motion_planner'])"
file="$(dirname)/planning_pipeline.launch.xml">
<arg name="pipeline" value="$(arg pipeline)" />
</include>
</group>
<!-- 还有这里这里👇 -->
<!-- Trajectory Execution Functionality -->
<include ns="move_group" file="$(dirname)/trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">
<arg name="moveit_manage_controllers" value="true" />
<arg name="moveit_controller_manager" value="$(arg moveit_controller_manager)" unless="$(arg fake_execution)" />
<arg name="moveit_controller_manager" value="fake" if="$(arg fake_execution)" />
</include>
<!-- Sensors Functionality -->
<include ns="move_group" file="$(dirname)/sensor_manager.launch.xml" if="$(arg allow_trajectory_execution)">
<arg name="moveit_sensor_manager" value="rml_63_description" />
</include>
<!-- Start the actual move_group node/action server -->
<node name="move_group" launch-prefix="$(arg launch_prefix)" pkg="moveit_ros_move_group" type="move_group" respawn="false" output="screen" args="$(arg command_args)">
<!-- Set the display variable, in case OpenGL code is used internally -->
<env name="DISPLAY" value="$(optenv DISPLAY :0)" />
<param name="allow_trajectory_execution" value="$(arg allow_trajectory_execution)"/>
<param name="sense_for_plan/max_safe_path_cost" value="$(arg max_safe_path_cost)"/>
<param name="default_planning_pipeline" value="$(arg pipeline)" />
<param name="capabilities" value="$(arg capabilities)" />
<param name="disable_capabilities" value="$(arg disable_capabilities)" />
<!-- do not copy dynamics information from /joint_states to internal robot monitoring
default to false, because almost nothing in move_group relies on this information -->
<param name="monitor_dynamics" value="false" />
<!-- Publish the planning scene of the physical robot so that rviz plugin can know actual robot -->
<param name="planning_scene_monitor/publish_planning_scene" value="$(arg publish_monitored_planning_scene)" />
<param name="planning_scene_monitor/publish_geometry_updates" value="$(arg publish_monitored_planning_scene)" />
<param name="planning_scene_monitor/publish_state_updates" value="$(arg publish_monitored_planning_scene)" />
<param name="planning_scene_monitor/publish_transforms_updates" value="$(arg publish_monitored_planning_scene)" />
</node>
</launch>
-----------------------------------------------------------------------------------------------------------------
接下来 moveit_controller_manager 参数传入 trajectory_execution.launch.xml 文件中，让我们进入这个文件进行修改。
参考链接在这步修改的也没啥问题，只是有一两行可以加上去：
<launch>
<!-- This file summarizes all settings required for trajectory execution -->
<!-- 这里这里👇 -->
<!-- Define moveit controller manager plugin: fake, simple, or ros_control -->
<arg name="moveit_controller_manager" default="rml_63_description" />
<arg name="fake_execution_type" default="interpolate" />
<!-- Flag indicating whether MoveIt is allowed to load/unload or switch controllers -->
<arg name="moveit_manage_controllers" default="true"/>
<param name="moveit_manage_controllers" value="$(arg moveit_manage_controllers)"/>
<!-- When determining the expected duration of a trajectory, this multiplicative factor is applied to get the allowed duration of execution -->
<param name="trajectory_execution/allowed_execution_duration_scaling" value="1.2"/> <!-- default 1.2 -->
<!-- Allow more than the expected execution time before triggering a trajectory cancel (applied after scaling) -->
<param name="trajectory_execution/allowed_goal_duration_margin" value="0.5"/> <!-- default 0.5 -->
<!-- Allowed joint-value tolerance for validation that trajectory's first point matches current robot state -->
<param name="trajectory_execution/allowed_start_tolerance" value="0.10"/> <!-- default 0.01 -->
<!-- We use pass_all_args=true here to pass fake_execution_type, which is required by fake controllers, but not by real-robot controllers.
As real-robot controller_manager.launch files shouldn't be required to define this argument, we use the trick of passing all args. -->
<!-- 这里这里👇 -->
<include file="$(find rml_63_moveit_config)/launch/$(arg moveit_controller_manager)_moveit_controller_manager.launch.xml">
<arg name="fake_execution_type" value="$(arg fake_execution_type)" />
</include>
<!-- 这里这里👇 -->
<param name="trajectory_execution/execution_duration_monitoring" value="false" />
</launch>
-----------------------------------------------------------------------------------------------------------------------------
可以看到当moveit_controller_manager从fake修改为rml_63_description之后，该此时此处加载的文件为rml_63_description_moveit_controller_manager.launch.xml，参考链接说这个文件在配置助手里面就已经生成好了，但是我的launch文件里面却没有找到，所以需要自己创建这么一个文件，然后把内容放进去：其他没啥改的
<launch>
<arg name="fake_execution_type" default="FollowJointTrajectory" />
<!-- 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)"/>
<!-- loads ros_controllers to the param server -->
<rosparam file="$(find rml_63_moveit_config)/config/realRobot_controllers.yaml"/>
</launch>
------------------------------------------------------------------------------------------------------------------------
可以看到上一步代码最后一句又指向了config文件夹内的 realRobot_controllers.yaml 这个配置文件决定了所使用 moveit 控制器的基本参数：这个控制器文件要自己创建，默认的是fake_controllers.yaml，就是演示用的那个虚拟控制器。
controller_list:
- name: rml_63_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
- joint1
- joint2
- joint3
- joint4
- joint5
- joint6
# initial: # Define initial robot poses per group
# - group: rml_arm
# pose: Home
这里有个超级大坑，看到name和joint 1前面那个短横杠“-“没，不要删！！！，那个短横杠表示controller_list 参数的定义格式应该是一个数组（列表）的格式，否则会报错。
而这里的name+action_ns就是这个moveit client的话题名词了，之后写sever的时候就要用rml_63_controller/follow_joint_trajectory这个话题名称了。
----------------------------------------------------------------------------------------------------------------------
所有文件就算是配置完了，但是！运行的时候有可能会报一个很难发现的错误！报错如下：
大概意思就是说找不到控制这些关节的控制器，这时候需要把joint需要的控制库install下来就行了，命令行：
sudo apt install ros-neotic-joint-*---------------------------------------------------------------------------------------------------------------------
接下来运行demo_realRobot.launch文件的时候把moveit server端那个也运行起来就可以发现能接收到路点数据啦，可以用rostopic echo /joint_states 来看看joint的关节信息是否发布哦。
5.三次样条处理moveit数据并按一定时间间隔发布给robot_driver
唉，终于实现这部分了，路途坎坷啊......
有几个注意点呢，
🕐首先是接收数据这里，就像我第二部分那里写的那样，很高兴的发现有moveit提供的moveit_msgs::RobotTrajectory这个数据类型来接收moveit轨迹规划出来的路点信息（当时想法：嘿嘿，不用自己写msg了），然后兴致冲冲的在三次样条这里使用了，写到一半发现不对劲了，我需要把全部路点下的某个关节角度传到三次样条的函数里面进行处理阿，要是用moveit_tra.joint_trajectory.points[i].positions[j]这个方式存储路点信息，我不也还得先通过for循环把全部路点下的第一个关节角度取出来吗，多绕了这一步，多麻烦，还不如直接就每个关节的positions、velocities、accelerations创建一个数组来存储对应的数据，反正进行样条插值是每个关节按顺序传入函数里面处理并接收返回数据的。不过这里也提供一个有点简便的做法（如果你已经是用moveit_msgs::RobotTrajectory了，那可以这么处理）：
（没想到for循环也可以写得这么优美👇）
# 提取所有路点的第一个关节位置
joint_positions = [point.positions[0] for point in moveit_tra.joint_trajectory.points]
# 生成对应的时间序列
time_stamps = [point.time_from_start.to_sec() for point in moveit_tra.joint_trajectory.points]🕑第二个点也很惊喜啊，“重复发送最后一个关节位置”，这个处理方式我也是受教了，就是在发送完全部节点数据之后，重复几次发送最后一个关节位置。这里面有说法的哦：
//在机械臂运动过程中，由于惯性和摩擦等因素，机械臂可能会稍微偏离目标位置。通过多次发送最后一个关节位置，可以确保机械臂能够更精确地到达目标位置，减少误差。
//在某些情况下，机械臂可能会因为微小的扰动而产生抖动。通过多次发送相同的目标位置，可以减少这种抖动，使机械臂的运动更加平滑和稳定。
//通过多次发送相同的目标位置，可以确认机械臂确实已经到达了目标位置。这对于某些应用场景（如精密装配、焊接等）非常重要。🕒第三点就是使用ros::Timer定时器按一定节奏来发布每个路点的关节角度给robot_driver节点了，Timer回调函数的时间间隔和插值之后每个路点的步长一样呢，即机械臂0.01s运动到下一个点就0.01s后发布下一个点角度。（差点用写publisher的时候那个while循环了，果然还是Timer好用）
🕔一个bug排查了三天！！！！！终于也是解决了。首先描述一下这个bug：提示"段错误（核心已转储）" ，我启动了moveit_client那个节点在rviz移动机械臂然后paln&excute的时候，moveit_server_spline这个节点就报这个错误，本来以为是系统的问题或者编译器的问题（当时我甚至以为是vscode的问题，因为那两三天vscode老是犯病，要么无法跳转要么经常死机要么就各种链接不到的问题，所以为怀疑它也是情有可原吧，但是我又想想vscode充气量就是个编辑器，真不应该怪它。然后我就找会不会是编译器出问题了呢，但是我单独测试了moevit_server那个节点，能正常运行啊，看来不关这个的事。那就只能是代码本身出错了，问了一下chatgpt，说确实在数组越界、空指针访问到异常地址、内存泄漏等问题会报这个错，好家伙，果然有了ai之后思考就下降了，然后就把代码各种复制过去给gpt分析，这么拉拉扯扯一天愣是没解决问题。）
然后转折点出现了，睡了一觉，第二天半梦半醒的时候有个可行想法突然出现（睡觉真好～），debug要有技巧啊，大型项目交给chatgpt分析还是分析不出来的，所以我就把想法记录下来了：
要学会debug:发现程序运行不起来但是不知道问题在哪的时候，要学会分段输出验证来定位问题，比如ROS_INFO或cout，一步步排查找到问题。（还有一个点可以避免奇奇怪怪的错误的就是，数据之间存在交互操作的，尽量保证数据都是同一个类型，别一会儿double一会儿vector<double>的，有时候理解不透彻的话容易影响程序鲁棒性）（按道理来说我平时就是这么干的，但是有时候依赖上外部资源和chatgpt之后，好像总在避免思考）
扯远了，还没说问题在哪找到的，发布插值数据的时候自己创建了一个msg文件，其实就是float32[] joint，然后进行发布👇，这么一看确实没啥问题，但是每次运行就卡在这里的ROS_INFO，结果发现，是少了这一步：joint_pos_msg.joint.resize(6);数据大小要进行初始化才可以使用的。
//beginner_tutorials::JointPos joint_pos_msg;
ROS_INFO("target publish begin...");
joint_pos_msg.joint[0] = p_joint1_[p2.vector_cnt];
joint_pos_msg.joint[1] = p_joint2_[p2.vector_cnt];
joint_pos_msg.joint[2] = p_joint3_[p2.vector_cnt];
joint_pos_msg.joint[3] = p_joint4_[p2.vector_cnt];
joint_pos_msg.joint[4] = p_joint5_[p2.vector_cnt];
joint_pos_msg.joint[5] = p_joint6_[p2.vector_cnt];
ROS_INFO("target ready to publish...");
target_pub.publish(joint_pos_msg);-----------------------------------------------------------------------------------------------------------------------
其他的暂时没啥注意事项了，完整代码过段时间补上吧....
先附上三次样条插值的原理和算法实现代码的链接给大家吧，看懂原理之后可能对接下来的代码实现有更好的理解（原理我看得也有点迷糊，大家有推荐的好资料或者讲得比较透彻的视频也欢迎在评论区分享一下哦，感谢感谢😀）。三次样条插值原理 ---- 三次样条算法代码
三次样条处理代码实现：
//命名为cubicSpline.h
#ifndef CUBIC_SPLINE_H_ // 防止头文件重复包含
#define CUBIC_SPLINE_H_
#include <algorithm>
#include <cstring>
#include <stdio.h>
#include <vector>
#include <iostream>
using namespace std;
/* 初始化输入输出速度加速度 */
double acc = 0, vel = 0;
class cubicSpline
{
public:
// 定义枚举类型BoundType，指定样条曲线边界条件的类型；
typedef enum _BoundType // enum _BoundType枚举的名称；
{
BoundType_First_Derivative, // 0，一阶导数边界
BoundType_Second_Derivative // 1，二阶导数边界
} BoundType; // 枚举的别名，后面可以直接用BoundType type 创建枚举变量
public:
cubicSpline();
~cubicSpline();
void initParam();
void releaseMem();
bool loadData(vector<double>& x_data, vector<double>& y_data, int count, double bound1, double bound2, BoundType type);
bool getYbyX(double &x_in, double &y_out);
protected:
bool spline(BoundType type);
protected:
vector<double> x_sample_;
vector<double> y_sample_;
vector<double> M_;
int sample_count_;
double bound1_, bound2_;
};
cubicSpline::cubicSpline()
{
}
cubicSpline::~cubicSpline()
{
releaseMem();
}
void cubicSpline::initParam()
{
cout<<"Initial cubicSpline successed..."<<endl;
sample_count_ = 0;
bound1_ = bound2_ = 0;
}
void cubicSpline::releaseMem()
{
initParam();
}
//加载样本数据，把规划出来的稀疏路点数据放进来充当样本数据用于计算函数参数
bool cubicSpline::loadData(vector<double>& x_data, vector<double>& y_data, int count, double bound1, double bound2, BoundType type)
{
if ((x_data.size() == 0) || (y_data.size() == 0) || (count < 3) || (type < BoundType_First_Derivative) || (type > BoundType_Second_Derivative))
{
return false;
}
cout<<"cubicSpline loadData begin..."<<endl;
initParam();
x_sample_.resize(count);
y_sample_.resize(count);
M_.resize(count);
sample_count_ = count;
// x_sample_.assign(x_data, x_data+count);
// y_sample_.assign(y_data, y_data+count);
x_sample_ = x_data;
y_sample_ = y_data;
bound1_ = bound1;
bound2_ = bound2;
cout<<"cubicSpline loadData successed..."<<endl;
return spline(type);
}
//三次样条本质就是在每两个相邻的样本点间构造一段三次函数来表示
//通过已知的sample样本值，求解这个三次函数的系数参数，将这个三次函数构造出来，每一段有自己的参数，因此参数向量也用数组表示
//其具体计算涉及各种公式和矩阵的运算可以去原理那个链接看看
bool cubicSpline::spline(BoundType type)
{
if ((type < BoundType_First_Derivative) || (type > BoundType_Second_Derivative))
{
return false;
}
cout<<"cubicSpline spline begin..."<<endl;
// 追赶法解方程求二阶偏导数
double f1 = bound1_, f2 = bound2_;
double *a = new double[sample_count_]; // a:稀疏矩阵最下边一串数
double *b = new double[sample_count_]; // b:稀疏矩阵最中间一串数
double *c = new double[sample_count_]; // c:稀疏矩阵最上边一串数
double *d = new double[sample_count_];
double *f = new double[sample_count_];
double *bt = new double[sample_count_];
double *gm = new double[sample_count_];
double *h = new double[sample_count_];
for (int i = 0; i < sample_count_; i++)
b[i] = 2; // 中间一串数为2
for (int i = 0; i < sample_count_ - 1; i++)
{
if (x_sample_[i + 1] == x_sample_[i])
{
h[i] = 0.005;
}
else
{
h[i] = x_sample_[i + 1] - x_sample_[i]; // 各段步长
}
}
for (int i = 1; i < sample_count_ - 1; i++)
a[i] = h[i - 1] / (h[i - 1] + h[i]);
a[sample_count_ - 1] = 1;
c[0] = 1;
for (int i = 1; i < sample_count_ - 1; i++)
c[i] = h[i] / (h[i - 1] + h[i]);
for (int i = 0; i < sample_count_ - 1; i++)
{
if (x_sample_[i + 1] == x_sample_[i])
{
f[i] = (y_sample_[i + 1] - y_sample_[i]) / 0.005;
}
else
{
f[i] = (y_sample_[i + 1] - y_sample_[i]) / (x_sample_[i + 1] - x_sample_[i]);
}
}
for (int i = 1; i < sample_count_ - 1; i++)
d[i] = 6 * (f[i] - f[i - 1]) / (h[i - 1] + h[i]);
// 追赶法求解方程
if (BoundType_First_Derivative == type)
{
d[0] = 6 * (f[0] - f1) / h[0];
d[sample_count_ - 1] = 6 * (f2 - f[sample_count_ - 2]) / h[sample_count_ - 2];
bt[0] = c[0] / b[0];
for (int i = 1; i < sample_count_ - 1; i++)
bt[i] = c[i] / (b[i] - a[i] * bt[i - 1]);
gm[0] = d[0] / b[0];
for (int i = 1; i <= sample_count_ - 1; i++)
gm[i] = (d[i] - a[i] * gm[i - 1]) / (b[i] - a[i] * bt[i - 1]);
M_[sample_count_ - 1] = gm[sample_count_ - 1];
for (int i = sample_count_ - 2; i >= 0; i--)
M_[i] = gm[i] - bt[i] * M_[i + 1];
}
else if (BoundType_Second_Derivative == type)
{
d[1] = d[1] - a[1] * f1;
d[sample_count_ - 2] = d[sample_count_ - 2] - c[sample_count_ - 2] * f2;
bt[1] = c[1] / b[1];
for (int i = 2; i < sample_count_ - 2; i++)
bt[i] = c[i] / (b[i] - a[i] * bt[i - 1]);
gm[1] = d[1] / b[1];
for (int i = 2; i <= sample_count_ - 2; i++)
gm[i] = (d[i] - a[i] * gm[i - 1]) / (b[i] - a[i] * bt[i - 1]);
M_[sample_count_ - 2] = gm[sample_count_ - 2];
for (int i = sample_count_ - 3; i >= 1; i--)
M_[i] = gm[i] - bt[i] * M_[i + 1];
M_[0] = f1;
M_[sample_count_ - 1] = f2;
}
else
return false;
delete[] a;
delete[] b;
delete[] c;
delete[] d;
delete[] gm;
delete[] bt;
delete[] f;
delete[] h;
cout<<"cubicSpline spline successed..."<<endl;
return true;
}
//使用这个分段的三次函数，给x，求y，在这里x就是细分出来的时间点，y就是位置，还可以求速度和加速度
bool cubicSpline::getYbyX(double &x_in, double &y_out)
{
cout<<"cubicSpline getYbyX begin..."<<endl;
int klo, khi, k;
klo = 0;
khi = sample_count_ - 1;
double hh, bb, aa;
// 二分法查找x所在区间段
while (khi - klo > 1)
{
k = (khi + klo) >> 1;
if (x_sample_[k] > x_in)
khi = k;
else
klo = k;
}
hh = x_sample_[khi] - x_sample_[klo];
aa = (x_sample_[khi] - x_in) / hh;
bb = (x_in - x_sample_[klo]) / hh;
y_out = aa * y_sample_[klo] + bb * y_sample_[khi] + ((aa * aa * aa - aa) * M_[klo] + (bb * bb * bb - bb) * M_[khi]) * hh * hh / 6.0;
// test
acc = (M_[klo] * (x_sample_[khi] - x_in) + M_[khi] * (x_in - x_sample_[klo])) / hh;
vel = M_[khi] * (x_in - x_sample_[klo]) * (x_in - x_sample_[klo]) / (2 * hh) - M_[klo] * (x_sample_[khi] - x_in) * (x_sample_[khi] - x_in) / (2 * hh) + (y_sample_[khi] - y_sample_[klo]) / hh - hh * (M_[khi] - M_[klo]) / 6;
// printf("[---位置、速度、加速度---]");
// printf("%0.9f, %0.9f, %0.9f\n",y_out, vel, acc);
// test end
cout<<"cubicSpline getYbyX successed..."<<endl;
return true;
}
#endif /*CUBIC_SPLINE_H_*/
moveit_server_spline端代码实现：（接收路点数据、三次样条插值、发布插值后的数据）
#include "beginner_tutorials/cubicSpline.h"
#include <actionlib/server/simple_action_server.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <iostream>
#include <moveit_msgs/RobotTrajectory.h>
#include <ros/ros.h>
#include <beginner_tutorials/JointPos.h>
using namespace std;
// 三次样条后各关节数据存储的地方,而不损坏原数据
vector<double> time_from_start_;
vector<double> p_joint1_;
vector<double> p_joint2_;
vector<double> p_joint3_;
vector<double> p_joint4_;
vector<double> p_joint5_;
vector<double> p_joint6_;
vector<double> p_joint7_;
vector<double> v_joint1_;
vector<double> v_joint2_;
vector<double> v_joint3_;
vector<double> v_joint4_;
vector<double> v_joint5_;
vector<double> v_joint6_;
vector<double> v_joint7_;
vector<double> a_joint1_;
vector<double> a_joint2_;
vector<double> a_joint3_;
vector<double> a_joint4_;
vector<double> a_joint5_;
vector<double> a_joint6_;
vector<double> a_joint7_;
/* 存储的结构体 p2*/
struct vel_data
{
int vector_len; //toatal num
int vector_cnt; //current num
};
/* 数据收发判断结构体实例化对象 */
struct vel_data p2;
//创建关节位置对象并初始化
beginner_tutorials::JointPos joint_pos_msg;
joint_pos_msg.joint.resize(6);
double x_out = 0, y_out = 0;
/*三次样条插值周期*/
double rate = 0.010; // 10ms
float min_interval = 0.01; //透传周期,单位:秒
float wait_move_finish_time = 1.5; //等待运动到位时间,单位:秒
/*最后路点关节数据重复发送判断*/
double count_final_joint = 0;
double count_keep_send = 0;
/* 判断路点数据是否改变 */
bool point_changed = false;
// typedef 是重命名的意思
typedef actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> Server;
ros::Publisher target_pub;
// // 存储 moveit 发送出来的轨迹数据，可用于后续处理
// moveit_msgs::RobotTrajectory moveit_tra;
void execute_callback(
const control_msgs::FollowJointTrajectoryGoalConstPtr &goalPtr,
Server *moveit_server) //
{
ROS_INFO("The execute_callback function begin successed....");
// 解析获取的目标值（多少个关节和多少个路点）
int jointsNum = goalPtr->trajectory.joint_names.size();
int PointsNum = goalPtr->trajectory.points.size();
ROS_INFO("The jointsNum and PointsNum received successed: %d, %d",jointsNum,PointsNum);
if (PointsNum > 3) {
// 各关节数据单独存储便于传入三次样条函数进行插值处理
// 关节位置
vector<double> p_joint1;
vector<double> p_joint2;
vector<double> p_joint3;
vector<double> p_joint4;
vector<double> p_joint5;
vector<double> p_joint6;
vector<double> p_joint7;
/* 各个关节速度 */
vector<double> v_joint1;
vector<double> v_joint2;
vector<double> v_joint3;
vector<double> v_joint4;
vector<double> v_joint5;
vector<double> v_joint6;
vector<double> v_joint7;
/* 各个关节加速度 */
vector<double> a_joint1;
vector<double> a_joint2;
vector<double> a_joint3;
vector<double> a_joint4;
vector<double> a_joint5;
vector<double> a_joint6;
vector<double> a_joint7;
//时间数组
vector<double> time_from_start;
for (int i = 0; i < PointsNum; i++) {
//把每个路点的数据分别存在对应关节数组中，方便每个关节的数据传入三次样条函数进行处理
p_joint1.push_back(goalPtr->trajectory.points[i].positions[0]);
p_joint2.push_back(goalPtr->trajectory.points[i].positions[1]);
p_joint3.push_back(goalPtr->trajectory.points[i].positions[2]);
p_joint4.push_back(goalPtr->trajectory.points[i].positions[3]);
p_joint5.push_back(goalPtr->trajectory.points[i].positions[4]);
p_joint6.push_back(goalPtr->trajectory.points[i].positions[5]);
v_joint1.push_back(goalPtr ->trajectory.points[i].velocities[0]);
v_joint2.push_back(goalPtr ->trajectory.points[i].velocities[1]);
v_joint3.push_back(goalPtr ->trajectory.points[i].velocities[2]);
v_joint4.push_back(goalPtr ->trajectory.points[i].velocities[3]);
v_joint5.push_back(goalPtr ->trajectory.points[i].velocities[4]);
v_joint6.push_back(goalPtr ->trajectory.points[i].velocities[5]);
a_joint1.push_back(goalPtr->trajectory.points[i].accelerations[0]);
a_joint2.push_back(goalPtr->trajectory.points[i].accelerations[1]);
a_joint3.push_back(goalPtr->trajectory.points[i].accelerations[2]);
a_joint4.push_back(goalPtr->trajectory.points[i].accelerations[3]);
a_joint5.push_back(goalPtr->trajectory.points[i].accelerations[4]);
a_joint6.push_back(goalPtr->trajectory.points[i].accelerations[5]);
time_from_start.push_back(goalPtr->trajectory.points[i].time_from_start.toSec());
ROS_INFO("Saving waypoints successed...");
ROS_INFO("p_joint1: %lf", p_joint1[i]);
}
// cout << moveit_tra;
//ROS_INFO("The number of joints is %d.", jointsNum);
//ROS_INFO("The waypoints number of the trajectory is %d.", PointsNum);
//ROS_INFO("Receive trajectory successfully");
// 三次样条处理
cubicSpline spline;
double max_time = time_from_start[PointsNum-1];
time_from_start_.clear();
//ROS_INFO("The Move_group max_time is %d", max_time);
// joint 1
if (spline.loadData(time_from_start, p_joint1, PointsNum, 0, 0, cubicSpline::BoundType_First_Derivative)) {
p_joint1_.clear();
v_joint1_.clear();
a_joint1_.clear();
x_out = -rate;
while(x_out<max_time)
{
x_out += rate;
spline.getYbyX(x_out, y_out);
time_from_start_.push_back(x_out);
p_joint1_.push_back(y_out);
v_joint1_.push_back(vel);
a_joint1_.push_back(acc);
ROS_INFO("The spline new waypoints successed...");
ROS_INFO("new waypoint of p_joint1 : %lf", p_joint1_[130]);
ROS_INFO("new waypoint of p_joint1 : %lf", p_joint1_[150]);
}
//采用层次的方式可以在前一个关节出现问题时就暂停
if (spline.loadData(time_from_start, p_joint2, PointsNum, 0, 0, cubicSpline::BoundType_First_Derivative)) {
p_joint2_.clear();
v_joint2_.clear();
a_joint2_.clear();
x_out = -rate;
while(x_out<max_time)
{
x_out += rate;
spline.getYbyX(x_out, y_out);
//time_from_start_.push_back(x_out); // 将新的时间存储，只需操作一次即可
p_joint2_.push_back(y_out);
v_joint2_.push_back(vel);
a_joint2_.push_back(acc);
}
if (spline.loadData(time_from_start, p_joint3, PointsNum, 0, 0, cubicSpline::BoundType_First_Derivative)) {
p_joint3_.clear();
v_joint3_.clear();
a_joint3_.clear();
x_out = -rate;
while(x_out<max_time)
{
x_out += rate;
spline.getYbyX(x_out, y_out);
//time_from_start_.push_back(x_out); // 将新的时间存储，只需操作一次即可
p_joint3_.push_back(y_out);
v_joint3_.push_back(vel);
a_joint3_.push_back(acc);
}
if (spline.loadData(time_from_start, p_joint4, PointsNum, 0, 0, cubicSpline::BoundType_First_Derivative)) {
p_joint4_.clear();
v_joint4_.clear();
a_joint4_.clear();
x_out = -rate;
while(x_out<max_time)
{
x_out += rate;
spline.getYbyX(x_out, y_out);
//time_from_start_.push_back(x_out); // 将新的时间存储，只需操作一次即可
p_joint4_.push_back(y_out);
v_joint4_.push_back(vel);
a_joint4_.push_back(acc);
}
if (spline.loadData(time_from_start, p_joint5, PointsNum, 0, 0, cubicSpline::BoundType_First_Derivative)) {
p_joint5_.clear();
v_joint5_.clear();
a_joint5_.clear();
x_out = -rate;
while(x_out<max_time)
{
x_out += rate;
spline.getYbyX(x_out, y_out);
//time_from_start_.push_back(x_out); // 将新的时间存储，只需操作一次即可
p_joint5_.push_back(y_out);
v_joint5_.push_back(vel);
a_joint5_.push_back(acc);
}
if (spline.loadData(time_from_start, p_joint6, PointsNum, 0, 0, cubicSpline::BoundType_First_Derivative)) {
p_joint6_.clear();
v_joint6_.clear();
a_joint6_.clear();
x_out = -rate;
while(x_out<max_time)
{
x_out += rate;
spline.getYbyX(x_out, y_out);
//time_from_start_.push_back(x_out); // 将新的时间存储，只需操作一次即可
p_joint6_.push_back(y_out);
v_joint6_.push_back(vel);
a_joint6_.push_back(acc);
ROS_INFO("new waypoint of p_joint6 : %lf", p_joint6_[130]);
ROS_INFO("new waypoint of p_joint6 : %lf", p_joint6_[150]);
}
cout<<"The number of new waypoints: "<<p_joint6_.size()<<endl;
//ROS_INFO("Now We get all trajectory num %d by cubicSpline.", time_from_start_.size());
p2.vector_len = time_from_start_.size();
p2.vector_cnt = 0;
cout<<"p2.vector_len: "<<p2.vector_len<<endl;
point_changed = true;
while(point_changed)
{
if(moveit_server -> isPreemptRequested() || !ros::ok())
{
ROS_INFO("************************Action Server: Preempted");
point_changed = false;
moveit_server -> setPreempted();
return;
}
}
}
}
}
}
}
}
}
else if(PointsNum > 0)
{
p_joint1_.clear();
v_joint1_.clear();
a_joint1_.clear();
p_joint2_.clear();
v_joint2_.clear();
a_joint2_.clear();
p_joint3_.clear();
v_joint3_.clear();
a_joint3_.clear();
p_joint4_.clear();
v_joint4_.clear();
a_joint4_.clear();
p_joint5_.clear();
v_joint5_.clear();
a_joint5_.clear();
p_joint6_.clear();
v_joint6_.clear();
a_joint6_.clear();
for(int i =0; i<PointsNum; i++){
p_joint1_.push_back(goalPtr->trajectory.points[i].positions[0]);
v_joint1_.push_back(goalPtr->trajectory.points[i].velocities[0]);
a_joint1_.push_back(goalPtr->trajectory.points[i].accelerations[0]);
p_joint2_.push_back(goalPtr->trajectory.points[i].positions[1]);
v_joint2_.push_back(goalPtr->trajectory.points[i].velocities[1]);
a_joint2_.push_back(goalPtr->trajectory.points[i].accelerations[1]);
p_joint3_.push_back(goalPtr->trajectory.points[i].positions[2]);
v_joint3_.push_back(goalPtr->trajectory.points[i].velocities[2]);
a_joint3_.push_back(goalPtr->trajectory.points[i].accelerations[2]);
p_joint4_.push_back(goalPtr->trajectory.points[i].positions[3]);
v_joint4_.push_back(goalPtr->trajectory.points[i].velocities[3]);
a_joint4_.push_back(goalPtr->trajectory.points[i].accelerations[3]);
p_joint5_.push_back(goalPtr->trajectory.points[i].positions[4]);
v_joint5_.push_back(goalPtr->trajectory.points[i].velocities[4]);
a_joint5_.push_back(goalPtr->trajectory.points[i].accelerations[4]);
p_joint6_.push_back(goalPtr->trajectory.points[i].positions[5]);
v_joint6_.push_back(goalPtr->trajectory.points[i].velocities[5]);
a_joint6_.push_back(goalPtr->trajectory.points[i].accelerations[5]);
}
p2.vector_len = PointsNum;
p2.vector_cnt = 0;
point_changed = true;
while(point_changed)
{
if (moveit_server->isPreemptRequested() || !ros::ok())
{
ROS_INFO("************************Action Server: Preempted");
point_changed = false;
moveit_server->setPreempted();
// timerSwitch.data = false;
// pub_getArmStateTimerSwitch.publish(timerSwitch);
return;
}
}
}
moveit_server->setSucceeded();
}
void timer_callback(const ros::TimerEvent) //这是ROS定时器的事件对象，包含了定时器的触发时间和上次触发时间等信息。
{
ROS_INFO("The timer_callback begin...");
cout<<"point_changed: "<<point_changed<<endl;
if(point_changed)
{
cout<<"p2.vector_cnt: "<<p2.vector_cnt<<" p2.vector_len: "<<p2.vector_len<<endl;
if(p2.vector_cnt < p2.vector_len)
{
ROS_INFO("Test if p_joint1_.at(p2.vector_cnt) can be runned...");
ROS_INFO("Pos:[%lf, %lf, %lf, %lf, %lf, %lf]", p_joint1_.at(p2.vector_cnt), p_joint2_.at(p2.vector_cnt), p_joint3_.at(p2.vector_cnt), p_joint4_.at(p2.vector_cnt), p_joint5_.at(p2.vector_cnt), p_joint6_.at(p2.vector_cnt));
ROS_INFO("target publish begin...");
joint_pos_msg.joint[0] = p_joint1_[p2.vector_cnt];
joint_pos_msg.joint[1] = p_joint2_[p2.vector_cnt];
joint_pos_msg.joint[2] = p_joint3_[p2.vector_cnt];
joint_pos_msg.joint[3] = p_joint4_[p2.vector_cnt];
joint_pos_msg.joint[4] = p_joint5_[p2.vector_cnt];
joint_pos_msg.joint[5] = p_joint6_[p2.vector_cnt];
ROS_INFO("target ready to publish...");
target_pub.publish(joint_pos_msg);
p2.vector_cnt++;
ROS_INFO("target publish successed...");
}
/*
重复发送最后一个关节位置：
在机械臂运动过程中，由于惯性和摩擦等因素，机械臂可能会稍微偏离目标位置。通过多次发送最后一个关节位置，可以确保机械臂能够更精确地到达目标位置，减少误差。
在某些情况下，机械臂可能会因为微小的扰动而产生抖动。通过多次发送相同的目标位置，可以减少这种抖动，使机械臂的运动更加平滑和稳定。
通过多次发送相同的目标位置，可以确认机械臂确实已经到达了目标位置。这对于某些应用场景（如精密装配、焊接等）非常重要。
*/
else{
if(count_final_joint <= count_keep_send)
{
ROS_INFO("final target publish...");
joint_pos_msg.joint[0] = p_joint1_.at(p2.vector_cnt-1);
joint_pos_msg.joint[1] = p_joint2_.at(p2.vector_cnt-1);
joint_pos_msg.joint[2] = p_joint3_.at(p2.vector_cnt-1);
joint_pos_msg.joint[3] = p_joint4_.at(p2.vector_cnt-1);
joint_pos_msg.joint[4] = p_joint5_.at(p2.vector_cnt-1);
joint_pos_msg.joint[5] = p_joint6_.at(p2.vector_cnt-1);
target_pub.publish(joint_pos_msg);
count_final_joint++;
}
else{
count_final_joint = 0;
p2.vector_cnt = 0;
p2.vector_len = 0;
point_changed = false;
ROS_INFO("final target publish end...");
ROS_INFO("Param reset...");
}
}
}
}
int main(int argc, char **argv) {
ros::init(argc, argv, "moveit_action_server_spline");
ros::NodeHandle nh;
// ros::AsyncSpinner spinner(1);
ROS_INFO("The main function begin successed....");
ros::Timer State_Timer;
count_final_joint = 0;
count_keep_send = wait_move_finish_time / min_interval;
//创建定时器，在指定时间间隔内调用回调函数，时间间隔与三次样条插值中插值的时间间隔rate一样,即机械臂0.01s运动到下一个点就0.01s后发布下一个点角度
State_Timer = nh.createTimer(ros::Duration(min_interval), timer_callback);
//创建发布节点，给驱动包发布关节角度
target_pub = nh.advertise<beginner_tutorials::JointPos>("/robot_driver/JointPos", 300);
// 话题名称一般为“<goup_name>_controller/follow_joint_trajectory”
// 创建 SimpleActionServer
// 对象(其实就是action了)并直接初始化(NodeHandle，话题名称，回调函数解析传入的目标值，服务器是否自启动)
Server moveit_server(nh, "rml_63_controller/follow_joint_trajectory",
boost::bind(&execute_callback, _1, &moveit_server),
false);
moveit_server.start();
ros::spin();
return 0;
}6.通过MoveGroupInterface接口给moveit发送关节目标和位姿目标进行规划和执行输出
moveit提供的这个接口可以对机械臂规划组进行相关的操作，如设置目标、设置最大速度/加速度、规划、执行、抓取、放置、获取当前状态等。同样在规划后会通过action client把数据点发布出去，moveit server同样可以接收到goal数据并进行处理。
🕐发送目标关节角给moveit进行轨迹规划并执行
//发送关节角给moveit进行轨迹规划并执行
#include<ros/ros.h>
#include<iostream>
#include<moveit/move_group_interface/move_group_interface.h> //Moveit的c++接口,调用其MoveGroupInterface类对规划组进行操作
//MoveGroupInterface类包含与机械臂规划组相关的操作，如设置目标、设置最大速度/加速度、规划、执行、抓取、放置、获取当前状态等。
using namespace std;
int main(int argc, char ** argv)
{
ros::init(argc, argv, "moveit_joints_pose_demo");
ros::NodeHandle nh;
//创建异步线程(1表示线程数)处理回调函数，即在不阻塞主线程的前提下在后台启动线程进行回调处理；
//适用于需要在主线程继续执行其他任务，并且希望回调函数能够被后台线程并行处理的场景，可以提高系统的实时性和响应能力。
//ros::spin()属于单线程
ros::AsyncSpinner spinner(1);
spinner.start();
moveit::planning_interface::MoveGroupInterface arm("rml_arm"); //传入规划组名，创建规划组的MoveGourpInterface对象，并用arm连接。
//可以创建多个对象
//moveit::planning_interface::MoveGroupInterface gripper("rml_gripper");
arm.setGoalJointTolerance(0.01); //设置容差
arm.setMaxVelocityScalingFactor(0.8); //设置最大关节速度比例因子；
vector<double> arm_joint_positions = {-0.664, -0.775, 0.657, -1.241, -0.473, -1.281}; //目标关节位置，弧度制
arm.setJointValueTarget(arm_joint_positions); //调用设置函数传入目标
//MoveGroupInterface::Plan：这是 MoveIt! 中的一个结构体，用于存储规划的路径（即机械臂从起始位置移动到目标位置的运动轨迹）；
moveit::planning_interface::MoveGroupInterface::Plan plan; //创建规划对象，plan：该对象用于存储规划好的路径（轨迹信息、关节值、执行时间等）
//arm.plan(plan)：调用 MoveGroupInterface 的 plan() 方法，为机械臂生成一条路径，并将结果存储在 plan 对象中，返回类型为MoveItErrorCode
bool success = (arm.plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("moveit_joint_pose_demo", "Visualizing plan 1 (joint space goal) %s", success ? "" : "FAILED" );
if(success){
arm.execute(plan);
}
ROS_INFO("Moving successfully....");
//还可以直接调用预设位置
/*
arm.setNamedTarget("Home");
arm.move(); //move()相当于plan并execute
*/
ros::shutdown(); //没有回调的话，执行完主程序记得把节点关闭
return 0;
}有个需要关注的点：如果你这个demo.cpp文件不在你开发机械臂的那个工作空间里面（就是说你随便在一个你的什么beginner_tutorials这么一个功能包里面创建的这个demo.cpp），就会导致一个问题，你这个功能包在rosrun beginner_tutorials demo.cpp的时候会报一个错误：
大致意思是它会去/opt/ros/noetic/share/rm_description路径下面找你的机械臂STL模型，虽然你.urdf文件里面已经通过相对路径“package://”的方式告诉它应该在该工作空间中寻找STL文件了，
filename="package://rm_description/meshes/RML63/base_link.STL" />但是由于你demo.cpp这个文件不在存放rm_description文件的那个工作空间下面，所以它就会去默认的/opt/ros/noetic/share/rm_description这个路径里面，所以就会报这个错误，虽然moveit也能成功规划和发布数据点，但是rviz里面没办法更新模型。
--------------------------------------------------------------------------------------------------------------------
🕑发送目标位姿给moveit进行轨迹规划并执行
前沿知识点：四元数和欧拉角。
四元数（Quaternion） 是一种常用的表示三维空间中旋转的数学工具。下面这个概念图可以理解为，x、y、z表示要旋转哪个轴，三者构成旋转轴的分量，w表示旋转的角度，整个四元数就可以表示目标的姿态了。
欧拉角（Roll, Pitch, Yaw）也是一种表达三维空间旋转的工具，与四元数之间存在转换关系，但是由于欧拉角存在“万向节锁”（可以粗略的理解为当旋转过程中两个旋转轴重合时，系统失去了一个自由度，使旋转操作受限），而四元数可以避免这个问题（这里就不展开了），因此在机器人规划中常用四元数表示旋转。
代码实现：
#include <ros/ros.h>
#include <iostream>
#include <moveit/move_group_interface/move_group_interface.h>
#include <geometry_msgs/PoseStamped.h> //位姿数据类型pose.position/pose.orientation
#include <tf2/LinearMath/Quaternion.h> //处理四元数的相关数学运算，如将欧拉角转换为四元数
using namespace std;
int main(int argc, char** argv)
{
ros::init(argc, argv, "moveit_pose_demo");
ros::NodeHandle nh;
ros::AsyncSpinner spinner(2);
spinner.start();
//创建运动规划接口对象arm并连接到规划组rml_arm
moveit::planning_interface::MoveGroupInterface arm("rml_arm");
//获取规划组的规划参考系，一般为基坐标系
//运动规划中的所有目标位置、路径规划都是相对于这个参考系来定义的。
string planning_frame = arm.getPlanningFrame();
cout<<"Planning frame: "<<planning_frame<<endl;
//获取末端关节或末端执行器的link
//这用于设定目标位置和姿态时，定义相对于哪个链接的位姿。
//当我们使用 setPoseTarget() 或 setJointValueTarget() 设定目标位置时，默认使用末端执行器的链接作为参考。
//通过获取末端执行器链接名称，可以明确是哪一个链接在进行规划，避免使用默认值导致错误。
string eef_link = arm.getEndEffectorLink();
cout<<"End effector link: "<<eef_link<<endl;
//允许在规划失败后重新规划
arm.allowReplanning(true);
//容差，最大速度比例因子
arm.setGoalPositionTolerance(0.02);
arm.setGoalOrientationTolerance(0.02);
arm.setMaxVelocityScalingFactor(0.8);
geometry_msgs::PoseStamped target_pose;
target_pose.header.frame_id = planning_frame;
target_pose.header.stamp = ros::Time::now();
target_pose.pose.position.x = 0.3;
target_pose.pose.position.y = 0.1;
target_pose.pose.position.z = 0.25;
tf2::Quaternion quaternion; //创建四元数对象
quaternion.setRPY(0, 3.1415926/2.0, 0); //把这个欧拉角转换成四元数了
//setRPY(roll, pitch, yaw)，根据欧拉角（RPY，Roll-Pitch-Yaw），分别表示绕 x、y、z 轴的旋转
//这个四元数表示的是绕 y 轴顺时针旋转 90 度的旋转
target_pose.pose.orientation.x = quaternion.x();
target_pose.pose.orientation.y = quaternion.y();
target_pose.pose.orientation.z = quaternion.z();
target_pose.pose.orientation.w = quaternion.w();
//把开始状态设置为机械臂的当前状态
arm.setStartStateToCurrentState();
//传入目标、规划、执行
arm.setPoseTarget(target_pose);
moveit::planning_interface::MoveGroupInterface::Plan plan;
bool success = (arm.plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("moveit_pose_demo", "Visualizing plan 1 (joint space goal) %s", success ? "" : "FAILED" );
if(success){
arm.execute(plan);
}
ROS_INFO("Moving successfully....");
//获取末端关节或执行器的当前位姿
geometry_msgs::PoseStamped current_pose = arm.getCurrentPose();
//获取当前六个关节角度
vector<double> current_joint_positions = arm.getCurrentJointValues();
//完成上面之后还可以接着设定新目标
ROS_INFO("Move forward 5cm ...");
target_pose = current_pose; //由于误差的存在，执行结束之后的current_pose与target_pose还是会有点偏差的，这也是为什么在三次样条那一节重复发布最后一次目标状态那么多次
target_pose.pose.position.x += 0.05;
arm.setPoseTarget(target_pose);
arm.move();
ROS_INFO("Move to Home...");
arm.setNamedTarget("Home");
arm.move();
ros::shutdown(); //没有回调的话，执行完主程序记得把节点关闭，要不然spinner也会一直在等待有没有数据进来可以回调的
return 0;
}CMakeLists.txt编译文件主要得把moveit一些内核和接口工具包包含进去：
cmake_minimum_required(VERSION 3.0.2)
project(rm_moveit_demo)
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
## 重点在这👇
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
moveit_msgs
moveit_core
moveit_ros_planning_interface
moveit_visual_tools
)
## System dependencies are found with CMake's conventions
find_package(Boost REQUIRED COMPONENTS system)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
generate_messages(
DEPENDENCIES
std_msgs
moveit_msgs
)
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES rm_moveit_demo
# CATKIN_DEPENDS roscpp rospy std_msgs
CATKIN_DEPENDS message_runtime moveit_msgs
# DEPENDS system_lib
)
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
include
${CMAKE_SOURCE_DIR}
${catkin_INCLUDE_DIRS}
${Boost_INCLUDE_DIRS}
)
## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/rm_moveit_demo.cpp
# )
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/rm_moveit_demo_node.cpp)
add_executable(moveit_pose_demo src/moveit_pose_demo.cpp)
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(moveit_pose_demo ${rm_moveit_demo_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
# ${catkin_LIBRARIES}
# )
target_link_libraries(moveit_pose_demo ${catkin_LIBRARIES})
package.xml依赖文件也是把moveit接口的几个依赖加进来就行：
<?xml version="1.0"?>
<package format="2">
<name>rm_moveit_demo</name>
<version>0.0.0</version>
<description>The rm_moveit_demo package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="robot@todo.todo">robot</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<!-- 在这里👇 -->
<build_export_depend>moveit_msgs</build_export_depend>
<exec_depend>moveit_msgs</exec_depend>
<build_depend>moveit_msgs</build_depend>
<build_depend>moveit_core</build_depend>
<build_depend>moveit_ros_planning_interface</build_depend>
<build_depend>moveit_visual_tools</build_depend>
<exec_depend>moveit_core</exec_depend>
<exec_depend>moveit_ros_planning_interface</exec_depend>
<exec_depend>moveit_visual_tools</exec_depend>
<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>
<exec_depend>message_generation</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
7.ros_control实现硬件控制
学习这个大模块之前，先来一点知识铺垫：
电机运动大概需要这么一个流程：上位机（Ubuntu下的ros）发送一个我们可以理解的target speed 或 target position数据出来，给到Motor Controller微控制器（也就是熟悉的stm32）进行PID控制算法运算将收到的上位机的target数据转换成PWM信号，然后输入(PWM In)到Motor Driver（电机驱动器，由于电机运动需要较大的电压电流供应，驱动器可以起到一个电压信号放大的作用）中，然后将放大的PWM信号输出(PWM out)给电机执行运动。
而ros_control这个框架在整个运动控制链中的作用呢，相当于集成了Motor Controller那部分功能，实现了PID运动控制算法将target数据转换成PWM信号输出给Motor Driver来驱动电机运动。
那就好说了，暂且不用学stm32的固件代码了。但是呢，存在及合理，stm32既然能一直存在那必然有其不可替代的原因，使用ros_control虽好，集成度高、框架清晰明了、控制方便，但是有几点问题：🕐上位机运算压力变大了，之前上位机只需要处理轨迹规划、运动学解算等高级目标，现在把硬件控制集成之后，就相当于增加了PID运动控制算法的实现这些事儿了；🕑实时性可能会变差，stm32这种嵌入式微控制器的频率听说达到1k~10kHz左右，而上位机的控制频率在100~1000Hz，也就会带来延迟性的问题。
但是呢，存在及合理，在一些不需要这么强的实时性的场合下，利用ros_control做开发也是很不错的。而且ros2_control肯定实时性更强，之后可能是个趋势。
----------------------------------------------------------------------------------------------------------------
经过一段时间摸索，似乎这个框架没有那么必要，框架之中也得实现硬件层面的驱动编写，笔者目前学术短浅暂且感受不到这个框架的优势，因此这部分先搁置，以后如果发现它实实在在好用的点再更新。
8.关节驱动控制
前面已经实现了moevit和运动轨迹规划和解算，实现了与实体机器人数据交互的条件，也完成了三次样条插值细化轨迹点并发布的目标，同时自中向上通过MoveGroupInterface接口也实现了关节目标和位置目标的初步控制。
因此机械臂的大框架基本实现了，这部分就是将上面那些层面实现的东西发送给电机进行真正的硬件驱动了。这里采用的是达妙的电机，之前笔者一直在思考和搜集关于如何写电机驱动控制的这部分代码，经过一段时间的思考发现，这部分嵌入式的通信按道理来说并不需要作为机械臂开发的我们去实现才对呀，这种具有标准格式的通信程序关节厂商应该已经写好了才对，我买这个关节应该只需要去知道我应该调用它控制类的哪个成员函数来传递我规划出来的关节角度就行。所以不要本末倒置了哦，要清除自己在干嘛，需要做什么。
driver_main.cpp实现：
#include "ros/ros.h"
#include "std_msgs/String.h"
#include <rm_control/JointPos.h>
#include "rm_driver/damiao.h"
#include "unistd.h"
#include <cmath>
#include <algorithm> // for std::max and std::min
#include <iostream> // for std::cout
// 限幅函数
void clamp(float* value, float min_value, float max_value) {
if (*value < min_value) {
*value = min_value;
} else if (*value > max_value) {
*value = max_value;
}
}
damiao::Motor M1(damiao::DM4310,0x01, 0x11);
damiao::Motor M2(damiao::DM4310,0x02, 0x12);
damiao::Motor M3(damiao::DM4310,0x03, 0x13);
damiao::Motor M4(damiao::DM4310,0x04, 0x14);
damiao::Motor M5(damiao::DM4310,0x05, 0x15);
damiao::Motor M6(damiao::DM4310,0x06, 0x16);
std::shared_ptr<SerialPort> serial;
damiao::Motor_Control dm(serial);
void jointstatesCallback(const rm_control::JointPosConstPtr& msg)
{
float pos[6];
// pos=msg.position;
pos[0]=msg->joint[0];
pos[1]=msg->joint[1];
pos[2]=msg->joint[2];
pos[3]=msg->joint[3];
pos[4]=msg->joint[4];
pos[5]=msg->joint[5];
clamp(&pos[0], -2.f, 2.f);//第一个关节
pos[1]=4.673 - pos[1];
clamp(&pos[1], -1.2f, 1.2f);//第二个关节
clamp(&pos[2], -3.f, 0);//第三个关节
clamp(&pos[3], -2.5f, 2.5f);//第四个关节
clamp(&pos[4], -1.4f, 1.4f);//第五个关节
dm.control_pos_vel(M6, pos[5], 1);
dm.control_pos_vel(M5, pos[4], 1);
dm.control_pos_vel(M4, pos[3], 1);
dm.control_pos_vel(M3, pos[2], 1);
dm.control_pos_vel(M2, pos[1], 1);
dm.control_pos_vel(M1, pos[0], 1);
// dm.control_pos_vel(M2, pos[0], 5);
ROS_INFO("[%f] [%f] [%f] [%f] [%f] [%f] ",pos[0],pos[1],pos[2],pos[3],pos[4],pos[5]);
}
int main(int argc, char *argv[])
{
ros::init(argc,argv,"RRdriver");
ros::NodeHandle nh;
serial = std::make_shared<SerialPort>("/dev/ttyACM0", B921600);
dm = damiao::Motor_Control(serial);
dm.addMotor(&M1);
dm.addMotor(&M2);
dm.addMotor(&M3);
dm.addMotor(&M4);
dm.addMotor(&M5);
dm.addMotor(&M6);
if(*argv[1] == 0)
{
dm.set_zero_position(M1);
dm.set_zero_position(M2);
dm.set_zero_position(M3);
dm.set_zero_position(M4);
dm.set_zero_position(M5);
dm.set_zero_position(M6);
dm.refresh_motor_status(M1);
dm.refresh_motor_status(M2);
dm.refresh_motor_status(M3);
dm.refresh_motor_status(M4);
dm.refresh_motor_status(M5);
dm.refresh_motor_status(M6);
std::cout<<"motor1--- POS:"<<M1.Get_Position()<<" VEL:"<<M1.Get_Velocity()<<" CUR:"<<M1.Get_tau()<<std::endl;
std::cout<<"motor2--- POS:"<<M2.Get_Position()<<" VEL:"<<M2.Get_Velocity()<<" CUR:"<<M2.Get_tau()<<std::endl;
std::cout<<"motor3--- POS:"<<M3.Get_Position()<<" VEL:"<<M3.Get_Velocity()<<" CUR:"<<M3.Get_tau()<<std::endl;
std::cout<<"motor4--- POS:"<<M4.Get_Position()<<" VEL:"<<M4.Get_Velocity()<<" CUR:"<<M4.Get_tau()<<std::endl;
std::cout<<"motor5--- POS:"<<M5.Get_Position()<<" VEL:"<<M5.Get_Velocity()<<" CUR:"<<M5.Get_tau()<<std::endl;
std::cout<<"motor6--- POS:"<<M6.Get_Position()<<" VEL:"<<M6.Get_Velocity()<<" CUR:"<<M6.Get_tau()<<std::endl;
}
// dm.addMotor(&M5);
dm.enable(M1);
// dm.enable(M2);
dm.enable(M2);
dm.enable(M3);
dm.enable(M4);
dm.enable(M5);
dm.enable(M6);
sleep(0.01);
// dm.enable(M5);
dm.switchControlMode(M1, damiao::POS_VEL_MODE);
dm.switchControlMode(M2, damiao::POS_VEL_MODE);
dm.switchControlMode(M3, damiao::POS_VEL_MODE);
dm.switchControlMode(M4, damiao::POS_VEL_MODE);
dm.switchControlMode(M5, damiao::POS_VEL_MODE);
dm.switchControlMode(M6, damiao::POS_VEL_MODE);
sleep(0.01);
// dm.switchControlMode(M5, damiao::POS_VEL_MODE);
// dm.save_motor_param(M1);
// dm.save_motor_param(M2);
sleep(3);
ros::Subscriber target_sub = nh.subscribe("/robot_driver/JointPos",300,jointstatesCallback);
ros::spin();
return 0;
}
damiao.h根据电机通信协议实现的头文件：
#ifndef DAMIAO_H
#define DAMIAO_H
#include "SerialPort.h"
#include <cmath>
#include <utility>
#include <vector>
#include <unordered_map>
#include <array>
#include <variant>
#include <cstdint>
#include <cmath>
#define POS_MODE 0x100
#define SPEED_MODE 0x200
#define POSI_MODE 0x300
#define max_retries 20
#define retry_interval 50000
namespace damiao
{
#pragma pack(1)
#define Motor_id uint32_t
/*!
* @brief Motor Type 电机类型
*/
enum DM_Motor_Type
{
DM4310,
DM4310_48V,
DM4340,
DM4340_48V,
DM6006,
DM8006,
DM8009,
DM10010L,
DM10010,
DMH3510,
DMH6215,
DMG6220,
Num_Of_Motor
};
/*
* @brief 电机控制模式
*/
enum Control_Mode
{
MIT_MODE=1,
POS_VEL_MODE=2,
VEL_MODE=3,
POS_FORCE_MODE=4,
};
/*
* @brief 寄存器列表 具体参考达妙手册
*/
enum DM_REG
{
UV_Value = 0,
KT_Value = 1,
OT_Value = 2,
OC_Value = 3,
ACC = 4,
DEC = 5,
MAX_SPD = 6,
MST_ID = 7,
ESC_ID = 8,
TIMEOUT = 9,
CTRL_MODE = 10,
Damp = 11,
Inertia = 12,
hw_ver = 13,
sw_ver = 14,
SN = 15,
NPP = 16,
Rs = 17,
LS = 18,
Flux = 19,
Gr = 20,
PMAX = 21,
VMAX = 22,
TMAX = 23,
I_BW = 24,
KP_ASR = 25,
KI_ASR = 26,
KP_APR = 27,
KI_APR = 28,
OV_Value = 29,
GREF = 30,
Deta = 31,
V_BW = 32,
IQ_c1 = 33,
VL_c1 = 34,
can_br = 35,
sub_ver = 36,
u_off = 50,
v_off = 51,
k1 = 52,
k2 = 53,
m_off = 54,
dir = 55,
p_m = 80,
xout = 81,
};
typedef struct
{
uint8_t FrameHeader;
uint8_t CMD;// 命令 0x00: 心跳
// 0x01: receive fail 0x11: receive success
// 0x02: send fail 0x12: send success
// 0x03: set baudrate fail 0x13: set baudrate success
// 0xEE: communication error 此时格式段为错误码
// 8: 超压 9: 欠压 A: 过流 B: MOS过温 C: 电机线圈过温 D: 通讯丢失 E: 过载
uint8_t canDataLen: 6; // 数据长度
uint8_t canIde: 1; // 0: 标准帧 1: 扩展帧
uint8_t canRtr: 1; // 0: 数据帧 1: 远程帧
uint32_t canId; // 电机反馈的ID
uint8_t canData[8];
uint8_t frameEnd; // 帧尾
} CAN_Receive_Frame;
typedef struct can_send_frame
{
uint8_t FrameHeader[2] = {0x55, 0xAA}; // 帧头
uint8_t FrameLen = 0x1e; // 帧长
uint8_t CMD = 0x03; // 命令 1：转发CAN数据帧 2：PC与设备握手，设备反馈OK 3: 非反馈CAN转发，不反馈发送状态
uint32_t sendTimes = 1; // 发送次数
uint32_t timeInterval = 10; // 时间间隔
uint8_t IDType = 0; // ID类型 0：标准帧 1：扩展帧
uint32_t canId=0x01; // CAN ID 使用电机ID作为CAN ID
uint8_t frameType = 0; // 帧类型 0： 数据帧 1：远程帧
uint8_t len = 0x08; // len
uint8_t idAcc=0;
uint8_t dataAcc=0;
uint8_t data[8]={0};
uint8_t crc=0; // 未解析，任意值
void modify(const Motor_id id, const uint8_t* send_data)
{
canId = id;
std::copy(send_data, send_data+8, data);
}
} can_send_frame;
#pragma pack()
typedef struct
{
float Q_MAX;
float DQ_MAX;
float TAU_MAX;
}Limit_param;
//电机PMAX DQMAX TAUMAX参数
Limit_param limit_param[Num_Of_Motor]=
{
{12.5, 30, 10 }, // DM4310
{12.5, 50, 10 }, // DM4310_48V
{12.5, 8, 28 }, // DM4340
{12.5, 10, 28 }, // DM4340_48V
{12.5, 45, 20 }, // DM6006
{12.5, 45, 40 }, // DM8006
{12.5, 45, 54 }, // DM8009
{12.5,25, 200}, // DM10010L
{12.5,20, 200}, // DM10010
{12.5,280,1}, // DMH3510
{12.5,45,10}, // DMH6215
{12.5,45,10} // DMG6220
};
class Motor
{
private:
/* data */
Motor_id Master_id;
Motor_id Slave_id;
float state_q=0;
float state_dq=0;
float state_tau=0;
Limit_param limit_param{};
DM_Motor_Type Motor_Type;
union ValueUnion {
float floatValue;
uint32_t uint32Value;
};
struct ValueType {
ValueUnion value;
bool isFloat;
};
std::unordered_map<uint32_t , ValueType> param_map;
public:
/**
* @brief Construct a new Motor object
*
* @param Motor_Type 电机类型
* @param Slave_id canId 从机ID即电机ID
* @param Master_id 主机ID建议主机ID不要都设为0x00
*
*/
Motor(DM_Motor_Type Motor_Type, Motor_id Slave_id, Motor_id Master_id)
: Master_id(Master_id), Slave_id(Slave_id), Motor_Type(Motor_Type) {
this->limit_param = damiao::limit_param[Motor_Type];
}
Motor() : Master_id(0x01), Slave_id(0x11), Motor_Type(DM4310) {
this->limit_param = damiao::limit_param[DM4310];
}
void receive_data(float q, float dq, float tau)
{
this->state_q = q;
this->state_dq = dq;
this->state_tau = tau;
}
DM_Motor_Type GetMotorType() const { return this->Motor_Type; }
/*
* @brief get master id 获取主机ID
* @return MasterID
*/
Motor_id GetMasterId() const { return this->Master_id; }
/*
* @brief get motor slave id(can id) 获取电机CAN ID
* @return SlaveID
*/
Motor_id GetSlaveId() const { return this->Slave_id; }
/*
* @brief get motor position 获取电机位置
* @return motor position 电机位置
*/
float Get_Position() const { return this->state_q; }
/*
* @brief get motor velocity 获取电机速度
* @return motor velocity 电机速度
*/
float Get_Velocity() const { return this->state_dq; }
/*
* @brief get torque of the motor 获取电机实际输出扭矩
* @return motor torque 电机实际输出扭矩
*/
float Get_tau() const { return this->state_tau; }
/*
* @brief get limit param 获取电机限制参数
* @return limit_param 电机限制参数
*/
Limit_param get_limit_param() { return limit_param; }
void set_param(int key, float value)
{
ValueType v{};
v.value.floatValue = value;
v.isFloat = true;
param_map[key] = v;
}
void set_param(int key, uint32_t value)
{
ValueType v{};
v.value.uint32Value = value;
v.isFloat = false;
param_map[key] = v;
}
float get_param_as_float(int key) const
{
auto it = param_map.find(key);
if (it != param_map.end())
{
if (it->second.isFloat)
{
return it->second.value.floatValue;
}
else
{
return 0;
}
}
return 0;
}
uint32_t get_param_as_uint32(int key) const {
auto it = param_map.find(key);
if (it != param_map.end()) {
if (!it->second.isFloat) {
return it->second.value.uint32Value;
}
else
{
return 0;
}
}
return 0;
}
bool is_have_param(int key) const
{
return param_map.find(key) != param_map.end();
}
};
/**
* @brief motor control class 电机控制类
*
* 使用USB转CAN进行通信，linux做虚拟串口
*/
class Motor_Control
{
public:
/*
* @brief 定义电机控制对象
* @param serial 串口对象
* 默认串口为/dev/ttyACM0
*/
Motor_Control(SerialPort::SharedPtr serial = nullptr): serial_(std::move(serial))
{
if (serial_ == nullptr) {
//default serial port
serial_ = std::make_shared<SerialPort>("/dev/ttyACM0", B921600);
}
}
~Motor_Control()
= default;
/*
* @brief enable the motor 使能电机
* @param motor 电机对象
*/
void enable(const Motor& motor)
{
control_cmd(motor.GetSlaveId(), 0xFC);
usleep(100000);//100ms
this->receive();
}
/*
* @brief enable motor which is old version 使能达妙旧款电机固件 使用旧版本固件建议尽快升级成新版本
* @param motor object 电机对象
* @param mode 控制模式 damiao::MIT_MODE, damiao::POS_VEL_MODE, damiao::VEL_MODE, damiao::POS_FORCE_MODE
*/
void enable_old(const Motor& motor, Control_Mode mode)
{
uint32_t id = ((mode -1) << 2) + motor.GetSlaveId();
control_cmd(id, 0xFC);
usleep(100000);
this->receive();
}
/*
* @brief refresh motor status 刷新电机状态
* @param motor object 电机对象
*/
void refresh_motor_status(const Motor& motor)
{
uint32_t id = 0x7FF;
uint8_t can_low = motor.GetSlaveId() & 0xff; // id low 8 bit
uint8_t can_high = (motor.GetSlaveId() >> 8) & 0xff; //id high 8 bit
std::array<uint8_t, 8> data_buf = {can_low,can_high, 0xCC, 0x00, 0x00, 0x00, 0x00, 0x00};
send_data.modify(id, data_buf.data());
serial_->send((uint8_t*)&send_data, sizeof(can_send_frame));
this->receive();
}
/*
* @brief disable the motor 失能电机
* @param motor object 电机对象
*/
void disable(const Motor& motor) {
control_cmd(motor.GetSlaveId(), 0xFD);
usleep(100000);
this->receive();
}
/*
* @brief set the now position as zero point 保存当前位置为电机零点
* @param motor object 电机对象
*/
void set_zero_position(const Motor& motor)
{
control_cmd(motor.GetSlaveId(), 0xFE);
usleep(100000);
this->receive();
}
/* @description: MIT Control Mode MIT控制模式 具体描述请参考达妙手册
*@param kp: 比例系数
*@param kd: 微分系数
*@param q: position 位置
*@param dq: velocity 速度
*@param tau: torque 扭矩
*/
void control_mit(Motor &DM_Motor, float kp, float kd, float q, float dq, float tau)
{
// 位置、速度和扭矩采用线性映射的关系将浮点型数据转换成有符号的定点数据
static auto float_to_uint = [](float x, float xmin, float xmax, uint8_t bits) -> uint16_t {
float span = xmax - xmin;
float data_norm = (x - xmin) / span;
uint16_t data_uint = data_norm * ((1 << bits) - 1);
return data_uint;
};
Motor_id id = DM_Motor.GetSlaveId();
if(motors.find(id) == motors.end())
{
throw std::runtime_error("Motor_Control id not found");
}
auto& m = motors[id];
uint16_t kp_uint = float_to_uint(kp, 0, 500, 12);
uint16_t kd_uint = float_to_uint(kd, 0, 5, 12);
Limit_param limit_param_cmd = m->get_limit_param();
uint16_t q_uint = float_to_uint(q, -limit_param_cmd.Q_MAX, limit_param_cmd.Q_MAX, 16);
uint16_t dq_uint = float_to_uint(dq, -limit_param_cmd.DQ_MAX,limit_param_cmd.DQ_MAX, 12);
uint16_t tau_uint = float_to_uint(tau, -limit_param_cmd.TAU_MAX, limit_param_cmd.TAU_MAX, 12);
std::array<uint8_t, 8> data_buf{};
data_buf[0] = (q_uint >> 8) & 0xff;
data_buf[1] = q_uint & 0xff;
data_buf[2] = dq_uint >> 4;
data_buf[3] = ((dq_uint & 0xf) << 4) | ((kp_uint >> 8) & 0xf);
data_buf[4] = kp_uint & 0xff;
data_buf[5] = kd_uint >> 4;
data_buf[6] = ((kd_uint & 0xf) << 4) | ((tau_uint >> 8) & 0xf);
data_buf[7] = tau_uint & 0xff;
send_data.modify(id, data_buf.data());
serial_->send((uint8_t*)&send_data, sizeof(can_send_frame));
this->receive();
}
/*
* @description: Position Control Mode with velocity 位置速度控制模式
* @param pos: position 位置
* @param vel: velocity 速度
* @param DM_Motor: Motor object 电机对象
*/
void control_pos_vel(Motor &DM_Motor,float pos,float vel)
{
Motor_id id = DM_Motor.GetSlaveId();
if(motors.find(id) == motors.end())
{
throw std::runtime_error("POS_VEL ERROR : Motor_Control id not found");
}
std::array<uint8_t, 8> data_buf{};
memcpy(data_buf.data(), &pos, sizeof(float));
memcpy(data_buf.data() + 4, &vel, sizeof(float));
id += POS_MODE;
send_data.modify(id, data_buf.data());
serial_->send(reinterpret_cast<uint8_t*>(&send_data), sizeof(can_send_frame));
this->receive();
}
/*
* @description: velocity control mode 速度控制模式
* @param DM_Motor: motor object 电机对象
* @param vel: velocity 速度
*/
void control_vel(Motor &DM_Motor,float vel)
{
Motor_id id =DM_Motor.GetSlaveId();
if(motors.find(id) == motors.end())
{
throw std::runtime_error("VEL ERROR : id not found");
}
std::array<uint8_t, 8> data_buf = {0};
memcpy(data_buf.data(), &vel, sizeof(float));
id=id+SPEED_MODE;
send_data.modify(id, data_buf.data());
serial_->send((uint8_t*)&send_data, sizeof(can_send_frame));
this->receive();
}
/*
* @description: Position control mode with torque 力位混合控制模式
* @param DM_Motor: motor object 电机对象
* @param pos: position 位置
* @param vel: velocity 速度 范围0-10000 具体参考达妙手册
* @param i: current 电流 范围0-10000具体参考达妙手册
*/
void control_pos_force(Motor &DM_Motor,float pos, uint16_t vel, uint16_t i)
{
Motor_id id =DM_Motor.GetSlaveId();
if(motors.find(id) == motors.end())
{
throw std::runtime_error("pos_force ERROR : Motor_Control id not found");
}
std::array<uint8_t, 8> data_buf{};
memcpy(data_buf.data(), &pos, sizeof(float));
memcpy(data_buf.data() + 4, &vel, sizeof(uint16_t));
memcpy(data_buf.data() + 6, &i, sizeof(uint16_t));
id=id+POSI_MODE;
send_data.modify(id, data_buf.data());
serial_->send((uint8_t*)&send_data, sizeof(can_send_frame));
this->receive();
}
/*
* @description 函数库内部调用,用于解算电机can线返回的电机参数
*/
void receive()
{
serial_->recv((uint8_t*)&receive_data, 0xAA, sizeof(CAN_Receive_Frame));
if(receive_data.CMD == 0x11 && receive_data.frameEnd == 0x55) // receive success
{
static auto uint_to_float = [](uint16_t x, float xmin, float xmax, uint8_t bits) -> float {
float span = xmax - xmin;
float data_norm = float(x) / ((1 << bits) - 1);
float data = data_norm * span + xmin;
return data;
};
auto & data = receive_data.canData;
uint16_t q_uint = (uint16_t(data[1]) << 8) | data[2];
uint16_t dq_uint = (uint16_t(data[3]) << 4) | (data[4] >> 4);
uint16_t tau_uint = (uint16_t(data[4] & 0xf) << 8) | data[5];
if(receive_data.canId != 0x00) //make sure the motor id is not 0x00
{
if(motors.find(receive_data.canId) == motors.end())
{
return;
}
auto m = motors[receive_data.canId];
Limit_param limit_param_receive = m->get_limit_param();
float receive_q = uint_to_float(q_uint, -limit_param_receive.Q_MAX, limit_param_receive.Q_MAX, 16);
float receive_dq = uint_to_float(dq_uint, -limit_param_receive.DQ_MAX, limit_param_receive.DQ_MAX, 12);
float receive_tau = uint_to_float(tau_uint, -limit_param_receive.TAU_MAX, limit_param_receive.TAU_MAX, 12);
m->receive_data(receive_q, receive_dq, receive_tau);
}
else //why the user set the masterid as 0x00 ???
{
uint32_t slaveID = data[0] & 0x0f;
if(motors.find(slaveID) == motors.end())
{
return;
}
auto m = motors[slaveID];
Limit_param limit_param_receive = m->get_limit_param();
float receive_q = uint_to_float(q_uint, -limit_param_receive.Q_MAX, limit_param_receive.Q_MAX, 16);
float receive_dq = uint_to_float(dq_uint, -limit_param_receive.DQ_MAX, limit_param_receive.DQ_MAX, 12);
float receive_tau = uint_to_float(tau_uint, -limit_param_receive.TAU_MAX, limit_param_receive.TAU_MAX, 12);
m->receive_data(receive_q, receive_dq, receive_tau);
}
return;
}
else if (receive_data.CMD == 0x01) // receive fail
{
/* code */
}
else if (receive_data.CMD == 0x02) // send fail
{
/* code */
}
else if (receive_data.CMD == 0x03) // send success
{
/* code */
}
else if (receive_data.CMD == 0xEE) // communication error
{
/* code */
}
}
void receive_param()
{
serial_->recv((uint8_t*)&receive_data, 0xAA, sizeof(CAN_Receive_Frame));
if(receive_data.CMD == 0x11 && receive_data.frameEnd == 0x55) // receive success
{
auto & data = receive_data.canData;
if(data[2]==0x33 or data[2]==0x55)
{
uint32_t slaveID = (uint32_t(data[1]) << 8) | data[0];
uint8_t RID = data[3];
if (motors.find(slaveID) == motors.end())
{
//can not found motor id
return;
}
if(is_in_ranges(RID))
{
uint32_t data_uint32 = (uint32_t(data[7]) << 24) | (uint32_t(data[6]) << 16) | (uint32_t(data[5]) << 8) | data[4];
motors[slaveID]->set_param(RID, data_uint32);
}
else
{
float data_float = uint8_to_float(data + 4);
motors[slaveID]->set_param(RID, data_float);
}
}
return ;
}
}
/**
* @brief add motor to class 添加电机
* @param DM_Motor : motor object 电机对象
*/
void addMotor(Motor *DM_Motor)
{
motors.insert({DM_Motor->GetSlaveId(), DM_Motor});
if (DM_Motor->GetMasterId() != 0)
{
motors.insert({DM_Motor->GetMasterId(), DM_Motor});
}
}
/*
* @description: read motor register param 读取电机内部寄存器参数，具体寄存器列表请参考达妙的手册
* @param DM_Motor: motor object 电机对象
* @param RID: register id 寄存器ID example: damiao::UV_Value
* @return: motor param 电机参数 如果没查询到返回的参数为0
*/
float read_motor_param(Motor &DM_Motor,uint8_t RID)
{
uint32_t id = DM_Motor.GetSlaveId();
uint8_t can_low = id & 0xff;
uint8_t can_high = (id >> 8) & 0xff;
std::array<uint8_t, 8> data_buf{can_low, can_high, 0x33, RID, 0x00, 0x00, 0x00, 0x00};
send_data.modify(0x7FF, data_buf.data());
serial_->send((uint8_t*)&send_data, sizeof(can_send_frame));
for(uint8_t i =0;i<max_retries;i++)
{
usleep(retry_interval);
receive_param();
if (motors[DM_Motor.GetSlaveId()]->is_have_param(RID))
{
if (is_in_ranges(RID))
{
return float(motors[DM_Motor.GetSlaveId()]->get_param_as_uint32(RID));
}
else
{
return motors[DM_Motor.GetSlaveId()]->get_param_as_float(RID);
}
}
}
return 0;
}
/*
* @description: switch control mode 切换电机控制模式
* @param DM_Motor: motor object 电机对象
* @param mode: control mode 控制模式 like:damiao::MIT_MODE, damiao::POS_VEL_MODE, damiao::VEL_MODE, damiao::POS_FORCE_MODE
*/
bool switchControlMode(Motor &DM_Motor,Control_Mode mode)
{
uint8_t write_data[4]={(uint8_t)mode, 0x00, 0x00, 0x00};
uint8_t RID = 10;
write_motor_param(DM_Motor,RID,write_data);
if (motors.find(DM_Motor.GetSlaveId()) == motors.end())
{
return false;
}
for(uint8_t i =0;i<max_retries;i++)
{
usleep(retry_interval);
receive_param();
if (motors[DM_Motor.GetSlaveId()]->is_have_param(RID))
{
return motors[DM_Motor.GetSlaveId()]->get_param_as_uint32(RID) == mode;
}
}
return false;
}
/*
* @description: change motor param 修改电机内部寄存器参数 具体寄存器列表请参考达妙手册
* @param DM_Motor: motor object 电机对象
* @param RID: register id 寄存器ID
* @param data: param data 参数数据,大部分数据是float类型，其中如果是uint32类型的数据也可以直接输入整型的就行，函数内部有处理
* @return: bool true or false 是否修改成功
*/
bool change_motor_param(Motor &DM_Motor,uint8_t RID,float data)
{
if(is_in_ranges(RID)) {
//居然传进来的是整型的范围 救一下
uint32_t data_uint32 = float_to_uint32(data);
uint8_t *data_uint8;
data_uint8=(uint8_t*)&data_uint32;
write_motor_param(DM_Motor,RID,data_uint8);
}
else
{
//is float
uint8_t *data_uint8;
data_uint8=(uint8_t*)&data;
write_motor_param(DM_Motor,RID,data_uint8);
}
if (motors.find(DM_Motor.GetSlaveId()) == motors.end())
{
return false;
}
for(uint8_t i =0;i<max_retries;i++)
{
usleep(retry_interval);
receive_param();
if (motors[DM_Motor.GetSlaveId()]->is_have_param(RID))
{
if (is_in_ranges(RID))
{
return motors[DM_Motor.GetSlaveId()]->get_param_as_uint32(RID) == float_to_uint32(data);
}
else
{
return fabsf(motors[DM_Motor.GetSlaveId()]->get_param_as_float(RID) - data)<0.1f;
}
}
}
return false;
}
/*
* @description: save all param to motor flash 保存电机的所有参数到flash里面
* @param DM_Motor: motor object 电机对象
* 电机默认参数不会写到flash里面，需要进行写操作
*/
void save_motor_param(Motor &DM_Motor)
{
disable(DM_Motor);
uint32_t id = DM_Motor.GetSlaveId();
uint8_t id_low = id & 0xff;
uint8_t id_high = (id >> 8) & 0xff;
std::array<uint8_t, 8> data_buf{id_low, id_high, 0xAA, 0x01, 0x00, 0x00, 0x00, 0x00};
send_data.modify(0x7FF, data_buf.data());
serial_->send((uint8_t*)&send_data, sizeof(can_send_frame));
usleep(100000);//100ms wait for save
}
/*
* @description: change motor limit param 修改电机限制参数，这个修改的不是电机内部的寄存器参数，而是电机的限制参数
* @param DM_Motor: motor object 电机对象
* @param P_MAX: position max 位置最大值
* @param Q_MAX: velocity max 速度最大值
* @param T_MAX: torque max 扭矩最大值
*/
static void changeMotorLimit(Motor &DM_Motor,float P_MAX,float Q_MAX,float T_MAX)
{
limit_param[DM_Motor.GetMotorType()]={P_MAX,Q_MAX,T_MAX};
}
/*
* @description: change motor PMAX 修改电机的最大PMAX
* @param DM_Motor: motor object 电机对象
* @param P_MAX: position max 位置最大值
*/
static void changeMotorPMAX(Motor &DM_Motor,float P_MAX)
{
limit_param[DM_Motor.GetMotorType()].Q_MAX=P_MAX;
}
private:
void control_cmd(Motor_id id , uint8_t cmd)
{
std::array<uint8_t, 8> data_buf = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, cmd};
send_data.modify(id, data_buf.data());
serial_->send((uint8_t*)&send_data, sizeof(can_send_frame));
}
void write_motor_param(Motor &DM_Motor,uint8_t RID,const uint8_t data[4])
{
uint32_t id = DM_Motor.GetSlaveId();
uint8_t can_low = id & 0xff;
uint8_t can_high = (id >> 8) & 0xff;
std::array<uint8_t, 8> data_buf{can_low, can_high, 0x55, RID, 0x00, 0x00, 0x00, 0x00};
data_buf[4] = data[0];
data_buf[5] = data[1];
data_buf[6] = data[2];
data_buf[7] = data[3];
send_data.modify(0x7FF, data_buf.data());
serial_->send((uint8_t*)&send_data, sizeof(can_send_frame));
}
static bool is_in_ranges(int number) {
return (7 <= number && number <= 10) ||
(13 <= number && number <= 16) ||
(35 <= number && number <= 36);
}
static uint32_t float_to_uint32(float value) {
return static_cast<uint32_t>(value);
}
static float uint32_to_float(uint32_t value) {
return static_cast<float>(value);
}
static float uint8_to_float(const uint8_t data[4]) {
uint32_t combined = (static_cast<uint32_t>(data[3]) << 24) |
(static_cast<uint32_t>(data[2]) << 16) |
(static_cast<uint32_t>(data[1]) << 8) |
static_cast<uint32_t>(data[0]);
float result;
memcpy(&result, &combined, sizeof(result));
return result;
}
std::unordered_map<Motor_id, Motor*> motors;
SerialPort::SharedPtr serial_; //serial port
can_send_frame send_data; //send data frame
CAN_Receive_Frame receive_data{};//receive data frame
};
};
#endifSerialPort.h串口识别需要的头文件：
#ifndef SERIAL_PORT_H
#define SERIAL_PORT_H
#include <termios.h>
#include <sys/select.h>
#include <string>
#include <string.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <linux/serial.h>
#include <unistd.h>
#include <iostream>
#include <memory>
#include <chrono>
#include <queue>
void print_data(const uint8_t* data, uint8_t len)
{
for (int i = 0; i < len; i++)
{
printf("%02x ", data[i]);
}
printf("\n");
}
class SerialPort
{
public:
using SharedPtr = std::shared_ptr<SerialPort>;
SerialPort(std::string port, speed_t baudrate, int timeout_ms = 2)
{
set_timeout(timeout_ms);
Init(port, baudrate);
}
~SerialPort()
{
close(fd_);
}
ssize_t send(const uint8_t* data, size_t len)
{
// tcflush(fd_, TCIFLUSH);
ssize_t ret = ::write(fd_, data, len);
// tcdrain(fd_);
return ret;
}
ssize_t recv(uint8_t* data, size_t len)
{
FD_ZERO(&rSet_);
FD_SET(fd_, &rSet_);
ssize_t recv_len = 0;
switch (select(fd_ + 1, &rSet_, NULL, NULL, &timeout_))
{
case -1: // error
// std::cout << "communication error" << std::endl;
break;
case 0: // timeout
// std::cout << "timeout" << std::endl;
break;
default:
recv_len = ::read(fd_, data, len);
break;
}
return recv_len;
}
void recv(uint8_t* data, uint8_t head, ssize_t len)
{
// 存入队列
ssize_t recv_len = this->recv(recv_buf.data(), len);
for (int i = 0; i < recv_len; i++)
{
recv_queue.push(recv_buf[i]);
}
// 查找帧头
while (recv_queue.size() >= len)
{
if(recv_queue.front() != head)
{
recv_queue.pop();
continue;
}
break;
}
if(recv_queue.size() < len) return;
// 读取数据
for(int i = 0; i < len; i++)
{
data[i] = recv_queue.front();
recv_queue.pop();
}
}
void set_timeout(int timeout_ms)
{
timeout_.tv_sec = timeout_ms / 1000;
timeout_.tv_usec = (timeout_ms % 1000) * 1000;
}
private:
void Init(std::string port, speed_t baudrate)
{
int ret;
// Open serial port
fd_ = open(port.c_str(), O_RDWR | O_NOCTTY);
if (fd_ < 0)
{
printf("Open serial port %s failed\n", port.c_str());
exit(-1);
}
// Set attributes
struct termios option;
memset(&option, 0, sizeof(option));
ret = tcgetattr(fd_, &option);
option.c_oflag = 0;
option.c_lflag = 0;
option.c_iflag = 0;
cfsetispeed(&option, baudrate);
cfsetospeed(&option, baudrate);
option.c_cflag &= ~CSIZE;
option.c_cflag |= CS8; // 8
option.c_cflag &= ~PARENB; // no parity
option.c_iflag &= ~INPCK; // no parity
option.c_cflag &= ~CSTOPB; // 1 stop bit
option.c_cc[VTIME] = 0;
option.c_cc[VMIN] = 0;
option.c_lflag |= CBAUDEX;
ret = tcflush(fd_, TCIFLUSH);
ret = tcsetattr(fd_, TCSANOW, &option);
}
int fd_;
fd_set rSet_;
timeval timeout_;
std::queue<uint8_t> recv_queue;
std::array<uint8_t, 1024> recv_buf;
};
#endif // SERIAL_PORT_H其他的CMakeLists.txt和package.xml前面写过很多次了，这里就不写了，有一点需要注意的是如果你在driver这个功能包里面使用了control这个功能包里面的msg或其他的消息文件，记得在package.xml里面包含这个包：
<depend>rm_control</depend>CMakeLists.txt里面也是：
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
message_generation
rm_control
)----------------------------------------------------------------------------------------------------------------------
补充一下笔者对硬件控制的理解吧：
（笔者不是嵌入式出身，理解可能有出入，欢迎批评指正！！）
上面这部分关节驱动的控制呢是通过usb转can模块就能实现了，并不需要单片机，所以我一直很好奇单片机在硬件控制的作用，粗浅的理解为：在硬件驱动控制中，由于pc端并没有直接与关节电机连接的什么can口、串口或其他乱七八糟的接口，所以需要一个中间媒介，也就是这里用到的usb转can通信模块或者是单片机。
对于前者这种简单的通信转换模块，直接在pc上面根据关节电机的通信协议去编写驱动代码将接收到的关节角度数据转换为通信协议要求的can接口数据帧然后通过转换模块发布给关节电机实现控制。
而对于后者这种单片机的形式，就相当于除了担任通信转换这个角色之外，还有很多拓展性，比如可以在板子上面装一个指示灯、装一个应急开关、装一些额外的传感器等，这部分就可以在单片机上面写代码进行操作。除此之外，还可以把通讯协议这部分代码写在单片机上面，pc端实现与单片机的连通就行。
当然，不要纠结为什么有单片机之后，pc端的driver那部分还是那么一长串代码，这个问题有点蠢但是也难免偶尔会搞混，因为虽然你单片机可以实现通信协议的封装，但是你pc端不也得与它交流吗，比如你main里面总不能写那些与单片机直接通信的代码吧，你肯定得在driver里面把这些比如使能、失能、控制模式、控制转速、控制运动等等这些与单片机里面对应部分的代码需要指示单片机去执行的事情再封装一层提供出来函数接口让你可以在main函数里面用呀。
所以单片机并不是非要不可，只不过它在拓展性上面好一些，方面后期拓展功能，同时大家也一直在说的单片机驱动的实时性比pc要好，但也都是毫秒级别的，除非高速场景下，一般用起来应该也不至于会有很明显的区别。
-----------------------------------------------------------------------------------------------------------------
9.RealSense SDK & Realsense-ros驱动包
sdk--软件开发工具包
参考链接：realsense sdk& realsense ros
直接通过apt的形式安装就行。
RealSense 官网：realsense sdk & ros information
扫一扫
3183
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
