Merge pull request #12 from fmrico/occupancy_works

Occupancy works

Author: fmrico

navmap_ros/include/navmap_ros/conversions.hpp

@@ -57,6 +57,26 @@
 namespace navmap_ros
 {
 
+/**
+ * @name Costmap value semantics
+ * @brief Standardized occupancy/cost values used when projecting NavMap layers
+ *        onto a 2D grid (compatible with `costmap_2d` conventions).
+ *
+ * These constants follow the same meaning as in `costmap_2d`:
+ *  - `NO_INFORMATION` (255): Unknown or unobserved area.
+ *  - `LETHAL_OBSTACLE` (254): Non-traversable obstacle.
+ *  - `INSCRIBED_INFLATED_OBSTACLE` (253): Inside the robot’s inscribed radius.
+ *  - `MAX_NON_OBSTACLE` (252): Highest cost still considered traversable.
+ *  - `FREE_SPACE` (0): Known free space.
+ * @{
+ */
+constexpr uint8_t NO_INFORMATION = 255;
+constexpr uint8_t LETHAL_OBSTACLE = 254;
+constexpr uint8_t INSCRIBED_INFLATED_OBSTACLE = 253;
+constexpr uint8_t MAX_NON_OBSTACLE = 252;
+constexpr uint8_t FREE_SPACE = 0;
+/** @} */  // end of Costmap value semantics group
+
 // --------- NavMap <-> ROS message ---------
 
 /**

navmap_ros/src/navmap_ros/conversions.cpp

@@ -53,15 +53,15 @@ using navmap_ros_interfaces::msg::NavMapSurface;
 
 static inline uint8_t occ_to_u8(int8_t v)
 {
-  if (v < 0) {return 255u;}
-  if (v >= 100) {return 254u;}
-  return static_cast<uint8_t>(std::lround((v / 100.0) * 254.0));
+  if (v < 0) {return NO_INFORMATION;}
+  if (v >= 100) {return LETHAL_OBSTACLE;}
+  return static_cast<uint8_t>(std::lround((v / 100.0) * static_cast<double>(LETHAL_OBSTACLE)));
 }
 
 static inline int8_t u8_to_occ(uint8_t u)
 {
-  if (u == 255u) {return -1;}
-  return static_cast<int8_t>(std::lround((u / 254.0) * 100.0));
+  if (u == NO_INFORMATION) {return -1;}
+  return static_cast<int8_t>(std::lround((u / static_cast<double>(LETHAL_OBSTACLE)) * 100.0));
 }
 
 static inline navmap::NavCelId tri_index_for_cell(uint32_t i, uint32_t j, uint32_t W)

navmap_rviz_plugin/src/NavMapDisplay.cpp

@@ -76,8 +76,8 @@ inline Ogre::ColourValue colorFromRainbow(float value, float max_value, float al
 
 inline Ogre::ColourValue colorFromU8(uint8_t v, float alpha)
 {
-  if (v == 0) {return Ogre::ColourValue(0.5f, 0.5f, 0.5f, alpha);}
-  if (v == 255) {return Ogre::ColourValue(0.0f, 0.39f, 0.0f, alpha);}
+  if (v == 0) {return Ogre::ColourValue(1.0f, 1.0f, 1.0f, alpha);}
+  if (v == 255) {return Ogre::ColourValue(0.25f, 0.25f, 0.25f, alpha);}
   if (v == 254) {return Ogre::ColourValue(0.0f, 0.0f, 0.0f, alpha);}
   float occ = static_cast<float>(v) / 253.0f;
   float c = 1.0f - occ;