MoveIt!之ROS1Melodic版本发布（MoveItCpp教程）

时间:2022-07-22
本文章向大家介绍MoveIt!之ROS1Melodic版本发布（MoveItCpp教程），主要内容包括其使用实例、应用技巧、基本知识点总结和需要注意事项，具有一定的参考价值，需要的朋友可以参考一下。

Moveit！（1.0.5）之ROS1Melodic版本发布~~

moveit_cpp：

教程版本：专业级

这是正在积极开发的最新版本。对于初学者，我们推荐稳定的Melodic教程。如果仍在运行Kinetic版本，请使用Kinetic教程。

MoveItCpp教程

介绍

MoveItCpp是一个新的高级接口，它是不需要使用ROS操作，服务和消息即可访问核心MoveIt! 功能的统一C++ API，并且是现有MoveGroup API的替代方法（不是完全替代），建议该界面适用于需要更多实时控制的高级用户或行业应用。PickNik Robotics已根据许多商业应用开发了此接口。

入门

如果尚未这样做，请确保已完成“入门”中的步骤。

运行代码

打开一个shell，运行启动文件：

roslaunch moveit_tutorials moveit_cpp_tutorial.launch

注意：本教程使用RvizVisualToolsGui面板逐步演示。要将此面板添加到RViz，请遵循“ 可视化教程”中的说明。

片刻之后，将出现RViz窗口，其外观类似于此页面顶部的窗口。要通过每一步演示或者按进度在接下来的按钮RvizVisualToolsGui在屏幕的底部面板或选择关键工具的工具，在屏幕的顶部面板，然后按你的键盘上N，此时RViz为活动窗口。

整个代码

完整的代码可以在MoveIt GitHub项目中找到。接下来，逐步地讲解代码，解释其功能。

配置

static const std::string PLANNING_GROUP = "panda_arm";
static const std::string LOGNAME = "moveit_cpp_tutorial";

/* Otherwise robot with zeros joint_states  */
/ *否则，机器人的关节编号为零* / 
ros::Duration(1.0).sleep();

ROS_INFO_STREAM_NAMED(LOGNAME, "Starting MoveIt Tutorials...");

auto moveit_cpp_ptr = std::make_shared<moveit::planning_interface::MoveItCpp>(nh);

auto planning_components =
    std::make_shared<moveit::planning_interface::PlanningComponent>(PLANNING_GROUP, moveit_cpp_ptr);
auto robot_model_ptr = moveit_cpp_ptr->getRobotModel();
auto robot_start_state = planning_components->getStartState();
auto joint_model_group_ptr = robot_model_ptr->getJointModelGroup(PLANNING_GROUP);

可视化

MoveItVisualTools软件包提供了许多功能，用于可视化RViz中的对象，机器人和轨迹以及调试工具（例如脚本的逐步自省）

moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0", rvt::RVIZ_MARKER_TOPIC,
                                                    moveit_cpp_ptr->getPlanningSceneMonitor());
visual_tools.deleteAllMarkers();
visual_tools.loadRemoteControl();

Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
text_pose.translation().z() = 1.75;
visual_tools.publishText(text_pose, "MoveItCpp Demo", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();

开始演示

visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");

使用MoveItCpp进行规划

在以下计划示例中说明了设置规划开始和目标状态的多种方法。

规划1

我们可以将规划的开始状态设置为机器人的当前状态

planning_components->setStartStateToCurrentState();

设置计划目标的第一种方法是使用geometry_msgs :: PoseStamped ROS消息类型，如下所示

geometry_msgs::PoseStamped target_pose1;
target_pose1.header.frame_id = "panda_link0";
target_pose1.pose.orientation.w = 1.0;
target_pose1.pose.position.x = 0.28;
target_pose1.pose.position.y = -0.2;
target_pose1.pose.position.z = 0.5;
planning_components->setGoal(target_pose1, "panda_link8");

现在，调用PlanningComponents来计算规划并对其进行可视化。请注意，我们只是在规划

auto plan_solution1 = planning_components->plan();

检查PlanningComponents是否成功找到规划的解

if (plan_solution1)
{

在Rviz中可视化起始姿势

visual_tools.publishAxisLabeled(robot_start_state->getGlobalLinkTransform("panda_link8"), "start_pose");
visual_tools.publishText(text_pose, "Start Pose", rvt::WHITE, rvt::XLARGE);

在Rviz中可视化目标姿势

visual_tools.publishAxisLabeled(target_pose1.pose, "target_pose");
visual_tools.publishText(text_pose, "Goal Pose", rvt::WHITE, rvt::XLARGE);

在Rviz中可视化轨迹

  visual_tools.publishTrajectoryLine(plan_solution1.trajectory, joint_model_group_ptr);
  visual_tools.trigger();

  /* Uncomment if you want to execute the plan */
  /* planning_components->execute(); // Execute the plan */
}

开始下一个规划

visual_tools.deleteAllMarkers();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

规划2

在这里，我们将使用moveit :: core :: RobotState设置规划的当前状态。

auto start_state = *(moveit_cpp_ptr->getCurrentState());
geometry_msgs::Pose start_pose;
start_pose.orientation.w = 1.0;
start_pose.position.x = 0.55;
start_pose.position.y = 0.0;
start_pose.position.z = 0.6;

start_state.setFromIK(joint_model_group_ptr, start_pose);

planning_components->setStartState(start_state);

将重用曾经的旧目标并对其进行规划。

auto plan_solution2 = planning_components->plan();
if (plan_solution2)
{
  moveit::core::RobotState robot_state(robot_model_ptr);
  moveit::core::robotStateMsgToRobotState(plan_solution2.start_state, robot_state);

  visual_tools.publishText(text_pose, "Start Pose", rvt::WHITE, rvt::XLARGE);
  visual_tools.publishAxisLabeled(robot_state.getGlobalLinkTransform("panda_link8"), "start_pose");
  visual_tools.publishText(text_pose, "Goal Pose", rvt::WHITE, rvt::XLARGE);
  visual_tools.publishAxisLabeled(target_pose1.pose, "target_pose");
  visual_tools.publishTrajectoryLine(plan_solution2.trajectory, joint_model_group_ptr);
  visual_tools.trigger();

  /* Uncomment if you want to execute the plan */
 / *如果要执行计划，请取消注释* / 
  /* planning_components->execute(); // Execute the plan */
}

开始下一个规划

visual_tools.deleteAllMarkers();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

规划3

我们还可以使用moveit :: core :: RobotState设置规划的目标

auto target_state = *robot_start_state;
geometry_msgs::Pose target_pose2;
target_pose2.orientation.w = 1.0;
target_pose2.position.x = 0.55;
target_pose2.position.y = -0.05;
target_pose2.position.z = 0.8;

target_state.setFromIK(joint_model_group_ptr, target_pose2);

planning_components->setGoal(target_state);

将重用以前的开始，并从中规划。

auto plan_solution3 = planning_components->plan();
if (plan_solution3)
{
  moveit::core::RobotState robot_state(robot_model_ptr);
  moveit::core::robotStateMsgToRobotState(plan_solution3.start_state, robot_state);

  visual_tools.publishText(text_pose, "Start Pose", rvt::WHITE, rvt::XLARGE);
  visual_tools.publishAxisLabeled(robot_state.getGlobalLinkTransform("panda_link8"), "start_pose");
  visual_tools.publishText(text_pose, "Goal Pose", rvt::WHITE, rvt::XLARGE);
  visual_tools.publishAxisLabeled(target_pose2, "target_pose");
  visual_tools.publishTrajectoryLine(plan_solution3.trajectory, joint_model_group_ptr);
  visual_tools.trigger();

  /* Uncomment if you want to execute the plan */
 / *如果要执行计划，请取消注释* / 
  /* planning_components->execute(); // Execute the plan */
}

开始下一个规划

visual_tools.deleteAllMarkers();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

规划4

我们可以将规划的开始状态设置为机器人的当前状态，可以使用熊猫机器人的组状态名称来设置计划的目标，我们为“ panda_arm”计划组命名为“ ready”的机器人状态参见panda_arm.xacro

/* // Set the start state of the plan from a named robot state */
/ * //从已命名的机器人状态设置计划的开始状态* / 
/* planning_components->setStartState("ready"); // Not implemented yet */

从命名的机器人状态设置计划的目标状态

planning_components->setGoal("ready");

再次，我们将重用我们曾经的旧起点并从中计划。

auto plan_solution4 = planning_components->plan();
if (plan_solution4)
{
  moveit::core::RobotState robot_state(robot_model_ptr);
  moveit::core::robotStateMsgToRobotState(plan_solution4.start_state, robot_state);

  visual_tools.publishText(text_pose, "Start Pose", rvt::WHITE, rvt::XLARGE);
  visual_tools.publishAxisLabeled(robot_state.getGlobalLinkTransform("panda_link8"), "start_pose");
  visual_tools.publishText(text_pose, "Goal Pose", rvt::WHITE, rvt::XLARGE);
  visual_tools.publishAxisLabeled(robot_start_state->getGlobalLinkTransform("panda_link8"), "target_pose");
  visual_tools.publishTrajectoryLine(plan_solution4.trajectory, joint_model_group_ptr);
  visual_tools.trigger();

  /* Uncomment if you want to execute the plan */
  / *如果要执行计划，请取消注释* / 
  /* planning_components->execute(); // Execute the plan */
}

启动文件

整个启动文件是这里 GitHub上。本教程中的所有代码都可以从MoveIt设置中包含的moveit_tutorials包中运行。

附加功能：

  • 允许时间最优轨迹生成的输入轨迹密度的参数化
  • 通过ompl_planning.yaml在ModelBasedPlanningContext中添加对杂交/内插标志的支持
  • 提高PlanningSceneDisplay的响应速度
  • 在PlanningComponent中使用plan_request_params
  • 提供在rviz中缩放大网格
  • 加快PlanningSceneDisplay
  • 添加对PS4游戏杆的支持
  • 将use_rviz添加到setup_assistant配置助手中的demo.launch

Bug修复：包括异常行为，程序段故障，内存泄漏和Python3支持等。

随机文章
本站知识点必读
最近更新