规划[5/5]：抓取、放置

• mgi：moveit::planning_interface::MoveGroupInterface
• mgii：moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl

抓取指机器人夹起工作空间内某个物体，放置指机器人把夹着的物体放到目标位置。处理它们的MoveGroupCapability是MoveGroupPickPlaceAction，在后者，对应实现了两个action server来进行处理，分别是pickup_action_server_、place_action_server_。

• 注1：mgi还提供其它参数简的api，像pick(const std::string& object, bool plan_only)，这时mgi会根据object、plan_only，用填充默认参数生成PickupGoal。

上面所说的放置、抓取是相互独立的两个操作，实际使用时往往是把它们合在一起认为一个动作，那mgii是否提供了认为是一个操作api？——没有。

moveit::core::MoveItErrorCode planGraspsAndPick(const moveit_msgs::CollisionObject& object, bool plan_only);

它处理的应该是是抓取。调用这个api时，caller需实现一个字符串标识是“plan_grasps”的actionlib Server。在抓取前，mgi会先调用plan_grasps server的一个自定义名称（名称存储在support_surface_变量）的服务，成功后再调用pick。

更具体说下抓放是怎么个过程。urdf定义两个判划组：robot_arm、robot_claw。robot_arm是手臂部分，需定义运动学求解器。robot_claw则是爪子部分，没有运动学求解器，但须以它为group值定义一个未端执行器effetor，此个effector的parent_link需要是robot_arm的末端link，假设这个effetor的名称是robot_eff。物体要放到的位姿记为。

1. 在工作空间定义一个物体，放在world树。物体标识记为box1，它现在在base_link坐标系的位姿。
2. [pick]。在robot_arm规划组，规划出一条从当前位置到能夹起box1的路径，并执行。
3. 控制robot_arm规划组，夹起box1。把box1加到机器人树，同时从world树中移除。
4. [place]。在robot_arm规划组，规定出一条从当前位置到的路径，并执行。
5. 控制robot_arm规划组，放下box1。

一、client

class MoveGroupTestFixture
{
public:
	...

	void ModifyPlanningSceneAsyncInterfaces()
	{
		////////////////////////////////////////////////////////////////////
		// Define a collision object ROS message.
		moveit_msgs::CollisionObject collision_object;
		collision_object.header.frame_id = move_group_.getPlanningFrame();

		// The id of the object is used to identify it.
		collision_object.id = "box1";

		// Define a box to add to the world.
		shape_msgs::SolidPrimitive primitive;
		primitive.type = primitive.BOX;
		primitive.dimensions.resize(3);
		primitive.dimensions[0] = 0.1;
		primitive.dimensions[1] = 1.0;
		primitive.dimensions[2] = 1.0;

		// Define a pose for the box (specified relative to frame_id)
		geometry_msgs::Pose box_pose;
		box_pose.orientation.w = 1.0;
		box_pose.position.x = 0.5;
		box_pose.position.y = 0.0;
		box_pose.position.z = 0.5;

box1的位姿和抓、放可说没关系，它影响的是box1和其它障碍物碰撞检测的结果。它和dimensions一起决定了box1在层级树中的包围盒数值。

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

		// Now, let's add the collision object into the world
		synchronizeGeometryUpdate([&]() { planning_scene_interface_.addCollisionObjects(collision_objects); });
		size_t s = planning_scene_interface_.getObjects().size();
		VALIDATE(s == collision_objects.size(), null_str);
	}

	void attachObject()
	{
		// attach and detach collision object
		synchronizeGeometryUpdate([&]() { 
			bool ret = move_group_.attachObject("box1", "grasping_frame"); // base_link
			VALIDATE(ret, null_str);
		});
		size_t s = planning_scene_interface_.getAttachedObjects().size();
		VALIDATE(s == 1, null_str);
	}

private:
	moveit::planning_interface::MoveGroupInterface& move_group_;
	moveit::planning_interface::PlanningSceneInterface planning_scene_interface_;
	planning_scene_monitor::PlanningSceneMonitorPtr psm_;
};

MoveGroupTestFixture构造函数、synchronizeGeometryUpdate的实现见“规划[4/5]：避障”。

moveit::planning_interface::MoveGroupInterface::Options opt("robot_arm", "robot_description", nh);
moveit::planning_interface::MoveGroupInterface group(opt);

MoveGroupTestFixture test(group, cbqueue);
test.ModifyPlanningSceneAsyncInterfaces();

不论抓取还是放置，都先执行上面的<4行代码>。至此，group针对的是“robot_arm”规划组，物体box1增加到了world树。

1.1 抓取

void fillGrasps(moveit_msgs::PickupGoal& goal, const Eigen::Isometry3d& desire_pose)
{
  ROS_DEBUG_NAMED("manipulation", "Using default grasp poses");
  goal.minimize_object_distance = true;

  const Eigen::Vector3d& trans = desire_pose.translation();
  const Eigen::Quaterniond quat(desire_pose.rotation());
  SDL_Log("{fillGrasps}desire_pose: %s", verbose_Isometry3d(desire_pose).c_str());
  
  // add a number of default grasp points
  // \todo add more!
  moveit_msgs::Grasp g;
  // g.grasp_pose.header.frame_id = goal.target_name;
  g.grasp_pose.header.frame_id = "base_link";
  g.grasp_pose.pose.position.x = trans.x();
  g.grasp_pose.pose.position.y = trans.y();
  g.grasp_pose.pose.position.z = trans.z();
  g.grasp_pose.pose.orientation.x = quat.x();
  g.grasp_pose.pose.orientation.y = quat.y();
  g.grasp_pose.pose.orientation.z = quat.z();
  g.grasp_pose.pose.orientation.w = quat.w();

  // g.pre_grasp_approach.direction.header.frame_id = lscene->getPlanningFrame();
  g.pre_grasp_approach.direction.header.frame_id = "base_link";
  g.pre_grasp_approach.direction.vector.x = 1.0;
  g.pre_grasp_approach.min_distance = 0.1;
  g.pre_grasp_approach.desired_distance = 0.2;
  
  // g.post_grasp_retreat.direction.header.frame_id = lscene->getPlanningFrame();
  g.post_grasp_retreat.direction.header.frame_id = "base_link";
  g.post_grasp_retreat.direction.vector.z = 1.0;
  g.post_grasp_retreat.min_distance = 0.1;
  g.post_grasp_retreat.desired_distance = 0.2;

  {
    int ii = 0;
    g.pre_grasp_approach.direction.vector.x = 0.0;
    g.post_grasp_retreat.direction.vector.z = 0.0;
  }
  goal.possible_grasps.push_back(g);
}

void test_pick()
{
  <4行代码>
  moveit_msgs::PickupGoal goal = group.constructPickupGoal("box1", std::vector<moveit_msgs::Grasp>(), false);
  fillGrasps(goal, box1_near_pose);

box1_near_pose即，移到它时，就可夹住box1。

  goal.minimize_object_distance = 0;		
  moveit::planning_interface::MoveItErrorCode success = group.pick(goal);
}

1.2 放置

void test_place()
{
  <4行代码>
  test.attachObject();

先把box1加到机器人树，同时移出world树。

  geometry_msgs::PoseStamped pose;
  pose.header.stamp = ros::Time::now();
  pose.header.frame_id = "base_link";
  pose.pose = tf2::toMsg(place_pose);

place_pose即，是希望把物体放置到的位姿。

  std::vector pls = group.posesToPlaceLocations({ pose });
  moveit_msgs::PlaceLocation& pl = pls.back();
  pl.pre_place_approach.direction.vector.x = 0.0;
  pl.pre_place_approach.direction.vector.y = 0.0;
  pl.pre_place_approach.direction.vector.z = 0.0;

  pl.post_place_retreat.direction.vector.x = 0.0;
  pl.post_place_retreat.direction.vector.y = 0.0;
  pl.post_place_retreat.direction.vector.z = 0.0;

  moveit_msgs::PlaceGoal goal = group.constructPlaceGoal("box1", pls, false);
  goal.place_eef = 1;
  moveit::planning_interface::MoveItErrorCode success = group.place(goal);
}

二、server

2.1 PlanExecution::planAndExecuteHelper

planAndExecuteHelper是pick、place都要执行的逻辑，是个同步函数，会等到机器人操作完后再返回。它依次执行两个任务。

1. opt.plan_callback_。规划出此次pick或place的路径轨迹。opt.plan_callback_是个函数指针，pick和place指向了不同函数。
2. executeAndMonitor。让机器人按上面得出的路径去操作，并同步等到机器人操作完后再返回。

void plan_execution::PlanExecution::planAndExecuteHelper(ExecutableMotionPlan& plan, const Options& opt)
{
  // perform initial configuration steps & various checks
  preempt_.checkAndClear();  // clear any previous preempt_ request

  bool preempt_requested = false;

  // run the actual motion plan & execution
  unsigned int max_replan_attempts =
      opt.replan_ ? (opt.replan_attempts_ > 0 ? opt.replan_attempts_ : default_max_replan_attempts_) : 1;
  unsigned int replan_attempts = 0;
  bool previously_solved = false;

  // run a planning loop for at most the maximum replanning attempts;
  // re-planning is executed only in case of known types of failures (e.g., environment changed)
  do
  {
    replan_attempts++;
    ROS_INFO_NAMED("plan_execution", "Planning attempt %u of at most %u", replan_attempts, max_replan_attempts);

    if (opt.before_plan_callback_)
      opt.before_plan_callback_();

    new_scene_update_ = false;  // we clear any scene updates to be evaluated because we are about to compute a new
                                // plan, which should consider most recent updates already

    // if we never had a solved plan, or there is no specified way of fixing plans, just call the planner; otherwise,
    // try to repair the plan we previously had;
    bool solved =
        (!previously_solved || !opt.repair_plan_callback_) ?
            opt.plan_callback_(plan) :
            opt.repair_plan_callback_(plan, trajectory_execution_manager_->getCurrentExpectedTrajectoryIndex());

previously_solved初值是false，因而总会调用opt.plan_callback_，这是一个函数指针。

pick时，opt.plan_callback_指向MoveGroupPickPlaceAction::planUsingPickPlacePickup，后者依次执行两个步骤。

1. 调用PickPlace::planPick。这是主要操作，planPick执行着pick规划，假设变量pick_plan是它的返回值，类型pick_place::PickPlan，派生于PickPlacePlanBase。pick_plan->pipeline_.success_存储着规划出的路径。success_类型是std::vector<ManipulationPlan>，其中success_.back()的认为最好的那个ManipulationPlan，记为result。注：当client生成moveit_msgs::PickupGoal时填的plan=true，即只计算规划路径，不执行，那它就是直接调用PickPlace::planPick，得到路径后转换为result.trajectories_转成client希望的moveit_msgs::RobotTrajectory，并存储在action_res.trajectory_stages，就结束了。
2. plan.plan_components_ = result。plan就是opt.plan_callback_传下参数。

place时，opt.plan_callback_指向MoveGroupPickPlaceAction::planUsingPickPlacePlace，后者依次执行两个步骤。

1. 调用PickPlace::planPlace。这是主要操作，planPlace执行着place规划，假设变量place_plan是它的返回值，类型pick_place::place_plan，它和PickPlan一样，派生于PickPlacePlanBase。和PickPlan一样，success_.back()是认为最好的那个ManipulationPlan，记为result。注：当client生成moveit_msgs::PlaceGoal时填的plan=true，即只计算规划路径，不执行，那它就是直接调用PickPlace::planPlace，得到路径后转换为result.trajectories_转成client希望的moveit_msgs::RobotTrajectory，并存储在action_res.trajectory_stages，就结束了。
2. plan.plan_components_ = result。

至此plan.plan_components_存储着规划出的路径，接下就是要让机器人按这路径去执行。

    preempt_requested = preempt_.checkAndClear();
    if (preempt_requested)
      break;

    // if planning fails in a manner that is not recoverable, we exit the loop,
    // otherwise, we attempt to continue, if replanning attempts are left
    if (plan.error_code_.val == moveit_msgs::MoveItErrorCodes::PLANNING_FAILED ||
        plan.error_code_.val == moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN ||
        plan.error_code_.val == moveit_msgs::MoveItErrorCodes::UNABLE_TO_AQUIRE_SENSOR_DATA)
    {
      if (plan.error_code_.val == moveit_msgs::MoveItErrorCodes::UNABLE_TO_AQUIRE_SENSOR_DATA && opt.replan_delay_ > 0.0)
      {
        ros::WallDuration d(opt.replan_delay_);
        d.sleep();
      }
      continue;
    }

    // abort if no plan was found
    if (solved)
      previously_solved = true;
    else
      break;

    if (plan.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
    {
      if (opt.before_execution_callback_)
        opt.before_execution_callback_();

      preempt_requested = preempt_.checkAndClear();
      if (preempt_requested)
        break;

      // execute the trajectory, and monitor its execution
      plan.error_code_ = executeAndMonitor(plan, false);

executeAndMonitor功能是让机器人按上面得出的路径去操作，并同步等到机器人操作完后再返回。

    }

    if (plan.error_code_.val == moveit_msgs::MoveItErrorCodes::PREEMPTED)
      preempt_requested = true;

    // if execution succeeded or failed in a manner that we do not consider recoverable, we exit the loop (with failure)
    if (plan.error_code_.val != moveit_msgs::MoveItErrorCodes::MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE)
      break;
    else
    {
      // otherwise, we wait (if needed)
      if (opt.replan_delay_ > 0.0)
      {
        ROS_INFO_NAMED("plan_execution", "Waiting for a %lf seconds before attempting a new plan ...",
                       opt.replan_delay_);
        ros::WallDuration d(opt.replan_delay_);
        d.sleep();
        ROS_INFO_NAMED("plan_execution", "Done waiting");
      }
    }

    preempt_requested = preempt_.checkAndClear();
    if (preempt_requested)
      break;

  } while (replan_attempts < max_replan_attempts);

  if (preempt_requested)
  {
    ROS_DEBUG_NAMED("plan_execution", "PlanExecution was preempted");
    plan.error_code_.val = moveit_msgs::MoveItErrorCodes::PREEMPTED;
  }

  if (opt.done_callback_)
    opt.done_callback_();

  if (plan.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
    ROS_DEBUG_NAMED("plan_execution", "PlanExecution finished successfully.");
  else
    ROS_DEBUG_NAMED("plan_execution", "PlanExecution terminating with error code %d - '%s'", plan.error_code_.val,
                    getErrorCodeString(plan.error_code_).c_str());
}

2.2 PlanExecution::executeAndMonitor

输入参数plan.plan_components_存储着pick或place规划出的路径，executeAndMonitor负责让机器人按这路径去操作，要等操作完后，函数才返回。既然是按路径去操作机器人，那和“server处理exectraj action：MoveGroupExecuteTrajectoryAction”(https://www.cswamp.com/post/123)中说的有很多相似之处。因为那里介绍得更详细，可以看完那里后再细看这函数。

moveit_msgs::MoveItErrorCodes plan_execution::PlanExecution::executeAndMonitor(ExecutableMotionPlan& plan,
                                                                               bool reset_preempted)
{
  if (reset_preempted)
    preempt_.checkAndClear();

  if (!plan.planning_scene_monitor_)
    plan.planning_scene_monitor_ = planning_scene_monitor_;
  if (!plan.planning_scene_)
    plan.planning_scene_ = planning_scene_monitor_->getPlanningScene();

  moveit_msgs::MoveItErrorCodes result;

  // try to execute the trajectory
  execution_complete_ = true;

  if (!trajectory_execution_manager_)
  {
    ROS_ERROR_NAMED("plan_execution", "No trajectory execution manager");
    result.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
    return result;
  }

  if (plan.plan_components_.empty())
  {
    result.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
    return result;
  }

  execution_complete_ = false;

  // push the trajectories we have slated for execution to the trajectory execution manager
  int prev = -1;
  for (std::size_t i = 0; i < plan.plan_components_.size(); ++i)
  {

plan.plan_components_可能包含多个路径段，每个路径段对应push到TrajectoryExecutionManager，生成一个Part。

    // \todo should this be in trajectory_execution ? Maybe. Then that will have to use kinematic_trajectory too;
    // spliting trajectories for controllers becomes interesting: tied to groups instead of joints. this could cause
    // some problems
    // in the meantime we do a hack:

    bool unwound = false;
    for (std::size_t j = 0; j < i; ++j)
      // if we ran unwind on a path for the same group
      if (plan.plan_components_[j].trajectory_ &&
          plan.plan_components_[j].trajectory_->getGroup() == plan.plan_components_[i].trajectory_->getGroup() &&
          !plan.plan_components_[j].trajectory_->empty())
      {
        plan.plan_components_[i].trajectory_->unwind(plan.plan_components_[j].trajectory_->getLastWayPoint());
        unwound = true;
        break;
      }

    if (!unwound)
    {
      // unwind the path to execute based on the current state of the system
      if (prev < 0)
        plan.plan_components_[i].trajectory_->unwind(
            plan.planning_scene_monitor_ && plan.planning_scene_monitor_->getStateMonitor() ?
                *plan.planning_scene_monitor_->getStateMonitor()->getCurrentState() :
                plan.planning_scene_->getCurrentState());
      else
        plan.plan_components_[i].trajectory_->unwind(plan.plan_components_[prev].trajectory_->getLastWayPoint());
    }

    if (plan.plan_components_[i].trajectory_ && !plan.plan_components_[i].trajectory_->empty())
      prev = i;

    // convert to message, pass along
    moveit_msgs::RobotTrajectory msg;
    plan.plan_components_[i].trajectory_->getRobotTrajectoryMsg(msg);
    if (!trajectory_execution_manager_->push(msg, plan.plan_components_[i].controller_names_))
    {
      trajectory_execution_manager_->clear();
      ROS_ERROR_STREAM_NAMED("plan_execution", "Apparently trajectory initialization failed");
      execution_complete_ = true;
      result.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
      return result;
    }
  }

  if (!trajectory_monitor_ && planning_scene_monitor_->getStateMonitor())
  {
    // Pass current value of reconfigurable parameter plan_execution/record_trajectory_state_frequency
    double sampling_frequency = 0.0;
    node_handle_.getParam("plan_execution/record_trajectory_state_frequency", sampling_frequency);
    trajectory_monitor_ = std::make_shared<planning_scene_monitor::TrajectoryMonitor>(
        planning_scene_monitor_->getStateMonitor(), sampling_frequency);
  }

  // start recording trajectory states
  if (trajectory_monitor_)
    trajectory_monitor_->startTrajectoryMonitor();

  // start a trajectory execution thread
  trajectory_execution_manager_->execute(
      std::bind(&PlanExecution::doneWithTrajectoryExecution, this, std::placeholders::_1),
      std::bind(&PlanExecution::successfulTrajectorySegmentExecution, this, &plan, std::placeholders::_1));
  // wait for path to be done, while checking that the path does not become invalid

trajectory_execution_manager_->execute(...)不是一个同步函数，它只是启动TrajectoryExecutionManager::executeThread线程，就返回了。要做到同步，后面代码需等待TrajectoryExecutionManager::executeThread线程执行结束。

在MoveGroupExecuteTrajectoryAction，用的同步方法是调用“context_->trajectory_execution_manager_->waitForExecution()”，这里则是利用了

executeThread结束时会调用这里设置的回调doneWithTrajectoryExecution，该函数只一个动作，把execution_complete_设为true。用了它后，executeAndMonitor通过轮询execution_complete_，来判断executeThread是否已结束。

另一个回调successfulTrajectorySegmentExecution是干什么用的？——executeThread在执行完一个“executePart(...)”后，会回调它。它内中有个操作，如果接下要执行的路径段是不可到达的，则把path_became_invalid_设为true。

  ros::Rate r(100);
  path_became_invalid_ = false;
  bool preempt_requested = false;

  while (node_handle_.ok() && !execution_complete_ && !path_became_invalid_)
  {

execution_complete_==false，意味着还没调用过doneWithTrajectoryExecution，须继续等待executeThread线程结束。

path_became_invalid_==true，意味着后绪路径无效，不必再发送到机器人了。如果因它退出while，并不意味着executeThread线程结束，后面需手动调用stopExecution()来结束executeThread。

    r.sleep();
    // check the path if there was an environment update in the meantime
    if (new_scene_update_)
    {
      new_scene_update_ = false;
      std::pair<int, int> current_index = trajectory_execution_manager_->getCurrentExpectedTrajectoryIndex();
      if (!isRemainingPathValid(plan, current_index))
      {
        ROS_INFO_NAMED("plan_execution", "Trajectory component '%s' is invalid after scene update",
                       plan.plan_components_[current_index.first].description_.c_str());
        path_became_invalid_ = true;
        break;
      }
    }

    preempt_requested = preempt_.checkAndClear();
    if (preempt_requested)
      break;
  }

能到这里，应该是结束executeThread线程（execution_complete_ = true），或出现路径错误（path_became_invalid_=true）。

  // stop execution if needed
  if (preempt_requested)
  {
    ROS_INFO_NAMED("plan_execution", "Stopping execution due to preempt request");
    trajectory_execution_manager_->stopExecution();
  }
  else if (path_became_invalid_)
  {
    ROS_INFO_NAMED("plan_execution", "Stopping execution because the path to execute became invalid"
                                     "(probably the environment changed)");
    trajectory_execution_manager_->stopExecution();

路径错误（path_became_invalid_=true）的话，并不意味着executeThread线程结束，需手动调用stopExecution()来结束executeThread。

  }
  else if (!execution_complete_)
  {
    ROS_WARN_NAMED("plan_execution", "Stopping execution due to unknown reason."
                                     "Possibly the node is about to shut down.");
    trajectory_execution_manager_->stopExecution();
  }

  // stop recording trajectory states
  if (trajectory_monitor_)
  {
    trajectory_monitor_->stopTrajectoryMonitor();
    plan.executed_trajectory_ =
        std::make_shared<robot_trajectory::RobotTrajectory>(planning_scene_monitor_->getRobotModel(), "");
    trajectory_monitor_->swapTrajectory(*plan.executed_trajectory_);
  }

  // decide return value
  if (path_became_invalid_)
    result.val = moveit_msgs::MoveItErrorCodes::MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE;
  else
  {
    if (preempt_requested)
    {
      result.val = moveit_msgs::MoveItErrorCodes::PREEMPTED;
    }
    else
    {
      if (trajectory_execution_manager_->getLastExecutionStatus() ==
          moveit_controller_manager::ExecutionStatus::SUCCEEDED)
        result.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
      else if (trajectory_execution_manager_->getLastExecutionStatus() ==
               moveit_controller_manager::ExecutionStatus::TIMED_OUT)
        result.val = moveit_msgs::MoveItErrorCodes::TIMED_OUT;
      else
        result.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
    }
  }
  return result;
}