costmap_2d_ros.cpp
Go to the documentation of this file.
1 /*****************************************************************************
2 ** Includes
3 *****************************************************************************/
4 
5 #include <gtest/gtest.h>
6 #include <iostream>
7 #include <string>
8 
9 #include <ros/ros.h>
10 #if ROS_VERSION_MINIMUM(1,14,0)
12 #include <tf/transform_datatypes.h>
13 #else
15 #endif
16 
17 #include "costmap_2d_ros.hpp"
18 
19 /*****************************************************************************
20 ** Helpers
21 *****************************************************************************/
22 
24 {
25  broadcaster.add("base_link_5x5", tf::Vector3(1.0, 1.0, 0.0), tf::Quaternion(0, 0, 0, 1));
26  broadcaster.add("base_link_4x4", tf::Vector3(1.0, -3.0, 0.0), tf::Quaternion(0, 0, 0, 1));
27  broadcaster.add("base_link_5x5_3x3_offset", tf::Vector3(-3.7, 2.4, 0.0), tf::Quaternion(0, 0, 0, 1));
28  broadcaster.add("base_link_5x5_3x3_centre", tf::Vector3(-3.5, -3.5, 0.0), tf::Quaternion(0, 0, 0, 1));
29  broadcaster.add("base_link_5x5_2_5x2_5_offset", tf::Vector3(-9.7, 2.4, 0.0), tf::Quaternion(0, 0, 0, 1));
30  broadcaster.startBroadCastingThread();
31 }
32 
33 /*****************************************************************************
34 ** ROS Costmap Server
35 *****************************************************************************/
36 
37 ROSCostmapServer::ROSCostmapServer(const std::string& name,
38  const std::string& baseLinkTransformName,
39  const grid_map::Position& origin, const double& width,
40  const double& height)
41 #if ROS_VERSION_MINIMUM(1,14,0)
42  : tfBuffer_(ros::Duration(1.0)),
43  tfListener_(tfBuffer_)
44 #else
45  : tfListener_(ros::Duration(1.0))
46 #endif
47 {
48  ros::NodeHandle privateNodeHandle("~");
49  // lots of parameters here affect the construction ( e.g. rolling window)
50  // if you don't have any parameters set, then this
51  // - alot of defaults which get dumped on the ros param server
52  // - fires up an obstacle layer and an inflation layer
53  // - creates a publisher for an occupancy grid
54  privateNodeHandle.setParam(name + "/robot_base_frame", baseLinkTransformName);
55  privateNodeHandle.setParam(name + "/origin_x", origin.x());
56  privateNodeHandle.setParam(name + "/origin_y", origin.y());
57  privateNodeHandle.setParam(name + "/width", width);
58  privateNodeHandle.setParam(name + "/height", height);
59  privateNodeHandle.setParam(name + "/plugins", std::vector<std::string>());
60  privateNodeHandle.setParam(name + "/resolution", 0.5);
61  privateNodeHandle.setParam(name + "/robot_radius", 0.03); // clears 1 cell if inside, up to 4 cells on a vertex
62 #if ROS_VERSION_MINIMUM(1,14,0)
63  costmap_ = std::make_shared<ROSCostmap>(name, tfBuffer_);
64 #else
65  costmap_ = std::make_shared<ROSCostmap>(name, tfListener_);
66 #endif
67 
68  for ( unsigned int index = 0; index < costmap_->getCostmap()->getSizeInCellsY(); ++index ) {
69  unsigned int dimension = costmap_->getCostmap()->getSizeInCellsX();
70  // @todo assert dimension > 1
71  // set the first row to costmap_2d::FREE_SPACE? but it shows up invisibly in rviz, so don't bother
72  for ( unsigned int fill_index = 0; fill_index < dimension - 2; ++fill_index )
73  {
74  double fraction = static_cast<double>(fill_index + 1) / static_cast<double>(costmap_->getCostmap()->getSizeInCellsX());
75  costmap_->getCostmap()->setCost(fill_index, index, fraction*costmap_2d::INSCRIBED_INFLATED_OBSTACLE );
76  }
77  costmap_->getCostmap()->setCost(dimension - 2, index, costmap_2d::LETHAL_OBSTACLE);
78  costmap_->getCostmap()->setCost(dimension - 1, index, costmap_2d::NO_INFORMATION);
79  }
80 }
81 
82 /*****************************************************************************
83 ** TransformBroadcaster
84 *****************************************************************************/
85 
87 {
88  broadcastingThread_.join();
89 }
90 
92 {
93  shutdownFlag_ = true;
94 }
95 
96 void TransformBroadcaster::add(const std::string& name, tf::Vector3 origin,
97  const tf::Quaternion& orientation)
98 {
99  tf::Transform transform;
100  transform.setOrigin(origin);
101  transform.setRotation(orientation);
102  transforms_.insert(std::pair<std::string, tf::Transform>(name, transform));
103 }
104 
107 }
108 
110 {
111 #if ROS_VERSION_MINIMUM(1,14,0)
112  tf2_ros::TransformBroadcaster tfBroadcaster;
113 #else
114  tf::TransformBroadcaster tfBroadcaster;
115 #endif
116  while (ros::ok() && !shutdownFlag_) {
117  for (std::pair<std::string, tf::Transform> p : transforms_) {
118  tf::StampedTransform transform(p.second, ros::Time::now(), "map", p.first);
119 #if ROS_VERSION_MINIMUM(1,14,0)
120  geometry_msgs::TransformStamped transformMsg;
121  tf::transformStampedTFToMsg(transform, transformMsg);
122  tfBroadcaster.sendTransform(transformMsg);
123 #else
124  tfBroadcaster.sendTransform(transform);
125 #endif
126  }
127  ros::Duration(0.1).sleep();
128  }
129 }
130 
131 /*****************************************************************************
132 ** Converter Functions
133 *****************************************************************************/
134 
135 bool fromCostmap2DROS(costmap_2d::Costmap2DROS& ros_costmap, const std::string& layer_name,
137 {
139  boost::lock_guard<costmap_2d::Costmap2D::mutex_t> lock(*(ros_costmap.getCostmap()->getMutex()));
140  converter.initializeFromCostmap2D(ros_costmap, grid_map);
141  if (!converter.addLayerFromCostmap2D(ros_costmap, layer_name, grid_map)) {
142  return false;
143  }
144  return true;
145 }
146 
148  const grid_map::Length& geometry, const std::string& layer_name,
150 {
152  boost::lock_guard<costmap_2d::Costmap2D::mutex_t> lock(*(ros_costmap.getCostmap()->getMutex()));
153  if (!converter.initializeFromCostmap2DAtRobotPose(ros_costmap, geometry, grid_map)) {
154  return false;
155  }
156  if (!converter.addLayerFromCostmap2DAtRobotPose(ros_costmap, layer_name, grid_map)) {
157  return false;
158  }
159  return true;
160 }
161 
162 /*****************************************************************************
163 ** Tests
164 *****************************************************************************/
165 
166 TEST(Costmap2DROSConversion, full_window)
167 {
168  std::cout << std::endl;
169  ROS_INFO("***********************************************************");
170  ROS_INFO(" Copy Full Window");
171  ROS_INFO("***********************************************************");
172  // preparation
173  std::string layer_name = "obstacle_costs";
174  ROSCostmapServer ros_costmap_5x5("five_by_five", "base_link_5x5", grid_map::Position(0.0, 0.0), 5.0, 5.0);
175  grid_map::GridMap grid_map_5x5;
176  fromCostmap2DROS(*(ros_costmap_5x5.getROSCostmap()), layer_name, grid_map_5x5);
177  // assert map properties
178  ASSERT_EQ(grid_map_5x5.getFrameId(), ros_costmap_5x5.getROSCostmap()->getGlobalFrameID());
179  ASSERT_EQ(
180  grid_map_5x5.getLength().x(),
181  ros_costmap_5x5.getROSCostmap()->getCostmap()->getSizeInCellsX()
182  * ros_costmap_5x5.getROSCostmap()->getCostmap()->getResolution());
183  ASSERT_EQ(
184  grid_map_5x5.getLength().y(),
185  ros_costmap_5x5.getROSCostmap()->getCostmap()->getSizeInCellsY()
186  * ros_costmap_5x5.getROSCostmap()->getCostmap()->getResolution());
187  ASSERT_EQ(grid_map_5x5.getSize()[0], ros_costmap_5x5.getROSCostmap()->getCostmap()->getSizeInCellsX());
188  ASSERT_EQ(grid_map_5x5.getSize()[1], ros_costmap_5x5.getROSCostmap()->getCostmap()->getSizeInCellsY());
189  grid_map::Length position = grid_map_5x5.getPosition() - 0.5 * grid_map_5x5.getLength().matrix();
190  ASSERT_EQ(position.x(), ros_costmap_5x5.getROSCostmap()->getCostmap()->getOriginX());
191  ASSERT_EQ(position.y(), ros_costmap_5x5.getROSCostmap()->getCostmap()->getOriginY());
192 
193  // assert map data
194  for (unsigned int i = 0; i < 5; ++i) {
195  for (unsigned int j = 0; j < 5; ++j) {
196  std::cout << static_cast<int>(ros_costmap_5x5.getROSCostmap()->getCostmap()->getCost(i, j))
197  << " ";
198  }
199  std::cout << std::endl;
200  }
201  for (unsigned int i = 0; i < 5; ++i) {
202  for (unsigned int j = 0; j < 5; ++j) {
203  std::cout << static_cast<int>(grid_map_5x5.at(layer_name, grid_map::Index(i, j))) << " ";
204  }
205  std::cout << std::endl;
206  }
207  // TODO a function which does the index conversion
208 
209  std::cout << "Original cost: " << static_cast<int>(ros_costmap_5x5.getROSCostmap()->getCostmap()->getCost(8,0)) << std::endl;
210  std::cout << "New Cost: " << grid_map_5x5.at(layer_name, grid_map::Index(1,9)) << std::endl;
211  std::vector<float> cost_translation_table;
213  unsigned char cost = ros_costmap_5x5.getROSCostmap()->getCostmap()->getCost(8,0);
214  ASSERT_EQ(grid_map_5x5.at(layer_name, grid_map::Index(1,9)), cost_translation_table[cost]);
215  std::cout << std::endl;
216 }
217 
218 TEST(Costmap2DROSConversion, cost_map_centres)
219 {
220  std::cout << std::endl;
221  ROS_INFO("***********************************************************");
222  ROS_INFO(" Check Subwindow Centres");
223  ROS_INFO("***********************************************************");
224  ROS_INFO("Subwindows are centred as closely as possible to the robot");
225  ROS_INFO("pose, though not exactly. They still need to align with");
226  ROS_INFO("the underlying ros costmap so that they don't introduce a");
227  ROS_INFO("new kind of error. As a result, the centre is shifted from");
228  ROS_INFO("the robot pose to the nearest appropriate point which aligns");
229  ROS_INFO("the new cost map exactly on top of the original ros costmap.");
230  std::cout << std::endl;
231  std::string layer_name = "obstacle_costs";
232  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);
233  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);
234  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);
235  grid_map::GridMap grid_map_5x5_3x3_offset, grid_map_5x5_3x3_centre, grid_map_5x5_2_5x2_5_offset;
236  grid_map::Length geometry_3x3(3.0, 3.0);
237  fromCostmap2DROSAtRobotPose(*(ros_costmap_5x5_3x3_offset.getROSCostmap()), geometry_3x3, layer_name, grid_map_5x5_3x3_offset);
238  fromCostmap2DROSAtRobotPose(*(ros_costmap_5x5_3x3_centre.getROSCostmap()), geometry_3x3, layer_name, grid_map_5x5_3x3_centre);
239  grid_map::Length geometry_2_5x2_5(2.5, 2.5);
240  fromCostmap2DROSAtRobotPose(*(ros_costmap_5x5_2_5x2_5_offset.getROSCostmap()), geometry_2_5x2_5, layer_name, grid_map_5x5_2_5x2_5_offset);
241  ROS_INFO_STREAM(" grid_map_5x5_3x3_offset : " << grid_map_5x5_3x3_offset.getPosition().transpose());
242  ROS_INFO_STREAM(" grid_map_5x5_3x3_offset : " << grid_map_5x5_3x3_centre.getPosition().transpose());
243  ROS_INFO_STREAM(" grid_map_5x5_3x3_offset : " << grid_map_5x5_2_5x2_5_offset.getPosition().transpose());
244  ASSERT_EQ(-3.5, grid_map_5x5_3x3_offset.getPosition().x());
245  ASSERT_EQ(2.5, grid_map_5x5_3x3_offset.getPosition().y());
246  ASSERT_EQ(-3.5, grid_map_5x5_3x3_centre.getPosition().x());
247  ASSERT_EQ(-3.5, grid_map_5x5_3x3_centre.getPosition().y());
248  ASSERT_EQ(-9.75, grid_map_5x5_2_5x2_5_offset.getPosition().x());
249  ASSERT_EQ(2.25, grid_map_5x5_2_5x2_5_offset.getPosition().y());
250  std::cout << std::endl;
251 }
252 
253 /*****************************************************************************
254 ** Main program
255 *****************************************************************************/
256 
257 int main(int argc, char **argv)
258 {
259  ros::init(argc, argv, "test_from_ros_costmaps");
260 
261  TransformBroadcaster broadcaster;
263 
264  testing::InitGoogleTest(&argc, argv);
265  int result = RUN_ALL_TESTS();
266  broadcaster.shutdown();
267  return result;
268 }
const Length & getLength() const
Eigen::Array2i Index
Convert Costmap2DRos objects into cost or grid maps.
TFSIMD_FORCE_INLINE void setRotation(const Quaternion &q)
Broadcast a set of transforms useful for various demos.
bool fromCostmap2DROS(costmap_2d::Costmap2DROS &ros_costmap, const std::string &layer_name, grid_map::GridMap &grid_map)
std::thread broadcastingThread_
bool getPosition(const Index &index, Position &position) const
void initializeFromCostmap2D(costmap_2d::Costmap2DROS &costmap2d, MapType &outputMap)
const std::string & getFrameId() const
bool sleep() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool fromCostmap2DROSAtRobotPose(costmap_2d::Costmap2DROS &ros_costmap, const grid_map::Length &geometry, const std::string &layer_name, grid_map::GridMap &grid_map)
std::map< std::string, tf::Transform > transforms_
bool addLayerFromCostmap2DAtRobotPose(costmap_2d::Costmap2DROS &costmap2d, const std::string &layer, MapType &outputMap)
void broadcastCostmap2DROSTestSuiteTransforms(TransformBroadcaster &broadcaster)
void add(const std::string &name, tf::Vector3 origin, const tf::Quaternion &orientation)
ROSCostmapServer(const std::string &name, const std::string &baseLinkTransformName, const grid_map::Position &origin, const double &width, const double &height)
ROSCostmapPtr getROSCostmap()
static const unsigned char INSCRIBED_INFLATED_OBSTACLE
Eigen::Vector2d Position
#define ROS_INFO(...)
ROSCPP_DECL bool ok()
void sendTransform(const StampedTransform &transform)
bool initializeFromCostmap2DAtRobotPose(costmap_2d::Costmap2DROS &costmap2d, const Length &length, MapType &outputMap)
virtual ~TransformBroadcaster()
float & at(const std::string &layer, const Index &index)
bool addLayerFromCostmap2D(const costmap_2d::Costmap2D &costmap2d, const std::string &layer, MapType &outputMap)
#define ROS_INFO_STREAM(args)
static const unsigned char LETHAL_OBSTACLE
static const unsigned char NO_INFORMATION
TEST(Costmap2DROSConversion, full_window)
static Time now()
static void transformStampedTFToMsg(const StampedTransform &bt, geometry_msgs::TransformStamped &msg)
int main(int argc, char **argv)
Costmap2D * getCostmap()
static void create(std::vector< DataType > &costTranslationTable)
void setParam(const std::string &key, const XmlRpc::XmlRpcValue &v) const
std::atomic< bool > shutdownFlag_
TFSIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
Eigen::Array2d Length
const Size & getSize() const


grid_map_costmap_2d
Author(s): Péter Fankhauser
autogenerated on Tue Jun 25 2019 20:02:11