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

  planning_interface::PlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene,
                                                            const planning_interface::MotionPlanRequest& req,
                                                            moveit_msgs::MoveItErrorCodes& error_code) const override
  {
    error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;

    if (req.group_name.empty())
    {
      ROS_ERROR("No group specified to plan for");
      error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
      return planning_interface::PlanningContextPtr();
    }

    if (!planning_scene)
    {
      ROS_ERROR("No planning scene supplied as input");
      error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE;
      return planning_interface::PlanningContextPtr();
    }

    // create PlanningScene using hybrid collision detector
    planning_scene::PlanningScenePtr ps = planning_scene->diff();
    ps->setActiveCollisionDetector(collision_detection::CollisionDetectorAllocatorHybrid::create(), true);

    // retrieve and configure existing context
    const CHOMPPlanningContextPtr& context = planning_contexts_.at(req.group_name);
    context->setPlanningScene(ps);
    context->setMotionPlanRequest(req);
    error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
    return context;
  }

PlacePlanPtr PickPlace::planPlace(const planning_scene::PlanningSceneConstPtr &planning_scene, const moveit_msgs::PlaceGoal &goal) const
{
  PlacePlanPtr p(new PlacePlan(shared_from_this()));
  if (planning_scene::PlanningScene::isEmpty(goal.planning_options.planning_scene_diff))
    p->plan(planning_scene, goal);
  else
    p->plan(planning_scene->diff(goal.planning_options.planning_scene_diff), goal);

  if (display_computed_motion_plans_)
  {
    const std::vector<pick_place::ManipulationPlanPtr> &success = p->getSuccessfulManipulationPlans();
    if (!success.empty())
      visualizePlan(success.back());
  }

  if (display_grasps_)
  {
    const std::vector<pick_place::ManipulationPlanPtr> &success = p->getSuccessfulManipulationPlans();
    visualizeGrasps(success);
    const std::vector<pick_place::ManipulationPlanPtr> &failed = p->getFailedManipulationPlans();
    visualizeGrasps(failed);
  }

  return p;
}

// ******************************************************************************************
// Generates an adjacency list of links that are always and never in collision, to speed up collision detection
// ******************************************************************************************
LinkPairMap
computeDefaultCollisions(const planning_scene::PlanningSceneConstPtr &parent_scene, unsigned int * progress,
                         const bool include_never_colliding, const unsigned int num_trials, const double min_collision_fraction,
                         const bool verbose)
{
  // Create new instance of planning scene using pointer
  planning_scene::PlanningScenePtr scene = parent_scene->diff();

  // Map of disabled collisions that contains a link as a key and an ordered list of links that are connected.
  LinkPairMap link_pairs;

  // Track unique edges that have been found to be in collision in some state
  StringPairSet links_seen_colliding;

  // LinkGraph is a custom type of a map with a LinkModel as key and a set of LinkModels as second
  LinkGraph link_graph;

  //ROS_INFO_STREAM("Initial allowed Collision Matrix Size = " << scene.getAllowedCollisions().getSize() );

  // 0. GENERATE ALL POSSIBLE LINK PAIRS -------------------------------------------------------------
  // Generate a list of unique link pairs for all links with geometry. Order pairs alphabetically.
  // There should be n choose 2 pairs
  computeLinkPairs( *scene, link_pairs );
  *progress = 1;

  // 1. FIND CONNECTING LINKS ------------------------------------------------------------------------
  // For each link, compute the set of other links it connects to via a single joint (adjacent links)
  // or via a chain of joints with intermediate links with no geometry (like a socket joint)

  // Create Connection Graph
  computeConnectionGraph(scene->getRobotModel()->getRootLink(), link_graph);
  *progress = 2; // Progress bar feedback

  // 2. DISABLE ALL ADJACENT LINK COLLISIONS ---------------------------------------------------------
  // if 2 links are adjacent, or adjacent with a zero-shape between them, disable collision checking for them
  unsigned int num_adjacent = disableAdjacentLinks( *scene, link_graph, link_pairs);
  *progress = 4; // Progress bar feedback

  // 3. INITIAL CONTACTS TO CONSIDER GUESS -----------------------------------------------------------
  // Create collision detection request object
  collision_detection::CollisionRequest req;
  req.contacts = true;
  // max number of contacts to compute. initial guess is number of links on robot
  req.max_contacts = int(link_graph.size());
  req.max_contacts_per_pair = 1;
  req.verbose = false;

  // 4. DISABLE "DEFAULT" COLLISIONS --------------------------------------------------------
  // Disable all collision checks that occur when the robot is started in its default state
  unsigned int num_default = disableDefaultCollisions(*scene, link_pairs, req);
  *progress = 6; // Progress bar feedback

  // 5. ALWAYS IN COLLISION --------------------------------------------------------------------
  // Compute the links that are always in collision
  unsigned int num_always = disableAlwaysInCollision(*scene, link_pairs, req, links_seen_colliding, min_collision_fraction);
  //ROS_INFO("Links seen colliding total = %d", int(links_seen_colliding.size()));
  *progress = 8; // Progress bar feedback

  // 6. NEVER IN COLLISION -------------------------------------------------------------------
  // Get the pairs of links that are never in collision
  unsigned int num_never = 0;
  if (include_never_colliding) // option of function
  {
    num_never = disableNeverInCollision(num_trials, *scene, link_pairs, req, links_seen_colliding, progress);
  }

  //ROS_INFO("Link pairs seen colliding ever: %d", int(links_seen_colliding.size()));

  if(verbose)
  {
    // Calculate number of disabled links:
    unsigned int num_disabled = 0;
    for ( LinkPairMap::const_iterator pair_it = link_pairs.begin() ; pair_it != link_pairs.end() ; ++pair_it)
    {
      if( pair_it->second.disable_check ) // has a reason to be disabled
        ++num_disabled;
    }

    ROS_INFO("-------------------------------------------------------------------------------");
    ROS_INFO("Statistics:");
    unsigned int num_links = int(link_graph.size());
    double num_possible = boost::math::binomial_coefficient<double>(num_links, 2); // n choose 2
    unsigned int num_sometimes = num_possible - num_disabled;

    ROS_INFO("%6d : %s",   num_links, "Total Links");
    ROS_INFO("%6.0f : %s", num_possible, "Total possible collisions");
    ROS_INFO("%6d : %s",   num_always, "Always in collision");
    ROS_INFO("%6d : %s",   num_never, "Never in collision");
    ROS_INFO("%6d : %s",   num_default, "Default in collision");
    ROS_INFO("%6d : %s",   num_adjacent, "Adjacent links disabled");
    ROS_INFO("%6d : %s",   num_sometimes, "Sometimes in collision");
    ROS_INFO("%6d : %s",   num_disabled, "TOTAL DISABLED");

    /*ROS_INFO("Copy to Spreadsheet:");
    ROS_INFO_STREAM(num_links << "\t" << num_possible << "\t" << num_always << "\t" << num_never
                    << "\t" << num_default << "\t" << num_adjacent << "\t" << num_sometimes
                    << "\t" << num_disabled);
//.........这里部分代码省略.........