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