navigation_visualizations.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 *
00003 * Software License Agreement (BSD License)
00004 *
00005 *  Copyright (c) 2009, Willow Garage, Inc.
00006 *  All rights reserved.
00007 *
00008 *  Redistribution and use in source and binary forms, with or without
00009 *  modification, are permitted provided that the following conditions
00010 *  are met:
00011 *
00012 *   * Redistributions of source code must retain the above copyright
00013 *     notice, this list of conditions and the following disclaimer.
00014 *   * Redistributions in binary form must reproduce the above
00015 *     copyright notice, this list of conditions and the following
00016 *     disclaimer in the documentation and/or other materials provided
00017 *     with the distribution.
00018 *   * Neither the name of Willow Garage, Inc. nor the names of its
00019 *     contributors may be used to endorse or promote products derived
00020 *     from this software without specific prior written permission.
00021 *
00022 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 *  POSSIBILITY OF SUCH DAMAGE.
00034 *
00035 * Author: Eitan Marder-Eppstein
00036 *********************************************************************/
00037 #include "ros/ros.h"
00038 #include "geometry_msgs/PointStamped.h"
00039 #include "sensor_msgs/PointCloud.h"
00040 #include "sensor_msgs/LaserScan.h"
00041 #include "tf/transform_listener.h"
00042 #include "tf/message_filter.h"
00043 #include "message_filters/subscriber.h"
00044 #include "laser_geometry/laser_geometry.h"
00045 #include "message_filters/time_synchronizer.h"
00046 #include "nav_msgs/GridCells.h"
00047 #include "nav_msgs/OccupancyGrid.h"
00048 
00049 namespace navigation_application {
00050 class NavigationVisualization {
00051   public:
00052     NavigationVisualization(): tf_(ros::Duration(10.0)), laser_sub_(NULL), tf_laser_filter_(NULL),
00053     tf_obs_(NULL), tf_inf_(NULL),
00054     obstacle_sub_(NULL), inflated_obstacle_sub_(NULL), obs_sync_(NULL){
00055       ros::NodeHandle n;
00056       ros::NodeHandle nav_nh("nav_app_visualizations");
00057 
00058       //we'll get the size of the grid from the parameters used to construct the local costmap, same defaults as in the costmap
00059       //wish that this info was available over ROS, but it isn't so oh well
00060       ros::NodeHandle local_map_nh("move_base_node/local_costmap");
00061       local_map_nh.param("resolution", resolution_, 0.05); 
00062       local_map_nh.param("width", width_, 10.0); 
00063       local_map_nh.param("height", height_, 10.0); 
00064 
00065       laser_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(n, "base_scan_throttled", 10);
00066       tf_laser_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*laser_sub_, tf_, "/map", 10);
00067 
00068       tf_laser_filter_->registerCallback(boost::bind(&NavigationVisualization::laserScanCallback, this, _1));
00069       laser_point_pub_ = nav_nh.advertise<sensor_msgs::PointCloud>("base_scan", 1);
00070 
00071       obs_pub_ = nav_nh.advertise<nav_msgs::OccupancyGrid>("costmap", 1);
00072 
00073       obstacle_sub_ = new message_filters::Subscriber<nav_msgs::GridCells>(n, "move_base_node/local_costmap/obstacles", 1);
00074       inflated_obstacle_sub_ = new message_filters::Subscriber<nav_msgs::GridCells>(n, "move_base_node/local_costmap/inflated_obstacles", 1);
00075 
00076       tf_obs_ = new tf::MessageFilter<nav_msgs::GridCells>(*obstacle_sub_, tf_, "/map", 10);
00077       tf_inf_ = new tf::MessageFilter<nav_msgs::GridCells>(*inflated_obstacle_sub_, tf_, "/map", 10);
00078 
00079       std::vector<std::string> frames;
00080       frames.push_back("/map");
00081       frames.push_back("base_link");
00082       tf_obs_->setTargetFrames(frames);
00083       tf_inf_->setTargetFrames(frames);
00084 
00085       obs_sync_ = new message_filters::TimeSynchronizer<nav_msgs::GridCells, nav_msgs::GridCells>(*tf_obs_, *tf_inf_, 10);
00086       obs_sync_->registerCallback(boost::bind(&NavigationVisualization::obstaclesCallback, this, _1, _2));
00087 
00088     }
00089 
00090     ~NavigationVisualization(){
00091       delete tf_laser_filter_;
00092       delete laser_sub_;
00093       delete obs_sync_;
00094       delete tf_obs_;
00095       delete tf_inf_;
00096       delete obstacle_sub_;
00097       delete inflated_obstacle_sub_;
00098     }
00099 
00100     inline int getIndex(double wx, double wy, const nav_msgs::OccupancyGrid& grid) {
00101       //make sure to bounds check
00102       double upper_bound_x = grid.info.origin.position.x + (grid.info.width * grid.info.resolution);
00103       double upper_bound_y = grid.info.origin.position.y + (grid.info.height * grid.info.resolution);
00104       if(wx < grid.info.origin.position.x || wx >= upper_bound_x || wy < grid.info.origin.position.y || wy >= upper_bound_y)
00105         return -1;
00106 
00107       unsigned int mx = int((wx - grid.info.origin.position.x) / grid.info.resolution);
00108       unsigned int my = int((wy - grid.info.origin.position.y) / grid.info.resolution);
00109 
00110       return my * grid.info.width + mx;
00111     }
00112 
00113     void obstaclesCallback(const nav_msgs::GridCells::ConstPtr& obstacles, const nav_msgs::GridCells::ConstPtr& inflated_obstacles){
00114       //we need a grid to send out
00115       nav_msgs::OccupancyGrid grid;
00116 
00117       //we'll build the grid in the odometric frame and transform it later
00118       grid.header.frame_id = obstacles->header.frame_id;
00119       grid.header.stamp = obstacles->header.stamp;
00120 
00121       grid.info.resolution = resolution_;
00122       grid.info.width = int(width_ / resolution_);
00123       grid.info.height = int(height_ / resolution_);
00124       grid.data.resize(grid.info.width * grid.info.height);
00125 
00126       //first, we'll get the transform we want from tf
00127       tf::StampedTransform  robot_transform;
00128       try{
00129         tf_.lookupTransform(obstacles->header.frame_id, "base_link", obstacles->header.stamp, robot_transform);
00130       }
00131       catch(tf::TransformException &ex){
00132         ROS_ERROR("This should never happen: %s", ex.what());
00133         return;
00134       }
00135 
00136       //get the position of the robot in the map frame
00137       tf::Point robot_position = robot_transform * tf::Point(0.0, 0.0, 0.0);
00138 
00139       //we assume that the robot is centered in the costmap and can compute the origin from there
00140       grid.info.origin.position.x = robot_position.getX() - width_ / 2.0;
00141       grid.info.origin.position.y = robot_position.getY() - height_ / 2.0;
00142       grid.info.origin.position.z = 0.0;
00143 
00144       //put the obstacles into the grid
00145       for(unsigned int i = 0; i < obstacles->cells.size(); ++i){
00146         int index = getIndex(obstacles->cells[i].x, obstacles->cells[i].y, grid);
00147         if(index >= 0)
00148           grid.data[index] = 254;
00149       }
00150 
00151       //put the inflated obstacles into the grid
00152       for(unsigned int i = 0; i < inflated_obstacles->cells.size(); ++i){
00153         int index = getIndex(inflated_obstacles->cells[i].x, inflated_obstacles->cells[i].y, grid);
00154         if(index >= 0)
00155           grid.data[index] = 253;
00156       }
00157 
00158       //we actually want to publish the grid in the map frame, so we'll change the origin here
00159       geometry_msgs::PoseStamped orig_origin, map_origin;
00160       orig_origin.header = obstacles->header;
00161       orig_origin.pose.position = grid.info.origin.position;
00162       orig_origin.pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
00163 
00164       try{
00165         tf_.transformPose("/map", orig_origin, map_origin);
00166       }
00167       catch(tf::TransformException &ex){
00168         ROS_ERROR("This should never happen: %s", ex.what());
00169         return;
00170       }
00171 
00172       //now... the grid is in the map frame, we'll update the header
00173       grid.header = map_origin.header;
00174       grid.info.origin = map_origin.pose;
00175 
00176       ROS_DEBUG("Publishing a grid with x, y, th: (%.2f, %.2f, %.2f)", grid.info.origin.position.x, grid.info.origin.position.y, tf::getYaw(grid.info.origin.orientation));
00177       obs_pub_.publish(grid);
00178     }
00179 
00180     void laserScanCallback(const sensor_msgs::LaserScan::ConstPtr& scan){
00181       sensor_msgs::PointCloud base_scan_cloud;
00182       base_scan_cloud.header = scan->header;
00183 
00184       try{
00185         projector_.transformLaserScanToPointCloud(scan->header.frame_id, *scan, base_scan_cloud, tf_);
00186 
00187         tf_.transformPointCloud("map", base_scan_cloud, base_scan_cloud);
00188         base_scan_cloud.channels.resize(0);
00189         laser_point_pub_.publish(base_scan_cloud);
00190       }
00191       catch(tf::TransformException &ex){
00192         ROS_ERROR("This should not happen");
00193       }
00194     }
00195 
00196     tf::TransformListener tf_;
00197     laser_geometry::LaserProjection projector_;
00198     ros::Publisher laser_point_pub_;
00199     ros::Publisher obs_pub_;
00200     message_filters::Subscriber<sensor_msgs::LaserScan>* laser_sub_;
00201     tf::MessageFilter<sensor_msgs::LaserScan>* tf_laser_filter_;
00202     tf::MessageFilter<nav_msgs::GridCells>* tf_obs_, *tf_inf_;
00203 
00204     message_filters::Subscriber<nav_msgs::GridCells>* obstacle_sub_, *inflated_obstacle_sub_;
00205     message_filters::TimeSynchronizer<nav_msgs::GridCells, nav_msgs::GridCells>* obs_sync_;
00206 
00207     double width_, height_, resolution_;
00208 };
00209 
00210 };
00211 
00212 
00213 int main(int argc, char **argv){
00214   ros::init(argc, argv, "nav_app_visualizations");
00215   navigation_application::NavigationVisualization visualizer;
00216 
00217   ros::spin();
00218 
00219   return(0);
00220 }


navigation_application
Author(s): Scott Hassan
autogenerated on Thu Apr 24 2014 15:48:49