Skip to content

Commit 6efb5d1

Browse files
zusizusipre-commit-ci[bot]
zusizusi
and
pre-commit-ci[bot]
authored
revert(pacmod_interface): Revert "chore(pacmod_interface): separate pacmod callback (#68)" (#83)
* Revert "chore(pacmod_interface): separate pacmod callback (#68)" This reverts commit b779f84. Signed-off-by: Sho Iwasawa <sho.iwasawa.2@tier4.jp> * style(pre-commit): autofix * fix type Signed-off-by: Sho Iwasawa <sho.iwasawa.2@tier4.jp> --------- Signed-off-by: Sho Iwasawa <sho.iwasawa.2@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
1 parent d7adea3 commit 6efb5d1

File tree

2 files changed

+68
-93
lines changed

2 files changed

+68
-93
lines changed

‎pacmod_interface/include/pacmod_interface/pacmod_interface.hpp

+28-25
Original file line numberDiff line numberDiff line change
@@ -45,6 +45,10 @@
4545
#include <tier4_vehicle_msgs/msg/steering_wheel_status_stamped.hpp>
4646
#include <tier4_vehicle_msgs/msg/vehicle_emergency_stamped.hpp>
4747

48+
#include <message_filters/subscriber.h>
49+
#include <message_filters/sync_policies/approximate_time.h>
50+
#include <message_filters/synchronizer.h>
51+
4852
#include <algorithm>
4953
#include <cmath>
5054
#include <cstdlib>
@@ -62,6 +66,12 @@ class PacmodInterface : public rclcpp::Node
6266
PacmodInterface();
6367

6468
private:
69+
typedef message_filters::sync_policies::ApproximateTime<
70+
pacmod3_msgs::msg::SystemRptFloat, pacmod3_msgs::msg::WheelSpeedRpt,
71+
pacmod3_msgs::msg::SystemRptFloat, pacmod3_msgs::msg::SystemRptFloat,
72+
pacmod3_msgs::msg::SystemRptInt, pacmod3_msgs::msg::SystemRptInt, pacmod3_msgs::msg::GlobalRpt>
73+
PacmodFeedbacksSyncPolicy;
74+
6575
/* subscribers */
6676
// From Autoware
6777
rclcpp::Subscription<autoware_control_msgs::msg::Control>::SharedPtr control_cmd_sub_;
@@ -76,13 +86,16 @@ class PacmodInterface : public rclcpp::Node
7686
// From Pacmod
7787
rclcpp::Subscription<pacmod3_msgs::msg::SystemRptInt>::SharedPtr rear_door_rpt_sub_;
7888

79-
rclcpp::Subscription<pacmod3_msgs::msg::SystemRptFloat>::SharedPtr steer_wheel_rpt_sub_;
80-
rclcpp::Subscription<pacmod3_msgs::msg::WheelSpeedRpt>::SharedPtr wheel_speed_rpt_sub_;
81-
rclcpp::Subscription<pacmod3_msgs::msg::SystemRptFloat>::SharedPtr accel_rpt_sub_;
82-
rclcpp::Subscription<pacmod3_msgs::msg::SystemRptFloat>::SharedPtr brake_rpt_sub_;
83-
rclcpp::Subscription<pacmod3_msgs::msg::SystemRptInt>::SharedPtr shift_rpt_sub_;
84-
rclcpp::Subscription<pacmod3_msgs::msg::SystemRptInt>::SharedPtr turn_rpt_sub_;
85-
rclcpp::Subscription<pacmod3_msgs::msg::GlobalRpt>::SharedPtr global_rpt_sub_;
89+
std::unique_ptr<message_filters::Subscriber<pacmod3_msgs::msg::SystemRptFloat>>
90+
steer_wheel_rpt_sub_;
91+
std::unique_ptr<message_filters::Subscriber<pacmod3_msgs::msg::WheelSpeedRpt>>
92+
wheel_speed_rpt_sub_;
93+
std::unique_ptr<message_filters::Subscriber<pacmod3_msgs::msg::SystemRptFloat>> accel_rpt_sub_;
94+
std::unique_ptr<message_filters::Subscriber<pacmod3_msgs::msg::SystemRptFloat>> brake_rpt_sub_;
95+
std::unique_ptr<message_filters::Subscriber<pacmod3_msgs::msg::SystemRptInt>> shift_rpt_sub_;
96+
std::unique_ptr<message_filters::Subscriber<pacmod3_msgs::msg::SystemRptInt>> turn_rpt_sub_;
97+
std::unique_ptr<message_filters::Subscriber<pacmod3_msgs::msg::GlobalRpt>> global_rpt_sub_;
98+
std::unique_ptr<message_filters::Synchronizer<PacmodFeedbacksSyncPolicy>> pacmod_feedbacks_sync_;
8699

87100
/* publishers */
88101
// To Pacmod
@@ -113,14 +126,6 @@ class PacmodInterface : public rclcpp::Node
113126
/* ros param */
114127
std::string base_frame_id_;
115128
int command_timeout_ms_; // vehicle_cmd timeout [ms]
116-
bool is_steer_wheel_rpt_received_ = false;
117-
bool is_wheel_speed_rpt_received_ = false;
118-
bool is_accel_rpt_received_ = false;
119-
bool is_brake_rpt_received_ = false;
120-
bool is_shift_rpt_received_ = false;
121-
bool is_turn_rpt_received_ = false;
122-
bool is_global_rpt_received_ = false;
123-
124129
bool is_pacmod_rpt_received_ = false;
125130
bool is_pacmod_enabled_ = false;
126131
bool is_clear_override_needed_ = false;
@@ -198,19 +203,17 @@ class PacmodInterface : public rclcpp::Node
198203
const autoware_vehicle_msgs::msg::HazardLightsCommand::ConstSharedPtr msg);
199204
void callbackEngage(const autoware_vehicle_msgs::msg::Engage::ConstSharedPtr msg);
200205
void callbackRearDoor(const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr rear_door_rpt);
201-
void callbackSteerWheelRpt(
202-
const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr steer_wheel_rpt);
203-
void callbackWheelSpeedRpt(
204-
const pacmod3_msgs::msg::WheelSpeedRpt::ConstSharedPtr wheel_speed_rpt);
205-
void callbackAccelRpt(const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr accel_rpt);
206-
void callbackBrakeRpt(const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr brake_rpt);
207-
void callbackGearRpt(const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr gear_rpt);
208-
void callbackTurnRpt(const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr turn_rpt);
209-
void callbackGlobalRpt(const pacmod3_msgs::msg::GlobalRpt::ConstSharedPtr global_rpt);
206+
void callbackPacmodRpt(
207+
const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr steer_wheel_rpt,
208+
const pacmod3_msgs::msg::WheelSpeedRpt::ConstSharedPtr wheel_speed_rpt,
209+
const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr accel_rpt,
210+
const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr brake_rpt,
211+
const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr gear_cmd_rpt,
212+
const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr turn_rpt,
213+
const pacmod3_msgs::msg::GlobalRpt::ConstSharedPtr global_rpt);
210214

211215
/* functions */
212216
void publishCommands();
213-
void publishVehicleStatus();
214217
double calculateVehicleVelocity(
215218
const pacmod3_msgs::msg::WheelSpeedRpt & wheel_speed_rpt,
216219
const pacmod3_msgs::msg::SystemRptInt & shift_rpt);

‎pacmod_interface/src/pacmod_interface/pacmod_interface.cpp

+40-68
Original file line numberDiff line numberDiff line change
@@ -95,20 +95,32 @@ PacmodInterface::PacmodInterface()
9595
rear_door_rpt_sub_ = create_subscription<pacmod3_msgs::msg::SystemRptInt>(
9696
"/pacmod/rear_pass_door_rpt", 1, std::bind(&PacmodInterface::callbackRearDoor, this, _1));
9797

98-
steer_wheel_rpt_sub_ = create_subscription<pacmod3_msgs::msg::SystemRptFloat>(
99-
"/pacmod/steering_rpt", 1, std::bind(&PacmodInterface::callbackSteerWheelRpt, this, _1));
100-
wheel_speed_rpt_sub_ = create_subscription<pacmod3_msgs::msg::WheelSpeedRpt>(
101-
"/pacmod/wheel_speed_rpt", 1, std::bind(&PacmodInterface::callbackWheelSpeedRpt, this, _1));
102-
accel_rpt_sub_ = create_subscription<pacmod3_msgs::msg::SystemRptFloat>(
103-
"/pacmod/accel_rpt", 1, std::bind(&PacmodInterface::callbackAccelRpt, this, _1));
104-
brake_rpt_sub_ = create_subscription<pacmod3_msgs::msg::SystemRptFloat>(
105-
"/pacmod/brake_rpt", 1, std::bind(&PacmodInterface::callbackBrakeRpt, this, _1));
106-
shift_rpt_sub_ = create_subscription<pacmod3_msgs::msg::SystemRptInt>(
107-
"/pacmod/shift_rpt", 1, std::bind(&PacmodInterface::callbackGearRpt, this, _1));
108-
turn_rpt_sub_ = create_subscription<pacmod3_msgs::msg::SystemRptInt>(
109-
"/pacmod/turn_rpt", 1, std::bind(&PacmodInterface::callbackTurnRpt, this, _1));
110-
global_rpt_sub_ = create_subscription<pacmod3_msgs::msg::GlobalRpt>(
111-
"/pacmod/global_rpt", 1, std::bind(&PacmodInterface::callbackGlobalRpt, this, _1));
98+
steer_wheel_rpt_sub_ =
99+
std::make_unique<message_filters::Subscriber<pacmod3_msgs::msg::SystemRptFloat>>(
100+
this, "/pacmod/steering_rpt");
101+
wheel_speed_rpt_sub_ =
102+
std::make_unique<message_filters::Subscriber<pacmod3_msgs::msg::WheelSpeedRpt>>(
103+
this, "/pacmod/wheel_speed_rpt");
104+
accel_rpt_sub_ = std::make_unique<message_filters::Subscriber<pacmod3_msgs::msg::SystemRptFloat>>(
105+
this, "/pacmod/accel_rpt");
106+
brake_rpt_sub_ = std::make_unique<message_filters::Subscriber<pacmod3_msgs::msg::SystemRptFloat>>(
107+
this, "/pacmod/brake_rpt");
108+
shift_rpt_sub_ = std::make_unique<message_filters::Subscriber<pacmod3_msgs::msg::SystemRptInt>>(
109+
this, "/pacmod/shift_rpt");
110+
turn_rpt_sub_ = std::make_unique<message_filters::Subscriber<pacmod3_msgs::msg::SystemRptInt>>(
111+
this, "/pacmod/turn_rpt");
112+
global_rpt_sub_ = std::make_unique<message_filters::Subscriber<pacmod3_msgs::msg::GlobalRpt>>(
113+
this, "/pacmod/global_rpt");
114+
115+
pacmod_feedbacks_sync_ =
116+
std::make_unique<message_filters::Synchronizer<PacmodFeedbacksSyncPolicy>>(
117+
PacmodFeedbacksSyncPolicy(10), *steer_wheel_rpt_sub_, *wheel_speed_rpt_sub_, *accel_rpt_sub_,
118+
*brake_rpt_sub_, *shift_rpt_sub_, *turn_rpt_sub_, *global_rpt_sub_);
119+
120+
pacmod_feedbacks_sync_->registerCallback(std::bind(
121+
&PacmodInterface::callbackPacmodRpt, this, std::placeholders::_1, std::placeholders::_2,
122+
std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6,
123+
std::placeholders::_7));
112124

113125
/* publisher */
114126
// To pacmod
@@ -227,57 +239,24 @@ void PacmodInterface::callbackRearDoor(
227239
door_status_pub_->publish(toAutowareDoorStatusMsg(*rear_door_rpt));
228240
}
229241

230-
void PacmodInterface::callbackSteerWheelRpt(
231-
const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr steer_wheel_rpt)
242+
void PacmodInterface::callbackPacmodRpt(
243+
const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr steer_wheel_rpt,
244+
const pacmod3_msgs::msg::WheelSpeedRpt::ConstSharedPtr wheel_speed_rpt,
245+
const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr accel_rpt,
246+
const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr brake_rpt,
247+
const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr shift_rpt,
248+
const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr turn_rpt,
249+
const pacmod3_msgs::msg::GlobalRpt::ConstSharedPtr global_rpt)
232250
{
233-
is_steer_wheel_rpt_received_ = true;
251+
is_pacmod_rpt_received_ = true;
234252
steer_wheel_rpt_ptr_ = steer_wheel_rpt;
235-
}
236-
237-
void PacmodInterface::callbackWheelSpeedRpt(
238-
const pacmod3_msgs::msg::WheelSpeedRpt::ConstSharedPtr wheel_speed_rpt)
239-
{
240-
is_wheel_speed_rpt_received_ = true;
241253
wheel_speed_rpt_ptr_ = wheel_speed_rpt;
242-
}
243-
244-
void PacmodInterface::callbackAccelRpt(
245-
const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr accel_rpt)
246-
{
247-
is_accel_rpt_received_ = true;
248254
accel_rpt_ptr_ = accel_rpt;
249-
}
250-
251-
void PacmodInterface::callbackBrakeRpt(
252-
const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr brake_rpt)
253-
{
254-
is_brake_rpt_received_ = true;
255255
brake_rpt_ptr_ = brake_rpt;
256-
}
257-
258-
void PacmodInterface::callbackGearRpt(
259-
const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr gear_rpt)
260-
{
261-
is_shift_rpt_received_ = true;
262-
gear_cmd_rpt_ptr_ = gear_rpt;
263-
}
264-
265-
void PacmodInterface::callbackTurnRpt(
266-
const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr turn_rpt)
267-
{
268-
is_turn_rpt_received_ = true;
269-
turn_rpt_ptr_ = turn_rpt;
270-
}
271-
272-
void PacmodInterface::callbackGlobalRpt(
273-
const pacmod3_msgs::msg::GlobalRpt::ConstSharedPtr global_rpt)
274-
{
275-
is_global_rpt_received_ = true;
256+
gear_cmd_rpt_ptr_ = shift_rpt;
276257
global_rpt_ptr_ = global_rpt;
277-
}
258+
turn_rpt_ptr_ = turn_rpt;
278259

279-
void PacmodInterface::publishVehicleStatus()
280-
{
281260
is_pacmod_enabled_ =
282261
steer_wheel_rpt_ptr_->enabled && accel_rpt_ptr_->enabled && brake_rpt_ptr_->enabled;
283262
RCLCPP_DEBUG(
@@ -312,7 +291,7 @@ void PacmodInterface::publishVehicleStatus()
312291
autoware_vehicle_msgs::msg::ControlModeReport control_mode_msg;
313292
control_mode_msg.stamp = header.stamp;
314293

315-
if (global_rpt_ptr_->enabled && is_pacmod_enabled_) {
294+
if (global_rpt->enabled && is_pacmod_enabled_) {
316295
control_mode_msg.mode = autoware_vehicle_msgs::msg::ControlModeReport::AUTONOMOUS;
317296
} else {
318297
control_mode_msg.mode = autoware_vehicle_msgs::msg::ControlModeReport::MANUAL;
@@ -363,23 +342,18 @@ void PacmodInterface::publishVehicleStatus()
363342
{
364343
autoware_vehicle_msgs::msg::TurnIndicatorsReport turn_msg;
365344
turn_msg.stamp = header.stamp;
366-
turn_msg.report = toAutowareTurnIndicatorsReport(*turn_rpt_ptr_);
345+
turn_msg.report = toAutowareTurnIndicatorsReport(*turn_rpt);
367346
turn_indicators_status_pub_->publish(turn_msg);
368347

369348
autoware_vehicle_msgs::msg::HazardLightsReport hazard_msg;
370349
hazard_msg.stamp = header.stamp;
371-
hazard_msg.report = toAutowareHazardLightsReport(*turn_rpt_ptr_);
350+
hazard_msg.report = toAutowareHazardLightsReport(*turn_rpt);
372351
hazard_lights_status_pub_->publish(hazard_msg);
373352
}
374353
}
375354

376355
void PacmodInterface::publishCommands()
377356
{
378-
is_pacmod_rpt_received_ = is_steer_wheel_rpt_received_ && is_wheel_speed_rpt_received_ &&
379-
is_accel_rpt_received_ && is_brake_rpt_received_ &&
380-
is_shift_rpt_received_ && is_turn_rpt_received_ &&
381-
is_global_rpt_received_;
382-
383357
/* guard */
384358
if (!actuation_cmd_ptr_ || !control_cmd_ptr_ || !is_pacmod_rpt_received_ || !gear_cmd_ptr_) {
385359
RCLCPP_INFO_THROTTLE(
@@ -389,8 +363,6 @@ void PacmodInterface::publishCommands()
389363
return;
390364
}
391365

392-
publishVehicleStatus();
393-
394366
const rclcpp::Time current_time = get_clock()->now();
395367

396368
double desired_throttle = actuation_cmd_ptr_->actuation.accel_cmd + accel_pedal_offset_;

0 commit comments

Comments
 (0)