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();