19 #if ROS_VERSION_MINIMUM(1,14,0) // melodic 36 std::map<std::string, collision_detection::FCLObject>::iterator it =
fcl_objs_.begin();
41 obj.push_back(it->second.collision_objects_[0]);
50 CollisionRobotFCL(model)
54 std::vector<std::shared_ptr<fcl::CollisionObject> > &obj)
65 planning_scene::PlanningScenePtr planning_scene_ptr = ps->diff();
67 std::vector<std::shared_ptr<fcl::CollisionObject> > robot_obj, world_obj;
68 robot_state::RobotState robot_state(planning_scene_ptr->getCurrentState());
76 this->robot_links_.clear();
77 this->collision_objects_.clear();
79 for (
unsigned int i = 0; i < robot_obj.size(); i++)
83 this->robot_links_[robot_link->
getID()] = robot_obj[i];
85 for (
unsigned int i = 0; i < world_obj.size(); i++)
89 this->collision_objects_[collision_object->
getID()] = world_obj[i];
96 planning_scene::PlanningScenePtr planning_scene_ptr = ps->diff();
98 planning_scene_ptr->getPlanningSceneMsg(res.scene, req.components);
104 cob_srvs::SetString::Response &res)
106 boost::mutex::scoped_lock lock(registered_links_mutex_);
107 std::pair<std::set<std::string>::iterator,
bool> ret = registered_links_.insert(req.data);
110 res.message = (ret.second) ? (req.data +
" successfully registered") : (req.data +
" already registered");
115 cob_srvs::SetString::Response &res)
117 boost::mutex::scoped_lock lock(registered_links_mutex_);
118 std::set<std::string>::iterator it = registered_links_.find(req.data);
120 if (it != registered_links_.end())
122 registered_links_.erase(it);
125 res.message = req.data +
" successfully unregistered";
130 res.message = req.data +
" has not been registered before";
138 std::map<std::string, std::shared_ptr<fcl::CollisionObject> > robot_links = this->robot_links_;
139 std::map<std::string, std::shared_ptr<fcl::CollisionObject> > collision_objects = this->collision_objects_;
141 boost::mutex::scoped_lock lock(registered_links_mutex_);
142 cob_control_msgs::ObstacleDistances distance_infos;
145 planning_scene::PlanningScenePtr planning_scene_ptr = ps->diff();
147 std::string planning_frame = planning_scene_ptr->getPlanningFrame();
149 std::set<std::string>::iterator link_it;
150 for (link_it = registered_links_.begin(); link_it!=registered_links_.end(); ++link_it)
152 std::string robot_link_name = *link_it;
153 const std::shared_ptr<fcl::CollisionObject> robot_object = robot_links[robot_link_name];
154 ROS_DEBUG_STREAM(
"RobotLink: " << robot_link_name <<
", Type: " << robot_object->getObjectType());
156 std::map<std::string, std::shared_ptr<fcl::CollisionObject> >::iterator obj_it;
157 for (obj_it = collision_objects.begin(); obj_it != collision_objects.end(); ++obj_it)
159 std::string collision_object_name = obj_it->first;
160 const std::shared_ptr<fcl::CollisionObject> collision_object = collision_objects[collision_object_name];
161 ROS_DEBUG_STREAM(
"CollisionLink: " << collision_object_name <<
", Type: " << collision_object->getObjectType());
163 if(collision_object->getObjectType() == fcl::OT_OCTREE)
169 cob_control_msgs::ObstacleDistance info;
170 info = getDistanceInfo(robot_object, collision_object);
172 info.header.frame_id = planning_frame;
173 info.header.stamp =
event.current_real;
174 info.link_of_interest = robot_link_name;
175 info.obstacle_id = collision_object_name;
177 distance_infos.distances.push_back(info);
180 std::map<std::string, std::shared_ptr<fcl::CollisionObject> >::iterator selfcollision_it;
181 for (selfcollision_it = robot_links.begin(); selfcollision_it != robot_links.end(); ++selfcollision_it)
183 std::string robot_self_name = selfcollision_it->first;
186 if(acm_.getEntry(robot_link_name, robot_self_name, type))
190 const std::shared_ptr<fcl::CollisionObject> robot_self_object = robot_links[robot_self_name];
191 ROS_DEBUG_STREAM(
"CollisionLink: " << robot_self_name <<
", Type: " << robot_self_object->getObjectType());
193 cob_control_msgs::ObstacleDistance info;
194 info = getDistanceInfo(robot_object, robot_self_object);
196 info.header.frame_id = planning_frame;
197 info.header.stamp =
event.current_real;
198 info.link_of_interest = robot_link_name;
199 info.obstacle_id = robot_self_name;
201 distance_infos.distances.push_back(info);
210 distance_pub_.publish(distance_infos);
216 planning_scene::PlanningScenePtr planning_scene_ptr = ps->diff();
218 moveit_msgs::PlanningScene scene;
219 planning_scene_ptr->getPlanningSceneMsg(scene);
220 monitored_scene_pub_.publish(scene);
225 cob_control_msgs::GetObstacleDistance::Response &resp)
227 std::map<std::string, std::shared_ptr<fcl::CollisionObject> > robot_links = this->robot_links_;
228 std::map<std::string, std::shared_ptr<fcl::CollisionObject> > collision_objects = this->collision_objects_;
231 for (
unsigned int i=0; i< req.links.size(); ++i)
233 if (req.objects.size() == 0)
236 std::map<std::string, std::shared_ptr<fcl::CollisionObject> >::iterator it;
237 for (it = collision_objects.begin(); it != collision_objects.end(); ++it)
239 const std::shared_ptr<fcl::CollisionObject> collision_object = collision_objects[it->first];
240 if(collision_object->getObjectType() == fcl::OT_OCTREE)
245 resp.link_to_object.push_back(req.links[i] +
"_to_" + it->first);
246 resp.distances.push_back(getDistanceInfo(robot_links[req.links[i]], collision_object).distance);
252 for (
int y = 0;
y < req.objects.size();
y++)
254 const std::shared_ptr<fcl::CollisionObject> collision_object = collision_objects[req.objects[
y]];
255 if(collision_object->getObjectType() == fcl::OT_OCTREE)
260 resp.link_to_object.push_back(req.links[i] +
" to " + req.objects[
y]);
261 resp.distances.push_back(getDistanceInfo(robot_links[req.links[i]], collision_object).distance);
269 const std::shared_ptr<fcl::CollisionObject> object_b)
271 fcl::DistanceRequest
req(
true);
272 fcl::DistanceResult
res;
273 res.update(MAXIMAL_MINIMAL_DISTANCE,
NULL,
NULL, fcl::DistanceResult::NONE, fcl::DistanceResult::NONE);
275 Eigen::Vector3d np_object_a;
276 Eigen::Vector3d np_object_b;
278 double dist = fcl::distance(object_a.get(), object_b.get(),
req, res);
281 if(object_a->getObjectType() == fcl::OT_GEOM && object_b->getObjectType() == fcl::OT_BVH)
284 np_object_a(0) = res.nearest_points[1][0];
285 np_object_a(1) = res.nearest_points[1][1];
286 np_object_a(2) = res.nearest_points[1][2];
288 np_object_b(0) = res.nearest_points[0][0];
289 np_object_b(1) = res.nearest_points[0][1];
290 np_object_b(2) = res.nearest_points[0][2];
294 np_object_a(0) = res.nearest_points[0][0];
295 np_object_a(1) = res.nearest_points[0][1];
296 np_object_a(2) = res.nearest_points[0][2];
298 np_object_b(0) = res.nearest_points[1][0];
299 np_object_b(1) = res.nearest_points[1][1];
300 np_object_b(2) = res.nearest_points[1][2];
304 geometry_msgs::Vector3 np_object_a_msg;
308 geometry_msgs::Vector3 np_object_b_msg;
313 fcl::Transform3f fcl_trans_object_a = object_a->getTransform();
314 fcl::Vec3f fcl_vec_object_a = fcl_trans_object_a.getTranslation();
315 fcl::Quaternion3f fcl_quat_object_a = fcl_trans_object_a.getQuatRotation();
317 Eigen::Affine3d eigen_trans_object_a;
318 tf::Transform tf_trans_object_a(
tf::Quaternion(fcl_quat_object_a.getX(),fcl_quat_object_a.getY(),fcl_quat_object_a.getZ(),fcl_quat_object_a.getW()),
319 tf::Vector3(fcl_vec_object_a[0],fcl_vec_object_a[1],fcl_vec_object_a[2]));
322 geometry_msgs::Transform trans_object_a_msg;
327 fcl::Transform3f fcl_trans_object_b = object_b->getTransform();
328 fcl::Vec3f fcl_vec_object_b = fcl_trans_object_b.getTranslation();
329 fcl::Quaternion3f fcl_quat_object_b = fcl_trans_object_b.getQuatRotation();
331 Eigen::Affine3d eigen_trans_object_b;
332 tf::Transform tf_trans_object_b(
tf::Quaternion(fcl_quat_object_b.getX(),fcl_quat_object_b.getY(),fcl_quat_object_b.getZ(),fcl_quat_object_b.getW()),
333 tf::Vector3(fcl_vec_object_b[0],fcl_vec_object_b[1],fcl_vec_object_b[2]));
336 geometry_msgs::Transform trans_object_b_msg;
341 if(!(object_a->getObjectType() == fcl::OT_BVH && object_b->getObjectType() == fcl::OT_BVH))
343 np_object_a = eigen_trans_object_a * np_object_a;
344 np_object_b = eigen_trans_object_b * np_object_b;
348 if (dist < 0) dist = 0;
350 cob_control_msgs::ObstacleDistance info;
351 info.distance = dist;
355 ROS_DEBUG_STREAM(
"NearestPointTransformed OBJ_A: \n" << info.nearest_point_frame_vector);
356 ROS_DEBUG_STREAM(
"NearestPointTransformed OBJ_B: \n" << info.nearest_point_obstacle_vector);
363 MAXIMAL_MINIMAL_DISTANCE = 5.0;
364 double update_frequency = 50.0;
366 std::string robot_description =
"/robot_description";
367 std::string robot_description_semantic =
"/robot_description_semantic";
368 std::string distance_service =
"/calculate_distance";
369 std::string register_service =
"/register_links";
370 std::string unregister_service =
"/unregister_links";
371 std::string distance_topic =
"/obstacle_distances";
377 acm_ = pss.getAllowedCollisionMatrix();
380 #if ROS_VERSION_MINIMUM(1,14,0) // melodic 386 planning_scene_monitor_ = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>(robot_description, tf_listener_);
389 planning_scene_monitor_->setStateUpdateFrequency(update_frequency);
398 registered_links_.clear();
404 distance_pub_ = nh_.advertise<cob_control_msgs::ObstacleDistances>(distance_topic, 1);
406 monitored_scene_pub_ = nh_.advertise<moveit_msgs::PlanningScene>(
"/monitored_planning_scene", 1);
void getCollisionObject(std::vector< std::shared_ptr< fcl::CollisionObject > > &obj)
const srdf::ModelSharedPtr & getSRDF() const
bool registerCallback(cob_srvs::SetString::Request &req, cob_srvs::SetString::Response &res)
#define ROS_WARN_THROTTLE(rate,...)
bool planningSceneCallback(moveit_msgs::GetPlanningScene::Request &req, moveit_msgs::GetPlanningScene::Response &res)
CreateCollisionRobot(const robot_model::RobotModelConstPtr &model)
bool calculateDistanceServiceCallback(cob_control_msgs::GetObstacleDistance::Request &req, cob_control_msgs::GetObstacleDistance::Response &res)
static const std::string DEFAULT_PLANNING_SCENE_WORLD_TOPIC
void updatedScene(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType type)
const urdf::ModelInterfaceSharedPtr & getURDF() const
bool unregisterCallback(cob_srvs::SetString::Request &req, cob_srvs::SetString::Response &res)
void vectorEigenToMsg(const Eigen::Vector3d &e, geometry_msgs::Vector3 &m)
static const std::string DEFAULT_PLANNING_SCENE_TOPIC
CreateCollisionWorld(const collision_detection::WorldPtr &world)
void calculateDistanceTimerCallback(const ros::TimerEvent &event)
std::vector< FCLCollisionObjectPtr > collision_objects_
#define ROS_DEBUG_STREAM(args)
static const std::string DEFAULT_COLLISION_OBJECT_TOPIC
const std::string & getID() const
cob_control_msgs::ObstacleDistance getDistanceInfo(const std::shared_ptr< fcl::CollisionObject > object_a, const std::shared_ptr< fcl::CollisionObject > object_b)
void planningSceneTimerCallback(const ros::TimerEvent &event)
void transformTFToEigen(const tf::Transform &t, Eigen::Isometry3d &e)
static const std::string DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC
std::map< std::string, FCLObject > fcl_objs_
void constructFCLObject(const World::Object *obj, FCLObject &fcl_obj) const
static void transformTFToMsg(const Transform &bt, geometry_msgs::Transform &msg)
static const std::string DEFAULT_JOINT_STATES_TOPIC
void getCollisionObject(const robot_state::RobotState &state, std::vector< std::shared_ptr< fcl::CollisionObject > > &obj)