fix configuration and test bugs

Signed-off-by: lotusymt <[email protected]>

Author: Lotusymt

nav2_collision_monitor/CMakeLists.txt

@@ -72,6 +72,7 @@ add_library(${detector_library_name} SHARED
   src/scan.cpp
   src/pointcloud.cpp
   src/polygon_source.cpp
+  src/costmap.cpp
   src/range.cpp
   src/kinematics.cpp
 )

nav2_collision_monitor/include/nav2_collision_monitor/costmap.hpp

@@ -103,6 +103,12 @@ class CostmapSource : public Source
    */
   void getParameters(std::string & source_topic);
 
+  /**
+   * @brief Check if costmap data has been received.
+   * @return true if data has been received, false otherwise.
+   */
+  bool hasData() const {return data_ != nullptr;}
+
 private:
   /**
    * @brief Subscription callback to store the latest costmap message.

nav2_collision_monitor/launch/collision_monitor_node.launch.py

@@ -11,7 +11,7 @@
 # Unless required by applicable law or agreed to in writing, software
 # distributed under the License is distributed on an "AS IS" BASIS,
 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific languazge governing permissions and
+# See the License for the specific language governing permissions and
 # limitations under the License.
 
 import os

nav2_collision_monitor/test/collision_detector_node_test.cpp

@@ -508,44 +508,37 @@ void Tester::publishCostmap(const double dist, const rclcpp::Time & stamp)
   std::unique_ptr<nav2_msgs::msg::Costmap> msg =
     std::make_unique<nav2_msgs::msg::Costmap>();
 
-  msg->header.frame_id = SOURCE_FRAME_ID;
+  // Costmap in odom frame (more typical for Nav2 costmaps)
+  msg->header.frame_id = ODOM_FRAME_ID;
   msg->header.stamp = stamp;
 
-  // Set up metadata for a 20x20 costmap with 0.1m resolution
+  // Metadata: 20x20 grid, 0.1 m resolution
   msg->metadata.map_load_time = stamp;
   msg->metadata.update_time = stamp;
   msg->metadata.layer = "test_layer";
-  msg->metadata.resolution = 0.1;  // 10cm resolution
+  msg->metadata.resolution = 0.1;
   msg->metadata.size_x = 20;
   msg->metadata.size_y = 20;
-  // Place origin so that obstacle at distance 'dist' is in the costmap
-  msg->metadata.origin.position.x = -1.0;
-  msg->metadata.origin.position.y = dist - 1.0;
+
+  // Choose origin so that cell (0,0) center is exactly at (0, dist)
+  msg->metadata.origin.position.x = -0.05;       // 0.05 = 0.5 * resolution
+  msg->metadata.origin.position.y = dist - 0.05;   // dist - 0.5 * resolution
   msg->metadata.origin.position.z = 0.0;
   msg->metadata.origin.orientation.w = 1.0;
 
-  // Create a costmap with an obstacle at distance 'dist'
-  msg->data.resize(400, 0);  // 20x20 = 400 cells
+  // Initialize all cells as free
+  msg->data.assign(msg->metadata.size_x * msg->metadata.size_y, 0);
 
-  // Calculate cell index for obstacle at (0, dist) in costmap coordinates
-  // Costmap origin is at (-1.0, dist-1.0), so (0, dist) is at cell (10, 10)
-  const int obstacle_x = 10;
-  const int obstacle_y = 10;
+  // Put one lethal obstacle at cell (0,0)
+  const int obstacle_x = 0;
+  const int obstacle_y = 0;
   const int obstacle_idx = obstacle_y * msg->metadata.size_x + obstacle_x;
-
-  // Set obstacle cells to lethal cost
   msg->data[obstacle_idx] = nav2_costmap_2d::LETHAL_OBSTACLE;
-  // Set surrounding cells as well for better detection
-  if (obstacle_idx + 1 < 400) {
-    msg->data[obstacle_idx + 1] = nav2_costmap_2d::LETHAL_OBSTACLE;
-  }
-  if (obstacle_idx + msg->metadata.size_x < 400) {
-    msg->data[obstacle_idx + msg->metadata.size_x] = nav2_costmap_2d::LETHAL_OBSTACLE;
-  }
 
   costmap_pub_->publish(std::move(msg));
 }
 
+
 bool Tester::waitData(
   const double expected_dist,
   const std::chrono::nanoseconds & timeout,

nav2_collision_monitor/test/sources_test.cpp

@@ -412,7 +412,7 @@ class CostmapWrapper : public nav2_collision_monitor::CostmapSource
 
   bool dataReceived() const
   {
-    return data_ != nullptr;
+    return hasData();
   }
 };  // CostmapWrapper