hector_map_server.cpp
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2011, Stefan Kohlbrecher, Johannes Meyer TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Simulation, Systems Optimization and Robotics
00013 //       group, TU Darmstadt nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //=================================================================================================
00028 
00029 #include <stdio.h>
00030 #include <stdlib.h>
00031 //#include <libgen.h>
00032 //#include <fstream>
00033 
00034 #include "ros/ros.h"
00035 #include "ros/console.h"
00036 #include "nav_msgs/MapMetaData.h"
00037 #include "nav_msgs/OccupancyGrid.h"
00038 
00039 #include <boost/thread.hpp>
00040 
00041 #include <tf/transform_listener.h>
00042 
00043 #include "nav_msgs/GetMap.h"
00044 
00045 #include "hector_marker_drawing/HectorDrawings.h"
00046 #include "hector_map_tools/HectorMapTools.h"
00047 
00048 #include "hector_nav_msgs/GetDistanceToObstacle.h"
00049 #include "hector_nav_msgs/GetSearchPosition.h"
00050 
00051 
00052 class OccupancyGridContainer
00053 {
00054 public:
00055   OccupancyGridContainer(std::string sub_topic, std::string prefix, ros::NodeHandle& nh, HectorDrawings* drawing_provider, tf::TransformListener* tf_)
00056     : drawing_provider_(drawing_provider)
00057     , tf_(tf_)
00058   {
00059 
00060     std::string service_name = "map";
00061     map_service_ = nh.advertiseService(service_name, &OccupancyGridContainer::mapServiceCallback, this);
00062 
00063     ros::NodeHandle pnh("~");
00064     std::string lookup_service_name = "get_distance_to_obstacle";
00065     dist_lookup_service_ = pnh.advertiseService(lookup_service_name, &OccupancyGridContainer::lookupServiceCallback, this);
00066 
00067     std::string get_search_pos_service_name = "get_search_position";
00068     get_search_pos_service_ = pnh.advertiseService(get_search_pos_service_name, &OccupancyGridContainer::getSearchPosServiceCallback, this);
00069 
00070     map_sub_ = nh.subscribe("map", 1, &OccupancyGridContainer::mapCallback, this);
00071   }
00072 
00073   ~OccupancyGridContainer()
00074   {}
00075 
00076   bool mapServiceCallback(nav_msgs::GetMap::Request  &req,
00077                           nav_msgs::GetMap::Response &res )
00078   {
00079     ROS_INFO("hector_map_server map service called");
00080 
00081     if (!map_ptr_){
00082       ROS_INFO("map_server has no map yet, no map service available");
00083       return false;
00084     }
00085 
00086     res.map = *map_ptr_;
00087 
00088     return true;
00089   }
00090 
00091   bool lookupServiceCallback(hector_nav_msgs::GetDistanceToObstacle::Request  &req,
00092                           hector_nav_msgs::GetDistanceToObstacle::Response &res )
00093   {
00094     //ROS_INFO("hector_map_server lookup service called");
00095 
00096 
00097     if (!map_ptr_){
00098       ROS_INFO("map_server has no map yet, no lookup service available");
00099       return false;
00100     }
00101 
00102     tf::StampedTransform stamped_pose;
00103 
00104 
00105 
00106     try{
00107       tf_->waitForTransform(map_ptr_->header.frame_id,req.point.header.frame_id, req.point.header.stamp, ros::Duration(1.0));
00108       tf_->lookupTransform(map_ptr_->header.frame_id, req.point.header.frame_id, req.point.header.stamp, stamped_pose);
00109 
00110       tf::Point v2_tf;
00111       tf::pointMsgToTF(req.point.point,v2_tf);
00112 
00113       tf::Vector3 v1 = stamped_pose * tf::Vector3(0.0, 0.0, 0.0);
00114       tf::Vector3 v2 = stamped_pose * v2_tf;
00115       tf::Vector3 diff = v2 - v1;
00116       v2 = v1 + diff / tf::Vector3(diff.x(), diff.y(), 0.0).length() * 5.0f;
00117 
00118       Eigen::Vector2f start(v1.x(),v1.y());
00119       Eigen::Vector2f end(v2.x(),v2.y());
00120 
00121       Eigen::Vector2f hit_world;
00122       //float dist = dist_meas_.getDist(start,end, &hit_world);
00123       float dist = dist_meas_.getDist(start,end,&hit_world);
00124 
00125       if (dist >=0.0f){
00126         tf::Vector3 diff (v2-v1);
00127 
00128         float angle = diff.angle(tf::Vector3(diff.x(),diff.y(),0.0f));
00129 
00130         res.distance = dist/cos(angle);
00131 
00132       }else{
00133         res.distance = -1.0f;
00134       }
00135 
00136       //debug drawing
00137       if (false){
00138 
00139         float cube_scale = map_ptr_->info.resolution;
00140         drawing_provider_->setColor(1.0, 0.0, 0.0);
00141         drawing_provider_->setScale(static_cast<double>(cube_scale));
00142 
00143         drawing_provider_->drawPoint(start);
00144 
00145         drawing_provider_->setColor(0.0, 1.0, 0.0);
00146         drawing_provider_->drawPoint(end);
00147 
00148         if (dist >= 0.0f){
00149           drawing_provider_->setColor(0.0, 0.0, 1.0);
00150           drawing_provider_->drawPoint(hit_world);
00151         }
00152 
00153         drawing_provider_->sendAndResetData();
00154       }
00155 
00156       return true;
00157 
00158     }
00159     catch(tf::TransformException e)
00160     {
00161       ROS_ERROR("Transform failed in lookup distance service call: %s",e.what());
00162     }
00163 
00164     return false;
00165   }
00166 
00167   bool getSearchPosServiceCallback(hector_nav_msgs::GetSearchPosition::Request  &req,
00168                                    hector_nav_msgs::GetSearchPosition::Response &res )
00169   {
00170     if (!map_ptr_){
00171       ROS_INFO("map_server has no map yet, no get best search pos service available");
00172       return false;
00173     }
00174 
00175     try{
00176 
00177       tf::Stamped<tf::Pose> ooi_pose, transformed_pose, search_position;
00178       tf::poseStampedMsgToTF(req.ooi_pose, ooi_pose);
00179 
00180       tf_->waitForTransform(map_ptr_->header.frame_id, req.ooi_pose.header.frame_id, req.ooi_pose.header.stamp, ros::Duration(1.0));
00181       tf_->transformPose(map_ptr_->header.frame_id, ooi_pose, transformed_pose);
00182 
00183       tf::Vector3 direction(-req.distance, 0.0, 0.0);
00184       search_position = transformed_pose;
00185       search_position.setOrigin(transformed_pose.getOrigin() + transformed_pose.getBasis() * direction);
00186 
00187       tf::poseStampedTFToMsg(search_position, res.search_pose);
00188 
00189       return true;
00190 
00191 //      //tf::Point v2_tf;
00192 //      //tf::pointMsgToTF(req.point.point,v2_tf);
00193 
00194 //      tf::Vector3 v1 = stamped_pose * tf::Vector3(0.0, 0.0, 0.0);
00195 
00196 //      //warning: 3D!
00197 //      tf::Vector3 v2 = stamped_pose * tf::Vector3(-1.0, 0.0, 0.0);
00198 
00199 //      tf::Vector3 dir = v2-v1;
00200 
00201 //      Eigen::Vector2f dir_2d (dir.x(), dir.y());
00202 
00203 //      dir_2d.normalize();
00204 
00205 //      Eigen::Vector2f searchPos (Eigen::Vector2f(v1.x(),v1.y()) + (dir_2d*0.5f));
00206 
00207 //      //copy original pose message but set translation
00208 //      res.search_pose.pose = ooi_pose.pose;
00209 
00210 //      res.search_pose.pose.position.x = searchPos.x();
00211 //      res.search_pose.pose.position.y = searchPos.y();
00212 
00213       //return true;
00214 
00215 
00216       //Eigen::Vector2f ooi_pos(v1.x(),v1.y());
00217       //Eigen::Vector2f sample_point_pos(v2.x(),v2.y());
00218 
00219 
00220       //float dist_from_target = dist_meas_.getDist(ooi_pos,sample_point_pos);
00221       //float dist_from_sample_point = dist_meas_.getDist(sample_point_pos, ooi_pos);
00222 
00223 
00224 
00225 /*
00226       if (dist >=0.0f){
00227         tf::Vector3 diff (v2-v1);
00228 
00229         float angle = diff.angle(tf::Vector3(diff.x(),diff.y(),0.0f));
00230 
00231         res.distance = dist/cos(angle);
00232 
00233 
00234         //debug drawing
00235         if (true){
00236 
00237           float cube_scale = map_ptr_->info.resolution;
00238           drawing_provider_->setColor(1.0, 0.0, 0.0);
00239           drawing_provider_->setScale(static_cast<double>(cube_scale));
00240 
00241           drawing_provider_->drawPoint(start);
00242           drawing_provider_->drawPoint(end);
00243 
00244           if (dist >= 0.0f){
00245             drawing_provider_->setColor(0.0, 0.0, 1.0);
00246             drawing_provider_->drawPoint(hit_world);
00247           }
00248 
00249           drawing_provider_->sendAndResetData();
00250         }
00251 
00252       }
00253       */
00254       //return true;
00255 
00256     }    catch(tf::TransformException e){
00257       ROS_ERROR("Transform failed in getSearchPosition service call: %s",e.what());
00258     }
00259 
00260     return false;
00261   }
00262 
00263   void mapCallback(const nav_msgs::OccupancyGridConstPtr& map)
00264   {
00265     map_ptr_ = map;
00266 
00267     dist_meas_.setMap(map_ptr_);
00268   }
00269 
00270   //Services
00271   ros::ServiceServer map_service_;
00272   ros::ServiceServer dist_lookup_service_;
00273   ros::ServiceServer get_search_pos_service_;
00274 
00275   //Subscriber
00276   ros::Subscriber map_sub_;
00277 
00278   HectorMapTools::DistanceMeasurementProvider dist_meas_;
00279 
00280   HectorDrawings* drawing_provider_;
00281   tf::TransformListener* tf_;
00282 
00283   //nav_msgs::MapMetaData meta_data_message_;
00284   nav_msgs::GetMap::Response map_resp_;
00285 
00286   nav_msgs::OccupancyGridConstPtr map_ptr_;
00287 };
00288 
00289 class HectorMapServer
00290 {
00291   public:
00293     HectorMapServer(ros::NodeHandle& private_nh)
00294     {
00295       std::string frame_id;
00296 
00297       hector_drawings_ = new HectorDrawings();
00298       hector_drawings_->setNamespace("map_server");
00299 
00300       mapContainer = new OccupancyGridContainer("map", "" ,private_nh, hector_drawings_,&tf_);
00301     }
00302 
00303     ~HectorMapServer()
00304     {
00305       delete mapContainer;
00306 
00307 
00308       delete hector_drawings_;
00309     }
00310 
00311 
00312 public:
00313     OccupancyGridContainer* mapContainer;
00314 private:
00315 
00316     HectorDrawings* hector_drawings_;
00317     tf::TransformListener tf_;
00318 };
00319 
00320 int main(int argc, char **argv)
00321 {
00322   ros::init(argc, argv, "hector_map_server");
00323   ros::NodeHandle nh;
00324 
00325   HectorMapServer ms(nh);
00326 
00327   ros::spin();
00328 
00329   return 0;
00330 }
00331 


hector_map_server
Author(s): Stefan Kohlbrecher
autogenerated on Wed Aug 26 2015 11:45:00