cost_map_demos: from_ros_costmaps.cpp Source File

Go to the documentation of this file.
00001 
00004 /*****************************************************************************
00005 ** Includes
00006 *****************************************************************************/
00007 
00008 #include <atomic>
00009 #include <ros/common.h>
00010 #include <ros/node_handle.h>
00011 #include <ros/subscriber.h>
00012 #include <ros/publisher.h>
00013 
00014 #include <costmap_2d/costmap_2d.h>
00015 #include <costmap_2d/cost_values.h>
00016 #include <nav_msgs/OccupancyGrid.h>
00017 
00018 #include <cost_map_ros/converter.hpp>
00019 
00020 #include "../../include/cost_map_demos/from_ros_costmaps.hpp"
00021 
00022 /*****************************************************************************
00023 ** Interfaces
00024 *****************************************************************************/
00025 
00026 
00027 
00028 int main(int argc, char** argv)
00029 {
00030   ros::init(argc, argv, "converter"); // , ros::init_options::AnonymousName);
00031 
00032   /********************
00033   ** Transforms
00034   ********************/
00035   cost_map_demos::TransformBroadcaster broadcaster;
00036   cost_map_demos::broadcastCostmap2DROSTestSuiteTransforms(broadcaster);
00037 
00038   /********************
00039   ** ROS Costmaps
00040   ********************/
00041   // MAKE SURE THIS STAY IN SYNC WITH src/tests/rostests/from_ros_costmaps/from_ros_costmaps.cpp!
00042   // The following should result in costmaps with strips of increasing cost in the x-direction,
00043   // but also 1-4 cells cleared by the footprint (see rviz)
00044   cost_map_demos::ROSCostmapServer ros_costmap_5x5("five_by_five", "base_link_5x5", cost_map::Position(0.0, 0.0), 5.0, 5.0);
00045   cost_map_demos::ROSCostmapServer ros_costmap_4x4("four_by_four", "base_link_4x4", cost_map::Position(0.0, -5.0), 4.0, 4.0);
00046   cost_map_demos::ROSCostmapServer ros_costmap_5x5_3x3_offset("five_by_five_three_by_three_offset", "base_link_5x5_3x3_offset", cost_map::Position(-6.0, 0.0), 5.0, 5.0);
00047   cost_map_demos::ROSCostmapServer ros_costmap_5x5_3x3_centre("five_by_five_three_by_three_centre", "base_link_5x5_3x3_centre", cost_map::Position(-6.0, -6.0), 5.0, 5.0);
00048   cost_map_demos::ROSCostmapServer ros_costmap_5x5_2_5x2_5_offset("five_by_five_twohalf_by_twohalf_offset", "base_link_5x5_2_5x2_5_offset", cost_map::Position(-12.0, 0.0), 5.0, 5.0);
00049 
00050   /********************
00051   ** New Costmaps
00052   ********************/
00053   // MAKE SURE THIS STAY IN SYNC WITH src/tests/rostests/from_ros_costmaps/from_ros_costmaps.cpp!
00054   cost_map::CostMap cost_map_5x5, cost_map_4x4, cost_map_5x5_3x3_offset, cost_map_5x5_3x3_centre, cost_map_5x5_2_5x2_5_offset;
00055   cost_map::fromCostmap2DROS(*(ros_costmap_5x5.getROSCostmap()), "obstacle_costs", cost_map_5x5);
00056   cost_map::fromCostmap2DROS(*(ros_costmap_4x4.getROSCostmap()), "obstacle_costs", cost_map_4x4);
00057   cost_map::Length geometry_3x3(3.0, 3.0);
00058   cost_map::fromCostmap2DROSAtRobotPose(*(ros_costmap_5x5_3x3_offset.getROSCostmap()), geometry_3x3, "obstacle_costs", cost_map_5x5_3x3_offset);
00059   cost_map::fromCostmap2DROSAtRobotPose(*(ros_costmap_5x5_3x3_centre.getROSCostmap()), geometry_3x3, "obstacle_costs", cost_map_5x5_3x3_centre);
00060   cost_map::Length geometry_2_5x2_5(2.5, 2.5);
00061   cost_map::fromCostmap2DROSAtRobotPose(*(ros_costmap_5x5_2_5x2_5_offset.getROSCostmap()), geometry_2_5x2_5, "obstacle_costs", cost_map_5x5_2_5x2_5_offset);
00062 
00063   /********************
00064   ** Publishers
00065   ********************/
00066   ros::NodeHandle node_handle;
00067   ros::Publisher pub_5x5 = node_handle.advertise<nav_msgs::OccupancyGrid>("converted_5x5", 1, true);
00068   ros::Publisher pub_4x4 = node_handle.advertise<nav_msgs::OccupancyGrid>("converted_4x4", 1, true);
00069   ros::Publisher pub_5x5_3x3_offset = node_handle.advertise<nav_msgs::OccupancyGrid>("converted_5x5_3x3_offset", 1, true);
00070   ros::Publisher pub_5x5_3x3_centre = node_handle.advertise<nav_msgs::OccupancyGrid>("converted_5x5_3x3_centre", 1, true);
00071   ros::Publisher pub_5x5_2_5x2_5_offset = node_handle.advertise<nav_msgs::OccupancyGrid>("converted_5x5_2_5x2_5_offset", 1, true);
00072 
00073   nav_msgs::OccupancyGrid occupancy_msg;
00074   cost_map::toOccupancyGrid(cost_map_5x5, cost_map_5x5.getLayers()[0], occupancy_msg);
00075   pub_5x5.publish(occupancy_msg);
00076   cost_map::toOccupancyGrid(cost_map_4x4, cost_map_4x4.getLayers()[0], occupancy_msg);
00077   pub_4x4.publish(occupancy_msg);
00078   cost_map::toOccupancyGrid(cost_map_5x5_3x3_offset, cost_map_5x5_3x3_offset.getLayers()[0], occupancy_msg);
00079   pub_5x5_3x3_offset.publish(occupancy_msg);
00080   cost_map::toOccupancyGrid(cost_map_5x5_3x3_centre, cost_map_5x5_3x3_centre.getLayers()[0], occupancy_msg);
00081   pub_5x5_3x3_centre.publish(occupancy_msg);
00082   cost_map::toOccupancyGrid(cost_map_5x5_2_5x2_5_offset, cost_map_5x5_2_5x2_5_offset.getLayers()[0], occupancy_msg);
00083   pub_5x5_2_5x2_5_offset.publish(occupancy_msg);
00084 
00085   /********************
00086   ** Spin & Shutdown
00087   ********************/
00088   ros::spin();
00089 
00090   return 0;
00091 }