ros_tools.prompt 断点调试

Rviz虚拟调试工具

#include "moveit_visual_tools/moveit_visual_tools.h"
...
//绘制移动轨迹
string frame = group.getPlanningFrame();
moveit_visual_tools::MoveItVisualTools tools(frame);
tools.deleteAllMarkers();
//添加label
tools.publishAxisLabeled(pose, "target");
tools.trigger();

#include "moveit_visual_tools/moveit_visual_tools.h"
...
//绘制移动轨迹
string frame = group.getPlanningFrame();
moveit_visual_tools::MoveItVisualTools tools(frame);
tools.deleteAllMarkers();
//添加label
const moveit::core::JointModelGroup *jointModelGroup = group.getCurrentState()->getJointModelGroup(groupName);
tools.publishTrajectoryLine(plan.trajectory_, jointModelGroup);
tools.trigger();

可视化工具，目前只支持c++

Gui断点调试

// group库
#include "moveit/move_group_interface/move_group_interface.h"
// 可视化工具库
#include "moveit_visual_tools/moveit_visual_tools.h"
......

string groupName="manipulator";
moveit::planning_interface::PlanningSceneInterface scene;
moveit_visual_tools::MoveItVisualTools tools(group.getPlanningFrame());

tools.loadRemoteControl();

ROS_INFO_STREAM("1");
tools.prompt("Press 'next' to start demo");

ROS_INFO_STREAM("2");
tools.prompt("Press 'next' to start demo");

ROS_INFO_STREAM("3");
tools.prompt("Press 'next' to start demo");

ROS_INFO_STREAM("4");
tools.prompt("Press 'next' to start demo");

开启调试面板

 

完整代码示例: 

#include 
#include 

//moveit环境引入
#include 
#include 

#include 

#include "moveit_visual_tools/moveit_visual_tools.h"

using namespace std;


double deg2rad(double deg) {
    return deg * M_PI / 180.0;
}


void moveHome() {
    moveit::planning_interface::MoveGroupInterface group("manipulator_i5");
    group.setNamedTarget("home");
    group.move();
}

void moveZero() {
    moveit::planning_interface::MoveGroupInterface group("manipulator_i5");
    group.setNamedTarget("zero");
    group.move();
}

void movePose() {
    moveit::planning_interface::MoveGroupInterface group("manipulator_i5");
    //设置目标
//    group.setNamedTarget("home");

    //去到具体的位置
    geometry_msgs::Pose pose;
    // 位置
    pose.position.x = 0.364878;
    pose.position.y = -0.279625;
    pose.position.z = 0.388561 + 0.502;
    // 姿态(四元素)(欧拉角转四元素)
    tf::Quaternion quat;
    quat.setRPY(deg2rad(-166.969955), deg2rad(2.913350), deg2rad(38.519089));
    pose.orientation.x = quat.x();
    pose.orientation.y = quat.y();
    pose.orientation.z = quat.z();
    pose.orientation.w = quat.w();

    group.setPoseTarget(pose);

    //移动
    const moveit::planning_interface::MoveItErrorCode &code = group.move();

    if (code == moveit::planning_interface::MoveItErrorCode::SUCCESS) {
        // 成功
        ROS_INFO_STREAM("执行成功: Success");
    } else {
        // 失败
        ROS_INFO_STREAM("执行失败: Failed");
    }
}


int main(int argc, char **argv) {
    // 初始化节点
    string nodeName = "ur3_move_node";
    ros::init(argc, argv, nodeName);

    ros::NodeHandle node;

    ros::AsyncSpinner spinner(1);
    spinner.start();

    moveit::planning_interface::MoveGroupInterface group("manipulator_i5");
    moveit_visual_tools::MoveItVisualTools tools(group.getPlanningFrame());

    tools.prompt("start move home");
    moveHome();

    tools.prompt("start move zero");
    moveZero();

    tools.prompt("start move pose");
    movePose();

    //阻塞
    ros::waitForShutdown();

    return 0;
}