Rviz虚拟调试工具

路点添加

添加label

#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();

添加line

#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();

Warning

可视化工具，目前只支持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");

开启调试面板