C++ PlanningSceneConstPtr::checkCollision方法代码示例

//.........这里部分代码省略.........
// ======== Unpack solver result into constrained goal state. ========



  ROS_INFO("Finished solving in %ld iterations, unpacking data...", num_iters);
  Eigen::VectorXd joint_deltas(N);
  std::map<std::string, double> goal_update;
  for(size_t joint_index = 0; joint_index < joint_names.size(); joint_index++)
  {
    std::string joint_name = joint_names[joint_index];
    joint_deltas(joint_index) = cvx.vars.q_d[joint_index];
    goal_update[joint_name] = cvx.vars.q_d[joint_index] + start_state.getJointState(joint_name)->getVariableValues()[0];
    //ROS_INFO("Updated joint [%zd] [%s]: %.3f + %.3f", joint_index, joint_name.c_str(), start_state.getJointState(joint_name)->getVariableValues()[0], cvx.vars.q_d[joint_index]);
  }
  Eigen::VectorXd cartesian_deltas = ee_jacobian*joint_deltas;

  //ROS_INFO("Raw Error: translate [%.3f, %.3f, %.3f]",
  //         x_error(0), x_error(1), x_error(2));
  ROS_INFO("ClipError: translate [%.3f, %.3f, %.3f]  euler [%.2f, %.2f, %.2f]",
           delta_x(0), delta_x(1), delta_x(2),
           delta_euler[0], delta_euler[1], delta_euler[2]);
  ROS_INFO("Output:    translate [%.3f, %.3f, %.3f]  euler [%.2f, %.2f, %.2f]",
           cartesian_deltas(0), cartesian_deltas(1), cartesian_deltas(2),
           cartesian_deltas(3), cartesian_deltas(4), cartesian_deltas(5));


  //goal_update[vars.qdd_c[index] + ]
  constrained_goal_state.setStateValues(goal_update);


// ======== Step toward constrained goal, checking for new collisions along the way ========

  //double proxy_goal_tolerance = 0.1;
  double interpolation_progress = 0.0;
  double interpolation_step = 0.34;
  collision_detection::CollisionResult collision_result;
  while(interpolation_progress <= 1.0)
  {
    //ROS_INFO("Doing interpolation, with progress %.2f ", interpolation_progress);

    // TODO this interpolation scheme might not allow the arm to slide along contacts very well...
    planning_models::KinematicStatePtr point;
    point.reset(new planning_models::KinematicState(constrained_goal_state));
    start_state.interpolate(constrained_goal_state, interpolation_progress, *point);

    // get contact set
    collision_detection::CollisionRequest collision_request;
    // TODO magic number
    collision_request.max_contacts = 50;
    collision_request.contacts = true;
    collision_request.distance = false;
    collision_request.verbose = false;

    collision_result.clear();
    planning_scene->checkCollision(collision_request, collision_result, *point);
    if(collision_result.collision)
    {
      break;
    }
    else
    {
      proxy_state = *point;
    }
    // TODO Use proxy_goal_tolerance to exit if we are close enough!
    interpolation_progress += interpolation_step;
  }
  // Store the last collision result!
  last_collision_result = collision_result;

// ======== Convert proxy state to a "trajectory" ========
  //ROS_INFO("Converting proxy to a trajectory, and returning...");

  moveit_msgs::RobotTrajectory rt;
  trajectory_msgs::JointTrajectory traj;
  trajectory_msgs::JointTrajectoryPoint pt;
  sensor_msgs::JointState js;

  planning_models::kinematicStateToJointState(proxy_state, js);
  //const planning_models::KinematicModel::JointModelGroup *jmg = planning_scene->getKinematicModel()->getJointModelGroup(req.motion_plan_request.group_name);

  // getJointNames
  for(size_t i = 0 ; i < js.name.size(); i++)
  {
    std::string name = js.name[i];
    if( !jmg->hasJointModel(name) ) continue;

    traj.joint_names.push_back(js.name[i]);
    pt.positions.push_back(js.position[i]);
    if(js.velocity.size()) pt.velocities.push_back(js.velocity[i]);
  }
  pt.time_from_start = ros::Duration(req.motion_plan_request.allowed_planning_time*1.0);
  traj.points.push_back(pt);

  traj.header.stamp = ros::Time::now();
  traj.header.frame_id = "odom_combined";

  res.trajectory.joint_trajectory = traj;

  return true;
}

    virtual bool adaptAndPlan(const PlannerFn &planner,
                              const planning_scene::PlanningSceneConstPtr& planning_scene,
                              const planning_interface::MotionPlanRequest &req,
                              planning_interface::MotionPlanResponse &res,
                              std::vector<std::size_t> &added_path_index) const
    {
        ROS_DEBUG("Running '%s'", getDescription().c_str());

        // get the specified start state
        robot_state::RobotState start_state = planning_scene->getCurrentState();
        robot_state::robotStateMsgToRobotState(planning_scene->getTransforms(), req.start_state, start_state);

        collision_detection::CollisionRequest creq;
        creq.group_name = req.group_name;
        collision_detection::CollisionResult cres;
        planning_scene->checkCollision(creq, cres, start_state);
        if (cres.collision)
        {
            // Rerun in verbose mode
            collision_detection::CollisionRequest vcreq = creq;
            collision_detection::CollisionResult vcres;
            vcreq.verbose = true;
            planning_scene->checkCollision(vcreq, vcres, start_state);

            if (creq.group_name.empty())
                ROS_INFO("Start state appears to be in collision");
            else
                ROS_INFO_STREAM("Start state appears to be in collision with respect to group " << creq.group_name);

            robot_state::RobotStatePtr prefix_state(new robot_state::RobotState(start_state));
            random_numbers::RandomNumberGenerator &rng = prefix_state->getRandomNumberGenerator();

            const std::vector<const robot_model::JointModel*> &jmodels =
                planning_scene->getRobotModel()->hasJointModelGroup(req.group_name) ?
                planning_scene->getRobotModel()->getJointModelGroup(req.group_name)->getJointModels() :
                planning_scene->getRobotModel()->getJointModels();

            bool found = false;
            for (int c = 0 ; !found && c < sampling_attempts_ ; ++c)
            {
                for (std::size_t i = 0 ; !found && i < jmodels.size() ; ++i)
                {
                    std::vector<double> sampled_variable_values(jmodels[i]->getVariableCount());
                    const double *original_values = prefix_state->getJointPositions(jmodels[i]);
                    jmodels[i]->getVariableRandomPositionsNearBy(rng, &sampled_variable_values[0], original_values, jmodels[i]->getMaximumExtent() * jiggle_fraction_);
                    start_state.setJointPositions(jmodels[i], sampled_variable_values);
                    collision_detection::CollisionResult cres;
                    planning_scene->checkCollision(creq, cres, start_state);
                    if (!cres.collision)
                    {
                        found = true;
                        ROS_INFO("Found a valid state near the start state at distance %lf after %d attempts", prefix_state->distance(start_state), c);
                    }
                }
            }

            if (found)
            {
                planning_interface::MotionPlanRequest req2 = req;
                robot_state::robotStateToRobotStateMsg(start_state, req2.start_state);
                bool solved = planner(planning_scene, req2, res);
                if (solved && !res.trajectory_->empty())
                {
                    // heuristically decide a duration offset for the trajectory (induced by the additional point added as a prefix to the computed trajectory)
                    res.trajectory_->setWayPointDurationFromPrevious(0, std::min(max_dt_offset_, res.trajectory_->getAverageSegmentDuration()));
                    res.trajectory_->addPrefixWayPoint(prefix_state, 0.0);
                    // we add a prefix point, so we need to bump any previously added index positions
                    for (std::size_t i = 0 ; i < added_path_index.size() ; ++i)
                        added_path_index[i]++;
                    added_path_index.push_back(0);
                }
                return solved;
            }
            else
            {
                ROS_WARN("Unable to find a valid state nearby the start state (using jiggle fraction of %lf and %u sampling attempts). Passing the original planning request to the planner.",
                         jiggle_fraction_, sampling_attempts_);
                return planner(planning_scene, req, res);
            }
        }
        else
        {
            if (creq.group_name.empty())
                ROS_DEBUG("Start state is valid");
            else
                ROS_DEBUG_STREAM("Start state is valid with respect to group " << creq.group_name);
            return planner(planning_scene, req, res);
        }
    }