00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #include <cob_map_accessibility_analysis/map_accessibility_analysis_server.h>
00019 #include <pcl_ros/point_cloud.h>
00020
00021
00022
00023 MapAccessibilityAnalysisServer::MapAccessibilityAnalysisServer(ros::NodeHandle nh) : node_handle_(nh)
00024 {
00025
00026 ROS_INFO("\n--------------------------------------\nMap Accessibility Analysis "
00027 "Parameters:\n--------------------------------------");
00028 node_handle_.param("approach_path_accessibility_check", approach_path_accessibility_check_, false);
00029 ROS_INFO_STREAM("approach_path_accessibility_check = " << approach_path_accessibility_check_);
00030 node_handle_.param<std::string>("map_link_name", map_link_name_, "/map");
00031 ROS_INFO_STREAM("map_link_name = " << map_link_name_);
00032 node_handle_.param<std::string>("robot_base_link_name", robot_base_link_name_, "/base_link");
00033 ROS_INFO_STREAM("robot_base_link_name_ = " << robot_base_link_name_);
00034 node_handle_.param("obstacle_topic_update_rate", obstacle_topic_update_rate_, 5.0);
00035 ROS_INFO_STREAM("obstacle_topic_update_rate = " << obstacle_topic_update_rate_);
00036 obstacle_topic_update_delay_ = ros::Duration(1.0 / obstacle_topic_update_rate_);
00037 last_update_time_obstacles_ = ros::Time::now();
00038 node_handle_.param("publish_inflated_map", publish_inflated_map_, false);
00039 ROS_INFO_STREAM("publish_inflated_map = " << publish_inflated_map_);
00040 robot_radius_ = 0.;
00041 if (node_handle_.hasParam("/local_costmap_node/costmap/footprint"))
00042 {
00043
00044 XmlRpc::XmlRpcValue footprint_list;
00045 node_handle_.getParam("/local_costmap_node/costmap/footprint", footprint_list);
00046 std::vector<geometry_msgs::Point> footprint = loadRobotFootprint(footprint_list);
00047 std::stringstream footprint_string;
00048 footprint_string << "footprint = [ ";
00049 for (unsigned int i = 0; i < footprint.size(); ++i)
00050 {
00051 robot_radius_ = std::max<double>(robot_radius_, sqrt(footprint[i].x * footprint[i].x + footprint[i].y * footprint[i].y));
00052 footprint_string << "[" << footprint[i].x << ", " << footprint[i].y << "] ";
00053 }
00054 footprint_string << "]";
00055 ROS_INFO("%s", footprint_string.str().c_str());
00056 }
00057 if (robot_radius_ == 0.0)
00058 {
00059
00060 node_handle_.param("/local_costmap_node/costmap/robot_radius", robot_radius_, 0.8);
00061 }
00062
00063 robot_radius_ = 0.35;
00064 ROS_INFO_STREAM("robot_radius = " << robot_radius_);
00065
00066
00067 mapInit(node_handle_);
00068
00069
00070
00071 dynamicObstaclesInit(node_handle_);
00072
00073
00074 it_ = new image_transport::ImageTransport(node_handle_);
00075 inflated_map_image_pub_ = it_->advertise("inflated_map", 1);
00076
00077
00078 map_points_accessibility_check_server_ = node_handle_.advertiseService(
00079 "map_points_accessibility_check", &MapAccessibilityAnalysisServer::checkPose2DArrayCallback, this);
00080 map_perimeter_accessibility_check_server_ = node_handle_.advertiseService(
00081 "map_perimeter_accessibility_check", &MapAccessibilityAnalysisServer::checkPerimeterCallback, this);
00082 map_polygon_accessibility_check_server_ = node_handle_.advertiseService(
00083 "map_polygon_accessibility_check", &MapAccessibilityAnalysisServer::checkPolygonCallback, this);
00084
00085 ROS_INFO("MapPointAccessibilityCheck initialized.");
00086 }
00087
00088 MapAccessibilityAnalysisServer::~MapAccessibilityAnalysisServer()
00089 {
00090 if (it_ != 0)
00091 delete it_;
00092 }
00093
00094 std::vector<geometry_msgs::Point> MapAccessibilityAnalysisServer::loadRobotFootprint(XmlRpc::XmlRpcValue& footprint_list)
00095 {
00096 std::vector<geometry_msgs::Point> footprint;
00097 geometry_msgs::Point pt;
00098 double scale_factor = 1.0;
00099
00100
00101 std::string footprint_string;
00102 std::vector<std::string> footstring_list;
00103 if (footprint_list.getType() == XmlRpc::XmlRpcValue::TypeString)
00104 {
00105 footprint_string = std::string(footprint_list);
00106
00107
00108 if (footprint_string == "[]" || footprint_string == "")
00109 return footprint;
00110
00111 boost::erase_all(footprint_string, " ");
00112
00113 boost::char_separator<char> sep("[]");
00114 boost::tokenizer<boost::char_separator<char> > tokens(footprint_string, sep);
00115 footstring_list = std::vector<std::string>(tokens.begin(), tokens.end());
00116 }
00117
00118 if (!(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeArray && footprint_list.size() > 2) &&
00119 !(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeString && footstring_list.size() > 5))
00120 {
00121 ROS_FATAL("The footprint must be specified as list of lists on the parameter server, but it was specified as %s",
00122 std::string(footprint_list).c_str());
00123 throw std::runtime_error("The footprint must be specified as list of lists on the parameter server with at least 3 "
00124 "points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]");
00125 }
00126
00127 if (footprint_list.getType() == XmlRpc::XmlRpcValue::TypeArray)
00128 {
00129 for (int i = 0; i < footprint_list.size(); ++i)
00130 {
00131
00132 XmlRpc::XmlRpcValue point = footprint_list[i];
00133 if (!(point.getType() == XmlRpc::XmlRpcValue::TypeArray && point.size() == 2))
00134 {
00135 ROS_FATAL("The footprint must be specified as list of lists on the parameter server eg: [[x1, y1], [x2, y2], "
00136 "..., [xn, yn]], but this spec is not of that form");
00137 throw std::runtime_error("The footprint must be specified as list of lists on the parameter server eg: [[x1, "
00138 "y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form");
00139 }
00140
00141
00142 if (!(point[0].getType() == XmlRpc::XmlRpcValue::TypeInt ||
00143 point[0].getType() == XmlRpc::XmlRpcValue::TypeDouble))
00144 {
00145 ROS_FATAL("Values in the footprint specification must be numbers");
00146 throw std::runtime_error("Values in the footprint specification must be numbers");
00147 }
00148 pt.x = point[0].getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(point[0]) : (double)(point[0]);
00149 pt.x *= scale_factor;
00150
00151
00152 if (!(point[1].getType() == XmlRpc::XmlRpcValue::TypeInt ||
00153 point[1].getType() == XmlRpc::XmlRpcValue::TypeDouble))
00154 {
00155 ROS_FATAL("Values in the footprint specification must be numbers");
00156 throw std::runtime_error("Values in the footprint specification must be numbers");
00157 }
00158 pt.y = point[1].getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(point[1]) : (double)(point[1]);
00159 pt.y *= scale_factor;
00160
00161 footprint.push_back(pt);
00162
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181 }
00182 }
00183 else if (footprint_list.getType() == XmlRpc::XmlRpcValue::TypeString)
00184 {
00185 std::vector<geometry_msgs::Point> footprint_spec;
00186 bool valid_foot = true;
00187 BOOST_FOREACH (std::string t, footstring_list)
00188 {
00189 if (t != ",")
00190 {
00191 boost::erase_all(t, " ");
00192 boost::char_separator<char> pt_sep(",");
00193 boost::tokenizer<boost::char_separator<char> > pt_tokens(t, pt_sep);
00194 std::vector<std::string> point(pt_tokens.begin(), pt_tokens.end());
00195
00196 if (point.size() != 2)
00197 {
00198 ROS_WARN("Each point must have exactly 2 coordinates");
00199 valid_foot = false;
00200 break;
00201 }
00202
00203 std::vector<double> tmp_pt;
00204 BOOST_FOREACH (std::string p, point)
00205 {
00206 std::istringstream iss(p);
00207 double temp;
00208 if (iss >> temp)
00209 {
00210 tmp_pt.push_back(temp);
00211 }
00212 else
00213 {
00214 ROS_WARN("Each coordinate must convert to a double.");
00215 valid_foot = false;
00216 break;
00217 }
00218 }
00219 if (!valid_foot)
00220 break;
00221
00222 geometry_msgs::Point pt;
00223 pt.x = tmp_pt[0];
00224 pt.y = tmp_pt[1];
00225
00226 footprint_spec.push_back(pt);
00227 }
00228 }
00229 if (valid_foot)
00230 {
00231 footprint = footprint_spec;
00232
00233 }
00234 else
00235 {
00236 ROS_FATAL("This footprint is not valid it must be specified as a list of lists with at least 3 points, you "
00237 "specified %s",
00238 footprint_string.c_str());
00239 throw std::runtime_error("The footprint must be specified as list of lists on the parameter server with at least "
00240 "3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]");
00241 }
00242 }
00243
00244 return footprint;
00245 }
00246
00247 void MapAccessibilityAnalysisServer::mapInit(ros::NodeHandle& nh)
00248 {
00249 map_data_recieved_ = false;
00250 map_msg_sub_ = nh.subscribe<nav_msgs::OccupancyGrid>("map", 1, &MapAccessibilityAnalysisServer::mapDataCallback, this);
00251 ROS_INFO("MapPointAccessibilityCheck: Waiting to receive map...");
00252 while (map_data_recieved_ == false)
00253 ros::spinOnce();
00254 ROS_INFO("MapPointAccessibilityCheck: Map received.");
00255 }
00256
00257 void MapAccessibilityAnalysisServer::inflationInit(ros::NodeHandle& nh)
00258 {
00259 obstacles_sub_.subscribe(nh, "obstacles", 1);
00260 inflated_obstacles_sub_.subscribe(nh, "inflated_obstacles", 1);
00261
00262 inflated_obstacles_sub_sync_ = boost::shared_ptr<message_filters::Synchronizer<InflatedObstaclesSyncPolicy> >(
00263 new message_filters::Synchronizer<InflatedObstaclesSyncPolicy>(InflatedObstaclesSyncPolicy(5)));
00264 inflated_obstacles_sub_sync_->connectInput(obstacles_sub_, inflated_obstacles_sub_);
00265 inflated_obstacles_sub_sync_->registerCallback(
00266 boost::bind(&MapAccessibilityAnalysisServer::inflatedObstacleDataCallback, this, _1, _2));
00267 }
00268
00269 void MapAccessibilityAnalysisServer::dynamicObstaclesInit(ros::NodeHandle& nh)
00270 {
00271 obstacles_sub_.subscribe(nh, "obstacles", 1);
00272 obstacles_sub_.registerCallback(boost::bind(&MapAccessibilityAnalysisServer::obstacleDataCallback, this, _1));
00273 }
00274
00275 void MapAccessibilityAnalysisServer::mapDataCallback(const nav_msgs::OccupancyGrid::ConstPtr& map_msg_data)
00276 {
00277 map_resolution_ = map_msg_data->info.resolution;
00278 inverse_map_resolution_ = 1. / map_resolution_;
00279 map_origin_ = cv::Point2d(map_msg_data->info.origin.position.x, map_msg_data->info.origin.position.y);
00280 ROS_INFO_STREAM("map resolution: " << map_msg_data->info.resolution);
00281
00282
00283 original_map_ = 255 * cv::Mat::ones(map_msg_data->info.height, map_msg_data->info.width, CV_8UC1);
00284 for (unsigned int v = 0, i = 0; v < map_msg_data->info.height; v++)
00285 {
00286 for (unsigned int u = 0; u < map_msg_data->info.width; u++, i++)
00287 {
00288 if (map_msg_data->data[i] != 0)
00289 original_map_.at<unsigned char>(v, u) = 0;
00290 }
00291 }
00292
00293
00294 ROS_INFO_STREAM("inflation thickness: " << cvRound(robot_radius_ * inverse_map_resolution_));
00295 inflateMap(original_map_, inflated_original_map_, cvRound(robot_radius_ * inverse_map_resolution_));
00296 if (inflated_map_.empty() == true)
00297 inflated_map_ = inflated_original_map_;
00298
00299 map_data_recieved_ = true;
00300 map_msg_sub_.shutdown();
00301 }
00302
00303 void MapAccessibilityAnalysisServer::inflatedObstacleDataCallback(const nav_msgs::GridCells::ConstPtr& obstacles_data,
00304 const nav_msgs::GridCells::ConstPtr& inflated_obstacles_data)
00305 {
00306 if (obstacle_topic_update_rate_ != 0.0 &&
00307 (ros::Time::now() - last_update_time_obstacles_) > obstacle_topic_update_delay_)
00308 {
00309 boost::mutex::scoped_lock lock(mutex_inflated_map_);
00310
00311 inflated_map_ = inflated_original_map_.clone();
00312 for (unsigned int i = 0; i < obstacles_data->cells.size(); ++i)
00313 inflated_map_.at<uchar>((obstacles_data->cells[i].y - map_origin_.y) * inverse_map_resolution_,
00314 (obstacles_data->cells[i].x - map_origin_.x) * inverse_map_resolution_) = 0;
00315
00316 for (unsigned int i = 0; i < inflated_obstacles_data->cells.size(); i++)
00317 inflated_map_.at<uchar>((inflated_obstacles_data->cells[i].y - map_origin_.y) * inverse_map_resolution_,
00318 (inflated_obstacles_data->cells[i].x - map_origin_.x) * inverse_map_resolution_) = 0;
00319
00320 last_update_time_obstacles_ = ros::Time::now();
00321 }
00322 }
00323
00324 void MapAccessibilityAnalysisServer::obstacleDataCallback(const nav_msgs::GridCells::ConstPtr& obstacles_data)
00325 {
00326 if (obstacle_topic_update_rate_ != 0.0 &&
00327 (ros::Time::now() - last_update_time_obstacles_) > obstacle_topic_update_delay_)
00328 {
00329 double radius = cvRound(robot_radius_ * inverse_map_resolution_);
00330
00331 boost::mutex::scoped_lock lock(mutex_inflated_map_);
00332
00333 inflated_map_ = inflated_original_map_.clone();
00334 for (unsigned int i = 0; i < obstacles_data->cells.size(); ++i)
00335 {
00336 const int u = (obstacles_data->cells[i].x - map_origin_.x) * inverse_map_resolution_;
00337 const int v = (obstacles_data->cells[i].y - map_origin_.y) * inverse_map_resolution_;
00338 inflated_map_.at<uchar>(v, u) = 0;
00339 cv::circle(inflated_map_, cv::Point(u, v), radius, cv::Scalar(0), -1);
00340 }
00341
00342 if (publish_inflated_map_ == true)
00343 {
00344
00345 cv_bridge::CvImage cv_ptr;
00346 cv_ptr.image = inflated_map_;
00347 cv_ptr.encoding = "mono8";
00348 inflated_map_image_pub_.publish(cv_ptr.toImageMsg());
00349 }
00350
00351 last_update_time_obstacles_ = ros::Time::now();
00352 }
00353 }
00354
00355 bool MapAccessibilityAnalysisServer::checkPose2DArrayCallback(
00356 cob_map_accessibility_analysis::CheckPointAccessibility::Request& req,
00357 cob_map_accessibility_analysis::CheckPointAccessibility::Response& res)
00358 {
00359 ROS_INFO("Received request to check accessibility of %u points.", (unsigned int)req.points_to_check.size());
00360
00361
00362 cv::Point robot_location(0, 0);
00363 if (approach_path_accessibility_check_ == true)
00364 robot_location = getRobotLocationInPixelCoordinates();
00365
00366
00367 std::vector<cv::Point> points_to_check(req.points_to_check.size());
00368 for (size_t i=0; i<req.points_to_check.size(); ++i)
00369 points_to_check[i] = cv::Point(cvRound((req.points_to_check[i].x - map_origin_.x) * inverse_map_resolution_),
00370 cvRound((req.points_to_check[i].y - map_origin_.y) * inverse_map_resolution_));
00371 std::vector<bool> accessibility_flags(req.points_to_check.size(), false);
00372
00373
00374 {
00375 boost::mutex::scoped_lock lock(mutex_inflated_map_);
00376 checkPoses(points_to_check, accessibility_flags, inflated_map_, req.approach_path_accessibility_check, robot_location);
00377 }
00378
00379
00380 res.accessibility_flags.resize(req.points_to_check.size());
00381 for (size_t i=0; i<accessibility_flags.size(); ++i)
00382 res.accessibility_flags[i] = accessibility_flags[i];
00383
00384 return true;
00385 }
00386
00387 bool MapAccessibilityAnalysisServer::checkPerimeterCallback(
00388 cob_map_accessibility_analysis::CheckPerimeterAccessibility::Request& req,
00389 cob_map_accessibility_analysis::CheckPerimeterAccessibility::Response& res)
00390 {
00391 ROS_INFO("Received request to check accessibility of a circle with center (%f,%f), radius %f and sampling step %f.",
00392 req.center.x, req.center.y, req.radius, req.rotational_sampling_step);
00393
00394 if (req.rotational_sampling_step == 0.0)
00395 {
00396 req.rotational_sampling_step = 10. / 180. * CV_PI;
00397 ROS_WARN("rotational_sampling_step was provided as 0.0 . Automatically changed it to %f.", req.rotational_sampling_step);
00398 }
00399
00400
00401 cv::Point robot_location(0, 0);
00402 if (approach_path_accessibility_check_ == true)
00403 robot_location = getRobotLocationInPixelCoordinates();
00404
00405
00406 Pose center((req.center.x-map_origin_.x)*inverse_map_resolution_, (req.center.y-map_origin_.y)*inverse_map_resolution_, req.center.theta);
00407 std::vector<Pose> accessible_poses_on_perimeter;
00408
00409
00410 {
00411 boost::mutex::scoped_lock lock(mutex_inflated_map_);
00412 checkPerimeter(accessible_poses_on_perimeter, center, req.radius*inverse_map_resolution_, req.rotational_sampling_step, inflated_map_,
00413 approach_path_accessibility_check_, robot_location);
00414 }
00415
00416
00417 for (size_t i=0; i<accessible_poses_on_perimeter.size(); ++i)
00418 {
00419 geometry_msgs::Pose2D pose;
00420 pose.x = accessible_poses_on_perimeter[i].x*map_resolution_ + map_origin_.x;
00421 pose.y = accessible_poses_on_perimeter[i].y*map_resolution_ + map_origin_.y;
00422 pose.theta = accessible_poses_on_perimeter[i].orientation;
00423 res.accessible_poses_on_perimeter.push_back(pose);
00424 }
00425
00426 return true;
00427 }
00428
00429 bool MapAccessibilityAnalysisServer::checkPolygonCallback(cob_3d_mapping_msgs::GetApproachPoseForPolygon::Request& req,
00430 cob_3d_mapping_msgs::GetApproachPoseForPolygon::Response& res)
00431 {
00432 ROS_INFO("Received request to check accessibility around a polygon.");
00433
00434
00435 cv::Point robot_location(0, 0);
00436 if (approach_path_accessibility_check_ == true)
00437 robot_location = getRobotLocationInPixelCoordinates();
00438
00439
00440 std::vector<std::vector<cv::Point> > polygon_contours;
00441 for (unsigned int i = 0; i < req.polygon.points.size(); i++)
00442 {
00443 if (req.polygon.holes[i] == false)
00444 {
00445 pcl::PointCloud<pcl::PointXYZ> pc;
00446 pcl::fromROSMsg(req.polygon.points[i], pc);
00447 std::vector<cv::Point> p_vec(pc.size());
00448 for (unsigned int j = 0; j < pc.size(); j++)
00449 {
00450 p_vec[j] = convertFromMeterToPixelCoordinates<cv::Point>(Pose(pc.points[j].x, pc.points[j].y, 0), map_origin_, inverse_map_resolution_);
00451 }
00452 polygon_contours.push_back(p_vec);
00453 }
00454 }
00455
00456
00457 cv::Mat polygon_expanded = 255 * cv::Mat::ones(original_map_.rows, original_map_.cols, original_map_.type());
00458 cv::drawContours(polygon_expanded, polygon_contours, -1, cv::Scalar(128), CV_FILLED);
00459 cv::erode(polygon_expanded, polygon_expanded, cv::Mat(), cv::Point(-1, -1), cvRound(robot_radius_ * inverse_map_resolution_));
00460
00461
00462 cv::Mat inflated_map;
00463 {
00464 boost::mutex::scoped_lock lock(mutex_inflated_map_);
00465 inflated_map = cv::min(polygon_expanded, inflated_map_);
00466 }
00467 #ifdef __DEBUG_DISPLAYS__
00468 cv::imshow("inflated polygon map", inflated_map);
00469 cv::waitKey();
00470 #endif
00471
00472
00473 std::vector<std::vector<cv::Point> > area_contours;
00474 if (approach_path_accessibility_check_ == true)
00475 {
00476 cv::Mat inflated_map_copy = inflated_map.clone();
00477 cv::findContours(inflated_map_copy, area_contours, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE);
00478 }
00479
00480
00481
00482 #ifdef __DEBUG_DISPLAYS__
00483 cv::Mat map_expanded_copy = inflated_map.clone();
00484 cv::drawContours(map_expanded_copy, area_contours, -1, cv::Scalar(128, 128, 128, 128), 2);
00485 #endif
00486 for (int y = 1; y < inflated_map.rows - 1; y++)
00487 {
00488 for (int x = 1; x < inflated_map.cols - 1; x++)
00489 {
00490 if (inflated_map.at<uchar>(y, x) == 255)
00491 {
00492 bool close_to_polygon = false;
00493 for (int ky = -1; ky <= 1; ky++)
00494 for (int kx = -1; kx <= 1; kx++)
00495 {
00496 if (inflated_map.at<uchar>(y + ky, x + kx) == 128)
00497 {
00498 close_to_polygon = true;
00499 break;
00500 }
00501 }
00502 if (close_to_polygon == true)
00503 {
00504
00505 if (approach_path_accessibility_check_ == false ||
00506 isApproachPositionAccessible(robot_location, cv::Point(x, y), area_contours) == true)
00507 {
00508 geometry_msgs::Pose pose;
00509 Pose pose_p(x, y, 0);
00510 Pose pose_m = convertFromPixelCoordinatesToMeter<Pose>(pose_p, map_origin_, map_resolution_);
00511 pose.position.x = pose_m.x;
00512 pose.position.y = pose_m.y;
00513 pose.position.z = 0;
00514
00515 Pose closest_point_on_polygon;
00516 computeClosestPointOnPolygon(inflated_map, pose_p, closest_point_on_polygon);
00517
00518 tf::quaternionTFToMsg(tf::createQuaternionFromYaw(atan2(closest_point_on_polygon.y - pose_p.y,
00519 closest_point_on_polygon.x - pose_p.x)),
00520 pose.orientation);
00521
00522
00523 res.approach_poses.poses.push_back(pose);
00524 }
00525
00526 #ifdef __DEBUG_DISPLAYS__
00527
00528 cv::circle(map_expanded_copy, robot_location, 3, cv::Scalar(200, 200, 200, 200), -1);
00529 cv::circle(map_expanded_copy, cv::Point(x, y), 3, cv::Scalar(200, 200, 200, 200), -1);
00530
00531 #endif
00532 }
00533 }
00534 }
00535 }
00536 #ifdef __DEBUG_DISPLAYS__
00537 cv::imshow("contour areas", map_expanded_copy);
00538 cv::waitKey();
00539 #endif
00540
00541 return true;
00542 }
00543
00544 cv::Point MapAccessibilityAnalysisServer::getRobotLocationInPixelCoordinates()
00545 {
00546 tf::StampedTransform transform;
00547 try
00548 {
00549 ros::Time request_time = ros::Time(0);
00550 tf_listener_.waitForTransform(map_link_name_, robot_base_link_name_, request_time, ros::Duration(10));
00551 tf_listener_.lookupTransform(map_link_name_, robot_base_link_name_, request_time, transform);
00552 }
00553 catch (tf::TransformException ex)
00554 {
00555 ROS_ERROR("[registration] : %s", ex.what());
00556 return cv::Point(0, 0);
00557 }
00558 tf::Vector3 pose = transform.getOrigin();
00559 cv::Point robot_location = convertFromMeterToPixelCoordinates<cv::Point>(Pose(pose.x(), pose.y(), 0), map_origin_, inverse_map_resolution_);
00560
00561 return robot_location;
00562 }
00563
00564
00565 int main(int argc, char** argv)
00566 {
00567 ros::init(argc, argv, "map_accessibility_analysis_server");
00568
00569 ros::NodeHandle nh;
00570
00571 MapAccessibilityAnalysisServer map_accessibility_analysis(nh);
00572
00573 ros::spin();
00574
00575 return 0;
00576 }