Moveit-C++接口

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2013, SRI International
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of SRI International nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 * 
 * **以上是开源许可证信息**
 *********************************************************************/

/* Author: Sachin Chitta, Dave Coleman, Mike Lautman */

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>

#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>

#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>

#include <moveit_visual_tools/moveit_visual_tools.h>

int main(int argc, char** argv)
{
//ros的初始化操作和设置
  ros::init(argc, argv, "move_group_interface_tutorial");
  ros::NodeHandle node_handle;
  ros::AsyncSpinner spinner(1);
  spinner.start();

  //Moveit操作名为‘planning groups’规划组的一系列关节joints ，将其存储在‘JointModelGroup’关节模组中。
  //"planning group" 或者"joint model group"可以互换
  static const std::string PLANNING_GROUP = "panda_arm";

  //:move_group_interface:`MoveGroupInterface` 类
  ///可以仅通过planning group的名称来设置，从而控制和规划关节
  moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);

  //使用 :planning_scene_interface:`PlanningSceneInterface`类
  //来添加和移除虚拟场景中的碰撞对象
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
  
  // 在planning group中频繁使用原始指针以提高性能
  const robot_state::JointModelGroup* joint_model_group =
      move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

  // 可视化
  // ^^^^^^^^^^^^^
  // MoveItVisualTools 包在RViz中提供了许多可视化对象、机器人的功能和轨迹，
  // 以及调试工具，例如脚本的单步调试
  namespace rvt = rviz_visual_tools;
  moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
  visual_tools.deleteAllMarkers();

  // 远程控制是一种自检工具，允许用户通过 RViz 中的按钮和键盘快捷键逐步执行高级脚本
  visual_tools.loadRemoteControl();

  // RViz 提供了多种类型的标记，在这个演示中我们将使用文本、圆柱体和球体
  Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
  text_pose.translation().z() = 1.75;
  visual_tools.publishText(text_pose, "MoveGroupInterface Demo", rvt::WHITE, rvt::XLARGE);

  // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
  // 批量发布可减少发送到 RViz 的大型可视化消息数量
  visual_tools.trigger();

  // 获取基本信息
  // ^^^^^^^^^^^^^^^^^^^^^^^^^
  //
  // 可以打印输出这个机器人的参考系名称
  ROS_INFO_NAMED("tutorial", "Planning frame: %s", move_group.getPlanningFrame().c_str());

  // 还可以打印该组的末端执行器链接的名称。
  ROS_INFO_NAMED("tutorial", "End effector link: %s", move_group.getEndEffectorLink().c_str());

  // 可以得到机器人中所有组的列表：
  ROS_INFO_NAMED("tutorial", "Available Planning Groups:");
  std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
            std::ostream_iterator<std::string>(std::cout, ", "));

  // 开始演示
  // ^^^^^^^^^^^^^^^^^^^^^^^^^
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");

  // .. _move_group_interface-planning-to-pose-goal:
  //
  //  规划到姿势目标
  // ^^^^^^^^^^^^^^^^^^^^^^^
  // 可以为该组做运动规划,得到末端执行器所需的姿势
  geometry_msgs::Pose target_pose1;
  target_pose1.orientation.w = 1.0;
  target_pose1.position.x = 0.28;
  target_pose1.position.y = -0.2;
  target_pose1.position.z = 0.5;
  move_group.setPoseTarget(target_pose1);

  // 现在，调用规划器planner 来计算规划并将其可视化。
  // 请注意，我们只是在规划，而不是要求 move_group 实际移动机器人。
  moveit::planning_interface::MoveGroupInterface::Plan my_plan;

  bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

  ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");

  // 可视化规划
  // ^^^^^^^^^^^^^^^^^
  // 我们还可以将规划路径可视化为 RViz 中带有标记的线。
  ROS_INFO_NAMED("tutorial", "Visualizing plan 1 as trajectory line");
  visual_tools.publishAxisLabeled(target_pose1, "pose1");
  visual_tools.publishText(text_pose, "Pose Goal", rvt::WHITE, rvt::XLARGE);
  visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
  visual_tools.trigger();
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

  // 移动到姿势目标
  // ^^^^^^^^^^^^^^^^^^^^^
  //
  // Moving to a pose goal is similar to the step above except we now use the move() function. Note that the pose goal we had set earlier is still active and so the robot will try to move to that goal. We will not use that function in this tutorial since it is a blocking function and requires a controller to be active and report success on execution of a trajectory.
  // 移动到姿势目标与上面的步骤类似，除了现在使用 move() 函数。请注意，我们之前设置的姿势目标仍然处于活动状态，因此机器人将尝试移动到该目标。我们不会在本教程中使用该函数，因为它是一个阻塞函数，需要一个控制器处于活动状态并报告轨迹执行成功。
  /* 使用真正的机器人时取消注释下面的行 */
  /* move_group.move(); */

  // 规划关节空间目标
  // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  //
  // 设定并移动至一个关节空间目标。这将替换上面设置的姿势目标。
  // 首先，我们将创建一个引用当前机器人状态的指针。
  // RobotState 是包含所有当前位置/速度/加速度数据的对象。
  moveit::core::RobotStatePtr current_state = move_group.getCurrentState();
  //
  // 接下来获取关节组的当前关节值集合。
  std::vector<double> joint_group_positions;
  current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);

  // 现在，让我们修改其中一个关节，规划到新的关节空间目标并可视化规划路径
  joint_group_positions[0] = -1.0;  // radians
  move_group.setJointValueTarget(joint_group_positions);

  success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  ROS_INFO_NAMED("tutorial", "Visualizing plan 2 (joint space goal) %s", success ? "" : "FAILED");

  // 在 RViz 中可视化规划路径
  visual_tools.deleteAllMarkers();
  visual_tools.publishText(text_pose, "Joint Space Goal", rvt::WHITE, rvt::XLARGE);
  visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
  visual_tools.trigger();
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

  // 使用路径约束Path Constraints进行规划
  // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  //
  // 可以在机器人上轻松地添加链接用于指定路径约束Path Constraints。
  // 为规划组指定路径约束path constraint和姿势目标pose goal
  // 首先定义路径约束
  moveit_msgs::OrientationConstraint ocm;
  ocm.link_name = "panda_link7";
  ocm.header.frame_id = "panda_link0";
  ocm.orientation.w = 1.0;
  ocm.absolute_x_axis_tolerance = 0.1;
  ocm.absolute_y_axis_tolerance = 0.1;
  ocm.absolute_z_axis_tolerance = 0.1;
  ocm.weight = 1.0;

  // 将其设置为规划组的路径约束。
  moveit_msgs::Constraints test_constraints;
  test_constraints.orientation_constraints.push_back(ocm);
  move_group.setPathConstraints(test_constraints);

  // 重用已有的旧目标并规划
  // 请注意，这仅在当前状态已经满足路径约束时才有效。
  // 因此，我们需要将开始状态 start state设置为新姿势。
  robot_state::RobotState start_state(*move_group.getCurrentState());
  geometry_msgs::Pose start_pose2;
  start_pose2.orientation.w = 1.0;
  start_pose2.position.x = 0.55;
  start_pose2.position.y = -0.05;
  start_pose2.position.z = 0.8;
  start_state.setFromIK(joint_model_group, start_pose2);
  move_group.setStartState(start_state);

  // 现在将从刚刚创建的新的开始状态new start state规划到之前的姿势目标earlier pose target。
  move_group.setPoseTarget(target_pose1);

  // 使用约束进行规划可能会很慢，因为每个样本都必须调用逆运动学求解器。
  //让我们从默认的 5 秒起增加规划时间，以确保规划器有足够的时间成功解算
  move_group.setPlanningTime(10.0);

  success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  ROS_INFO_NAMED("tutorial", "Visualizing plan 3 (constraints) %s", success ? "" : "FAILED");

  // 在RViz中可视化规划路径
  visual_tools.deleteAllMarkers();                              // 删除所有的markers
  visual_tools.publishAxisLabeled(start_pose2, "start");        // 
  visual_tools.publishAxisLabeled(target_pose1, "goal");        // 设置发布起始和目标的标签
  visual_tools.publishText(text_pose, "Constrained Goal", rvt::WHITE, rvt::XLARGE);  // 显示发布文本，白色 超大
  visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);  // 显示发布路径轨迹线
  visual_tools.trigger();                  // 触发
  visual_tools.prompt("next step");        // 打印‘下一步’提示
    
  // 完成路径约束后，请务必清除它。
  move_group.clearPathConstraints();

  // 笛卡尔路径
  // ^^^^^^^^^^^^^^^
  // 可以通过指定末端执行器要经过的路标(航点)waypoints列表来直接规划笛卡尔路径。
  // 请注意，是从上面的新开始状态new start state开始的。
  // 初始姿势（开始状态）不需要添加到航点列表，但添加它可以帮助可视化
  std::vector<geometry_msgs::Pose> waypoints;
  waypoints.push_back(start_pose2);  //添加初始初始位姿到航点列表

  geometry_msgs::Pose target_pose3 = start_pose2;

  target_pose3.position.z -= 0.2;
  waypoints.push_back(target_pose3);  // down  //然后向z轴-0.2米

  target_pose3.position.y -= 0.2;
  waypoints.push_back(target_pose3);  // right 向y轴-0.2米

  target_pose3.position.z += 0.2;
  target_pose3.position.y += 0.2;
  target_pose3.position.x -= 0.2;
  waypoints.push_back(target_pose3);  // up and left   向量移动（-0.2，0.2，0.2）

  // 笛卡尔运动Cartesian motions经常需要较慢的动作，例如接近和撤退抓取运动approach and retreat grasp motions
  // 在这里，我们演示了如何通过每个关节的最大速度的比例因子scaling factor来降低机器人手臂的速度。
  // 请注意，这不是末端执行器点的速度。
  move_group.setMaxVelocityScalingFactor(0.1);// 最大速度的比例因子scaling factor设置为0.1

  // 我们希望笛卡尔路径以 1 厘米的分辨率进行插值，这就是为什么我们将指定 0.01 作为笛卡尔平移的最大步长。
  // 我们将跳跃阈值jump threshold指定为 0.0，从而有效地禁用它。
  // 警告 - 在操作真实硬件时禁用跳跃阈值会导致冗余关节发生大量不可预测的运动，并且可能是一个安全问题
  moveit_msgs::RobotTrajectory trajectory;
  const double jump_threshold = 0.0;// 禁用跳跃阈值
  const double eef_step = 0.01; // 指定 0.01 作为笛卡尔平移的最大步长
  double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
  ROS_INFO_NAMED("tutorial", "Visualizing plan 4 (Cartesian path) (%.2f%% acheived)", fraction * 100.0);

  // 在RViz中可视化规划路径
  visual_tools.deleteAllMarkers();
  visual_tools.publishText(text_pose, "Joint Space Goal", rvt::WHITE, rvt::XLARGE);
  visual_tools.publishPath(waypoints, rvt::LIME_GREEN, rvt::SMALL);
  for (std::size_t i = 0; i < waypoints.size(); ++i)
    visual_tools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::SMALL);
  visual_tools.trigger();
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

  // 添加/删除Adding/Removing对象    附加/分离Attaching/Detaching对象
  // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  //
  // 定义一个碰撞对象 ROS 消息collision object ROS message
  moveit_msgs::CollisionObject collision_object;
  collision_object.header.frame_id = move_group.getPlanningFrame();

  // 对象的 id 用于标识它。
  collision_object.id = "box1";

  // 定义一个box添加到世界中
  shape_msgs::SolidPrimitive primitive;
  primitive.type = primitive.BOX;
  primitive.dimensions.resize(3);
  primitive.dimensions[0] = 0.4;
  primitive.dimensions[1] = 0.1;
  primitive.dimensions[2] = 0.4;

  // Define a pose for the box (specified relative to frame_id)定义box的姿态（指定关联frame_id）
  geometry_msgs::Pose box_pose;
  box_pose.orientation.w = 1.0;
  box_pose.position.x = 0.4;
  box_pose.position.y = -0.2;
  box_pose.position.z = 1.0;

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

  std::vector<moveit_msgs::CollisionObject> collision_objects;
  collision_objects.push_back(collision_object);

  // 现在，让我们将碰撞对象添加到世界中
  ROS_INFO_NAMED("tutorial", "Add an object into the world");
  planning_scene_interface.addCollisionObjects(collision_objects);

  // 在RViz 中显示状态文本
  visual_tools.publishText(text_pose, "Add object", rvt::WHITE, rvt::XLARGE);
  visual_tools.trigger();

  // 等待 MoveGroup 接收并处理碰撞对象消息
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz");

  // 现在，当我们计划一条轨迹时，它将避开障碍物
  move_group.setStartState(*move_group.getCurrentState());
  geometry_msgs::Pose another_pose;
  another_pose.orientation.w = 1.0;
  another_pose.position.x = 0.4;
  another_pose.position.y = -0.4;
  another_pose.position.z = 0.9;
  move_group.setPoseTarget(another_pose);

  success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  ROS_INFO_NAMED("tutorial", "Visualizing plan 5 (pose goal move around cuboid) %s", success ? "" : "FAILED");

  // Visualize the plan in RViz
  visual_tools.deleteAllMarkers();
  visual_tools.publishText(text_pose, "Obstacle Goal", rvt::WHITE, rvt::XLARGE);
  visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
  visual_tools.trigger();
  visual_tools.prompt("next step");

  // 将碰撞对象附加到机器人上。
  ROS_INFO_NAMED("tutorial", "Attach the object to the robot");
  move_group.attachObject(collision_object.id);

  // Show text in RViz of status
  visual_tools.publishText(text_pose, "Object attached to robot", rvt::WHITE, rvt::XLARGE);
  visual_tools.trigger();

  /* 等待 MoveGroup 接收并处理附加的碰撞对象消息 */
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object attaches to the "
                      "robot");

  // 从机器人上分离碰撞对象。
  ROS_INFO_NAMED("tutorial", "Detach the object from the robot");
  move_group.detachObject(collision_object.id);

  // Show text in RViz of status
  visual_tools.publishText(text_pose, "Object dettached from robot", rvt::WHITE, rvt::XLARGE);
  visual_tools.trigger();

  /* Wait for MoveGroup to recieve and process the attached collision object message */
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object detaches to the "
                      "robot");

  // 从世界中移除碰撞对象
  ROS_INFO_NAMED("tutorial", "Remove the object from the world");
  std::vector<std::string> object_ids;
  object_ids.push_back(collision_object.id);
  planning_scene_interface.removeCollisionObjects(object_ids);

  // Show text in RViz of status
  visual_tools.publishText(text_pose, "Object removed", rvt::WHITE, rvt::XLARGE);
  visual_tools.trigger();

  /* Wait for MoveGroup to recieve and process the attached collision object message */
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object disapears");

  // END_TUTORIAL

  ros::shutdown();
  return 0;
}