moveit！学习心得（三）路径规划

在完成gazebo仿真后，我们就可以开始着手于机械臂的路径规划。

首先，我们需要在工作空间的src下新建一个功能包，用于存放我们路径规划的一些代码，另外由于涉及到路径规划，所以我们会用到trajectory_msgs等依赖

catkin_create_pkg eng_arm_planning roscpp actionlib moveit_ros_planning_interface tf trajectory_msgs 

建立完功能包后，我们可在其src下新建一个.cpp文件，即可开始编程。

可以编写主函数如下所示

int main(int argc, char **argv)
{
    ros::init(argc, argv, "cartesian_path_demo", ros::init_options::AnonymousName);

    // 创建一个异步的自旋线程（spinning thread）
    ros::AsyncSpinner spinner(1);
    spinner.start();

    moveit::planning_interface::MoveGroupInterface move_group("arm");

    // 设置一个目标位置（路点）
    geometry_msgs::Pose target_pose;
    target_pose.orientation.w = 0.8129126437415342;
    target_pose.orientation.x = 0.23297924420790708;
    target_pose.orientation.y = 0.509018876993605;
    target_pose.orientation.z = 0.16060351265657682;
    target_pose.position.x = 0.4607054862676324;
    target_pose.position.y = 0.13631097298751307;
    target_pose.position.z = 0.4230179971879574;
    std::vector<geometry_msgs::Pose> waypoints;
    waypoints.push_back(target_pose);

    // 笛卡尔空间下的路径规划
    moveit_msgs::RobotTrajectory trajectory;
    const double jump_threshold = 0.0;
    const double eef_step = 0.01;
    double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
   
    // 生成机械臂的运动规划数据
    moveit::planning_interface::MoveGroupInterface::Plan plan;
    plan.trajectory_ = trajectory;

    scale_trajectory_speed(plan,0.3);

    // 执行运动
    move_group.execute(plan);

    ros::shutdown(); 
    return 0;
}

因为在moveit中我们所给的机械臂位姿的形式用的是四元数，不清楚的话可以去看看我之前发的那篇，看完就大概差不多能理解了。

https://blog.csdn.net/weixin_52498332/article/details/128405897?spm=1001.2014.3001.5502

然后呢，就你输进去的四元数如果模不是1，就是说这个数是错误的话，那你在跑这个路径规划的时候可能就会报错。那如果你对这个不是很清楚,不知道怎么转换的话。你可以先开rviz，把模型调到某一个位置,然后去查看当前机械臂的位姿。

依次输入以下指令即可查看当前机械臂位姿

rosrun moveit_commander moveit_commander_cmdline.py

use arm

current

最后得到的数据大概就是这样，把得到的这七个数填入即可。

然后由于机械臂模型的运动速度是无法修改的，所以如果我们想要机械臂按我们预期的速度运行，就要对其轨迹进行重构，编写重构函数如下。

void scale_trajectory_speed(moveit::planning_interface::MoveGroupInterface::Plan &plan, double scale)
{
    int n_joints = plan.trajectory_.joint_trajectory.joint_names.size();//获取关节个数
    
    for(int i=0; i<plan.trajectory_.joint_trajectory.points.size(); i++)//通过for循环对plan中所有的轨迹点作一个遍历
    {
        plan.trajectory_.joint_trajectory.points[i].time_from_start *= 1/scale;//速度变慢，时间变长为 1/scale 倍
        
        for(int j=0; j<n_joints; j++)//遍历各个关节，每一个关节的速度和加速度数据都要作一个尺度的变化
        {
            plan.trajectory_.joint_trajectory.points[i].velocities[j] *= scale;//速度变化为原来的 scale
            plan.trajectory_.joint_trajectory.points[i].accelerations[j] *= scale*scale;//加速度变化为原来的 scale * scale
        }
    }
}

最后在CmakeList中加入节点即可运行。

add_executable(test_cartesian_path_node src/test_cartesian_path.cpp)
target_link_libraries(test_cartesian_path_node ${catkin_LIBRARIES})

在机械臂模型运行的过程中，我们可以输入相应指令去查看机械臂位姿的变换

rostopic echo /joint state

另外，我们也可以通过话题的方式来监听机械臂的位姿。

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "sensor_msgs/JointState.h"
 
void jointstatesCallback(const sensor_msgs::JointStateConstPtr& msg)
{
  float pos[3];
  pos[0]=msg->position[0];
  pos[1]=msg->position[1];
  pos[2]=msg->position[2];
  pos[3]=msg->position[3];
  pos[4]=msg->position[4];
  pos[5]=msg->position[5];
  ROS_INFO("I heard: [%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, "listener");
 
 
  ros::NodeHandle n;
 
 
  ros::Subscriber sub = n.subscribe("/joint_states", 100, jointstatesCallback);
 
 
  ros::spin();
 
 
  return 0;
}

add_executable(listener src/listener.cpp)
target_link_libraries(listener ${catkin_LIBRARIES})