Skip to content

Commit b89bfab

Browse files
LotusymtSteveMacenski
Lotusymt
and
SteveMacenski
authored
[Draft] CollisionMonitor: add CostmapSource + dataset-based bag test (#5642)
* Collision Monitor: add costmap sourced(unit tests) Signed-off-by: Mengting Yang <[email protected]> * nav2_collision_monitor: modified CostmapSource + dataset-based bag test (v1) Signed-off-by: Mengting Yang <[email protected]> * modified: nav2_collision_monitor/README.md Signed-off-by: Mengting Yang <[email protected]> * - annotate files to indicate AI-assisted drafts Signed-off-by: Mengting Yang <[email protected]> * collision_monitor: fix linter and address issues in the comment Signed-off-by: lotusymt <[email protected]> * nav2_collision_monitor: fix lint, coverage, and collision_monitor_node_test.cpp Signed-off-by: lotusymt <[email protected]> * Update nav2_collision_monitor/src/costmap.cpp Co-authored-by: Steve Macenski <[email protected]> Signed-off-by: Mengting Yang <[email protected]> * Update nav2_collision_monitor/src/costmap.cpp Co-authored-by: Steve Macenski <[email protected]> Signed-off-by: Mengting Yang <[email protected]> * Update nav2_collision_monitor/src/costmap.cpp Co-authored-by: Steve Macenski <[email protected]> Signed-off-by: Mengting Yang <[email protected]> * Reverse format change Signed-off-by: lotusymt <[email protected]> * Reverse format changes and comments Signed-off-by: lotusymt <[email protected]> * Refactor in_range and count_stopped functions Signed-off-by: Mengting Yang <[email protected]> Signed-off-by: lotusymt <[email protected]> * added costmap source to collision detector and tests( for failure path and collision detector) Signed-off-by: lotusymt <[email protected]> * fix configuration and test bugs Signed-off-by: lotusymt <[email protected]> --------- Signed-off-by: Mengting Yang <[email protected]> Signed-off-by: lotusymt <[email protected]> Signed-off-by: Mengting Yang <[email protected]> Co-authored-by: Steve Macenski <[email protected]>
1 parent 2f741fd commit b89bfab

21 files changed

+1193
-1
lines changed

‎nav2_collision_monitor/CMakeLists.txt‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -37,7 +37,9 @@ add_library(${monitor_library_name} SHARED
3737
src/polygon_source.cpp
3838
src/range.cpp
3939
src/kinematics.cpp
40+
src/costmap.cpp
4041
)
42+
4143
target_include_directories(${monitor_library_name}
4244
PUBLIC
4345
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
@@ -70,6 +72,7 @@ add_library(${detector_library_name} SHARED
7072
src/scan.cpp
7173
src/pointcloud.cpp
7274
src/polygon_source.cpp
75+
src/costmap.cpp
7376
src/range.cpp
7477
src/kinematics.cpp
7578
)
@@ -146,6 +149,8 @@ install(DIRECTORY params DESTINATION share/${PROJECT_NAME})
146149

147150
if(BUILD_TESTING)
148151
find_package(ament_lint_auto REQUIRED)
152+
find_package(launch_testing_ament_cmake REQUIRED)
153+
find_package(rosgraph_msgs REQUIRED)
149154
# the following line skips the linter which checks for copyrights
150155
set(ament_cmake_copyright_FOUND TRUE)
151156
set(ament_cmake_cpplint_FOUND TRUE)
@@ -165,6 +170,7 @@ ament_export_dependencies(
165170
nav2_costmap_2d
166171
nav2_msgs
167172
nav2_util
173+
rosgraph_msgs
168174
rclcpp
169175
rclcpp_lifecycle
170176
sensor_msgs

‎nav2_collision_monitor/README.md‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -44,6 +44,12 @@ The data may be obtained from different data sources:
4444
* Laser scanners (`sensor_msgs::msg::LaserScan` messages)
4545
* PointClouds (`sensor_msgs::msg::PointCloud2` messages)
4646
* IR/Sonars (`sensor_msgs::msg::Range` messages)
47+
* Costmap (`nav2_msgs::msg::Costmap` messages)
48+
49+
> **⚠️ when using CostmapSource**
50+
> Collision Monitor normally **bypasses the costmap** to minimize reaction latency using fresh sensor data.
51+
> Use at your own caution or when using external costmap sources from derived sources.
52+
4753

4854
### Design
4955

‎nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -39,6 +39,7 @@
3939
#include "nav2_collision_monitor/pointcloud.hpp"
4040
#include "nav2_collision_monitor/range.hpp"
4141
#include "nav2_collision_monitor/polygon_source.hpp"
42+
#include "nav2_collision_monitor/costmap.hpp"
4243

4344
namespace nav2_collision_monitor
4445
{

‎nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -43,6 +43,7 @@
4343
#include "nav2_collision_monitor/scan.hpp"
4444
#include "nav2_collision_monitor/pointcloud.hpp"
4545
#include "nav2_collision_monitor/range.hpp"
46+
#include "nav2_collision_monitor/costmap.hpp"
4647
#include "nav2_collision_monitor/polygon_source.hpp"
4748

4849
namespace nav2_collision_monitor

‎nav2_collision_monitor/include/nav2_collision_monitor/costmap.hpp‎

Lines changed: 139 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,139 @@
1+
// Copyright (c) 2025 Angsa Robotics
2+
// Copyright (c) 2025 lotusymt
3+
//
4+
// Licensed under the Apache License, Version 2.0 (the "License");
5+
// you may not use this file except in compliance with the License.
6+
// You may obtain a copy of the License at
7+
//
8+
// http://www.apache.org/licenses/LICENSE-2.0
9+
//
10+
// Unless required by applicable law or agreed to in writing, software
11+
// distributed under the License is distributed on an "AS IS" BASIS,
12+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
// See the License for the specific language governing permissions and
14+
// limitations under the License.
15+
16+
#ifndef NAV2_COLLISION_MONITOR__COSTMAP_HPP_
17+
#define NAV2_COLLISION_MONITOR__COSTMAP_HPP_
18+
19+
/**
20+
* @file costmap.hpp
21+
* @brief Observation source that converts a Nav2 costmap topic into 2D points for Collision Monitor.
22+
*/
23+
24+
#include <memory>
25+
#include <string>
26+
#include <vector>
27+
28+
#include "nav2_collision_monitor/source.hpp"
29+
#include "nav2_msgs/msg/costmap.hpp"
30+
#include <nav2_ros_common/lifecycle_node.hpp>
31+
#include <nav2_ros_common/subscription.hpp>
32+
33+
namespace nav2_collision_monitor
34+
{
35+
36+
/**
37+
* @class CostmapSource
38+
* @brief Reads nav2_msgs::msg::Costmap and produces obstacle points for Collision Monitor.
39+
*
40+
* Cells with cost >= @ref cost_threshold_ are exported as points. Optionally, NO_INFORMATION (255)
41+
* can be treated as obstacles via @ref treat_unknown_as_obstacle_.
42+
*
43+
* Parameters (declared/queried in @ref getParameters):
44+
* - `topic` (std::string): costmap topic name (relative is recommended).
45+
* - `cost_threshold` (int, 0..255): minimum cost to consider a cell occupied.
46+
* - `treat_unknown_as_obstacle` (bool): whether cost 255 should be treated as occupied.
47+
*/
48+
class CostmapSource : public Source
49+
{
50+
public:
51+
/**
52+
* @brief Construct a CostmapSource.
53+
* @param node Weak pointer to the lifecycle node.
54+
* @param source_name Logical name of this source instance (for params/logs).
55+
* @param tf_buffer Shared TF buffer for frame transforms.
56+
* @param base_frame_id Robot base frame (e.g., "base_footprint").
57+
* @param global_frame_id Global frame of the costmap (e.g., "odom" or "map").
58+
* @param transform_tolerance Allowed TF age for transforms.
59+
* @param source_timeout Max age of data before it is considered stale.
60+
* @param base_shift_correction Whether to compensate robot motion during simulation checks.
61+
*/
62+
CostmapSource(
63+
const nav2::LifecycleNode::WeakPtr & node,
64+
const std::string & source_name,
65+
const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
66+
const std::string & base_frame_id,
67+
const std::string & global_frame_id,
68+
const tf2::Duration & transform_tolerance,
69+
const rclcpp::Duration & source_timeout,
70+
const bool base_shift_correction);
71+
72+
/// @brief Destructor.
73+
~CostmapSource();
74+
75+
/**
76+
* @brief Declare and get parameters; create the subscription.
77+
*
78+
* Must be called during the node’s configuration phase (after construction, before use).
79+
* Reads `topic`, `cost_threshold`, and `treat_unknown_as_obstacle`.
80+
*/
81+
void configure();
82+
83+
/**
84+
* @brief Produce current obstacle points from the latest costmap.
85+
* @param curr_time Current time used for staleness checks and TF queries.
86+
* @param[out] data Output vector of points in the base frame.
87+
* @return true if valid, non-stale data were produced; false otherwise.
88+
*
89+
* @details
90+
* - Returns false if no message has arrived or data are older than @ref source_timeout_.
91+
* - Transforms points from costmap frame to @ref base_frame_id using @ref tf_buffer_.
92+
* - Applies @ref cost_threshold_ and @ref treat_unknown_as_obstacle_.
93+
*/
94+
bool getData(
95+
const rclcpp::Time & curr_time,
96+
std::vector<Point> & data) override;
97+
98+
/**
99+
* @brief Read parameters specific to the costmap source.
100+
* @param[out] source_topic Resolved topic name to subscribe to.
101+
*
102+
* Declares/gets: `topic`, `cost_threshold`, `treat_unknown_as_obstacle`.
103+
*/
104+
void getParameters(std::string & source_topic);
105+
106+
/**
107+
* @brief Check if costmap data has been received.
108+
* @return true if data has been received, false otherwise.
109+
*/
110+
bool hasData() const {return data_ != nullptr;}
111+
112+
private:
113+
/**
114+
* @brief Subscription callback to store the latest costmap message.
115+
* @param msg Incoming costmap.
116+
*/
117+
void dataCallback(nav2_msgs::msg::Costmap::ConstSharedPtr msg);
118+
119+
/// @brief Latest costmap message.
120+
nav2_msgs::msg::Costmap::ConstSharedPtr data_;
121+
122+
/// @brief Subscription handle for the costmap topic.
123+
nav2::Subscription<nav2_msgs::msg::Costmap>::SharedPtr data_sub_;
124+
125+
/**
126+
* @brief Minimum cost (0..255) considered as an obstacle.
127+
* @note Typical values: 253 (inscribed), 254 (lethal). Inflation = 1..252.
128+
*/
129+
int cost_threshold_{253};
130+
131+
/**
132+
* @brief Whether cost 255 (NO_INFORMATION) is treated as an obstacle.
133+
*/
134+
bool treat_unknown_as_obstacle_{true};
135+
};
136+
137+
} // namespace nav2_collision_monitor
138+
139+
#endif // NAV2_COLLISION_MONITOR__COSTMAP_HPP_

‎nav2_collision_monitor/package.xml‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -31,6 +31,9 @@
3131
<test_depend>ament_cmake_gtest</test_depend>
3232
<test_depend>ament_lint_common</test_depend>
3333
<test_depend>ament_lint_auto</test_depend>
34+
<test_depend>launch_testing_ament_cmake</test_depend>
35+
<test_depend>rosgraph_msgs</test_depend>
36+
3437

3538
<export>
3639
<build_type>ament_cmake</build_type>

‎nav2_collision_monitor/params/collision_detector_params.yaml‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -27,3 +27,9 @@ collision_detector:
2727
min_height: 0.1
2828
max_height: 0.5
2929
enabled: True
30+
# costmap:
31+
# type: "costmap"
32+
# topic: "local_costmap/costmap"
33+
# cost_threshold: 254
34+
# treat_unknown_as_obstacle: True
35+
# enabled: True

‎nav2_collision_monitor/params/collision_monitor_params.yaml‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -102,3 +102,9 @@ collision_monitor:
102102
max_height: 0.5
103103
use_global_height: False
104104
enabled: True
105+
# costmap:
106+
# type: "costmap"
107+
# topic: "local_costmap/costmap"
108+
# cost_threshold: 254
109+
# enabled: True
110+
# treat_unknown_as_obstacle: True

‎nav2_collision_monitor/src/collision_detector_node.cpp‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -277,6 +277,14 @@ bool CollisionDetector::configureSources(
277277
ps->configure();
278278

279279
sources_.push_back(ps);
280+
} else if (source_type == "costmap") {
281+
auto src = std::make_shared<CostmapSource>(
282+
node, source_name, tf_buffer_, base_frame_id, odom_frame_id,
283+
transform_tolerance, source_timeout, base_shift_correction);
284+
285+
src->configure();
286+
287+
sources_.push_back(src);
280288
} else { // Error if something else
281289
RCLCPP_ERROR(
282290
get_logger(),

‎nav2_collision_monitor/src/collision_monitor_node.cpp‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -377,6 +377,14 @@ bool CollisionMonitor::configureSources(
377377
ps->configure();
378378

379379
sources_.push_back(ps);
380+
} else if (source_type == "costmap") {
381+
auto src = std::make_shared<CostmapSource>(
382+
node, source_name, tf_buffer_, base_frame_id, odom_frame_id,
383+
transform_tolerance, source_timeout, base_shift_correction);
384+
385+
src->configure();
386+
387+
sources_.push_back(src);
380388
} else { // Error if something else
381389
RCLCPP_ERROR(
382390
get_logger(),

0 commit comments

Comments
 (0)