grid_map_costmap_2d: costmap_2d

Go to the documentation of this file.
00001 /*****************************************************************************
00002 ** Includes
00003 *****************************************************************************/
00004 
00005 #include <gtest/gtest.h>
00006 #include <iostream>
00007 #include <string>
00008 
00009 #include "costmap_2d_ros.hpp"
00010 
00011 /*****************************************************************************
00012 ** Helpers
00013 *****************************************************************************/
00014 
00015 void broadcastCostmap2DROSTestSuiteTransforms(TransformBroadcaster& broadcaster)
00016 {
00017   broadcaster.add("base_link_5x5", tf::Vector3(1.0, 1.0, 0.0), tf::Quaternion(0, 0, 0, 1));
00018   broadcaster.add("base_link_4x4", tf::Vector3(1.0, -3.0, 0.0), tf::Quaternion(0, 0, 0, 1));
00019   broadcaster.add("base_link_5x5_3x3_offset", tf::Vector3(-3.7, 2.4, 0.0), tf::Quaternion(0, 0, 0, 1));
00020   broadcaster.add("base_link_5x5_3x3_centre", tf::Vector3(-3.5, -3.5, 0.0), tf::Quaternion(0, 0, 0, 1));
00021   broadcaster.add("base_link_5x5_2_5x2_5_offset", tf::Vector3(-9.7, 2.4, 0.0), tf::Quaternion(0, 0, 0, 1));
00022   broadcaster.startBroadCastingThread();
00023 }
00024 
00025 /*****************************************************************************
00026 ** ROS Costmap Server
00027 *****************************************************************************/
00028 
00029 ROSCostmapServer::ROSCostmapServer(const std::string& name,
00030                                    const std::string& baseLinkTransformName,
00031                                    const grid_map::Position& origin, const double& width,
00032                                    const double& height)
00033     : transformListener(ros::Duration(1.0))
00034 {
00035   ros::NodeHandle privateNodeHandle("~");
00036   // lots of parameters here affect the construction ( e.g. rolling window)
00037   // if you don't have any parameters set, then this
00038   //  - alot of defaults which get dumped on the ros param server
00039   //  - fires up an obstacle layer and an inflation layer
00040   //  - creates a publisher for an occupancy grid
00041   privateNodeHandle.setParam(name + "/robot_base_frame", baseLinkTransformName);
00042   privateNodeHandle.setParam(name + "/origin_x", origin.x());
00043   privateNodeHandle.setParam(name + "/origin_y", origin.y());
00044   privateNodeHandle.setParam(name + "/width", width);
00045   privateNodeHandle.setParam(name + "/height", height);
00046   privateNodeHandle.setParam(name + "/plugins", std::vector<std::string>());
00047   privateNodeHandle.setParam(name + "/resolution", 0.5);
00048   privateNodeHandle.setParam(name + "/robot_radius", 0.03); // clears 1 cell if inside, up to 4 cells on a vertex
00049   costmap = std::make_shared<ROSCostmap>(name, transformListener);
00050 
00051   for ( unsigned int index = 0; index < costmap->getCostmap()->getSizeInCellsY(); ++index ) {
00052     unsigned int dimension = costmap->getCostmap()->getSizeInCellsX();
00053     // @todo assert dimension > 1
00054     // set the first row to costmap_2d::FREE_SPACE? but it shows up invisibly in rviz, so don't bother
00055     for ( unsigned int fill_index = 0; fill_index < dimension - 2; ++fill_index )
00056     {
00057       double fraction = static_cast<double>(fill_index + 1) / static_cast<double>(costmap->getCostmap()->getSizeInCellsX());
00058       costmap->getCostmap()->setCost(fill_index, index, fraction*costmap_2d::INSCRIBED_INFLATED_OBSTACLE );
00059     }
00060     costmap->getCostmap()->setCost(dimension - 2, index, costmap_2d::LETHAL_OBSTACLE);
00061     costmap->getCostmap()->setCost(dimension - 1, index, costmap_2d::NO_INFORMATION);
00062   }
00063 }
00064 
00065 /*****************************************************************************
00066 ** TransformBroadcaster
00067 *****************************************************************************/
00068 
00069 TransformBroadcaster::~TransformBroadcaster()
00070 {
00071   broadcastingThread_.join();
00072 }
00073 
00074 void TransformBroadcaster::shutdown()
00075 {
00076   shutdownFlag_ = true;
00077 }
00078 
00079 void TransformBroadcaster::add(const std::string& name, tf::Vector3 origin,
00080                                const tf::Quaternion& orientation)
00081 {
00082   tf::Transform transform;
00083   transform.setOrigin(origin);
00084   transform.setRotation(orientation);
00085   transforms_.insert(std::pair<std::string, tf::Transform>(name, transform));
00086 }
00087 
00088 void TransformBroadcaster::startBroadCastingThread() {
00089   broadcastingThread_ = std::thread(&TransformBroadcaster::broadcast, this);
00090 }
00091 
00092 void TransformBroadcaster::broadcast()
00093 {
00094   tf::TransformBroadcaster tf_broadcaster;
00095   while (ros::ok() && !shutdownFlag_) {
00096     for (std::pair<std::string, tf::Transform> p : transforms_) {
00097       tf::StampedTransform stamped_transform(p.second, ros::Time::now(), "map", p.first);
00098       tf_broadcaster.sendTransform(stamped_transform);
00099     }
00100     ros::Duration(0.1).sleep();
00101   }
00102 }
00103 
00104 /*****************************************************************************
00105 ** Converter Functions
00106 *****************************************************************************/
00107 
00108 bool fromCostmap2DROS(costmap_2d::Costmap2DROS& ros_costmap, const std::string& layer_name,
00109                       grid_map::GridMap& grid_map)
00110 {
00111 
00112   grid_map::Costmap2DConverter<grid_map::GridMap> converter;
00113   boost::lock_guard<costmap_2d::Costmap2D::mutex_t> lock(*(ros_costmap.getCostmap()->getMutex()));
00114   converter.initializeFromCostmap2D(ros_costmap, grid_map);
00115   if (!converter.addLayerFromCostmap2D(ros_costmap, layer_name, grid_map)) {
00116     return false;
00117   }
00118   return true;
00119 }
00120 
00121 bool fromCostmap2DROSAtRobotPose(costmap_2d::Costmap2DROS& ros_costmap,
00122                                  const grid_map::Length& geometry, const std::string& layer_name,
00123                                  grid_map::GridMap& grid_map)
00124 {
00125   grid_map::Costmap2DConverter<grid_map::GridMap> converter;
00126   boost::lock_guard<costmap_2d::Costmap2D::mutex_t> lock(*(ros_costmap.getCostmap()->getMutex()));
00127   if (!converter.initializeFromCostmap2DAtRobotPose(ros_costmap, geometry, grid_map)) {
00128     return false;
00129   }
00130   if (!converter.addLayerFromCostmap2DAtRobotPose(ros_costmap, layer_name, grid_map)) {
00131     return false;
00132   }
00133   return true;
00134 }
00135 
00136 /*****************************************************************************
00137 ** Tests
00138 *****************************************************************************/
00139 
00140 TEST(Costmap2DROSConversion, full_window)
00141 {
00142   std::cout << std::endl;
00143   ROS_INFO("***********************************************************");
00144   ROS_INFO("                 Copy Full Window");
00145   ROS_INFO("***********************************************************");
00146   // preparation
00147   std::string layer_name =  "obstacle_costs";
00148   ROSCostmapServer ros_costmap_5x5("five_by_five", "base_link_5x5", grid_map::Position(0.0, 0.0), 5.0, 5.0);
00149   grid_map::GridMap grid_map_5x5;
00150   fromCostmap2DROS(*(ros_costmap_5x5.getROSCostmap()), layer_name, grid_map_5x5);
00151   // assert map properties
00152   ASSERT_EQ(grid_map_5x5.getFrameId(), ros_costmap_5x5.getROSCostmap()->getGlobalFrameID());
00153   ASSERT_EQ(
00154       grid_map_5x5.getLength().x(),
00155       ros_costmap_5x5.getROSCostmap()->getCostmap()->getSizeInCellsX()
00156           * ros_costmap_5x5.getROSCostmap()->getCostmap()->getResolution());
00157   ASSERT_EQ(
00158       grid_map_5x5.getLength().y(),
00159       ros_costmap_5x5.getROSCostmap()->getCostmap()->getSizeInCellsY()
00160           * ros_costmap_5x5.getROSCostmap()->getCostmap()->getResolution());
00161   ASSERT_EQ(grid_map_5x5.getSize()[0], ros_costmap_5x5.getROSCostmap()->getCostmap()->getSizeInCellsX());
00162   ASSERT_EQ(grid_map_5x5.getSize()[1], ros_costmap_5x5.getROSCostmap()->getCostmap()->getSizeInCellsY());
00163   grid_map::Length position = grid_map_5x5.getPosition() - 0.5 * grid_map_5x5.getLength().matrix();
00164   ASSERT_EQ(position.x(), ros_costmap_5x5.getROSCostmap()->getCostmap()->getOriginX());
00165   ASSERT_EQ(position.y(), ros_costmap_5x5.getROSCostmap()->getCostmap()->getOriginY());
00166 
00167   // assert map data
00168   for (unsigned int i = 0; i < 5; ++i) {
00169     for (unsigned int j = 0; j < 5; ++j) {
00170       std::cout << static_cast<int>(ros_costmap_5x5.getROSCostmap()->getCostmap()->getCost(i, j))
00171                 << " ";
00172     }
00173     std::cout << std::endl;
00174   }
00175   for (unsigned int i = 0; i < 5; ++i) {
00176     for (unsigned int j = 0; j < 5; ++j) {
00177       std::cout << static_cast<int>(grid_map_5x5.at(layer_name, grid_map::Index(i, j))) << " ";
00178     }
00179     std::cout << std::endl;
00180   }
00181   // TODO a function which does the index conversion
00182 
00183   std::cout << "Original cost: " << static_cast<int>(ros_costmap_5x5.getROSCostmap()->getCostmap()->getCost(8,0)) << std::endl;
00184   std::cout << "New Cost: " << grid_map_5x5.at(layer_name, grid_map::Index(1,9)) << std::endl;
00185   std::vector<float> cost_translation_table;
00186   grid_map::Costmap2DCenturyTranslationTable::create(cost_translation_table);
00187   unsigned char cost = ros_costmap_5x5.getROSCostmap()->getCostmap()->getCost(8,0);
00188   ASSERT_EQ(grid_map_5x5.at(layer_name, grid_map::Index(1,9)), cost_translation_table[cost]);
00189   std::cout << std::endl;
00190 }
00191 
00192 TEST(Costmap2DROSConversion, cost_map_centres)
00193 {
00194   std::cout << std::endl;
00195   ROS_INFO("***********************************************************");
00196   ROS_INFO("                 Check Subwindow Centres");
00197   ROS_INFO("***********************************************************");
00198   ROS_INFO("Subwindows are centred as closely as possible to the robot");
00199   ROS_INFO("pose, though not exactly. They still need to align with");
00200   ROS_INFO("the underlying ros costmap so that they don't introduce a");
00201   ROS_INFO("new kind of error. As a result, the centre is shifted from");
00202   ROS_INFO("the robot pose to the nearest appropriate point which aligns");
00203   ROS_INFO("the new cost map exactly on top of the original ros costmap.");
00204   std::cout << std::endl;
00205   std::string layer_name =  "obstacle_costs";
00206   ROSCostmapServer ros_costmap_5x5_3x3_offset("five_by_five_three_by_three_offset", "base_link_5x5_3x3_offset", grid_map::Position(-6.0, 0.0), 5.0, 5.0);
00207   ROSCostmapServer ros_costmap_5x5_3x3_centre("five_by_five_three_by_three_centre", "base_link_5x5_3x3_centre", grid_map::Position(-6.0, -6.0), 5.0, 5.0);
00208   ROSCostmapServer ros_costmap_5x5_2_5x2_5_offset("five_by_five_twohalf_by_twohalf_offset", "base_link_5x5_2_5x2_5_offset", grid_map::Position(-12.0, 0.0), 5.0, 5.0);
00209   grid_map::GridMap grid_map_5x5_3x3_offset, grid_map_5x5_3x3_centre, grid_map_5x5_2_5x2_5_offset;
00210   grid_map::Length geometry_3x3(3.0, 3.0);
00211   fromCostmap2DROSAtRobotPose(*(ros_costmap_5x5_3x3_offset.getROSCostmap()), geometry_3x3, layer_name, grid_map_5x5_3x3_offset);
00212   fromCostmap2DROSAtRobotPose(*(ros_costmap_5x5_3x3_centre.getROSCostmap()), geometry_3x3, layer_name, grid_map_5x5_3x3_centre);
00213   grid_map::Length geometry_2_5x2_5(2.5, 2.5);
00214   fromCostmap2DROSAtRobotPose(*(ros_costmap_5x5_2_5x2_5_offset.getROSCostmap()), geometry_2_5x2_5, layer_name, grid_map_5x5_2_5x2_5_offset);
00215   ROS_INFO_STREAM("  grid_map_5x5_3x3_offset : " << grid_map_5x5_3x3_offset.getPosition().transpose());
00216   ROS_INFO_STREAM("  grid_map_5x5_3x3_offset : " << grid_map_5x5_3x3_centre.getPosition().transpose());
00217   ROS_INFO_STREAM("  grid_map_5x5_3x3_offset : " << grid_map_5x5_2_5x2_5_offset.getPosition().transpose());
00218   ASSERT_EQ(-3.5, grid_map_5x5_3x3_offset.getPosition().x());
00219   ASSERT_EQ(2.5, grid_map_5x5_3x3_offset.getPosition().y());
00220   ASSERT_EQ(-3.5, grid_map_5x5_3x3_centre.getPosition().x());
00221   ASSERT_EQ(-3.5, grid_map_5x5_3x3_centre.getPosition().y());
00222   ASSERT_EQ(-9.75, grid_map_5x5_2_5x2_5_offset.getPosition().x());
00223   ASSERT_EQ(2.25, grid_map_5x5_2_5x2_5_offset.getPosition().y());
00224   std::cout << std::endl;
00225 }
00226 
00227 /*****************************************************************************
00228 ** Main program
00229 *****************************************************************************/
00230 
00231 int main(int argc, char **argv)
00232 {
00233   ros::init(argc, argv, "test_from_ros_costmaps");
00234 
00235   TransformBroadcaster broadcaster;
00236   broadcastCostmap2DROSTestSuiteTransforms(broadcaster);
00237 
00238   testing::InitGoogleTest(&argc, argv);
00239   int result = RUN_ALL_TESTS();
00240   broadcaster.shutdown();
00241   return result;
00242 }