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 MAP_ACCESSIBILITY_ANALYSIS_H 00019 #define MAP_ACCESSIBILITY_ANALYSIS_H 00020 00021 #include <iostream> 00022 #include <sstream> 00023 #include <string> 00024 #include <vector> 00025 #include <math.h> 00026 00027 #include <ros/ros.h> 00028 #include <XmlRpc.h> 00029 00030 #include <nav_msgs/OccupancyGrid.h> 00031 #include <geometry_msgs/Point.h> 00032 #include <geometry_msgs/Pose2D.h> 00033 #include <nav_msgs/GridCells.h> 00034 00035 #include <image_transport/image_transport.h> 00036 00037 #include <message_filters/subscriber.h> 00038 #include <message_filters/sync_policies/approximate_time.h> 00039 #include <message_filters/synchronizer.h> 00040 #include <message_filters/time_synchronizer.h> 00041 00042 #include <tf/transform_listener.h> 00043 00044 #include <cob_map_accessibility_analysis/CheckPointAccessibility.h> 00045 #include <cob_map_accessibility_analysis/CheckPerimeterAccessibility.h> 00046 #include <cob_3d_mapping_msgs/GetApproachPoseForPolygon.h> 00047 00048 // opencv 00049 #include <opencv2/opencv.hpp> 00050 #include <cv_bridge/cv_bridge.h> 00051 #include <sensor_msgs/image_encodings.h> 00052 00053 #include <pcl/conversions.h> 00054 #include <pcl/point_types.h> 00055 00056 #include <boost/thread/mutex.hpp> 00057 #include <boost/shared_ptr.hpp> 00058 #include <boost/tokenizer.hpp> 00059 #include <boost/foreach.hpp> 00060 #include <boost/algorithm/string.hpp> 00061 00062 #include <cob_map_accessibility_analysis/map_accessibility_analysis.h> 00063 00064 class MapAccessibilityAnalysisServer : public MapAccessibilityAnalysis 00065 { 00066 public: 00067 MapAccessibilityAnalysisServer(ros::NodeHandle nh); 00068 ~MapAccessibilityAnalysisServer(); 00069 00070 protected: 00071 00072 // reads out the robot footprint from a string or array 00073 // returns the polygonal footprint of the robot in [m] 00074 std::vector<geometry_msgs::Point> loadRobotFootprint(XmlRpc::XmlRpcValue& footprint_list); 00075 00076 // original map initializer 00077 void mapInit(ros::NodeHandle& nh_map); 00078 00079 // dynamic obstacles map initializer using obstacles and inflated obstacles topics 00080 void inflationInit(ros::NodeHandle& nh); 00081 00082 // dynamic obstacles map initializer using obstacles and robot radius 00083 void dynamicObstaclesInit(ros::NodeHandle& nh); 00084 00085 // map data call-back function to get the original map and also to write original inflated map for the obstacles 00086 void mapDataCallback(const nav_msgs::OccupancyGrid::ConstPtr& map_msg_data); 00087 00088 // to create dynamic obstacles map from obstacles and inflated obstacles topics 00089 void inflatedObstacleDataCallback(const nav_msgs::GridCells::ConstPtr& obstacles_data, 00090 const nav_msgs::GridCells::ConstPtr& inflated_obstacles_data); 00091 00092 // to create dynamic obstacles map from obstacles topic and robot radius 00093 void obstacleDataCallback(const nav_msgs::GridCells::ConstPtr& obstacles_data); 00094 00095 // callback for service checking the accessibility of a vector of points 00096 bool checkPose2DArrayCallback(cob_map_accessibility_analysis::CheckPointAccessibility::Request& req, 00097 cob_map_accessibility_analysis::CheckPointAccessibility::Response& res); 00098 00099 // callback for service checking the accessibility of a perimeter around a center point 00100 bool checkPerimeterCallback(cob_map_accessibility_analysis::CheckPerimeterAccessibility::Request& req, 00101 cob_map_accessibility_analysis::CheckPerimeterAccessibility::Response& res); 00102 00103 // callback for service checking the accessibility of a perimeter around a polygon 00104 bool checkPolygonCallback(cob_3d_mapping_msgs::GetApproachPoseForPolygon::Request& req, 00105 cob_3d_mapping_msgs::GetApproachPoseForPolygon::Response& res); 00106 00107 // reads the robot coordinates from tf 00108 cv::Point getRobotLocationInPixelCoordinates(); 00109 00110 ros::NodeHandle node_handle_; 00111 00112 ros::Subscriber map_msg_sub_; // subscriber to the map topic 00113 bool map_data_recieved_; // flag whether the map has already been received by the node 00114 00115 image_transport::ImageTransport* it_; 00116 image_transport::Publisher inflated_map_image_pub_; 00117 bool publish_inflated_map_; 00118 message_filters::Subscriber<nav_msgs::GridCells> obstacles_sub_; 00119 message_filters::Subscriber<nav_msgs::GridCells> inflated_obstacles_sub_; 00120 typedef message_filters::sync_policies::ApproximateTime<nav_msgs::GridCells, nav_msgs::GridCells> 00121 InflatedObstaclesSyncPolicy; 00122 boost::shared_ptr<message_filters::Synchronizer<InflatedObstaclesSyncPolicy> > 00123 inflated_obstacles_sub_sync_; //< Synchronizer 00124 double obstacle_topic_update_rate_; // defines how often this topic should be updated (set to a low rate to save 00125 // ressources of your computing system) 00126 ros::Duration obstacle_topic_update_delay_; // the inverse of the update rate 00127 ros::Time last_update_time_obstacles_; // time of last update from the obstacle topic 00128 00129 tf::TransformListener tf_listener_; 00130 00131 ros::ServiceServer map_points_accessibility_check_server_; // server handling requests for checking the accessibility 00132 // of a set of points 00133 ros::ServiceServer map_perimeter_accessibility_check_server_; // server handling requests for checking the 00134 // accessibility of any point on the perimeter of a 00135 // given position 00136 ros::ServiceServer map_polygon_accessibility_check_server_; // server handling requests for checking the 00137 // accessibility of any point around a given polygon 00138 // (obeying the safety margin around the polygon) 00139 00140 // maps 00141 cv::Mat original_map_; 00142 cv::Mat inflated_original_map_; // contains only the inflated static obstacles 00143 cv::Mat inflated_map_; // contains inflated static and dynamic obstacles 00144 00145 boost::mutex mutex_inflated_map_; // mutex for access on inflated_map 00146 00147 // map properties 00148 double map_resolution_; // in [m/cell] 00149 double inverse_map_resolution_; // in [cell/m] 00150 cv::Point2d map_origin_; // in [m,m,rad] 00151 std::string map_link_name_; 00152 00153 // robot 00154 double robot_radius_; // in [m] 00155 std::string robot_base_link_name_; 00156 bool approach_path_accessibility_check_; // if true, the path to a goal position must be accessible as well 00157 }; 00158 00159 #endif // MAP_ACCESSIBILITY_ANALYSIS_H