Skip to content

Commit 6d77841

Browse files
refactor(gyro_odometer): componentize node in gyro_odometer (#7090)
* refactor: install glog to gyro_odometer Signed-off-by: Motsu-san <masahiro.sakamoto@tier4.jp> * style(pre-commit): autofix * feat: change glog error setting --------- Signed-off-by: Motsu-san <masahiro.sakamoto@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
1 parent 27e1aa1 commit 6d77841

File tree

7 files changed

+68
-81
lines changed

7 files changed

+68
-81
lines changed

localization/gyro_odometer/CMakeLists.txt

+7-7
Original file line numberDiff line numberDiff line change
@@ -4,17 +4,12 @@ project(gyro_odometer)
44
find_package(autoware_cmake REQUIRED)
55
autoware_package()
66

7-
ament_auto_add_executable(${PROJECT_NAME}
8-
src/gyro_odometer_node.cpp
7+
ament_auto_add_library(${PROJECT_NAME} SHARED
98
src/gyro_odometer_core.cpp
109
)
1110

1211
target_link_libraries(${PROJECT_NAME} fmt)
1312

14-
ament_auto_add_library(gyro_odometer_node SHARED
15-
src/gyro_odometer_core.cpp
16-
)
17-
1813
if(BUILD_TESTING)
1914
ament_add_ros_isolated_gtest(test_gyro_odometer
2015
test/test_main.cpp
@@ -25,10 +20,15 @@ if(BUILD_TESTING)
2520
rclcpp
2621
)
2722
target_link_libraries(test_gyro_odometer
28-
gyro_odometer_node
23+
${PROJECT_NAME}
2924
)
3025
endif()
3126

27+
rclcpp_components_register_node(${PROJECT_NAME}
28+
PLUGIN "autoware::gyro_odometer::GyroOdometerNode"
29+
EXECUTABLE ${PROJECT_NAME}_node
30+
EXECUTOR SingleThreadedExecutor
31+
)
3232

3333
ament_auto_package(INSTALL_TO_SHARE
3434
launch

localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp

+8-3
Original file line numberDiff line numberDiff line change
@@ -36,14 +36,17 @@
3636
#include <memory>
3737
#include <string>
3838

39-
class GyroOdometer : public rclcpp::Node
39+
namespace autoware::gyro_odometer
40+
{
41+
42+
class GyroOdometerNode : public rclcpp::Node
4043
{
4144
private:
4245
using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX;
4346

4447
public:
45-
explicit GyroOdometer(const rclcpp::NodeOptions & options);
46-
~GyroOdometer();
48+
explicit GyroOdometerNode(const rclcpp::NodeOptions & node_options);
49+
~GyroOdometerNode();
4750

4851
private:
4952
void callbackVehicleTwist(
@@ -75,4 +78,6 @@ class GyroOdometer : public rclcpp::Node
7578
std::deque<sensor_msgs::msg::Imu> gyro_queue_;
7679
};
7780

81+
} // namespace autoware::gyro_odometer
82+
7883
#endif // GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_

localization/gyro_odometer/launch/gyro_odometer.launch.xml

+1-1
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,7 @@
1111

1212
<arg name="config_file" default="$(find-pkg-share gyro_odometer)/config/gyro_odometer.param.yaml"/>
1313

14-
<node pkg="gyro_odometer" exec="gyro_odometer" name="gyro_odometer" output="screen">
14+
<node pkg="gyro_odometer" exec="gyro_odometer_node" output="both">
1515
<remap from="vehicle/twist_with_covariance" to="$(var input_vehicle_twist_with_covariance_topic)"/>
1616

1717
<remap from="imu" to="$(var input_imu_topic)"/>

localization/gyro_odometer/package.xml

+2-3
Original file line numberDiff line numberDiff line change
@@ -19,14 +19,13 @@
1919

2020
<depend>fmt</depend>
2121
<depend>geometry_msgs</depend>
22+
<depend>rclcpp</depend>
23+
<depend>rclcpp_components</depend>
2224
<depend>sensor_msgs</depend>
2325
<depend>tf2</depend>
2426
<depend>tf2_geometry_msgs</depend>
2527
<depend>tier4_autoware_utils</depend>
2628

27-
<exec_depend>rclcpp</exec_depend>
28-
<exec_depend>rclcpp_components</exec_depend>
29-
3029
<test_depend>ament_cmake_ros</test_depend>
3130
<test_depend>ament_lint_auto</test_depend>
3231
<test_depend>autoware_lint_common</test_depend>

localization/gyro_odometer/src/gyro_odometer_core.cpp

+46-35
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,8 @@
1414

1515
#include "gyro_odometer/gyro_odometer_core.hpp"
1616

17+
#include <rclcpp/rclcpp.hpp>
18+
1719
#ifdef ROS_DISTRO_GALACTIC
1820
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
1921
#else
@@ -25,6 +27,42 @@
2527
#include <memory>
2628
#include <string>
2729

30+
namespace autoware::gyro_odometer
31+
{
32+
33+
GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options)
34+
: Node("gyro_odometer", node_options),
35+
output_frame_(declare_parameter<std::string>("output_frame")),
36+
message_timeout_sec_(declare_parameter<double>("message_timeout_sec")),
37+
vehicle_twist_arrived_(false),
38+
imu_arrived_(false)
39+
{
40+
transform_listener_ = std::make_shared<tier4_autoware_utils::TransformListener>(this);
41+
logger_configure_ = std::make_unique<tier4_autoware_utils::LoggerLevelConfigure>(this);
42+
43+
vehicle_twist_sub_ = create_subscription<geometry_msgs::msg::TwistWithCovarianceStamped>(
44+
"vehicle/twist_with_covariance", rclcpp::QoS{100},
45+
std::bind(&GyroOdometerNode::callbackVehicleTwist, this, std::placeholders::_1));
46+
47+
imu_sub_ = create_subscription<sensor_msgs::msg::Imu>(
48+
"imu", rclcpp::QoS{100},
49+
std::bind(&GyroOdometerNode::callbackImu, this, std::placeholders::_1));
50+
51+
twist_raw_pub_ = create_publisher<geometry_msgs::msg::TwistStamped>("twist_raw", rclcpp::QoS{10});
52+
twist_with_covariance_raw_pub_ = create_publisher<geometry_msgs::msg::TwistWithCovarianceStamped>(
53+
"twist_with_covariance_raw", rclcpp::QoS{10});
54+
55+
twist_pub_ = create_publisher<geometry_msgs::msg::TwistStamped>("twist", rclcpp::QoS{10});
56+
twist_with_covariance_pub_ = create_publisher<geometry_msgs::msg::TwistWithCovarianceStamped>(
57+
"twist_with_covariance", rclcpp::QoS{10});
58+
59+
// TODO(YamatoAndo) createTimer
60+
}
61+
62+
GyroOdometerNode::~GyroOdometerNode()
63+
{
64+
}
65+
2866
std::array<double, 9> transformCovariance(const std::array<double, 9> & cov)
2967
{
3068
using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX;
@@ -100,39 +138,7 @@ geometry_msgs::msg::TwistWithCovarianceStamped concatGyroAndOdometer(
100138
return twist_with_cov;
101139
}
102140

103-
GyroOdometer::GyroOdometer(const rclcpp::NodeOptions & options)
104-
: Node("gyro_odometer", options),
105-
output_frame_(declare_parameter<std::string>("output_frame")),
106-
message_timeout_sec_(declare_parameter<double>("message_timeout_sec")),
107-
vehicle_twist_arrived_(false),
108-
imu_arrived_(false)
109-
{
110-
transform_listener_ = std::make_shared<tier4_autoware_utils::TransformListener>(this);
111-
logger_configure_ = std::make_unique<tier4_autoware_utils::LoggerLevelConfigure>(this);
112-
113-
vehicle_twist_sub_ = create_subscription<geometry_msgs::msg::TwistWithCovarianceStamped>(
114-
"vehicle/twist_with_covariance", rclcpp::QoS{100},
115-
std::bind(&GyroOdometer::callbackVehicleTwist, this, std::placeholders::_1));
116-
117-
imu_sub_ = create_subscription<sensor_msgs::msg::Imu>(
118-
"imu", rclcpp::QoS{100}, std::bind(&GyroOdometer::callbackImu, this, std::placeholders::_1));
119-
120-
twist_raw_pub_ = create_publisher<geometry_msgs::msg::TwistStamped>("twist_raw", rclcpp::QoS{10});
121-
twist_with_covariance_raw_pub_ = create_publisher<geometry_msgs::msg::TwistWithCovarianceStamped>(
122-
"twist_with_covariance_raw", rclcpp::QoS{10});
123-
124-
twist_pub_ = create_publisher<geometry_msgs::msg::TwistStamped>("twist", rclcpp::QoS{10});
125-
twist_with_covariance_pub_ = create_publisher<geometry_msgs::msg::TwistWithCovarianceStamped>(
126-
"twist_with_covariance", rclcpp::QoS{10});
127-
128-
// TODO(YamatoAndo) createTimer
129-
}
130-
131-
GyroOdometer::~GyroOdometer()
132-
{
133-
}
134-
135-
void GyroOdometer::callbackVehicleTwist(
141+
void GyroOdometerNode::callbackVehicleTwist(
136142
const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_ptr)
137143
{
138144
vehicle_twist_arrived_ = true;
@@ -173,7 +179,7 @@ void GyroOdometer::callbackVehicleTwist(
173179
gyro_queue_.clear();
174180
}
175181

176-
void GyroOdometer::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr)
182+
void GyroOdometerNode::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr)
177183
{
178184
imu_arrived_ = true;
179185
if (!vehicle_twist_arrived_) {
@@ -241,7 +247,7 @@ void GyroOdometer::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_m
241247
gyro_queue_.clear();
242248
}
243249

244-
void GyroOdometer::publishData(
250+
void GyroOdometerNode::publishData(
245251
const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw)
246252
{
247253
geometry_msgs::msg::TwistStamped twist_raw;
@@ -269,3 +275,8 @@ void GyroOdometer::publishData(
269275
twist_pub_->publish(twist);
270276
twist_with_covariance_pub_->publish(twist_with_covariance);
271277
}
278+
279+
} // namespace autoware::gyro_odometer
280+
281+
#include <rclcpp_components/register_node_macro.hpp>
282+
RCLCPP_COMPONENTS_REGISTER_NODE(autoware::gyro_odometer::GyroOdometerNode)

localization/gyro_odometer/src/gyro_odometer_node.cpp

-30
This file was deleted.

localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp

+4-2
Original file line numberDiff line numberDiff line change
@@ -108,7 +108,8 @@ TEST(GyroOdometer, TestGyroOdometerWithImuAndVelocity)
108108
expected_output_twist.twist.twist.angular.y = input_imu.angular_velocity.y;
109109
expected_output_twist.twist.twist.angular.z = input_imu.angular_velocity.z;
110110

111-
auto gyro_odometer_node = std::make_shared<GyroOdometer>(getNodeOptionsWithDefaultParams());
111+
auto gyro_odometer_node =
112+
std::make_shared<autoware::gyro_odometer::GyroOdometerNode>(getNodeOptionsWithDefaultParams());
112113
auto imu_generator = std::make_shared<ImuGenerator>();
113114
auto velocity_generator = std::make_shared<VelocityGenerator>();
114115
auto gyro_odometer_validator_node = std::make_shared<GyroOdometerValidator>();
@@ -135,7 +136,8 @@ TEST(GyroOdometer, TestGyroOdometerImuOnly)
135136
{
136137
Imu input_imu = generateSampleImu();
137138

138-
auto gyro_odometer_node = std::make_shared<GyroOdometer>(getNodeOptionsWithDefaultParams());
139+
auto gyro_odometer_node =
140+
std::make_shared<autoware::gyro_odometer::GyroOdometerNode>(getNodeOptionsWithDefaultParams());
139141
auto imu_generator = std::make_shared<ImuGenerator>();
140142
auto gyro_odometer_validator_node = std::make_shared<GyroOdometerValidator>();
141143

0 commit comments

Comments
 (0)