motion_planning_api_tutorial.cpp详解

英文解释网站

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

#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/planning_interface/planning_interface.h>
#include <moveit/planning_scene/planning_scene.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>

#include <boost/scoped_ptr.hpp>

/** 
 * robot_model: 通过robot_model_loader得到robot_model
 * robot_state & joint_model_group: keep track of the current robot pose and planning group
 * planning_scene: maintains the state of the world (including the robot)
 */
int main(int argc, char **argv){
    const std::string node_name = "motion_planning_tutorial";
    ros::init(argc, argv, node_name);
    ros::AsyncSpinner spinner(1);
    spinner.start();
    ros::NodeHandle node_handle("~");
/** 
 * robot_model: 通过robot_model_loader得到robot_model
 * robot_state & joint_model_group: keep track of the current robot pose and planning group
 * planning_scene: maintains the state of the world (including the robot)
 */
    const std::string PLANNING_GROUP = "panda_arm";
    robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
    robot_model::RobotModelPtr robot_model = robot_model_loader.getModel();
    robot_state::RobotStatePtr robot_state(new robot_state::RobotState(robot_model));
    const robot_state::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP);
    planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model));
    /* 配置一个有效的机器人状态 */
    planning_scene->getCurrentStateNonConst().setToDefaultValues(joint_model_group, "ready");
    /* 构建一个加载器，能够通过名称来加载规划器，这里我们使用ROS插件库 */
    boost::scoped_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager>> planner_plugin_loader;
    planning_interface::PlannerManagerPtr planner_instance;
    std::string planner_plugin_name;
    /* 从ROS参数服务器上得到我们想要加载的规划插件的名字，然后加载规划器来捕捉所有的异常 */
    /* bool getParam(const std::string& key, std::string& s) const;根据key检索到参数并通过s传出 */
    if(!node_handle.getParam("planning_plugin", planner_plugin_name)){
        ROS_FATAL_STREAM("Could not find planner plugin name");
    }
    try{
        planner_plugin_loader.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>(
            "moveit_core", "planning_interface::PlannerManager"));
    }catch(pluginlib::PluginlibException& ex){
        ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what());
    }
    try{
        planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_plugin_name));
        if(!planner_instance->initialize(robot_model, node_handle.getNamespace())){
            ROS_FATAL_STREAM("Could not initialize planner instance");
        }
        ROS_INFO_STREAM("Using planning interface '" << planner_instance->getDescription() << "'");
    }catch(pluginlib::PluginlibException& ex){
        const std::vector<std::string>& classes = planner_plugin_loader->getDeclaredClasses();
        std::stringstream ss;
        for(std::size_t i = 0; i < classes.size(); ++i){
            ss << classes[i] << " ";
        }
        ROS_ERROR_STREAM("Exception while loading planner '" << planner_plugin_name << "': " << ex.what() << std::endl
                                                             << "Available plugins: " << ss.str());
    }
    
/***********************************************/
/********* Visualisation **********/
    namespace rvt = rviz_visual_tools;
    moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0"); /* panda_frame是构造函数的参数base_frame */
    visual_tools.loadRobotStatePub("/display_robot_state");
    visual_tools.enableBatchPublishing();
    visual_tools.deleteAllMarkers();
    visual_tools.trigger();
    visual_tools.loadRemoteControl();
    Eigen::Affine3d text_pose = Eigen::Affine3d::Identity();
    text_pose.translation().z() = 1.75;
    visual_tools.publishText(text_pose, "Motion Planning API Demo", rvt::WHITE, rvt::XLARGE);
    visual_tools.trigger();
    visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");

/***********************************************/
/********* 1. pose goal：为Panda机械臂创建一个运动规划请求，并指定末端效应器的pose作为输入 ************/
    visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN);
    visual_tools.trigger();
    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.4;
    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);
    moveit_msgs::Constraints pose_goal = 
        kinematic_constraints::constructGoalConstraints("panda_link8", pose, tolerance_pose, tolerance_angle);
    req.group_name = PLANNING_GROUP;
    req.goal_constraints.push_back(pose_goal);
    /* 根据当前场景和规划请求构造一个规划上下文 */
    planning_interface::PlanningContextPtr context = 
        planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
    context->solve(res);    /* 求解运动规划并将结果存储在res中 */
    if(res.error_code_.val != res.error_code_.SUCCESS){
        ROS_ERROR("Could not compute plan successfully");
        return 0;
    }
    /* topic为/move_group/display_planned_path */
    /* 排队等待传递给订阅者的最大传出消息数为1 */
    ros::Publisher display_publisher = 
        node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
    moveit_msgs::DisplayTrajectory display_trajectory;
    moveit_msgs::MotionPlanResponse response;
    res.getMessage(response);
    display_trajectory.trajectory_start = response.trajectory_start;
    display_trajectory.trajectory.push_back(response.trajectory);
    visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
    visual_tools.trigger();
    display_publisher.publish(display_trajectory);
    /* 将规划场景中的状态设置为上一个规划的最终状态 */
    robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
    planning_scene->setCurrentState(*robot_state.get());
    /* 展示目标状态 */
    visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN);
    visual_tools.publishAxisLabeled(pose.pose, "goal_1");
    visual_tools.publishText(text_pose, "Pose Goal (1)", rvt::WHITE, rvt::XLARGE);
    visual_tools.trigger();
    visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

/***********************************************/
/********* 2. Joint Space Goals **********/
    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);
    /* 调用规划器并可视化轨迹，重新构造规划上下文 */
    context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
    context->solve(res);    /* 调用规划器 */
    if(res.error_code_.val != res.error_code_.SUCCESS){     /* 检查规划是否成功 */
        ROS_ERROR("Could not compute plan successfully");
        return 0;
    }
    res.getMessage(response);
    display_trajectory.trajectory.push_back(response.trajectory);
    visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
    visual_tools.trigger();
    display_publisher.publish(display_trajectory);
    /* 我们将添加更多目标。但是首先，将规划场景中的状态设置为最后一个规划的最终状态，然后展示目标状态 */
    robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
    planning_scene->setCurrentState(*robot_state.get());
    visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN);
    visual_tools.publishAxisLabeled(pose.pose, "goal_2");
    visual_tools.publishText(text_pose, "Joint Space Goal (2)", rvt::WHITE, rvt::XLARGE);
    visual_tools.trigger();
    visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
    /* 现在，我们从joint space goal回到pose goal，为定位受限的规划做准备 */
    req.goal_constraints.clear();
    req.goal_constraints.push_back(pose_goal);
    context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
    context->solve(res);
    res.getMessage(response);
    display_trajectory.trajectory.push_back(response.trajectory);
    visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
    visual_tools.trigger();
    display_publisher.publish(display_trajectory);
    robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
    planning_scene->setCurrentState(*robot_state.get());
    visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN);
    visual_tools.trigger();
    visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

/***********************************************/
/********* 3. Adding Path Constraints **********/  
    /* 首先添加一个新的pose goal */  
    pose.pose.position.x = 0.32;
    pose.pose.position.y = -0.25;
    pose.pose.position.z = 0.65;
    pose.pose.orientation.w = 1.0;

    moveit_msgs::Constraints pose_goal_2 = 
        kinematic_constraints::constructGoalConstraints("panda_link8", pose, tolerance_pose, tolerance_angle);
    req.goal_constraints.clear();
    req.goal_constraints.push_back(pose_goal_2);
    /* 但是，让我们对运动施加路径限制。在这里，我们要求末端效应器保持水平 */
    geometry_msgs::QuaternionStamped quaternion;
    quaternion.header.frame_id = "panda_link0";
    quaternion.quaternion.w = 1.0;
    req.path_constraints = kinematic_constraints::constructGoalConstraints("panda_link8", quaternion);
    /** 
     * 施加路径限制要求规划器在末端执行器的可能位置空间（机器人的工作空间）内进行推理，
     * 因此我们还需要为允许的规划量指定一个界限。
     * 注意：默认边界由WorkspaceBounds请求适配器（属于OMPL管道，但在本例中未使用）自动填充。
     * 我们使用的界限肯定包括手臂的可到达空间。 这很好，因为在规划手臂时不会在此卷中进行采样； 
     * 边界仅用于确定采样的配置是否有效。
     */
    req.workspace_parameters.min_corner.x = req.workspace_parameters.min_corner.y = 
        req.workspace_parameters.min_corner.z = -2.0;
    req.workspace_parameters.max_corner.x = req.workspace_parameters.max_corner.y = 
        req.workspace_parameters.max_corner.z = 2.0;
    /* 调用规划器和可视化目前位置创建的所有规划 */
    context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
    context->solve(res);
    res.getMessage(response);
    display_trajectory.trajectory.push_back(response.trajectory);
    visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
    visual_tools.trigger();
    display_publisher.publish(display_trajectory);
    robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
    planning_scene->setCurrentState(*robot_state.get());
    visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN);
    visual_tools.publishAxisLabeled(pose.pose, "goal_3");
    visual_tools.publishText(text_pose, "Orientation Constrained Motion Plan (3)", rvt::WHITE, rvt::XLARGE);
    visual_tools.trigger();
    visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to exit the demo");
    planner_instance.reset();

    return 0;
}