19
19
#include < tier4_api_utils/tier4_api_utils.hpp>
20
20
#include < vehicle_info_util/vehicle_info_util.hpp>
21
21
22
- #include < autoware_auto_control_msgs /msg/ackermann_control_command .hpp>
23
- #include < autoware_auto_vehicle_msgs /msg/control_mode_report.hpp>
24
- #include < autoware_auto_vehicle_msgs /msg/engage.hpp>
25
- #include < autoware_auto_vehicle_msgs /msg/gear_command.hpp>
26
- #include < autoware_auto_vehicle_msgs /msg/gear_report.hpp>
27
- #include < autoware_auto_vehicle_msgs /msg/hazard_lights_command.hpp>
28
- #include < autoware_auto_vehicle_msgs /msg/hazard_lights_report.hpp>
29
- #include < autoware_auto_vehicle_msgs /msg/steering_report.hpp>
30
- #include < autoware_auto_vehicle_msgs /msg/turn_indicators_command.hpp>
31
- #include < autoware_auto_vehicle_msgs /msg/turn_indicators_report.hpp>
32
- #include < autoware_auto_vehicle_msgs /msg/velocity_report.hpp>
33
- #include < autoware_auto_vehicle_msgs /srv/control_mode_command.hpp>
22
+ #include < autoware_control_msgs /msg/control .hpp>
23
+ #include < autoware_vehicle_msgs /msg/control_mode_report.hpp>
24
+ #include < autoware_vehicle_msgs /msg/engage.hpp>
25
+ #include < autoware_vehicle_msgs /msg/gear_command.hpp>
26
+ #include < autoware_vehicle_msgs /msg/gear_report.hpp>
27
+ #include < autoware_vehicle_msgs /msg/hazard_lights_command.hpp>
28
+ #include < autoware_vehicle_msgs /msg/hazard_lights_report.hpp>
29
+ #include < autoware_vehicle_msgs /msg/steering_report.hpp>
30
+ #include < autoware_vehicle_msgs /msg/turn_indicators_command.hpp>
31
+ #include < autoware_vehicle_msgs /msg/turn_indicators_report.hpp>
32
+ #include < autoware_vehicle_msgs /msg/velocity_report.hpp>
33
+ #include < autoware_vehicle_msgs /srv/control_mode_command.hpp>
34
34
#include < pacmod3_msgs/msg/global_rpt.hpp>
35
35
#include < pacmod3_msgs/msg/steering_cmd.hpp>
36
36
#include < pacmod3_msgs/msg/system_cmd_float.hpp>
@@ -58,18 +58,17 @@ class PacmodInterface : public rclcpp::Node
58
58
using ActuationCommandStamped = tier4_vehicle_msgs::msg::ActuationCommandStamped;
59
59
using ActuationStatusStamped = tier4_vehicle_msgs::msg::ActuationStatusStamped;
60
60
using SteeringWheelStatusStamped = tier4_vehicle_msgs::msg::SteeringWheelStatusStamped;
61
- using ControlModeCommand = autoware_auto_vehicle_msgs ::srv::ControlModeCommand;
61
+ using ControlModeCommand = autoware_vehicle_msgs ::srv::ControlModeCommand;
62
62
PacmodInterface ();
63
63
64
64
private:
65
65
/* subscribers */
66
66
// From Autoware
67
- rclcpp::Subscription<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr
68
- control_cmd_sub_;
69
- rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::GearCommand>::SharedPtr gear_cmd_sub_;
70
- rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand>::SharedPtr
67
+ rclcpp::Subscription<autoware_control_msgs::msg::Control>::SharedPtr control_cmd_sub_;
68
+ rclcpp::Subscription<autoware_vehicle_msgs::msg::GearCommand>::SharedPtr gear_cmd_sub_;
69
+ rclcpp::Subscription<autoware_vehicle_msgs::msg::TurnIndicatorsCommand>::SharedPtr
71
70
turn_indicators_cmd_sub_;
72
- rclcpp::Subscription<autoware_auto_vehicle_msgs ::msg::HazardLightsCommand>::SharedPtr
71
+ rclcpp::Subscription<autoware_vehicle_msgs ::msg::HazardLightsCommand>::SharedPtr
73
72
hazard_lights_cmd_sub_;
74
73
rclcpp::Subscription<ActuationCommandStamped>::SharedPtr actuation_cmd_sub_;
75
74
rclcpp::Subscription<tier4_vehicle_msgs::msg::VehicleEmergencyStamped>::SharedPtr emergency_sub_;
@@ -97,15 +96,13 @@ class PacmodInterface : public rclcpp::Node
97
96
raw_steer_cmd_pub_; // only for debug
98
97
99
98
// To Autoware
100
- rclcpp::Publisher<autoware_auto_vehicle_msgs::msg::ControlModeReport>::SharedPtr
101
- control_mode_pub_;
102
- rclcpp::Publisher<autoware_auto_vehicle_msgs::msg::VelocityReport>::SharedPtr vehicle_twist_pub_;
103
- rclcpp::Publisher<autoware_auto_vehicle_msgs::msg::SteeringReport>::SharedPtr
104
- steering_status_pub_;
105
- rclcpp::Publisher<autoware_auto_vehicle_msgs::msg::GearReport>::SharedPtr gear_status_pub_;
106
- rclcpp::Publisher<autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport>::SharedPtr
99
+ rclcpp::Publisher<autoware_vehicle_msgs::msg::ControlModeReport>::SharedPtr control_mode_pub_;
100
+ rclcpp::Publisher<autoware_vehicle_msgs::msg::VelocityReport>::SharedPtr vehicle_twist_pub_;
101
+ rclcpp::Publisher<autoware_vehicle_msgs::msg::SteeringReport>::SharedPtr steering_status_pub_;
102
+ rclcpp::Publisher<autoware_vehicle_msgs::msg::GearReport>::SharedPtr gear_status_pub_;
103
+ rclcpp::Publisher<autoware_vehicle_msgs::msg::TurnIndicatorsReport>::SharedPtr
107
104
turn_indicators_status_pub_;
108
- rclcpp::Publisher<autoware_auto_vehicle_msgs ::msg::HazardLightsReport>::SharedPtr
105
+ rclcpp::Publisher<autoware_vehicle_msgs ::msg::HazardLightsReport>::SharedPtr
109
106
hazard_lights_status_pub_;
110
107
rclcpp::Publisher<ActuationStatusStamped>::SharedPtr actuation_status_pub_;
111
108
rclcpp::Publisher<SteeringWheelStatusStamped>::SharedPtr steering_wheel_status_pub_;
@@ -164,10 +161,10 @@ class PacmodInterface : public rclcpp::Node
164
161
165
162
/* input values */
166
163
ActuationCommandStamped::ConstSharedPtr actuation_cmd_ptr_;
167
- autoware_auto_control_msgs ::msg::AckermannControlCommand ::ConstSharedPtr control_cmd_ptr_;
168
- autoware_auto_vehicle_msgs ::msg::TurnIndicatorsCommand::ConstSharedPtr turn_indicators_cmd_ptr_;
169
- autoware_auto_vehicle_msgs ::msg::HazardLightsCommand::ConstSharedPtr hazard_lights_cmd_ptr_;
170
- autoware_auto_vehicle_msgs ::msg::GearCommand::ConstSharedPtr gear_cmd_ptr_;
164
+ autoware_control_msgs ::msg::Control ::ConstSharedPtr control_cmd_ptr_;
165
+ autoware_vehicle_msgs ::msg::TurnIndicatorsCommand::ConstSharedPtr turn_indicators_cmd_ptr_;
166
+ autoware_vehicle_msgs ::msg::HazardLightsCommand::ConstSharedPtr hazard_lights_cmd_ptr_;
167
+ autoware_vehicle_msgs ::msg::GearCommand::ConstSharedPtr gear_cmd_ptr_;
171
168
172
169
pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr steer_wheel_rpt_ptr_; // [rad]
173
170
pacmod3_msgs::msg::WheelSpeedRpt::ConstSharedPtr wheel_speed_rpt_ptr_; // [m/s]
@@ -188,18 +185,17 @@ class PacmodInterface : public rclcpp::Node
188
185
189
186
/* callbacks */
190
187
void callbackActuationCmd (const ActuationCommandStamped::ConstSharedPtr msg);
191
- void callbackControlCmd (
192
- const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg);
188
+ void callbackControlCmd (const autoware_control_msgs::msg::Control::ConstSharedPtr msg);
193
189
194
190
void callbackEmergencyCmd (
195
191
const tier4_vehicle_msgs::msg::VehicleEmergencyStamped::ConstSharedPtr msg);
196
192
197
- void callbackGearCmd (const autoware_auto_vehicle_msgs ::msg::GearCommand::ConstSharedPtr msg);
193
+ void callbackGearCmd (const autoware_vehicle_msgs ::msg::GearCommand::ConstSharedPtr msg);
198
194
void callbackTurnIndicatorsCommand (
199
- const autoware_auto_vehicle_msgs ::msg::TurnIndicatorsCommand::ConstSharedPtr msg);
195
+ const autoware_vehicle_msgs ::msg::TurnIndicatorsCommand::ConstSharedPtr msg);
200
196
void callbackHazardLightsCommand (
201
- const autoware_auto_vehicle_msgs ::msg::HazardLightsCommand::ConstSharedPtr msg);
202
- void callbackEngage (const autoware_auto_vehicle_msgs ::msg::Engage::ConstSharedPtr msg);
197
+ const autoware_vehicle_msgs ::msg::HazardLightsCommand::ConstSharedPtr msg);
198
+ void callbackEngage (const autoware_vehicle_msgs ::msg::Engage::ConstSharedPtr msg);
203
199
void callbackRearDoor (const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr rear_door_rpt);
204
200
void callbackSteerWheelRpt (
205
201
const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr steer_wheel_rpt);
@@ -219,14 +215,14 @@ class PacmodInterface : public rclcpp::Node
219
215
const pacmod3_msgs::msg::SystemRptInt & shift_rpt);
220
216
double calculateVariableGearRatio (const double vel, const double steer_wheel);
221
217
double calcSteerWheelRateCmd (const double gear_ratio);
222
- uint16_t toPacmodShiftCmd (const autoware_auto_vehicle_msgs ::msg::GearCommand & gear_cmd);
218
+ uint16_t toPacmodShiftCmd (const autoware_vehicle_msgs ::msg::GearCommand & gear_cmd);
223
219
uint16_t getGearCmdForPreventChatter (uint16_t gear_command);
224
220
uint16_t toPacmodTurnCmd (
225
- const autoware_auto_vehicle_msgs ::msg::TurnIndicatorsCommand & turn,
226
- const autoware_auto_vehicle_msgs ::msg::HazardLightsCommand & hazard);
221
+ const autoware_vehicle_msgs ::msg::TurnIndicatorsCommand & turn,
222
+ const autoware_vehicle_msgs ::msg::HazardLightsCommand & hazard);
227
223
uint16_t toPacmodTurnCmdWithHazardRecover (
228
- const autoware_auto_vehicle_msgs ::msg::TurnIndicatorsCommand & turn,
229
- const autoware_auto_vehicle_msgs ::msg::HazardLightsCommand & hazard);
224
+ const autoware_vehicle_msgs ::msg::TurnIndicatorsCommand & turn,
225
+ const autoware_vehicle_msgs ::msg::HazardLightsCommand & hazard);
230
226
231
227
std::optional<int32_t > toAutowareShiftReport (const pacmod3_msgs::msg::SystemRptInt & shift);
232
228
int32_t toAutowareTurnIndicatorsReport (const pacmod3_msgs::msg::SystemRptInt & turn);
0 commit comments