18 #ifndef COB_OBSTACLE_DISTANCE_MOVEIT__OBSTACLE_DISTANCE_H 19 #define COB_OBSTACLE_DISTANCE_MOVEIT__OBSTACLE_DISTANCE_H 23 #include <boost/thread.hpp> 24 #include <boost/thread/mutex.hpp> 30 #include <moveit_msgs/PlanningScene.h> 31 #include <moveit_msgs/GetPlanningScene.h> 33 #include <cob_srvs/SetString.h> 34 #include <cob_control_msgs/ObstacleDistances.h> 35 #include <cob_control_msgs/GetObstacleDistance.h> 55 bool planningSceneCallback(moveit_msgs::GetPlanningScene::Request &
req, moveit_msgs::GetPlanningScene::Response &res);
58 std::map<std::string, std::shared_ptr<fcl::CollisionObject> >
robot_links_;
65 cob_control_msgs::GetObstacleDistance::Response &res);
69 bool registerCallback(cob_srvs::SetString::Request &
req, cob_srvs::SetString::Response &res);
75 cob_control_msgs::ObstacleDistance
getDistanceInfo(
const std::shared_ptr<fcl::CollisionObject> object_a,
76 const std::shared_ptr<fcl::CollisionObject> object_b);
81 #endif // COB_OBSTACLE_DISTANCE_MOVEIT__OBSTACLE_DISTANCE_H ros::Timer distance_timer_
planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_
bool registerCallback(cob_srvs::SetString::Request &req, cob_srvs::SetString::Response &res)
bool planningSceneCallback(moveit_msgs::GetPlanningScene::Request &req, moveit_msgs::GetPlanningScene::Response &res)
ros::Timer planning_scene_timer_
bool calculateDistanceServiceCallback(cob_control_msgs::GetObstacleDistance::Request &req, cob_control_msgs::GetObstacleDistance::Response &res)
ros::ServiceServer monitored_scene_server_
std::map< std::string, std::shared_ptr< fcl::CollisionObject > > collision_objects_
ros::ServiceServer unregister_server_
ros::Publisher monitored_scene_pub_
void updatedScene(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType type)
collision_detection::AllowedCollisionMatrix acm_
std::map< std::string, std::shared_ptr< fcl::CollisionObject > > robot_links_
ros::Publisher distance_pub_
bool unregisterCallback(cob_srvs::SetString::Request &req, cob_srvs::SetString::Response &res)
ros::ServiceServer calculate_obstacle_distance_
boost::mutex registered_links_mutex_
std::set< std::string > registered_links_
void calculateDistanceTimerCallback(const ros::TimerEvent &event)
cob_control_msgs::ObstacleDistance getDistanceInfo(const std::shared_ptr< fcl::CollisionObject > object_a, const std::shared_ptr< fcl::CollisionObject > object_b)
float MAXIMAL_MINIMAL_DISTANCE
void planningSceneTimerCallback(const ros::TimerEvent &event)
ros::ServiceServer register_server_