diff --git a/CMakeLists.txt b/CMakeLists.txt
index b72552137..445f0f138 100644
--- a/CMakeLists.txt
+++ b/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}")
@@ -121,6 +124,7 @@ ament_target_dependencies(
tf2_geometry_msgs
tf2_ros
yaml_cpp_vendor
+ rclcpp_components
)
target_link_libraries(
@@ -163,6 +167,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)
diff --git a/include/robot_localization/ros_components.hpp b/include/robot_localization/ros_components.hpp
new file mode 100644
index 000000000..d5887a4b1
--- /dev/null
+++ b/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_
diff --git a/package.xml b/package.xml
index eb11a5103..bc35197cc 100644
--- a/package.xml
+++ b/package.xml
@@ -31,6 +31,7 @@
angles
rclcpp
rmw_implementation
+ rclcpp_components
sensor_msgs
std_msgs
std_srvs
diff --git a/src/ros_components.cpp b/src/ros_components.cpp
new file mode 100644
index 000000000..4bbb7ef10
--- /dev/null
+++ b/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
+
+namespace robot_localization{
+ rclcpp::NodeOptions _option_helper(std::string node_name, const rclcpp::NodeOptions & options){
+ rclcpp::NodeOptions o = options;
+ std::vector 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)
\ No newline at end of file
diff --git a/src/ros_filter.cpp b/src/ros_filter.cpp
index 4da645baf..98aae114e 100644
--- a/src/ros_filter.cpp
+++ b/src/ros_filter.cpp
@@ -1941,11 +1941,11 @@ template
void RosFilter::initialize()
{
diagnostic_updater_ = std::make_unique(
- shared_from_this());
+ this);
diagnostic_updater_->setHardwareID("none");
world_transform_broadcaster_ = std::make_shared(
- shared_from_this());
+ this);
loadParams();