|
14 | 14 |
|
15 | 15 | #include "gyro_odometer/gyro_odometer_core.hpp"
|
16 | 16 |
|
| 17 | +#include <rclcpp/rclcpp.hpp> |
| 18 | + |
17 | 19 | #ifdef ROS_DISTRO_GALACTIC
|
18 | 20 | #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
|
19 | 21 | #else
|
|
25 | 27 | #include <memory>
|
26 | 28 | #include <string>
|
27 | 29 |
|
| 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 | + |
28 | 66 | std::array<double, 9> transformCovariance(const std::array<double, 9> & cov)
|
29 | 67 | {
|
30 | 68 | using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX;
|
@@ -100,39 +138,7 @@ geometry_msgs::msg::TwistWithCovarianceStamped concatGyroAndOdometer(
|
100 | 138 | return twist_with_cov;
|
101 | 139 | }
|
102 | 140 |
|
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( |
136 | 142 | const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_ptr)
|
137 | 143 | {
|
138 | 144 | vehicle_twist_arrived_ = true;
|
@@ -173,7 +179,7 @@ void GyroOdometer::callbackVehicleTwist(
|
173 | 179 | gyro_queue_.clear();
|
174 | 180 | }
|
175 | 181 |
|
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) |
177 | 183 | {
|
178 | 184 | imu_arrived_ = true;
|
179 | 185 | if (!vehicle_twist_arrived_) {
|
@@ -241,7 +247,7 @@ void GyroOdometer::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_m
|
241 | 247 | gyro_queue_.clear();
|
242 | 248 | }
|
243 | 249 |
|
244 |
| -void GyroOdometer::publishData( |
| 250 | +void GyroOdometerNode::publishData( |
245 | 251 | const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw)
|
246 | 252 | {
|
247 | 253 | geometry_msgs::msg::TwistStamped twist_raw;
|
@@ -269,3 +275,8 @@ void GyroOdometer::publishData(
|
269 | 275 | twist_pub_->publish(twist);
|
270 | 276 | twist_with_covariance_pub_->publish(twist_with_covariance);
|
271 | 277 | }
|
| 278 | + |
| 279 | +} // namespace autoware::gyro_odometer |
| 280 | + |
| 281 | +#include <rclcpp_components/register_node_macro.hpp> |
| 282 | +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::gyro_odometer::GyroOdometerNode) |
0 commit comments