diff --git a/localization/gyro_odometer/CMakeLists.txt b/localization/gyro_odometer/CMakeLists.txt index 57589837dd529..a832383ff4761 100644 --- a/localization/gyro_odometer/CMakeLists.txt +++ b/localization/gyro_odometer/CMakeLists.txt @@ -4,17 +4,12 @@ project(gyro_odometer) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(${PROJECT_NAME} - src/gyro_odometer_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/gyro_odometer_core.cpp ) target_link_libraries(${PROJECT_NAME} fmt) -ament_auto_add_library(gyro_odometer_node SHARED - src/gyro_odometer_core.cpp -) - if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_gyro_odometer test/test_main.cpp @@ -25,10 +20,15 @@ if(BUILD_TESTING) rclcpp ) target_link_libraries(test_gyro_odometer - gyro_odometer_node + ${PROJECT_NAME} ) endif() +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::gyro_odometer::GyroOdometerNode" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) ament_auto_package(INSTALL_TO_SHARE launch diff --git a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp index 3a6358e62c21a..2926589bd2da9 100644 --- a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp +++ b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp @@ -36,14 +36,17 @@ #include #include -class GyroOdometer : public rclcpp::Node +namespace autoware::gyro_odometer +{ + +class GyroOdometerNode : public rclcpp::Node { private: using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; public: - explicit GyroOdometer(const rclcpp::NodeOptions & options); - ~GyroOdometer(); + explicit GyroOdometerNode(const rclcpp::NodeOptions & node_options); + ~GyroOdometerNode(); private: void callbackVehicleTwist( @@ -75,4 +78,6 @@ class GyroOdometer : public rclcpp::Node std::deque gyro_queue_; }; +} // namespace autoware::gyro_odometer + #endif // GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ diff --git a/localization/gyro_odometer/launch/gyro_odometer.launch.xml b/localization/gyro_odometer/launch/gyro_odometer.launch.xml index 21aff3e10d26c..aed6050858fe1 100644 --- a/localization/gyro_odometer/launch/gyro_odometer.launch.xml +++ b/localization/gyro_odometer/launch/gyro_odometer.launch.xml @@ -11,7 +11,7 @@ - + diff --git a/localization/gyro_odometer/package.xml b/localization/gyro_odometer/package.xml index a0a982ddedc16..3c979eb69ac8a 100644 --- a/localization/gyro_odometer/package.xml +++ b/localization/gyro_odometer/package.xml @@ -19,14 +19,13 @@ fmt geometry_msgs + rclcpp + rclcpp_components sensor_msgs tf2 tf2_geometry_msgs tier4_autoware_utils - rclcpp - rclcpp_components - ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/localization/gyro_odometer/src/gyro_odometer_core.cpp b/localization/gyro_odometer/src/gyro_odometer_core.cpp index 5de0ecd7cdc0a..bda7af74b8489 100644 --- a/localization/gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/gyro_odometer/src/gyro_odometer_core.cpp @@ -14,6 +14,8 @@ #include "gyro_odometer/gyro_odometer_core.hpp" +#include + #ifdef ROS_DISTRO_GALACTIC #include #else @@ -25,6 +27,42 @@ #include #include +namespace autoware::gyro_odometer +{ + +GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options) +: Node("gyro_odometer", node_options), + output_frame_(declare_parameter("output_frame")), + message_timeout_sec_(declare_parameter("message_timeout_sec")), + vehicle_twist_arrived_(false), + imu_arrived_(false) +{ + transform_listener_ = std::make_shared(this); + logger_configure_ = std::make_unique(this); + + vehicle_twist_sub_ = create_subscription( + "vehicle/twist_with_covariance", rclcpp::QoS{100}, + std::bind(&GyroOdometerNode::callbackVehicleTwist, this, std::placeholders::_1)); + + imu_sub_ = create_subscription( + "imu", rclcpp::QoS{100}, + std::bind(&GyroOdometerNode::callbackImu, this, std::placeholders::_1)); + + twist_raw_pub_ = create_publisher("twist_raw", rclcpp::QoS{10}); + twist_with_covariance_raw_pub_ = create_publisher( + "twist_with_covariance_raw", rclcpp::QoS{10}); + + twist_pub_ = create_publisher("twist", rclcpp::QoS{10}); + twist_with_covariance_pub_ = create_publisher( + "twist_with_covariance", rclcpp::QoS{10}); + + // TODO(YamatoAndo) createTimer +} + +GyroOdometerNode::~GyroOdometerNode() +{ +} + std::array transformCovariance(const std::array & cov) { using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; @@ -100,39 +138,7 @@ geometry_msgs::msg::TwistWithCovarianceStamped concatGyroAndOdometer( return twist_with_cov; } -GyroOdometer::GyroOdometer(const rclcpp::NodeOptions & options) -: Node("gyro_odometer", options), - output_frame_(declare_parameter("output_frame")), - message_timeout_sec_(declare_parameter("message_timeout_sec")), - vehicle_twist_arrived_(false), - imu_arrived_(false) -{ - transform_listener_ = std::make_shared(this); - logger_configure_ = std::make_unique(this); - - vehicle_twist_sub_ = create_subscription( - "vehicle/twist_with_covariance", rclcpp::QoS{100}, - std::bind(&GyroOdometer::callbackVehicleTwist, this, std::placeholders::_1)); - - imu_sub_ = create_subscription( - "imu", rclcpp::QoS{100}, std::bind(&GyroOdometer::callbackImu, this, std::placeholders::_1)); - - twist_raw_pub_ = create_publisher("twist_raw", rclcpp::QoS{10}); - twist_with_covariance_raw_pub_ = create_publisher( - "twist_with_covariance_raw", rclcpp::QoS{10}); - - twist_pub_ = create_publisher("twist", rclcpp::QoS{10}); - twist_with_covariance_pub_ = create_publisher( - "twist_with_covariance", rclcpp::QoS{10}); - - // TODO(YamatoAndo) createTimer -} - -GyroOdometer::~GyroOdometer() -{ -} - -void GyroOdometer::callbackVehicleTwist( +void GyroOdometerNode::callbackVehicleTwist( const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_ptr) { vehicle_twist_arrived_ = true; @@ -173,7 +179,7 @@ void GyroOdometer::callbackVehicleTwist( gyro_queue_.clear(); } -void GyroOdometer::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr) +void GyroOdometerNode::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr) { imu_arrived_ = true; if (!vehicle_twist_arrived_) { @@ -241,7 +247,7 @@ void GyroOdometer::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_m gyro_queue_.clear(); } -void GyroOdometer::publishData( +void GyroOdometerNode::publishData( const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw) { geometry_msgs::msg::TwistStamped twist_raw; @@ -269,3 +275,8 @@ void GyroOdometer::publishData( twist_pub_->publish(twist); twist_with_covariance_pub_->publish(twist_with_covariance); } + +} // namespace autoware::gyro_odometer + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::gyro_odometer::GyroOdometerNode) diff --git a/localization/gyro_odometer/src/gyro_odometer_node.cpp b/localization/gyro_odometer/src/gyro_odometer_node.cpp deleted file mode 100644 index 5bb6241506fbe..0000000000000 --- a/localization/gyro_odometer/src/gyro_odometer_node.cpp +++ /dev/null @@ -1,30 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "gyro_odometer/gyro_odometer_core.hpp" - -#include - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions options; - auto node = std::make_shared(options); - rclcpp::spin(node); - rclcpp::shutdown(); - - return 0; -} diff --git a/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp b/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp index 7f6416fbdda16..b7849ef03bfc5 100644 --- a/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp +++ b/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp @@ -108,7 +108,8 @@ TEST(GyroOdometer, TestGyroOdometerWithImuAndVelocity) expected_output_twist.twist.twist.angular.y = input_imu.angular_velocity.y; expected_output_twist.twist.twist.angular.z = input_imu.angular_velocity.z; - auto gyro_odometer_node = std::make_shared(getNodeOptionsWithDefaultParams()); + auto gyro_odometer_node = + std::make_shared(getNodeOptionsWithDefaultParams()); auto imu_generator = std::make_shared(); auto velocity_generator = std::make_shared(); auto gyro_odometer_validator_node = std::make_shared(); @@ -135,7 +136,8 @@ TEST(GyroOdometer, TestGyroOdometerImuOnly) { Imu input_imu = generateSampleImu(); - auto gyro_odometer_node = std::make_shared(getNodeOptionsWithDefaultParams()); + auto gyro_odometer_node = + std::make_shared(getNodeOptionsWithDefaultParams()); auto imu_generator = std::make_shared(); auto gyro_odometer_validator_node = std::make_shared();