Filters as ROS2 components

CMakeLists.txt

@@ -17,6 +17,7 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(angles REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
 find_package(diagnostic_msgs REQUIRED)
 find_package(diagnostic_updater REQUIRED)
 find_package(geographic_msgs REQUIRED)
@@ -64,6 +65,7 @@ include_directories(
 
 add_library(
   ${library_name}
+  SHARED
   src/ekf.cpp
   src/ukf.cpp
   src/filter_base.cpp
@@ -72,7 +74,8 @@ add_library(
   src/robot_localization_estimator.cpp
   src/ros_filter.cpp
   src/ros_filter_utilities.cpp
-  src/ros_robot_localization_listener.cpp)
+  src/ros_robot_localization_listener.cpp
+  src/ros_components.cpp)
 
 rosidl_get_typesupport_target(cpp_typesupport_target "${PROJECT_NAME}" "rosidl_typesupport_cpp")
 target_link_libraries(${library_name} "${cpp_typesupport_target}")
@@ -163,6 +166,16 @@ ament_target_dependencies(
   rclcpp
 )
 
+rclcpp_components_register_nodes(
+  ${library_name} 
+  "robot_localization::UkfFilter"
+)
+
+rclcpp_components_register_nodes(
+  ${library_name} 
+  "robot_localization::EkfFilter"
+)
+
 if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)
   find_package(ament_cmake_cppcheck REQUIRED)

include/robot_localization/ros_components.hpp

@@ -0,0 +1,52 @@
+/*
+ * Copyright (c) 2022, Patrick Roncagliolo
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * 3. Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef ROBOT_LOCALIZATION__ROS_COMPONENTS_HPP_
+#define ROBOT_LOCALIZATION__ROS_COMPONENTS_HPP_
+
+#include "robot_localization/ros_filter_types.hpp"
+
+namespace robot_localization
+{
+
+class EkfFilter : public RosEkf{
+public:
+    explicit EkfFilter(const rclcpp::NodeOptions & options);
+};
+
+class UkfFilter : public RosUkf{
+public:
+    explicit UkfFilter(const rclcpp::NodeOptions & options);
+};
+
+}
+#endif  // ROBOT_LOCALIZATION__ROS_COMPONENTS_HPP_

package.xml

@@ -31,6 +31,7 @@
   <depend>angles</depend>
   <build_depend>rclcpp</build_depend>
   <build_depend>rmw_implementation</build_depend>
+  <depend>rclcpp_components</depend>
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
   <depend>std_srvs</depend>

src/ros_components.cpp

@@ -0,0 +1,66 @@
+/*
+ * Copyright (c) 2022, Patrick Roncagliolo
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * 3. Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <robot_localization/ros_components.hpp>
+
+namespace robot_localization{
+   rclcpp::NodeOptions _option_helper(std::string node_name, const rclcpp::NodeOptions & options){
+      rclcpp::NodeOptions o = options;
+      std::vector<std::string> a;
+      a = o.arguments();
+      a.insert(a.begin(), node_name);
+      o.arguments(a);
+      return o;
+   }
+
+   EkfFilter::EkfFilter(const rclcpp::NodeOptions & options)
+   : RosEkf(_option_helper("ekf_filter_node", options)){
+      initialize();
+   }
+
+   UkfFilter::UkfFilter(const rclcpp::NodeOptions & options)
+   : RosUkf(_option_helper("ukf_filter_node", options)){
+      double alpha = declare_parameter("alpha", 0.001);
+      double kappa = declare_parameter("kappa", 0.0);
+      double beta = declare_parameter("beta", 2.0);
+      getFilter().setConstants(alpha, kappa, beta);
+      initialize();
+   }
+}
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+// Register the component with class_loader.
+// This acts as a sort of entry point, allowing the component to be discoverable when its library
+// is being loaded into a running process.
+RCLCPP_COMPONENTS_REGISTER_NODE(robot_localization::EkfFilter)
+RCLCPP_COMPONENTS_REGISTER_NODE(robot_localization::UkfFilter)

src/ros_filter.cpp

@@ -1941,11 +1941,11 @@ template<typename T>
 void RosFilter<T>::initialize()
 {
   diagnostic_updater_ = std::make_unique<diagnostic_updater::Updater>(
-    shared_from_this());
+    this);
   diagnostic_updater_->setHardwareID("none");
 
   world_transform_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(
-    shared_from_this());
+    this);
 
   loadParams();