diff --git a/pacmod_interface/include/pacmod_interface/pacmod_interface.hpp b/pacmod_interface/include/pacmod_interface/pacmod_interface.hpp index e34da0f..3055814 100644 --- a/pacmod_interface/include/pacmod_interface/pacmod_interface.hpp +++ b/pacmod_interface/include/pacmod_interface/pacmod_interface.hpp @@ -45,10 +45,6 @@ #include #include -#include -#include -#include - #include #include #include @@ -66,12 +62,6 @@ class PacmodInterface : public rclcpp::Node PacmodInterface(); private: - typedef message_filters::sync_policies::ApproximateTime< - pacmod3_msgs::msg::SystemRptFloat, pacmod3_msgs::msg::WheelSpeedRpt, - pacmod3_msgs::msg::SystemRptFloat, pacmod3_msgs::msg::SystemRptFloat, - pacmod3_msgs::msg::SystemRptInt, pacmod3_msgs::msg::SystemRptInt, pacmod3_msgs::msg::GlobalRpt> - PacmodFeedbacksSyncPolicy; - /* subscribers */ // From Autoware rclcpp::Subscription::SharedPtr @@ -87,16 +77,13 @@ class PacmodInterface : public rclcpp::Node // From Pacmod rclcpp::Subscription::SharedPtr rear_door_rpt_sub_; - std::unique_ptr> - steer_wheel_rpt_sub_; - std::unique_ptr> - wheel_speed_rpt_sub_; - std::unique_ptr> accel_rpt_sub_; - std::unique_ptr> brake_rpt_sub_; - std::unique_ptr> shift_rpt_sub_; - std::unique_ptr> turn_rpt_sub_; - std::unique_ptr> global_rpt_sub_; - std::unique_ptr> pacmod_feedbacks_sync_; + rclcpp::Subscription::SharedPtr steer_wheel_rpt_sub_; + rclcpp::Subscription::SharedPtr wheel_speed_rpt_sub_; + rclcpp::Subscription::SharedPtr accel_rpt_sub_; + rclcpp::Subscription::SharedPtr brake_rpt_sub_; + rclcpp::Subscription::SharedPtr shift_rpt_sub_; + rclcpp::Subscription::SharedPtr turn_rpt_sub_; + rclcpp::Subscription::SharedPtr global_rpt_sub_; /* publishers */ // To Pacmod @@ -129,6 +116,14 @@ class PacmodInterface : public rclcpp::Node /* ros param */ std::string base_frame_id_; int command_timeout_ms_; // vehicle_cmd timeout [ms] + bool is_steer_wheel_rpt_received_ = false; + bool is_wheel_speed_rpt_received_ = false; + bool is_accel_rpt_received_ = false; + bool is_brake_rpt_received_ = false; + bool is_shift_rpt_received_ = false; + bool is_turn_rpt_received_ = false; + bool is_global_rpt_received_ = false; + bool is_pacmod_rpt_received_ = false; bool is_pacmod_enabled_ = false; bool is_clear_override_needed_ = false; @@ -206,17 +201,19 @@ class PacmodInterface : public rclcpp::Node const autoware_auto_vehicle_msgs::msg::HazardLightsCommand::ConstSharedPtr msg); void callbackEngage(const autoware_auto_vehicle_msgs::msg::Engage::ConstSharedPtr msg); void callbackRearDoor(const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr rear_door_rpt); - void callbackPacmodRpt( - const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr steer_wheel_rpt, - const pacmod3_msgs::msg::WheelSpeedRpt::ConstSharedPtr wheel_speed_rpt, - const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr accel_rpt, - const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr brake_rpt, - const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr gear_cmd_rpt, - const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr turn_rpt, - const pacmod3_msgs::msg::GlobalRpt::ConstSharedPtr global_rpt); + void callbackSteerWheelRpt( + const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr steer_wheel_rpt); + void callbackWheelSpeedRpt( + const pacmod3_msgs::msg::WheelSpeedRpt::ConstSharedPtr wheel_speed_rpt); + void callbackAccelRpt(const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr accel_rpt); + void callbackBrakeRpt(const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr brake_rpt); + void callbackGearRpt(const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr gear_rpt); + void callbackTurnRpt(const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr turn_rpt); + void callbackGlobalRpt(const pacmod3_msgs::msg::GlobalRpt::ConstSharedPtr global_rpt); /* functions */ void publishCommands(); + void publishVehicleStatus(); double calculateVehicleVelocity( const pacmod3_msgs::msg::WheelSpeedRpt & wheel_speed_rpt, const pacmod3_msgs::msg::SystemRptInt & shift_rpt); diff --git a/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp b/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp index 698b80c..fa9850d 100644 --- a/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp +++ b/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp @@ -96,32 +96,20 @@ PacmodInterface::PacmodInterface() rear_door_rpt_sub_ = create_subscription( "/pacmod/rear_pass_door_rpt", 1, std::bind(&PacmodInterface::callbackRearDoor, this, _1)); - steer_wheel_rpt_sub_ = - std::make_unique>( - this, "/pacmod/steering_rpt"); - wheel_speed_rpt_sub_ = - std::make_unique>( - this, "/pacmod/wheel_speed_rpt"); - accel_rpt_sub_ = std::make_unique>( - this, "/pacmod/accel_rpt"); - brake_rpt_sub_ = std::make_unique>( - this, "/pacmod/brake_rpt"); - shift_rpt_sub_ = std::make_unique>( - this, "/pacmod/shift_rpt"); - turn_rpt_sub_ = std::make_unique>( - this, "/pacmod/turn_rpt"); - global_rpt_sub_ = std::make_unique>( - this, "/pacmod/global_rpt"); - - pacmod_feedbacks_sync_ = - std::make_unique>( - PacmodFeedbacksSyncPolicy(10), *steer_wheel_rpt_sub_, *wheel_speed_rpt_sub_, *accel_rpt_sub_, - *brake_rpt_sub_, *shift_rpt_sub_, *turn_rpt_sub_, *global_rpt_sub_); - - pacmod_feedbacks_sync_->registerCallback(std::bind( - &PacmodInterface::callbackPacmodRpt, this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, - std::placeholders::_7)); + steer_wheel_rpt_sub_ = create_subscription( + "/pacmod/steering_rpt", 1, std::bind(&PacmodInterface::callbackSteerWheelRpt, this, _1)); + wheel_speed_rpt_sub_ = create_subscription( + "/pacmod/wheel_speed_rpt", 1, std::bind(&PacmodInterface::callbackWheelSpeedRpt, this, _1)); + accel_rpt_sub_ = create_subscription( + "/pacmod/accel_rpt", 1, std::bind(&PacmodInterface::callbackAccelRpt, this, _1)); + brake_rpt_sub_ = create_subscription( + "/pacmod/brake_rpt", 1, std::bind(&PacmodInterface::callbackBrakeRpt, this, _1)); + shift_rpt_sub_ = create_subscription( + "/pacmod/shift_rpt", 1, std::bind(&PacmodInterface::callbackGearRpt, this, _1)); + turn_rpt_sub_ = create_subscription( + "/pacmod/turn_rpt", 1, std::bind(&PacmodInterface::callbackTurnRpt, this, _1)); + global_rpt_sub_ = create_subscription( + "/pacmod/global_rpt", 1, std::bind(&PacmodInterface::callbackGlobalRpt, this, _1)); /* publisher */ // To pacmod @@ -241,24 +229,57 @@ void PacmodInterface::callbackRearDoor( door_status_pub_->publish(toAutowareDoorStatusMsg(*rear_door_rpt)); } -void PacmodInterface::callbackPacmodRpt( - const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr steer_wheel_rpt, - const pacmod3_msgs::msg::WheelSpeedRpt::ConstSharedPtr wheel_speed_rpt, - const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr accel_rpt, - const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr brake_rpt, - const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr shift_rpt, - const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr turn_rpt, - const pacmod3_msgs::msg::GlobalRpt::ConstSharedPtr global_rpt) +void PacmodInterface::callbackSteerWheelRpt( + const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr steer_wheel_rpt) { - is_pacmod_rpt_received_ = true; + is_steer_wheel_rpt_received_ = true; steer_wheel_rpt_ptr_ = steer_wheel_rpt; +} + +void PacmodInterface::callbackWheelSpeedRpt( + const pacmod3_msgs::msg::WheelSpeedRpt::ConstSharedPtr wheel_speed_rpt) +{ + is_wheel_speed_rpt_received_ = true; wheel_speed_rpt_ptr_ = wheel_speed_rpt; +} + +void PacmodInterface::callbackAccelRpt( + const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr accel_rpt) +{ + is_accel_rpt_received_ = true; accel_rpt_ptr_ = accel_rpt; +} + +void PacmodInterface::callbackBrakeRpt( + const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr brake_rpt) +{ + is_brake_rpt_received_ = true; brake_rpt_ptr_ = brake_rpt; - gear_cmd_rpt_ptr_ = shift_rpt; - global_rpt_ptr_ = global_rpt; +} + +void PacmodInterface::callbackGearRpt( + const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr gear_rpt) +{ + is_shift_rpt_received_ = true; + gear_cmd_rpt_ptr_ = gear_rpt; +} + +void PacmodInterface::callbackTurnRpt( + const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr turn_rpt) +{ + is_turn_rpt_received_ = true; turn_rpt_ptr_ = turn_rpt; +} +void PacmodInterface::callbackGlobalRpt( + const pacmod3_msgs::msg::GlobalRpt::ConstSharedPtr global_rpt) +{ + is_global_rpt_received_ = true; + global_rpt_ptr_ = global_rpt; +} + +void PacmodInterface::publishVehicleStatus() +{ is_pacmod_enabled_ = steer_wheel_rpt_ptr_->enabled && accel_rpt_ptr_->enabled && brake_rpt_ptr_->enabled; RCLCPP_DEBUG( @@ -293,7 +314,7 @@ void PacmodInterface::callbackPacmodRpt( autoware_auto_vehicle_msgs::msg::ControlModeReport control_mode_msg; control_mode_msg.stamp = header.stamp; - if (global_rpt->enabled && is_pacmod_enabled_) { + if (global_rpt_ptr_->enabled && is_pacmod_enabled_) { control_mode_msg.mode = autoware_auto_vehicle_msgs::msg::ControlModeReport::AUTONOMOUS; } else { control_mode_msg.mode = autoware_auto_vehicle_msgs::msg::ControlModeReport::MANUAL; @@ -344,18 +365,23 @@ void PacmodInterface::callbackPacmodRpt( { autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport turn_msg; turn_msg.stamp = header.stamp; - turn_msg.report = toAutowareTurnIndicatorsReport(*turn_rpt); + turn_msg.report = toAutowareTurnIndicatorsReport(*turn_rpt_ptr_); turn_indicators_status_pub_->publish(turn_msg); autoware_auto_vehicle_msgs::msg::HazardLightsReport hazard_msg; hazard_msg.stamp = header.stamp; - hazard_msg.report = toAutowareHazardLightsReport(*turn_rpt); + hazard_msg.report = toAutowareHazardLightsReport(*turn_rpt_ptr_); hazard_lights_status_pub_->publish(hazard_msg); } } void PacmodInterface::publishCommands() { + is_pacmod_rpt_received_ = is_steer_wheel_rpt_received_ && is_wheel_speed_rpt_received_ && + is_accel_rpt_received_ && is_brake_rpt_received_ && + is_shift_rpt_received_ && is_turn_rpt_received_ && + is_global_rpt_received_; + /* guard */ if (!actuation_cmd_ptr_ || !control_cmd_ptr_ || !is_pacmod_rpt_received_ || !gear_cmd_ptr_) { RCLCPP_INFO_THROTTLE( @@ -365,6 +391,8 @@ void PacmodInterface::publishCommands() return; } + publishVehicleStatus(); + const rclcpp::Time current_time = get_clock()->now(); double desired_throttle = actuation_cmd_ptr_->actuation.accel_cmd + accel_pedal_offset_;