Add beluga_ros package (#270)

This patch creates a `beluga_ros` package in order to decouple common functionality to interface with ROS from `beluga_amcl`. The goal is to reduce the boilerplate needed to create a new ROS localization package using `beluga`. Signed-off-by: Nahuel Espinosa <[email protected]>
Ekumen-OS · Dec 11, 2023 · e51ac33 · e51ac33
1 parent bac478d
commit e51ac33
Show file tree

Hide file tree

Showing 31 changed files with 609 additions and 164 deletions.
diff --git a/.codecov.yml b/.codecov.yml
@@ -29,6 +29,11 @@ component_management:
       paths:
         - beluga_amcl/**
 
+    - component_id: package_beluga_ros
+      name: beluga_ros
+      paths:
+        - beluga_ros/**
+
 coverage:
   # Disable project and patch level coverage status check in favor of
   # individual component status checks.
@@ -49,4 +54,5 @@ github_checks:
 ignore:
   - "beluga/test"
   - "beluga_amcl/test"
+  - "beluga_ros/test"
   - "beluga_system_tests/test"
diff --git a/beluga_amcl/cmake/ament.cmake b/beluga_amcl/cmake/ament.cmake
@@ -14,17 +14,13 @@
 
 find_package(ament_cmake REQUIRED)
 find_package(beluga REQUIRED)
+find_package(beluga_ros REQUIRED)
 find_package(bondcpp REQUIRED)
-find_package(geometry_msgs REQUIRED)
 find_package(nav2_msgs REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_components REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
-find_package(sensor_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
-find_package(tf2 REQUIRED)
-find_package(tf2_eigen REQUIRED)
-find_package(tf2_geometry_msgs REQUIRED)
 
 add_library(amcl_node_utils SHARED)
 target_sources(amcl_node_utils PRIVATE src/amcl_node_utils.cpp
@@ -33,15 +29,7 @@ target_include_directories(
   amcl_node_utils PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
                          $<INSTALL_INTERFACE:include/${PROJECT_NAME}>)
 target_link_libraries(amcl_node_utils PUBLIC beluga::beluga)
-ament_target_dependencies(
-  amcl_node_utils
-  PUBLIC geometry_msgs
-         nav_msgs
-         sensor_msgs
-         tf2
-         tf2_eigen
-         tf2_geometry_msgs)
-target_compile_definitions(amcl_node_utils PUBLIC BELUGA_AMCL_ROS_VERSION=2)
+ament_target_dependencies(amcl_node_utils PUBLIC beluga_ros nav2_msgs)
 target_compile_features(amcl_node_utils PUBLIC cxx_std_17)
 
 add_library(amcl_node_component SHARED)
@@ -51,13 +39,12 @@ target_compile_features(amcl_node_component PUBLIC cxx_std_17)
 target_link_libraries(amcl_node_component PUBLIC beluga::beluga amcl_node_utils)
 ament_target_dependencies(
   amcl_node_component
-  PUBLIC bondcpp
-         nav_msgs
-         nav2_msgs
+  PUBLIC beluga
+         beluga_ros
+         bondcpp
          rclcpp
          rclcpp_components
          rclcpp_lifecycle
-         sensor_msgs
          std_srvs)
 rclcpp_components_register_node(
   amcl_node_component
@@ -66,17 +53,14 @@ rclcpp_components_register_node(
 
 ament_export_dependencies(
   beluga
+  beluga_ros
   bondcpp
   nav2_msgs
   rclcpp
   rclcpp_components
   rclcpp_lifecycle
-  sensor_msgs
-  tf2
-  tf2_eigen
-  tf2_geometry_msgs)
+  std_srvs)
 ament_export_include_directories("include/${PROJECT_NAME}")
-ament_export_definitions(BELUGA_AMCL_ROS_VERSION=2)
 ament_export_targets(amcl_node_utils HAS_LIBRARY_TARGET)
 
 install(

diff --git a/beluga_amcl/cmake/catkin.cmake b/beluga_amcl/cmake/catkin.cmake
@@ -18,37 +18,28 @@ find_package(beluga REQUIRED)
 find_package(
   catkin REQUIRED
   COMPONENTS bondcpp
+             beluga_ros
              diagnostic_updater
              dynamic_reconfigure
              message_filters
-             nav_msgs
              nodelet
              roscpp
-             sensor_msgs
-             std_srvs
-             tf2
-             tf2_geometry_msgs
-             tf2_msgs
-             tf2_ros)
+             std_srvs)
 
 generate_dynamic_reconfigure_options(config/Amcl.cfg)
 
 catkin_package(
   CATKIN_DEPENDS
     bondcpp
-    nav_msgs
+    beluga_ros
     roscpp
-    sensor_msgs
-    tf2
-    tf2_eigen
-    tf2_geometry_msgs
+    std_srvs
   DEPENDS beluga
   INCLUDE_DIRS include
-  LIBRARIES ${PROJECT_NAME}
-  CFG_EXTRAS ${PROJECT_NAME}-extras.cmake)
+  LIBRARIES ${PROJECT_NAME})
 
-add_compile_definitions(BELUGA_AMCL_ROS_VERSION=1)
 include_directories(include ${catkin_INCLUDE_DIRS})
+add_definitions(${catkin_DEFINITIONS})
 
 add_library(${PROJECT_NAME} SHARED)
 target_sources(${PROJECT_NAME} PRIVATE src/amcl_node_utils.cpp

diff --git a/beluga_amcl/include/beluga_amcl/amcl_node_utils.hpp b/beluga_amcl/include/beluga_amcl/amcl_node_utils.hpp
@@ -18,7 +18,7 @@
 #include <utility>
 #include <vector>
 
-#include <beluga_amcl/ros_interfaces.hpp>
+#include <beluga_ros/messages.hpp>
 
 #include <sophus/se3.hpp>
 
@@ -46,7 +46,7 @@ inline Eigen::IOFormat make_eigen_comma_format() {
  * \return A vector of pairs of coordinates in the robot's reference frame.
  */
 std::vector<std::pair<double, double>> make_points_from_laser_scan(
-    const beluga_amcl::messages::LaserScan& laser_scan,
+    const beluga_ros::msg::LaserScan& laser_scan,
     const Sophus::SE3d& laser_transform,
     std::size_t max_beam_count,
     float range_min,

diff --git a/beluga_amcl/include/beluga_amcl/filter_update_control/filter_update_control_mixin.hpp b/beluga_amcl/include/beluga_amcl/filter_update_control/filter_update_control_mixin.hpp
@@ -19,7 +19,7 @@
 #include <utility>
 #include <vector>
 
-#include <beluga_amcl/tf2_sophus.hpp>
+#include <beluga_ros/tf2_sophus.hpp>
 
 #include <ciabatta/ciabatta.hpp>
 

diff --git a/beluga_amcl/include/beluga_amcl/particle_filtering.hpp b/beluga_amcl/include/beluga_amcl/particle_filtering.hpp
@@ -28,7 +28,7 @@
 #include <beluga_amcl/filter_update_control/selective_resampling_policy.hpp>
 #include <beluga_amcl/filter_update_control/update_filter_when_moving_policy.hpp>
 
-#include <beluga_amcl/occupancy_grid.hpp>
+#include <beluga_ros/occupancy_grid.hpp>
 
 #include <sophus/se2.hpp>
 
@@ -41,7 +41,8 @@ static constexpr std::string_view kStationaryModelName = "stationary";
 static constexpr std::string_view kLikelihoodFieldModelName = "likelihood_field";
 static constexpr std::string_view kBeamSensorModelName = "beam";
 
-using LaserLocalizationInterface2d = beluga::LaserLocalizationInterface2d<OccupancyGrid, FilterUpdateControlInterface>;
+using LaserLocalizationInterface2d =
+    beluga::LaserLocalizationInterface2d<beluga_ros::OccupancyGrid, FilterUpdateControlInterface>;
 
 using Stationary = beluga::mixin::descriptor<beluga::StationaryModel>;
 using DifferentialDrive =
@@ -51,11 +52,12 @@ using OmnidirectionalDrive =
 
 using MotionDescriptor = std::variant<Stationary, DifferentialDrive, OmnidirectionalDrive>;
 
-using LikelihoodField = beluga::mixin::
-    descriptor<ciabatta::curry<beluga::LikelihoodFieldModel, OccupancyGrid>::mixin, beluga::LikelihoodFieldModelParam>;
+using LikelihoodField = beluga::mixin::descriptor<
+    ciabatta::curry<beluga::LikelihoodFieldModel, beluga_ros::OccupancyGrid>::mixin,
+    beluga::LikelihoodFieldModelParam>;
 
-using BeamSensorModel =
-    beluga::mixin::descriptor<ciabatta::curry<beluga::BeamSensorModel, OccupancyGrid>::mixin, beluga::BeamModelParam>;
+using BeamSensorModel = beluga::mixin::
+    descriptor<ciabatta::curry<beluga::BeamSensorModel, beluga_ros::OccupancyGrid>::mixin, beluga::BeamModelParam>;
 
 using SensorDescriptor = std::variant<LikelihoodField, BeamSensorModel>;
 
@@ -67,15 +69,15 @@ template <class MotionDescriptor, class SensorDescriptor>
 using MonteCarloLocalization2d = beluga::MonteCarloLocalization2d<
     MotionDescriptor,
     SensorDescriptor,
-    beluga_amcl::OccupancyGrid,
+    beluga_ros::OccupancyGrid,
     FilterUpdateControlInterface,
     ConcreteResamplingPoliciesPoller>;
 
 template <class MotionDescriptor, class SensorDescriptor>
 using AdaptiveMonteCarloLocalization2d = beluga::AdaptiveMonteCarloLocalization2d<
     MotionDescriptor,
     SensorDescriptor,
-    OccupancyGrid,
+    beluga_ros::OccupancyGrid,
     FilterUpdateControlInterface,
     ConcreteResamplingPoliciesPoller>;
 

diff --git a/beluga_amcl/include/beluga_amcl/private/amcl_node.hpp b/beluga_amcl/include/beluga_amcl/private/amcl_node.hpp
@@ -32,7 +32,6 @@
 #include <sensor_msgs/msg/laser_scan.hpp>
 #include <std_srvs/srv/empty.hpp>
 
-#include "beluga_amcl/occupancy_grid.hpp"
 #include "beluga_amcl/particle_filtering.hpp"
 #include "beluga_amcl/private/execution_policy.hpp"
 

diff --git a/beluga_amcl/package.xml b/beluga_amcl/package.xml
@@ -8,6 +8,7 @@
 
   <maintainer email="[email protected]">Gerardo Puga</maintainer>
   <maintainer email="[email protected]">Ivan Paunovic</maintainer>
+  <maintainer email="[email protected]">Michel Hidalgo</maintainer>
   <maintainer email="[email protected]">Nahuel Espinosa</maintainer>
 
   <license>Apache License 2.0</license>
@@ -16,9 +17,9 @@
   <buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
 
   <depend>beluga</depend>
-  <depend>geometry_msgs</depend>
+  <depend>beluga_ros</depend>
   <depend>message_filters</depend>
-  <depend>nav_msgs</depend>
+  <depend>std_srvs</depend>
 
   <depend condition="$ROS_VERSION == 1">diagnostic_updater</depend>
   <depend condition="$ROS_VERSION == 1">dynamic_reconfigure</depend>
@@ -30,13 +31,6 @@
   <depend condition="$ROS_VERSION == 2">rclcpp_components</depend>
   <depend condition="$ROS_VERSION == 2">rclcpp_lifecycle</depend>
 
-  <depend>sensor_msgs</depend>
-  <depend>std_srvs</depend>
-  <depend>tf2</depend>
-  <depend>tf2_eigen</depend>
-  <depend>tf2_geometry_msgs</depend>
-  <depend>tf2_ros</depend>
-
   <test_depend condition="$ROS_VERSION == 1">rostest</test_depend>
   <test_depend condition="$ROS_VERSION == 2">ament_cmake_gmock</test_depend>
   <test_depend condition="$ROS_VERSION == 2">ament_cmake_gtest</test_depend>

diff --git a/beluga_amcl/src/amcl_node.cpp b/beluga_amcl/src/amcl_node.cpp
@@ -37,12 +37,11 @@
 #include <range/v3/algorithm/transform.hpp>
 #include <range/v3/range/conversion.hpp>
 #include <rclcpp_components/register_node_macro.hpp>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
 #include "beluga_amcl/amcl_node_utils.hpp"
-#include "beluga_amcl/occupancy_grid.hpp"
 #include "beluga_amcl/private/execution_policy.hpp"
-#include "beluga_amcl/tf2_sophus.hpp"
+#include "beluga_ros/occupancy_grid.hpp"
+#include "beluga_ros/tf2_sophus.hpp"
 
 namespace beluga_amcl {
 
@@ -716,10 +715,11 @@ std::unique_ptr<LaserLocalizationInterface2d> AmclNode::make_particle_filter(
   };
 
   using LikelihoodField = beluga::mixin::descriptor<
-      ciabatta::curry<beluga::LikelihoodFieldModel, OccupancyGrid>::mixin, beluga::LikelihoodFieldModelParam>;
+      ciabatta::curry<beluga::LikelihoodFieldModel, beluga_ros::OccupancyGrid>::mixin,
+      beluga::LikelihoodFieldModelParam>;
 
-  using BeamSensorModel =
-      beluga::mixin::descriptor<ciabatta::curry<beluga::BeamSensorModel, OccupancyGrid>::mixin, beluga::BeamModelParam>;
+  using BeamSensorModel = beluga::mixin::descriptor<
+      ciabatta::curry<beluga::BeamSensorModel, beluga_ros::OccupancyGrid>::mixin, beluga::BeamModelParam>;
 
   using SensorDescriptor = std::variant<LikelihoodField, BeamSensorModel>;
   auto get_sensor_descriptor = [this](std::string_view name) -> SensorDescriptor {
@@ -750,7 +750,7 @@ std::unique_ptr<LaserLocalizationInterface2d> AmclNode::make_particle_filter(
     using beluga::mixin::make_mixin;
     return make_mixin<LaserLocalizationInterface2d, AdaptiveMonteCarloLocalization2d>(
         sampler_params, limiter_params, get_motion_descriptor(get_parameter("robot_model_type").as_string()),
-        get_sensor_descriptor(get_parameter("laser_model_type").as_string()), OccupancyGrid{map},
+        get_sensor_descriptor(get_parameter("laser_model_type").as_string()), beluga_ros::OccupancyGrid{map},
         resample_on_motion_params, resample_interval_params, selective_resampling_params);
   } catch (const std::invalid_argument& error) {
     RCLCPP_ERROR(get_logger(), "Coudn't instantiate the particle filter: %s", error.what());
@@ -782,7 +782,7 @@ void AmclNode::map_callback(nav_msgs::msg::OccupancyGrid::SharedPtr map) {
       should_reset_initial_pose = false;
     }
   } else {
-    particle_filter_->update_map(OccupancyGrid{std::move(map)});
+    particle_filter_->update_map(beluga_ros::OccupancyGrid{std::move(map)});
     should_reset_initial_pose = get_parameter("always_reset_initial_pose").as_bool();
   }
 

diff --git a/beluga_amcl/src/amcl_node_utils.cpp b/beluga_amcl/src/amcl_node_utils.cpp
@@ -17,7 +17,7 @@
 namespace beluga_amcl::utils {
 
 std::vector<std::pair<double, double>> make_points_from_laser_scan(
-    const beluga_amcl::messages::LaserScan& laser_scan,
+    const beluga_ros::msg::LaserScan& laser_scan,
     const Sophus::SE3d& laser_transform,
     std::size_t max_beam_count,
     float range_min,

diff --git a/beluga_amcl/src/amcl_nodelet.cpp b/beluga_amcl/src/amcl_nodelet.cpp
@@ -26,12 +26,12 @@
 
 #include <range/v3/algorithm/transform.hpp>
 
-#include "beluga_amcl/private/amcl_nodelet.hpp"
-
 #include "beluga_amcl/amcl_node_utils.hpp"
-#include "beluga_amcl/occupancy_grid.hpp"
+#include "beluga_amcl/private/amcl_nodelet.hpp"
 #include "beluga_amcl/private/execution_policy.hpp"
-#include "beluga_amcl/tf2_sophus.hpp"
+
+#include "beluga_ros/occupancy_grid.hpp"
+#include "beluga_ros/tf2_sophus.hpp"
 
 // LCOV_EXCL_BR_START: Disable branch coverage.
 
@@ -245,7 +245,7 @@ std::unique_ptr<LaserLocalizationInterface2d> AmclNodelet::make_particle_filter(
     using beluga::mixin::make_mixin;
     return make_mixin<LaserLocalizationInterface2d, AdaptiveMonteCarloLocalization2d>(
         sampler_params, limiter_params, get_motion_descriptor(config_.odom_model_type),
-        get_sensor_descriptor(config_.laser_model_type), OccupancyGrid{map}, resample_on_motion_params,
+        get_sensor_descriptor(config_.laser_model_type), beluga_ros::OccupancyGrid{map}, resample_on_motion_params,
         resample_interval_params, selective_resampling_params);
   } catch (const std::invalid_argument& error) {
     NODELET_ERROR("Could not instantiate the particle filter: %s", error.what());
@@ -301,7 +301,7 @@ bool AmclNodelet::set_map_callback(nav_msgs::SetMap::Request& request, nav_msgs:
   if (!particle_filter_) {
     particle_filter_ = make_particle_filter(map);
   } else {
-    particle_filter_->update_map(OccupancyGrid{map});
+    particle_filter_->update_map(beluga_ros::OccupancyGrid{map});
   }
   last_known_map_ = map;
 
@@ -343,7 +343,7 @@ void AmclNodelet::handle_map_with_default_initial_pose(const nav_msgs::Occupancy
     particle_filter_ = make_particle_filter(map);
     should_reset_initial_pose |= !last_known_estimate_.has_value();
   } else {
-    particle_filter_->update_map(OccupancyGrid{map});
+    particle_filter_->update_map(beluga_ros::OccupancyGrid{map});
     should_reset_initial_pose = config_.always_reset_initial_pose;
   }
 

diff --git a/beluga_amcl/test/test_amcl_node.cpp b/beluga_amcl/test/test_amcl_node.cpp
@@ -16,7 +16,7 @@
 #include <tf2_ros/create_timer_ros.h>
 
 #include <beluga_amcl/private/amcl_node.hpp>
-#include <beluga_amcl/tf2_sophus.hpp>
+#include <beluga_ros/tf2_sophus.hpp>
 
 #include <lifecycle_msgs/msg/state.hpp>