Exemplos de updateDistanceObject em C++ (Cpp)

Exemplo n.º 1

Arquivo: collision_world_distance_field.cpp Projeto: bigcmos/moveit_experimental

boost::shared_ptr<CollisionWorldDistanceField::DistanceFieldCacheEntry>
CollisionWorldDistanceField::generateDistanceFieldCacheEntry()
{
  boost::shared_ptr<DistanceFieldCacheEntry> dfce(new DistanceFieldCacheEntry());
  if(use_signed_distance_field_)
  {
    dfce->distance_field_.reset(new distance_field::PropagationDistanceField(size_x_,
                                                                             size_y_, 
                                                                             size_z_, 
                                                                             resolution_, 
                                                                             -(size_x_/2.0), 
                                                                             -(size_y_/2.0), 
                                                                             -(size_z_/2.0), 
                                                                             max_propogation_distance_, true));
  }
  else
  {
    dfce->distance_field_.reset(new distance_field::PropagationDistanceField(size_x_,
                                                                             size_y_, 
                                                                             size_z_, 
                                                                             resolution_, 
                                                                             -(size_x_/2.0), 
                                                                             -(size_y_/2.0), 
                                                                             -(size_z_/2.0), 
                                                                             max_propogation_distance_, false));
  }
  EigenSTL::vector_Vector3d add_points;
  EigenSTL::vector_Vector3d subtract_points;
  for(World::const_iterator it=getWorld()->begin(); it!=getWorld()->end(); ++it)
  {
    updateDistanceObject(it->first, dfce, add_points, subtract_points);
  }
  dfce->distance_field_->addPointsToField(add_points);
  return dfce;
}

Exemplo n.º 2

Arquivo: collision_world_distance_field.cpp Projeto: jonbinney/moveit-core

void CollisionWorldDistanceField::removeObject(const std::string& id)
{
  CollisionWorld::removeObject(id);
  EigenSTL::vector_Vector3d add_points;
  EigenSTL::vector_Vector3d subtract_points;
  updateDistanceObject(id, distance_field_cache_entry_, add_points, subtract_points);
  distance_field_cache_entry_->distance_field_->removePointsFromField(subtract_points);
}

Exemplo n.º 3

Arquivo: collision_world_distance_field.cpp Projeto: jonbinney/moveit-core

bool CollisionWorldDistanceField::removeShapeFromObject(const std::string& id,
                                                        const shapes::ShapeConstPtr& shape) {
  if(CollisionWorld::removeShapeFromObject(id, shape)) {
    EigenSTL::vector_Vector3d add_points;
    EigenSTL::vector_Vector3d subtract_points;
    updateDistanceObject(id, distance_field_cache_entry_, add_points, subtract_points);
    distance_field_cache_entry_->distance_field_->removePointsFromField(subtract_points);
  }
  return false;
}

Exemplo n.º 4

Arquivo: collision_world_distance_field.cpp Projeto: jonbinney/moveit-core

void CollisionWorldDistanceField::addToObject(const std::string& id,
                                              const shapes::ShapeConstPtr &shape, 
                                              const Eigen::Affine3d &pose)
{
  ros::WallTime n = ros::WallTime::now();
  CollisionWorld::addToObject(id, shape, pose);
  EigenSTL::vector_Vector3d add_points;
  EigenSTL::vector_Vector3d subtract_points;
  updateDistanceObject(id, distance_field_cache_entry_, add_points, subtract_points);
  distance_field_cache_entry_->distance_field_->removePointsFromField(subtract_points);
  distance_field_cache_entry_->distance_field_->addPointsToField(add_points);
  ROS_DEBUG_STREAM("Adding took " << (ros::WallTime::now()-n));
}