< >
Home » ROS2与MoveIt2教程 » ROS2与MoveIt2入门教程-MoveItCpp教程

ROS2与MoveIt2入门教程-MoveItCpp教程

说明：

  • 介绍MoveItCpp

概述：

  • MoveItCpp是一个新的高级接口，是一个统一的C++ API，不需要使用ROS的动作、服务和消息来访问MoveIt2核心功能，并且是现有MoveGroup API的一个替代（不是完全替代），我们将此接口推荐给需要更多实时控制的高级用户或工业应用。该接口是PickNik Robotics公司为许多商业应用程序开发的。

请输入图片描述

运行代码
打开一个终端窗口并运行启动文件：

ros2 launch moveit2_tutorials moveit_cpp_tutorial.launch.py

片刻之后，RViz窗口应该会出现，看起来应该与上面这张图相类似。要完成每个演示步骤，在RViz处于活动状态（聚焦）时按屏幕底部RvizVisualToolsGui面板中的“下一步（Next）”按钮，或选择屏幕顶部“工具（Tools）”面板中的“键盘工具（Key Tool）”后按键盘上的“N”字母键。

完整代码说明

本教程的完整代码可以在此处的MoveIt GitHub项目中看到。接下来会逐段代码地解释其功能。

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

/* Otherwise robot with zeros joint_states */
rclcpp::sleep_for(std::chrono::seconds(1));

RCLCPP_INFO(LOGGER, "Starting MoveIt Tutorials...");

auto moveit_cpp_ptr = std::make_shared<moveit_cpp::MoveItCpp>(node);
moveit_cpp_ptr->getPlanningSceneMonitor()->providePlanningSceneService();

auto planning_components = std::make_shared<moveit_cpp::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(node, "panda_link0", "moveit_cpp_tutorial",         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::msg::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");

然后在Rviz中可视化目标位姿：

visual_tools.publishAxisLabeled(target_pose1.pose, "target_pose");
visual_tools.publishText(text_pose, "setStartStateToCurrentState", 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 */
}

1#规划的可视化结果应该如下图所示：

请输入图片描述

最后启动下一个规划：

visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
visual_tools.deleteAllMarkers();
visual_tools.trigger();
  • 规划#2

在这个示例中会使用moveit::core::RobotState来设置规划的当前状态：

auto start_state = *(moveit_cpp_ptr->getCurrentState());
geometry_msgs::msg::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);

这里会重用1#规划中已有的旧目标并规划到该目标的轨迹：

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.publishAxisLabeled(robot_state.getGlobalLinkTransform("panda_link8"), "start_pose");
  visual_tools.publishAxisLabeled(target_pose1.pose, "target_pose");
  visual_tools.publishText(text_pose, "moveit::core::RobotState_Start_State", rvt::WHITE, rvt::XLARGE);
  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 */
}

2#规划的可视化结果应该如下图所示：

请输入图片描述

最后也是启动下一个规划：

visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
visual_tools.deleteAllMarkers();
visual_tools.trigger();
  • 规划#3

也可以使用moveit::core::RobotState来设置规划的目标状态：

auto target_state = *robot_start_state;
geometry_msgs::msg::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.publishAxisLabeled(robot_state.getGlobalLinkTransform("panda_link8"), "start_pose");
  visual_tools.publishAxisLabeled(target_pose2, "target_pose");
  visual_tools.publishText(text_pose, "moveit::core::RobotState_Goal_Pose", rvt::WHITE, rvt::XLARGE);
 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 */
}

3#规划的可视化结果应该如下图所示：

请输入图片描述

最后也是启动下一个规划：

visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
visual_tools.deleteAllMarkers();
visual_tools.trigger();
  • 规划#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.publishAxisLabeled(robot_state.getGlobalLinkTransform("panda_link8"), "start_pose");
  visual_tools.publishAxisLabeled(robot_start_state->getGlobalLinkTransform("panda_link8"), "target_pose");
  visual_tools.publishText(text_pose, "Goal_Pose_From_Named_State", rvt::WHITE, rvt::XLARGE);
  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 */
}

4#规划的可视化结果应该如下图所示：

请输入图片描述

最后也是启动下一个规划：

visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
visual_tools.deleteAllMarkers();
visual_tools.trigger();
  • 规划#5

也可以围绕着碰撞场景中的物体来生成运动规划。

首先要创建碰撞物体：

moveit_msgs::msg::CollisionObject collision_object;
collision_object.header.frame_id = "panda_link0";
collision_object.id = "box";

shape_msgs::msg::SolidPrimitive box;
box.type = box.BOX;
box.dimensions = { 0.1, 0.4, 0.1 };

geometry_msgs::msg::Pose box_pose;
box_pose.position.x = 0.4;
box_pose.position.y = 0.0;
box_pose.position.z = 1.0;

collision_object.primitives.push_back(box);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;

并将该物体添加到规划场景中：

{  // Lock PlanningScene
  planning_scene_monitor::LockedPlanningSceneRW scene(moveit_cpp_ptr->getPlanningSceneMonitor());
  scene->processCollisionObjectMsg(collision_object);
}  // Unlock PlanningScene
planning_components->setStartStateToCurrentState();
planning_components->setGoal("extended");

auto plan_solution5 = planning_components->plan();
if (plan_solution5)
{
  visual_tools.publishText(text_pose, "Planning_Around_Collision_Object", rvt::WHITE, rvt::XLARGE);
  visual_tools.publishTrajectoryLine(plan_solution5.trajectory, joint_model_group_ptr);
  visual_tools.trigger();

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

5#规划的可视化结果应该如下图所示：

请输入图片描述

  • 启动文件

完整的启动文件放在GitHub网站上此处。本教程中的所有代码都可以从作为MoveIt2安装组成部分之一的moveit2_tutorials软件包中运行。

英语原文地址：https://moveit2_tutorials.picknik.ai/doc/moveit_cpp/moveitcpp_tutorial.html%23

纠错,疑问,交流: 请进入讨论区或 点击加入Q群

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号


标签: ros2与moveit2入门教程