Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 2 additions & 1 deletion .circleci/config.yml
Original file line number Diff line number Diff line change
Expand Up @@ -503,7 +503,7 @@ _jobs:
type: boolean
default: false
rmw:
default: "rmw_cyclonedds_cpp"
default: "rmw_zenoh_cpp"
type: string
parallelism: 1
environment:
Expand Down Expand Up @@ -576,6 +576,7 @@ workflows:
rmw:
- rmw_cyclonedds_cpp
- rmw_fastrtps_cpp
- rmw_zenoh_cpp
triggers:
- schedule:
cron: "0 13 * * *"
Expand Down
1 change: 1 addition & 0 deletions Dockerfile
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,7 @@ RUN apt-get update && \
ros-$ROS_DISTRO-rmw-fastrtps-cpp \
ros-$ROS_DISTRO-rmw-connextdds \
ros-$ROS_DISTRO-rmw-cyclonedds-cpp \
ros-$ROS_DISTRO-rmw-zenoh-cpp \
&& pip3 install --break-system-packages \
fastcov \
git+https://github.com/ruffsl/colcon-cache.git@a937541bfc496c7a267db7ee9d6cceca61e470ca \
Expand Down
2 changes: 1 addition & 1 deletion nav2_amcl/src/amcl_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -72,12 +72,12 @@ AmclNode::on_configure(const rclcpp_lifecycle::State & /*state*/)
rclcpp::CallbackGroupType::MutuallyExclusive, false);
initParameters();
initTransforms();
initOdometry();
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think this needs to be after init pubsub and init services in order to be able to have last_published_pose_ be usable

Copy link
Contributor Author

@mini-1235 mini-1235 Oct 24, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Looking at the ros(1) code, https://github.com/ros-planning/navigation/blob/f44bb1fc2810399165115cc98b530fe4b9397c18/amcl/src/amcl_node.cpp#L793-L833, I think the init_pose_ is either [0,0,0] or [initial_pose_x,y,yaw] when set_initial_pose_ is enabled

However, I cannot find the parameters initial_cov_xx, initial_cov_yy in our code. Maybe it is removed at some point in Nav2?

So, I think

if (set_initial_pose_) {
auto msg = std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>();
msg->header.stamp = now();
msg->header.frame_id = global_frame_id_;
msg->pose.pose.position.x = initial_pose_x_;
msg->pose.pose.position.y = initial_pose_y_;
msg->pose.pose.position.z = initial_pose_z_;
msg->pose.pose.orientation = orientationAroundZAxis(initial_pose_yaw_);
initialPoseReceived(msg);
} else if (init_pose_received_on_inactive) {
handleInitialPose(last_published_pose_);
}

and
initOdometry should be called before initParticleFilter

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I am not familiar with the amcl codebase, so I may have missed something. A simpler option would be initializing init_pose_ to [0,0,0] in the constructor so that there will be no garbage value when configuring amcl

initParticleFilter();
initLaserScan();
initMessageFilters();
initPubSub();
initServices();
initOdometry();
executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
executor_->add_callback_group(callback_group_, get_node_base_interface());
executor_thread_ = std::make_unique<nav2::NodeThread>(executor_);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,7 @@ class DistanceControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTes
public:
void SetUp()
{
node_->declare_parameter("transform_tolerance", rclcpp::ParameterValue{0.1});
config_->input_ports["distance"] = 1.0;
config_->input_ports["global_frame"] = "map";
config_->input_ports["robot_base_frame"] = "base_link";
Expand Down
1 change: 1 addition & 0 deletions nav2_costmap_2d/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,7 @@
<test_depend>ament_lint_auto</test_depend>
<test_depend>nav2_map_server</test_depend>
<test_depend>ament_cmake_gtest</test_depend>
<test_depend>ament_cmake_ros</test_depend>
<test_depend>launch</test_depend>
<test_depend>launch_testing</test_depend>
<test_depend>nav2_lifecycle_manager</test_depend>
Expand Down
14 changes: 7 additions & 7 deletions nav2_costmap_2d/test/integration/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -72,7 +72,7 @@ target_link_libraries(test_costmap_subscriber_exec
nav2_costmap_2d_client
)

ament_add_test(test_collision_checker
ament_add_ros_isolated_test(test_collision_checker
GENERATE_RESULT_FOR_RETURN_CODE_ZERO
COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py"
WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
Expand All @@ -92,7 +92,7 @@ ament_add_test(footprint_tests
TEST_EXECUTABLE=$<TARGET_FILE:footprint_tests_exec>
)

ament_add_test(inflation_tests
ament_add_ros_isolated_test(inflation_tests
GENERATE_RESULT_FOR_RETURN_CODE_ZERO
COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py"
WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
Expand All @@ -102,7 +102,7 @@ ament_add_test(inflation_tests
TEST_EXECUTABLE=$<TARGET_FILE:inflation_tests_exec>
)

ament_add_test(obstacle_tests
ament_add_ros_isolated_test(obstacle_tests
GENERATE_RESULT_FOR_RETURN_CODE_ZERO
COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py"
WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
Expand All @@ -112,7 +112,7 @@ ament_add_test(obstacle_tests
TEST_EXECUTABLE=$<TARGET_FILE:obstacle_tests_exec>
)

ament_add_test(range_tests
ament_add_ros_isolated_test(range_tests
GENERATE_RESULT_FOR_RETURN_CODE_ZERO
COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py"
WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
Expand All @@ -122,7 +122,7 @@ ament_add_test(range_tests
TEST_EXECUTABLE=$<TARGET_FILE:range_tests_exec>
)

ament_add_test(plugin_container_tests
ament_add_ros_isolated_test(plugin_container_tests
GENERATE_RESULT_FOR_RETURN_CODE_ZERO
COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py"
WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
Expand All @@ -133,7 +133,7 @@ ament_add_test(plugin_container_tests
STATIC_ODOM_TO_BASE_LINK=true
)

ament_add_test(test_costmap_publisher_exec
ament_add_ros_isolated_test(test_costmap_publisher_exec
GENERATE_RESULT_FOR_RETURN_CODE_ZERO
COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py"
WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
Expand All @@ -144,7 +144,7 @@ ament_add_test(test_costmap_publisher_exec
STATIC_ODOM_TO_BASE_LINK=true
)

ament_add_test(test_costmap_subscriber_exec
ament_add_ros_isolated_test(test_costmap_subscriber_exec
GENERATE_RESULT_FOR_RETURN_CODE_ZERO
COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py"
WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
Expand Down
1 change: 1 addition & 0 deletions nav2_docking/opennav_docking/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,7 @@

<test_depend>ament_cmake_gtest</test_depend>
<test_depend>ament_cmake_pytest</test_depend>
<test_depend>ament_cmake_ros</test_depend>
<test_depend>ament_index_cpp</test_depend>
<test_depend>ament_lint_common</test_depend>
<test_depend>ament_lint_auto</test_depend>
Expand Down
8 changes: 4 additions & 4 deletions nav2_docking/opennav_docking/test/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -170,25 +170,25 @@ target_include_directories(test_docking_server_unit
)

# Test docking server (smoke)
ament_add_pytest_test(test_docking_server_with_charging_dock test_docking_server.py
ament_add_ros_isolated_pytest_test(test_docking_server_with_charging_dock test_docking_server.py
ENV
NON_CHARGING_DOCK=False
BACKWARD=False
)

ament_add_pytest_test(test_docking_server_with_non_charging_dock test_docking_server.py
ament_add_ros_isolated_pytest_test(test_docking_server_with_non_charging_dock test_docking_server.py
ENV
NON_CHARGING_DOCK=True
BACKWARD=False
)

ament_add_pytest_test(test_docking_server_backward test_docking_server.py
ament_add_ros_isolated_pytest_test(test_docking_server_backward test_docking_server.py
ENV
NON_CHARGING_DOCK=False
BACKWARD=True
)

ament_add_pytest_test(test_docking_server_backward_blind test_docking_server.py
ament_add_ros_isolated_pytest_test(test_docking_server_backward_blind test_docking_server.py
ENV
NON_CHARGING_DOCK=False
BACKWARD_BLIND=True
Expand Down
48 changes: 22 additions & 26 deletions nav2_docking/opennav_docking/test/test_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -291,8 +291,7 @@ TEST(ControllerTests, CollisionCheckerDockForward) {
node, tf, "test_base_frame", "test_base_frame");
collision_tester->configure();
collision_tester->activate();
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(node->get_node_base_interface());
auto executor_thread = std::make_unique<nav2::NodeThread>(node->get_node_base_interface());

// Set the pose of the dock at 1.75m in front of the robot
auto dock_pose = collision_tester->setPose(1.75, 0.0, 0.0);
Expand All @@ -304,23 +303,23 @@ TEST(ControllerTests, CollisionCheckerDockForward) {
// Publish an empty costmap
// It should not hit anything in an empty costmap
collision_tester->publishCostmap();
executor.spin_some();
std::this_thread::sleep_for(std::chrono::milliseconds(50));
EXPECT_TRUE(controller->isTrajectoryCollisionFree(dock_pose, true, false));

// Set a dock in the costmap of 0.2x1.5m at 2m in front of the robot
// It should hit the dock because the robot is 0.5m wide and the dock pose is at 1.75
// But it does not hit because the collision tolerance is 0.3m
collision_tester->setRectangle(0.2, 1.5, 2.0, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE);
collision_tester->publishCostmap();
executor.spin_some();
std::this_thread::sleep_for(std::chrono::milliseconds(50));
EXPECT_TRUE(controller->isTrajectoryCollisionFree(dock_pose, true, false));

// Set an object between the robot and the dock
// It should hit the object
collision_tester->clearCostmap();
collision_tester->setRectangle(0.2, 0.2, 1.0, -0.1, nav2_costmap_2d::LETHAL_OBSTACLE);
collision_tester->publishCostmap();
executor.spin_some();
std::this_thread::sleep_for(std::chrono::milliseconds(50));
EXPECT_FALSE(controller->isTrajectoryCollisionFree(dock_pose, true, false));

// Set the collision tolerance to 0 to ensure all obstacles in the path are detected
Expand All @@ -331,7 +330,7 @@ TEST(ControllerTests, CollisionCheckerDockForward) {
collision_tester->clearCostmap();
collision_tester->setRectangle(0.2, 1.5, 2.0, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE);
collision_tester->publishCostmap();
executor.spin_some();
std::this_thread::sleep_for(std::chrono::milliseconds(50));
EXPECT_FALSE(controller->isTrajectoryCollisionFree(dock_pose, true, false));

collision_tester->deactivate();
Expand All @@ -358,8 +357,7 @@ TEST(ControllerTests, CollisionCheckerDockBackward) {
node, tf, "test_base_frame", "test_base_frame");
collision_tester->configure();
collision_tester->activate();
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(node->get_node_base_interface());
auto executor_thread = std::make_unique<nav2::NodeThread>(node->get_node_base_interface());

// Set the pose of the dock at 1.75m behind the robot
auto dock_pose = collision_tester->setPose(-1.75, 0.0, 0.0);
Expand All @@ -371,23 +369,23 @@ TEST(ControllerTests, CollisionCheckerDockBackward) {
// Publish an empty costmap
// It should not hit anything in an empty costmap
collision_tester->publishCostmap();
executor.spin_some();
std::this_thread::sleep_for(std::chrono::milliseconds(50));
EXPECT_TRUE(controller->isTrajectoryCollisionFree(dock_pose, true, true));

// Set a dock in the costmap of 0.2x1.5m at 2m behind the robot
// It should hit the dock because the robot is 0.5m wide and the dock pose is at -1.75
// But it does not hit because the collision tolerance is 0.3m
collision_tester->setRectangle(0.2, 1.5, -2.1, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE);
collision_tester->publishCostmap();
executor.spin_some();
std::this_thread::sleep_for(std::chrono::milliseconds(50));
EXPECT_TRUE(controller->isTrajectoryCollisionFree(dock_pose, true, true));

// Set an object between the robot and the dock
// It should hit the object
collision_tester->clearCostmap();
collision_tester->setRectangle(0.2, 0.2, -1.0, 0.0, nav2_costmap_2d::LETHAL_OBSTACLE);
collision_tester->publishCostmap();
executor.spin_some();
std::this_thread::sleep_for(std::chrono::milliseconds(50));
EXPECT_FALSE(controller->isTrajectoryCollisionFree(dock_pose, true, true));

// Set the collision tolerance to 0 to ensure all obstacles in the path are detected
Expand All @@ -398,7 +396,7 @@ TEST(ControllerTests, CollisionCheckerDockBackward) {
collision_tester->clearCostmap();
collision_tester->setRectangle(0.2, 1.5, -2.1, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE);
collision_tester->publishCostmap();
executor.spin_some();
std::this_thread::sleep_for(std::chrono::milliseconds(50));
EXPECT_FALSE(controller->isTrajectoryCollisionFree(dock_pose, true, true));

collision_tester->deactivate();
Expand All @@ -425,8 +423,7 @@ TEST(ControllerTests, CollisionCheckerUndockBackward) {
node, tf, "test_base_frame", "test_base_frame");
collision_tester->configure();
collision_tester->activate();
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(node->get_node_base_interface());
auto executor_thread = std::make_unique<nav2::NodeThread>(node->get_node_base_interface());

// Set the staging pose at 1.75m behind the robot
auto staging_pose = collision_tester->setPose(-1.75, 0.0, 0.0);
Expand All @@ -438,31 +435,31 @@ TEST(ControllerTests, CollisionCheckerUndockBackward) {
// Publish an empty costmap
// It should not hit anything in an empty costmap
collision_tester->publishCostmap();
executor.spin_some();
std::this_thread::sleep_for(std::chrono::milliseconds(50));
EXPECT_TRUE(controller->isTrajectoryCollisionFree(staging_pose, false, true));

// Set a dock in the costmap of 0.2x1.5m in front of the robot. The robot is docked
// It should hit the dock because the robot is 0.5m wide and the robot pose is at 1.75
// But it does not hit because the collision tolerance is 0.3m
collision_tester->setRectangle(0.2, 1.5, 0.25, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE);
collision_tester->publishCostmap();
executor.spin_some();
std::this_thread::sleep_for(std::chrono::milliseconds(50));
EXPECT_TRUE(controller->isTrajectoryCollisionFree(staging_pose, false, true));

// Set an object beyond the staging pose
// It should hit the object
collision_tester->clearCostmap();
collision_tester->setRectangle(0.2, 0.2, -1.75 - 0.5, -0.1, nav2_costmap_2d::LETHAL_OBSTACLE);
collision_tester->publishCostmap();
executor.spin_some();
std::this_thread::sleep_for(std::chrono::milliseconds(50));
EXPECT_FALSE(controller->isTrajectoryCollisionFree(staging_pose, false, true));

// Set an object between the robot and the staging pose
// It should hit the object
collision_tester->clearCostmap();
collision_tester->setRectangle(0.2, 0.2, -1.0, -0.1, nav2_costmap_2d::LETHAL_OBSTACLE);
collision_tester->publishCostmap();
executor.spin_some();
std::this_thread::sleep_for(std::chrono::milliseconds(50));
EXPECT_FALSE(controller->isTrajectoryCollisionFree(staging_pose, false, true));

// Set the collision tolerance to 0 to ensure all obstacles in the path are detected
Expand All @@ -473,7 +470,7 @@ TEST(ControllerTests, CollisionCheckerUndockBackward) {
collision_tester->clearCostmap();
collision_tester->setRectangle(0.2, 1.5, 0.25, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE);
collision_tester->publishCostmap();
executor.spin_some();
std::this_thread::sleep_for(std::chrono::milliseconds(50));
EXPECT_FALSE(controller->isTrajectoryCollisionFree(staging_pose, false, true));

collision_tester->deactivate();
Expand All @@ -500,8 +497,7 @@ TEST(ControllerTests, CollisionCheckerUndockForward) {
node, tf, "test_base_frame", "test_base_frame");
collision_tester->configure();
collision_tester->activate();
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(node->get_node_base_interface());
auto executor_thread = std::make_unique<nav2::NodeThread>(node->get_node_base_interface());

// Set the staging pose at 1.75m in the front of the robot
auto staging_pose = collision_tester->setPose(1.75, 0.0, 0.0);
Expand All @@ -513,30 +509,30 @@ TEST(ControllerTests, CollisionCheckerUndockForward) {
// Publish an empty costmap
// It should not hit anything in an empty costmap
collision_tester->publishCostmap();
executor.spin_some();
std::this_thread::sleep_for(std::chrono::milliseconds(50));
EXPECT_TRUE(controller->isTrajectoryCollisionFree(staging_pose, false, false));

// Set a dock in the costmap of 0.2x1.5m at 0.5m behind the robot. The robot is docked
// It should not hit anything because the robot is docked and the trajectory is backward
collision_tester->setRectangle(0.2, 1.5, -0.35, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE);
collision_tester->publishCostmap();
executor.spin_some();
std::this_thread::sleep_for(std::chrono::milliseconds(50));
EXPECT_TRUE(controller->isTrajectoryCollisionFree(staging_pose, false, false));

// Set an object beyond the staging pose
// It should hit the object
collision_tester->clearCostmap();
collision_tester->setRectangle(0.2, 0.3, 1.75 + 0.5, 0.0, nav2_costmap_2d::LETHAL_OBSTACLE);
collision_tester->publishCostmap();
executor.spin_some();
std::this_thread::sleep_for(std::chrono::milliseconds(50));
EXPECT_FALSE(controller->isTrajectoryCollisionFree(staging_pose, false, false));

// Set an object between the robot and the staging pose
// It should hit the object
collision_tester->clearCostmap();
collision_tester->setRectangle(0.2, 0.2, 1.0, 0.0, nav2_costmap_2d::LETHAL_OBSTACLE);
collision_tester->publishCostmap();
executor.spin_some();
std::this_thread::sleep_for(std::chrono::milliseconds(50));
EXPECT_FALSE(controller->isTrajectoryCollisionFree(staging_pose, false, false));

// Set the collision tolerance to 0 to ensure all obstacles in the path are detected
Expand All @@ -547,7 +543,7 @@ TEST(ControllerTests, CollisionCheckerUndockForward) {
collision_tester->clearCostmap();
collision_tester->setRectangle(0.2, 1.5, -0.35, -0.75, nav2_costmap_2d::LETHAL_OBSTACLE);
collision_tester->publishCostmap();
executor.spin_some();
std::this_thread::sleep_for(std::chrono::milliseconds(50));
EXPECT_FALSE(controller->isTrajectoryCollisionFree(staging_pose, false, false));

collision_tester->deactivate();
Expand Down
1 change: 1 addition & 0 deletions nav2_following/opennav_following/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,7 @@

<test_depend>ament_cmake_gtest</test_depend>
<test_depend>ament_cmake_pytest</test_depend>
<test_depend>ament_cmake_ros</test_depend>
<test_depend>ament_index_cpp</test_depend>
<test_depend>ament_lint_common</test_depend>
<test_depend>ament_lint_auto</test_depend>
Expand Down
8 changes: 4 additions & 4 deletions nav2_following/opennav_following/test/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -12,22 +12,22 @@ target_link_libraries(test_following_server_unit
)

# Test following server (smoke)
ament_add_pytest_test(test_following_server_topic test_following_server.py
ament_add_ros_isolated_pytest_test(test_following_server_topic test_following_server.py
ENV
FOLLOWING_MODE=topic
)

ament_add_pytest_test(test_following_server_frame test_following_server.py
ament_add_ros_isolated_pytest_test(test_following_server_frame test_following_server.py
ENV
FOLLOWING_MODE=frame
)

ament_add_pytest_test(test_following_server_skip_pose test_following_server.py
ament_add_ros_isolated_pytest_test(test_following_server_skip_pose test_following_server.py
ENV
FOLLOWING_MODE=skip_pose
)

ament_add_pytest_test(test_following_server_search test_following_server.py
ament_add_ros_isolated_pytest_test(test_following_server_search test_following_server.py
ENV
FOLLOWING_MODE=search
)
1 change: 1 addition & 0 deletions nav2_lifecycle_manager/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,7 @@
<test_depend>ament_cmake_gtest</test_depend>
<test_depend>ament_cmake_pytest</test_depend>
<test_depend>rclcpp_lifecycle</test_depend>
<test_depend>ament_cmake_ros</test_depend>

<export>
<build_type>ament_cmake</build_type>
Expand Down
Loading
Loading