ros bridge: costmap mode for gridmap conversion

MRPT · Oct 7, 2024 · ad80afe · ad80afe
1 parent 60b7b60
commit ad80afe
Show file tree

Hide file tree

Showing 5 changed files with 40 additions and 12 deletions.
diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md
@@ -4,6 +4,8 @@
 - Changes in libraries:
   - \ref mrpt_nav_grp:
     - mrpt::nav::CWaypointsNavigator: New parameter "minimum_target_approach_per_step" and feature to keep approaching waypoints until no significant improvement is done.
+  - \ref mrpt_ros1bridge_grp and mrpt_ros2bridge_grp:
+    - Convert from MRPT occupancy grids to ROS: Add new optional parameter to interpret grid maps as cost maps.
 
 # Version 2.14.2: Released Oct 5th, 2024
 - Changes in libraries:

diff --git a/libs/ros1bridge/include/mrpt/ros1bridge/map.h b/libs/ros1bridge/include/mrpt/ros1bridge/map.h
@@ -96,16 +96,22 @@ bool fromROS(const nav_msgs::OccupancyGrid& src, mrpt::maps::COccupancyGridMap2D
  * @return true on sucessful conversion, false on any error.
  * @param src
  * @param header
+ * @param as_costmap If set to true, gridmap cell values will be copied without changes
+ *        (interpreted as int8_t instead of Log-odds)
  */
 bool toROS(
     const mrpt::maps::COccupancyGridMap2D& src,
     nav_msgs::OccupancyGrid& msg,
-    const std_msgs::Header& header);
+    const std_msgs::Header& header,
+    bool as_costmap = false);
 /**
  * converts mrpt object to ros msg
  * @return true on sucessful conversion, false on any error.
  */
-bool toROS(const mrpt::maps::COccupancyGridMap2D& src, nav_msgs::OccupancyGrid& msg);
+bool toROS(
+    const mrpt::maps::COccupancyGridMap2D& src,
+    nav_msgs::OccupancyGrid& msg,
+    bool as_costmap = false);
 
 /** @}
  * @}

diff --git a/libs/ros1bridge/src/map.cpp b/libs/ros1bridge/src/map.cpp
@@ -107,13 +107,17 @@ bool mrpt::ros1bridge::fromROS(const nav_msgs::OccupancyGrid& src, COccupancyGri
   MRPT_END
 }
 bool mrpt::ros1bridge::toROS(
-    const COccupancyGridMap2D& src, nav_msgs::OccupancyGrid& des, const std_msgs::Header& header)
+    const COccupancyGridMap2D& src,
+    nav_msgs::OccupancyGrid& des,
+    const std_msgs::Header& header,
+    bool as_costmap)
 {
   des.header = header;
-  return toROS(src, des);
+  return toROS(src, des, as_costmap);
 }
 
-bool mrpt::ros1bridge::toROS(const COccupancyGridMap2D& src, nav_msgs::OccupancyGrid& des)
+bool mrpt::ros1bridge::toROS(
+    const COccupancyGridMap2D& src, nav_msgs::OccupancyGrid& des, bool as_costmap)
 {
   des.info.width = src.getSizeX();
   des.info.height = src.getSizeY();
@@ -133,10 +137,14 @@ bool mrpt::ros1bridge::toROS(const COccupancyGridMap2D& src, nav_msgs::Occupancy
   for (unsigned int h = 0; h < des.info.height; h++)
   {
     const COccupancyGridMap2D::cellType* pSrc = src.getRow(h);
+    ASSERT_(pSrc);
     int8_t* pDes = &des.data[h * des.info.width];
     for (unsigned int w = 0; w < des.info.width; w++)
     {
-      *pDes++ = MapHdl::instance()->cellMrpt2Ros(*pSrc++);
+      if (as_costmap)
+        *pDes++ = static_cast<int8_t>(*pSrc++);
+      else
+        *pDes++ = MapHdl::instance()->cellMrpt2Ros(*pSrc++);
     }
   }
   return true;

diff --git a/libs/ros2bridge/include/mrpt/ros2bridge/map.h b/libs/ros2bridge/include/mrpt/ros2bridge/map.h
@@ -96,16 +96,22 @@ bool fromROS(const nav_msgs::msg::OccupancyGrid& src, mrpt::maps::COccupancyGrid
  * @return true on sucessful conversion, false on any error.
  * @param src
  * @param header
+ * @param as_costmap If set to true, gridmap cell values will be copied without changes
+ *        (interpreted as int8_t instead of Log-odds)
  */
 bool toROS(
     const mrpt::maps::COccupancyGridMap2D& src,
     nav_msgs::msg::OccupancyGrid& msg,
-    const std_msgs::msg::Header& header);
+    const std_msgs::msg::Header& header,
+    bool as_costmap = false);
 /**
  * converts mrpt object to ros msg
  * @return true on sucessful conversion, false on any error.
  */
-bool toROS(const mrpt::maps::COccupancyGridMap2D& src, nav_msgs::msg::OccupancyGrid& msg);
+bool toROS(
+    const mrpt::maps::COccupancyGridMap2D& src,
+    nav_msgs::msg::OccupancyGrid& msg,
+    bool as_costmap = false);
 
 /** @}
  * @}

diff --git a/libs/ros2bridge/src/map.cpp b/libs/ros2bridge/src/map.cpp
@@ -109,13 +109,15 @@ bool mrpt::ros2bridge::fromROS(const nav_msgs::msg::OccupancyGrid& src, COccupan
 bool mrpt::ros2bridge::toROS(
     const COccupancyGridMap2D& src,
     nav_msgs::msg::OccupancyGrid& des,
-    const std_msgs::msg::Header& header)
+    const std_msgs::msg::Header& header,
+    bool as_costmap)
 {
   des.header = header;
-  return toROS(src, des);
+  return toROS(src, des, as_costmap);
 }
 
-bool mrpt::ros2bridge::toROS(const COccupancyGridMap2D& src, nav_msgs::msg::OccupancyGrid& des)
+bool mrpt::ros2bridge::toROS(
+    const COccupancyGridMap2D& src, nav_msgs::msg::OccupancyGrid& des, bool as_costmap)
 {
   des.info.width = src.getSizeX();
   des.info.height = src.getSizeY();
@@ -135,10 +137,14 @@ bool mrpt::ros2bridge::toROS(const COccupancyGridMap2D& src, nav_msgs::msg::Occu
   for (unsigned int h = 0; h < des.info.height; h++)
   {
     const COccupancyGridMap2D::cellType* pSrc = src.getRow(h);
+    ASSERT_(pSrc);
     int8_t* pDes = &des.data[h * des.info.width];
     for (unsigned int w = 0; w < des.info.width; w++)
     {
-      *pDes++ = MapHdl::instance()->cellMrpt2Ros(*pSrc++);
+      if (as_costmap)
+        *pDes++ = static_cast<int8_t>(*pSrc++);
+      else
+        *pDes++ = MapHdl::instance()->cellMrpt2Ros(*pSrc++);
     }
   }
   return true;