motion_planning_pipeline_tutorial.cpp详解

英文解释网站

Motion Planning Pipeline解释

在MoveIt!中，运动规划器用于规划路径，但是，有时候我们可能需要预处理运动请求或者后处理规划好的路径（例如用于时间参数化）。在这种情况下，我们使用规划管道，该管道将运动规划器与预处理和后处理阶段连接在一起。可以通过ROS参数服务器中的名称来配置称为规划请求适配器的预处理和后处理阶段。在本教程中，我们将引导您完成C++代码以实例化并调用这样的规划管道。

#include <pluginlib/class_loader.h>
#include <ros/ros.h>

#include <moveit/robot_model_loader/robot_model_loader.h>
/* 这个头文件含有planning_pipeline类，这个类有助于加载规划插件和规划请求适应的插件，
   并允许以指定的顺序从已加载的规划插件和planning_request_adapter::PlanningRequestAdapter插件中
   调用planning_interface::PlanningContext::solve() */
#include <moveit/planning_pipeline/planning_pipeline.h>
#include <moveit/planning_interface/planning_interface.h>
#include <moveit/kinematic_constraints/utils.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/PlanningScene.h>
#include <moveit_visual_tools/moveit_visual_tools.h>

int main(int argc, char **argv){
    ros::init(argc, argv, "move_group_tutorial");
    ros::AsyncSpinner spinner(1);
    spinner.start();
    ros::NodeHandle node_handle("~");

    robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
    robot_model::RobotModelPtr robot_model = robot_model_loader.getModel();
    planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model));
    /* 
    \brief Given a robot model (\e model), a node handle (\e nh), initialize the planning pipeline.
    \param model The robot model for which this pipeline is initialized.
    \param nh The ROS node handle that should be used for reading parameters needed for configuration
    \param planning_plugin_param_name ROS参数的名称，在其下指定规划插件的名称
    \param adapter_plugins_param_name ROS参数的名称，用于指定请求适配器插件的名称（插件名称以空格分隔；顺序很重要）
  
    PlanningPipeline(const robot_model::RobotModelConstPtr& model, const ros::NodeHandle& nh = ros::NodeHandle("~"),
                    const std::string& planning_plugin_param_name = "planning_plugin",
                    const std::string& adapter_plugins_param_name = "request_adapters");
     */
    planning_pipeline::PlanningPipelinePtr planning_pipeline(
        new planning_pipeline::PlanningPipeline(robot_model, node_handle, "planning_plugin", "request_adapters"));

/***********************************************/
/********* Visualisation **********/    
    namespace rvt = rviz_visual_tools;
    moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
    visual_tools.deleteAllMarkers();
    visual_tools.loadRemoteControl();
    Eigen::Affine3d text_pose = Eigen::Affine3d::Identity();
    text_pose.translation().z() = 1.75;
    visual_tools.publishText(text_pose, "Motion Plannng Pipeline Demo", rvt::WHITE, rvt::XLARGE);
    visual_tools.trigger();
    ros::Duration(10).sleep();
    visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");

/***********************************************/
/********* 1. pose goal：为Panda机械臂创建一个运动规划请求，并指定末端效应器的pose作为输入 ************/
    planning_interface::MotionPlanRequest req;
    planning_interface::MotionPlanResponse res;
    geometry_msgs::PoseStamped pose;
    pose.header.frame_id = "panda_link0";
    pose.pose.position.x = 0.3;
    pose.pose.position.y = 0.0;
    pose.pose.position.z = 0.75;
    pose.pose.orientation.w = 1.0;
    std::vector<double> tolerance_pose(3, 0.01);
    std::vector<double> tolerance_angle(3, 0.01);
    req.group_name = "panda_arm";
    /* 将关节名称、期望pose和公差带入函数得到约束message */
    moveit_msgs::Constraints pose_goal = 
        kinematic_constraints::constructGoalConstraints("panda_link8", pose, tolerance_pose, tolerance_angle);
    req.goal_constraints.push_back(pose_goal);  /* 放入请求中 */
    /* 调用运动规划器插件和规划请求适配器的序列（如果有），planning_scene和req是传入参数，res是传出参数，保存运动规划的响应。*/
    planning_pipeline->generatePlan(planning_scene, req, res);
    if(res.error_code_.val != res.error_code_.SUCCESS){
        ROS_ERROR("Could not compute plan successfully");
        return 0;
    }
    /* 可视化结果 */
    ros::Publisher display_publisher =
        node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
    moveit_msgs::DisplayTrajectory display_trajectory;
    ROS_INFO("Visualizing the trajectory");
    moveit_msgs::MotionPlanResponse response;
    res.getMessage(response);
    display_trajectory.trajectory_start = response.trajectory_start;
    display_trajectory.trajectory.push_back(response.trajectory);
    display_publisher.publish(display_trajectory);
    visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

/***********************************************/
/********* 2. Joint Space Goals **********/
    /* 首先，将规划场景中的状态设置为最后一个规划的最终状态 */
    robot_state::RobotState& robot_state = planning_scene->getCurrentStateNonConst();
    planning_scene->setCurrentState(response.trajectory_start);
    const robot_model::JointModelGroup* joint_model_group = robot_state.getJointModelGroup("panda_arm");
    robot_state.setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
    /* 然后，配置关节空间目标 */
    robot_state::RobotState goal_state(robot_model);
    std::vector<double> joint_values = { -1.0, 0.7, 0.7, -1.5, -0.7, 2.0, 0.0 };
    goal_state.setJointGroupPositions(joint_model_group, joint_values);
    /* 使用默认的公差创建运动学约束 */
    moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group);
    req.goal_constraints.clear();
    req.goal_constraints.push_back(joint_goal);
    /* 调用pipeline并可视化轨迹 */
    planning_pipeline->generatePlan(planning_scene, req, res);
    if(res.error_code_.val != res.error_code_.SUCCESS){
        ROS_ERROR("Could not compute plan successfully");
        return 0;
    }
    ROS_INFO("Visualizing the trajectory");
    res.getMessage(response);
    display_trajectory.trajectory_start = response.trajectory_start;
    display_trajectory.trajectory.push_back(response.trajectory);
    display_publisher.publish(display_trajectory);
    visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

/***********************************************/
/********* 2. Using a Planning Request Adapter **********/ 
    /* 通过规划请求适配器，我们可以指定在进行规划之前或在结果路径上完成规划之后的一系列操作 */
    /* 首先，将规划场景中的状态设置为最后一个规划的最终状态 */
    robot_state = planning_scene->getCurrentStateNonConst();
    planning_scene->setCurrentState(response.trajectory_start);
    robot_state.setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
    /* 然后，将其中一个关节设置为稍微超出其上限 */
    const robot_model::JointModel* joint_model = joint_model_group->getJointModel("panda_joint3");
    /* 获取此关节的变量范围 */
    const robot_model::JointModel::Bounds& joint_bounds = joint_model->getVariableBounds();
    std::vector<double> tmp_values(1, 0.0); /* 创建一个vector容器，里面有一个元素0.0 */
    tmp_values[0] = joint_bounds[0].min_position_ - 0.01;
    robot_state.setJointPositions(joint_model, tmp_values);
    req.goal_constraints.clear();
    req.goal_constraints.push_back(pose_goal);
    /* 再次调用规划器并可视化轨迹 */
    planning_pipeline->generatePlan(planning_scene, req, res);
    if(res.error_code_.val != res.error_code_.SUCCESS){
        ROS_ERROR("Could not compute plan successfully");
        return 0;
    }
    ROS_INFO("Visualizing the trajectory");
    res.getMessage(response);
    display_trajectory.trajectory_start = response.trajectory_start;
    display_trajectory.trajectory.push_back(response.trajectory);
    /* 现在您应该看到三个规划的轨迹 */
    display_publisher.publish(display_trajectory);
    visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to finish the demo");
    ROS_INFO("Done");

    return 0;

}