00001 /* 00002 * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 00003 * 00004 * Licensed under the Apache License, Version 2.0 (the "License"); 00005 * you may not use this file except in compliance with the License. 00006 * You may obtain a copy of the License at 00007 * 00008 * http://www.apache.org/licenses/LICENSE-2.0 00009 00010 * Unless required by applicable law or agreed to in writing, software 00011 * distributed under the License is distributed on an "AS IS" BASIS, 00012 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 00013 * See the License for the specific language governing permissions and 00014 * limitations under the License. 00015 */ 00016 00017 00018 #ifndef COB_OBSTACLE_DISTANCE_MOVEIT__OBSTACLE_DISTANCE_H 00019 #define COB_OBSTACLE_DISTANCE_MOVEIT__OBSTACLE_DISTANCE_H 00020 00021 #include <map> 00022 #include <string.h> 00023 #include <boost/thread.hpp> 00024 #include <boost/thread/mutex.hpp> 00025 00026 #include <ros/ros.h> 00027 #include <moveit/planning_scene_monitor/planning_scene_monitor.h> 00028 #include <moveit/collision_detection_fcl/collision_robot_fcl.h> 00029 #include <moveit/collision_detection_fcl/collision_world_fcl.h> 00030 #include <moveit_msgs/PlanningScene.h> 00031 #include <moveit_msgs/GetPlanningScene.h> 00032 00033 #include <cob_srvs/SetString.h> 00034 #include <cob_control_msgs/ObstacleDistances.h> 00035 #include <cob_control_msgs/GetObstacleDistance.h> 00036 00037 #include <eigen_conversions/eigen_msg.h> 00038 #include <tf_conversions/tf_eigen.h> 00039 00040 class ObstacleDistanceMoveit 00041 { 00042 public: 00043 ObstacleDistanceMoveit(); 00044 00045 private: 00046 ros::NodeHandle nh_; 00047 float MAXIMAL_MINIMAL_DISTANCE; 00048 00049 planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; 00050 void updatedScene(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType type); 00051 00052 ros::Timer planning_scene_timer_; 00053 ros::Publisher monitored_scene_pub_; 00054 ros::ServiceServer monitored_scene_server_; 00055 bool planningSceneCallback(moveit_msgs::GetPlanningScene::Request &req, moveit_msgs::GetPlanningScene::Response &res); 00056 void planningSceneTimerCallback(const ros::TimerEvent& event); 00057 00058 std::map<std::string, boost::shared_ptr<fcl::CollisionObject> > robot_links_; 00059 std::map<std::string, boost::shared_ptr<fcl::CollisionObject> > collision_objects_; 00060 std::set< std::string > registered_links_; 00061 boost::mutex registered_links_mutex_; 00062 00063 ros::ServiceServer calculate_obstacle_distance_; 00064 bool calculateDistanceServiceCallback(cob_control_msgs::GetObstacleDistance::Request &req, 00065 cob_control_msgs::GetObstacleDistance::Response &res); 00066 00067 ros::Publisher distance_pub_; 00068 ros::ServiceServer register_server_, unregister_server_; 00069 bool registerCallback(cob_srvs::SetString::Request &req, cob_srvs::SetString::Response &res); 00070 bool unregisterCallback(cob_srvs::SetString::Request &req, cob_srvs::SetString::Response &res); 00071 00072 ros::Timer distance_timer_; 00073 void calculateDistanceTimerCallback(const ros::TimerEvent& event); 00074 00075 cob_control_msgs::ObstacleDistance getDistanceInfo(const boost::shared_ptr<fcl::CollisionObject> object_a, 00076 const boost::shared_ptr<fcl::CollisionObject> object_b); 00077 00078 collision_detection::AllowedCollisionMatrix acm_; 00079 }; 00080 00081 #endif // COB_OBSTACLE_DISTANCE_MOVEIT__OBSTACLE_DISTANCE_H