diff --git a/build_depends.repos b/build_depends.repos index 28f32e64ea1e5..471cfbe567991 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -47,3 +47,7 @@ repositories: type: git url: https://github.com/tier4/pointcloud_to_laserscan.git version: tier4/main + vendor/pacmod3_msgs: + type: git + url: https://github.com/astuff/pacmod3_msgs + version: main diff --git a/vehicle/pacmod_interface/CMakeLists.txt b/vehicle/pacmod_interface/CMakeLists.txt new file mode 100644 index 0000000000000..177e506806ab6 --- /dev/null +++ b/vehicle/pacmod_interface/CMakeLists.txt @@ -0,0 +1,45 @@ +cmake_minimum_required(VERSION 3.5) +project(pacmod_interface) + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +ament_auto_add_executable(pacmod_additional_debug_publisher + src/pacmod_additional_debug_publisher/pacmod_additional_debug_publisher_node.cpp + src/pacmod_additional_debug_publisher/pacmod_additional_debug_publisher_main.cpp +) + +ament_auto_add_executable(pacmod_dynamic_parameter_changer + src/pacmod_dynamic_parameter_changer/pacmod_dynamic_parameter_changer_node.cpp + src/pacmod_dynamic_parameter_changer/pacmod_dynamic_parameter_changer_main.cpp +) + +ament_auto_add_executable(pacmod_interface + src/pacmod_interface/pacmod_interface.cpp + src/pacmod_interface/pacmod_interface_node.cpp +) + +ament_auto_add_executable(pacmod_diag_publisher + src/pacmod_interface/pacmod_diag_publisher.cpp + src/pacmod_interface/pacmod_diag_publisher_node.cpp +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/vehicle/pacmod_interface/README.md b/vehicle/pacmod_interface/README.md new file mode 100644 index 0000000000000..c0087db38ae09 --- /dev/null +++ b/vehicle/pacmod_interface/README.md @@ -0,0 +1,81 @@ +# pacmod_interface + +`pacmod_interface` is the package to connect Autoware with Pacmod. + +## Input / Output + +### Input topics + +- From Autoware + + | Name | Type | Description | + | -------------------------------------- | -------------------------------------------------------- | ----------------------------------------------------- | + | `/control/command/control_cmd` | autoware_auto_control_msgs::msg::AckermannControlCommand | lateral and longitudinal control command | + | `/control/command/gear_cmd` | autoware_auto_vehicle_msgs::msg::GearCommand | gear command | + | `/control/command/turn_indicators_cmd` | autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand | turn indicators command | + | `/control/command/hazard_lights_cmd` | autoware_auto_vehicle_msgs::msg::HazardLightsCommand | hazard lights command | + | `/vehicle/engage` | autoware_auto_vehicle_msgs::msg::Engage | engage command | + | `/vehicle/command/actuation_cmd` | autoware_vehicle_msgs::msg::ActuationCommandStamped | actuation (accel/brake pedal, steering wheel) command | + | `/control/command/emergency_cmd` | autoware_vehicle_msgs::msg::VehicleEmergencyStamped | emergency command | + +- From Pacmod + + | Name | Type | Description | + | ------------------------- | --------------------------------- | ----------------------------------------------------------------------- | + | `/pacmod/steering_rpt` | pacmod3_msgs::msg::SystemRptFloat | current steering wheel angle | + | `/pacmod/wheel_speed_rpt` | pacmod3_msgs::msg::WheelSpeedRpt | current wheel speed | + | `/pacmod/accel_rpt` | pacmod3_msgs::msg::SystemRptFloat | current accel pedal | + | `/pacmod/brake_rpt` | pacmod3_msgs::msg::SystemRptFloat | current brake pedal | + | `/pacmod/shift_rpt` | pacmod3_msgs::msg::SystemRptInt | current gear status | + | `/pacmod/turn_rpt` | pacmod3_msgs::msg::SystemRptInt | current turn indicators status | + | `/pacmod/global_rpt` | pacmod3_msgs::msg::GlobalRpt | current status of other parameters (e.g. override_active, can_time_out) | + +### Output topics + +- To Pacmod + + | Name | Type | Description | + | ---------------------- | --------------------------------- | ----------------------------------------------------- | + | `pacmod/accel_cmd` | pacmod3_msgs::msg::SystemCmdFloat | accel pedal command | + | `pacmod/brake_cmd` | pacmod3_msgs::msg::SystemCmdFloat | brake pedal command | + | `pacmod/steering_cmd` | pacmod3_msgs::msg::SystemCmdFloat | steering wheel angle and angular velocity command | + | `pacmod/shift_cmd` | pacmod3_msgs::msg::SystemCmdInt | gear command | + | `pacmod/turn_cmd` | pacmod3_msgs::msg::SystemCmdInt | turn indicators command | + | `pacmod/raw_steer_cmd` | pacmod3_msgs::msg::SteerSystemCmd | raw steering wheel angle and angular velocity command | + +- To Autoware + + | Name | Type | Description | + | ---------------------------------------- | ------------------------------------------------------- | ---------------------------------------------------- | + | `/vehicle/status/control_mode` | autoware_auto_vehicle_msgs::msg::ControlModeReport | control mode | + | `/vehicle/status/velocity_status` | autoware_auto_vehicle_msgs::msg::VelocityReport | velocity | + | `/vehicle/status/steering_status` | autoware_auto_vehicle_msgs::msg::SteeringReport | steering wheel angle | + | `/vehicle/status/gear_status` | autoware_auto_vehicle_msgs::msg::GearReport | gear status | + | `/vehicle/status/turn_indicators_status` | autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport | turn indicators status | + | `/vehicle/status/hazard_lights_status` | autoware_auto_vehicle_msgs::msg::HazardLightsReport | hazard lights status | + | `/vehicle/status/actuation_status` | autoware_auto_vehicle_msgs::msg::ActuationStatusStamped | actuation (accel/brake pedal, steering wheel) status | + +## ROS Parameters + +| Name | Type | Description | +| --------------------------------- | ------ | ----------------------------------------------------------------------------------------- | +| `base_frame_id` | string | frame id (assigned in pacmod command, but it does not make sense) | +| `command_timeout_ms` | double | timeout [ms] | +| `loop_rate` | double | loop rate to publish commands | +| `steering_offset` | double | steering wheel angle offset | +| `enable_steering_rate_control` | bool | when enabled, max steering wheel rate is used for steering wheel angular velocity command | +| `emergency_brake` | double | brake pedal for emergency | +| `vgr_coef_a` | double | coefficient to calculate steering wheel angle | +| `vgr_coef_b` | double | coefficient to calculate steering wheel angle | +| `vgr_coef_c` | double | coefficient to calculate steering wheel angle | +| `accel_pedal_offset` | double | accel pedal offset | +| `brake_pedal_offset` | double | brake pedal offset | +| `max_throttle` | double | max accel pedal | +| `max_brake` | double | max brake pedal | +| `max_steering_wheel` | double | max steering wheel angle | +| `max_steering_wheel_rate` | double | max steering wheel angular velocity | +| `min_steering_wheel_rate` | double | min steering wheel angular velocity | +| `steering_wheel_rate_low_vel` | double | min steering wheel angular velocity when velocity is low | +| `steering_wheel_rate_low_stopped` | double | min steering wheel angular velocity when velocity is almost 0 | +| `low_vel_thresh` | double | threshold velocity to decide the velocity is low for `steering_wheel_rate_low_vel` | +| `hazard_thresh_time` | double | threshold time to keep hazard lights | diff --git a/vehicle/pacmod_interface/config/pacmod.param.yaml b/vehicle/pacmod_interface/config/pacmod.param.yaml new file mode 100644 index 0000000000000..11b4aca96f77e --- /dev/null +++ b/vehicle/pacmod_interface/config/pacmod.param.yaml @@ -0,0 +1,19 @@ +/**: + ros__parameters: + base_frame_id: "base_link" + command_timeout_ms: 1000 + loop_rate: 30.0 + emergency_brake: 0.7 + max_throttle: 0.4 + max_brake: 0.8 + max_steering_wheel: 11.0 + min_steering_wheel: -11.0 + max_steering_wheel_rate: 5.0 + min_steering_wheel_rate: 3.0 + steering_offset: 0.0 + enable_steering_rate_control: false + vgr_coef_a: 15.713 + vgr_coef_b: 0.053 + vgr_coef_c: 0.042 + accel_pedal_offset: 0.0 + brake_pedal_offset: 0.0 diff --git a/vehicle/pacmod_interface/config/pacmod_extra.param.yaml b/vehicle/pacmod_interface/config/pacmod_extra.param.yaml new file mode 100644 index 0000000000000..f20548d196bbd --- /dev/null +++ b/vehicle/pacmod_interface/config/pacmod_extra.param.yaml @@ -0,0 +1,20 @@ +/**: + ros__parameters: + straight_p_gain: 0.8 + straight_i_gain: 0.002 + straight_lugre_fc: 0.02 + straight_rtz_offset: 0.085 + curve_p_gain: 0.4 + curve_i_gain: 0.002 + curve_lugre_fc: 0.01 + curve_rtz_offset: 0.085 + min_steer_thresh: 0.1 + max_steer_thresh: 0.4 + p_gain_rate_max: 1.5 + p_gain_rate_min: -5.0 + i_gain_rate_max: 0.05 + i_gain_rate_min: -0.05 + lugre_fc_rate_max: 0.1 + lugre_fc_rate_min: -0.1 + rtz_offset_rate_max: 0.5 + rtz_offset_rate_min: -0.5 diff --git a/vehicle/pacmod_interface/include/pacmod_additional_debug_publisher/pacmod_additional_debug_publisher_node.hpp b/vehicle/pacmod_interface/include/pacmod_additional_debug_publisher/pacmod_additional_debug_publisher_node.hpp new file mode 100644 index 0000000000000..c4dddf8674f7d --- /dev/null +++ b/vehicle/pacmod_interface/include/pacmod_additional_debug_publisher/pacmod_additional_debug_publisher_node.hpp @@ -0,0 +1,47 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PACMOD_ADDITIONAL_DEBUG_PUBLISHER__PACMOD_ADDITIONAL_DEBUG_PUBLISHER_NODE_HPP_ +#define PACMOD_ADDITIONAL_DEBUG_PUBLISHER__PACMOD_ADDITIONAL_DEBUG_PUBLISHER_NODE_HPP_ + +#include + +#include +#include + +class PacmodAdditionalDebugPublisherNode : public rclcpp::Node +{ +private: + rclcpp::Publisher::SharedPtr debug_pub_; + rclcpp::Publisher::SharedPtr + accel_cal_rpt_pub_; + rclcpp::Publisher::SharedPtr + brake_cal_rpt_pub_; + rclcpp::Publisher::SharedPtr + steer_cal_rpt_pub_; + + rclcpp::Subscription::SharedPtr sub_; + autoware_debug_msgs::msg::Float32MultiArrayStamped debug_value_; + autoware_debug_msgs::msg::Float32MultiArrayStamped accel_cal_rpt_; + autoware_debug_msgs::msg::Float32MultiArrayStamped brake_cal_rpt_; + autoware_debug_msgs::msg::Float32MultiArrayStamped steer_cal_rpt_; + bool calibration_active_; + void canTxCallback(const can_msgs::msg::Frame::ConstSharedPtr msg); + +public: + PacmodAdditionalDebugPublisherNode(); + ~PacmodAdditionalDebugPublisherNode() {} +}; + +#endif // PACMOD_ADDITIONAL_DEBUG_PUBLISHER__PACMOD_ADDITIONAL_DEBUG_PUBLISHER_NODE_HPP_ diff --git a/vehicle/pacmod_interface/include/pacmod_dynamic_parameter_changer/pacmod_dynamic_parameter_changer_node.hpp b/vehicle/pacmod_interface/include/pacmod_dynamic_parameter_changer/pacmod_dynamic_parameter_changer_node.hpp new file mode 100644 index 0000000000000..40b52b54b16b9 --- /dev/null +++ b/vehicle/pacmod_interface/include/pacmod_dynamic_parameter_changer/pacmod_dynamic_parameter_changer_node.hpp @@ -0,0 +1,64 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PACMOD_DYNAMIC_PARAMETER_CHANGER__PACMOD_DYNAMIC_PARAMETER_CHANGER_NODE_HPP_ +#define PACMOD_DYNAMIC_PARAMETER_CHANGER__PACMOD_DYNAMIC_PARAMETER_CHANGER_NODE_HPP_ + +#include + +#include +#include +#include + +struct ParamList +{ + double p_gain; + double i_gain; + double lugre_fc; + double rtz_offset; +}; + +class PacmodDynamicParameterChangerNode : public rclcpp::Node +{ +private: + rclcpp::Publisher::SharedPtr debug_pub_; + rclcpp::Publisher::SharedPtr can_pub_; + rclcpp::Subscription::SharedPtr steer_rpt_sub_; + + ParamList straight_course_param_; + ParamList curve_course_param_; + ParamList param_max_rate_; + ParamList param_min_rate_; + double min_steer_thresh_; + double max_steer_thresh_; + + ParamList current_param_list_; + rclcpp::Time current_param_time_; + +public: + PacmodDynamicParameterChangerNode(); + void subSteerRpt(const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr msg); + ParamList calculateParam( + const double current_steer_cmd, const double current_steer, const bool enabled); + void sendCanMsg(const ParamList param_list); + void sendDebugMsg(const ParamList param_list); + + ParamList interpolateParam(const ParamList p1, const ParamList p2, const double p1_rate); + ParamList rateLimit(const ParamList new_param, const ParamList current_param); + double rateLimit( + const double new_value, const double current_value, const double delta_time, + const double max_rate, const double min_rate); +}; + +#endif // PACMOD_DYNAMIC_PARAMETER_CHANGER__PACMOD_DYNAMIC_PARAMETER_CHANGER_NODE_HPP_ diff --git a/vehicle/pacmod_interface/include/pacmod_interface/pacmod_diag_publisher.hpp b/vehicle/pacmod_interface/include/pacmod_interface/pacmod_diag_publisher.hpp new file mode 100644 index 0000000000000..a499864462397 --- /dev/null +++ b/vehicle/pacmod_interface/include/pacmod_interface/pacmod_diag_publisher.hpp @@ -0,0 +1,112 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PACMOD_INTERFACE__PACMOD_DIAG_PUBLISHER_HPP_ +#define PACMOD_INTERFACE__PACMOD_DIAG_PUBLISHER_HPP_ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +class PacmodDiagPublisher : public rclcpp::Node +{ +public: + PacmodDiagPublisher(); + +private: + using PacmodFeedbacksSyncPolicy = 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>; + + /* subscribers */ + + // From Pacmod + 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_; + + // From CAN + rclcpp::Subscription::SharedPtr can_sub_; + + /* ros parameters */ + double can_timeout_sec_; + double pacmod3_msgs_timeout_sec_; + + /* variables */ + rclcpp::Time last_can_received_time_; + rclcpp::Time last_pacmod3_msgs_received_time_; + bool is_pacmod_rpt_received_ = false; + pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr steer_wheel_rpt_ptr_; // [rad] + pacmod3_msgs::msg::WheelSpeedRpt::ConstSharedPtr wheel_speed_rpt_ptr_; // [m/s] + pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr accel_rpt_ptr_; + pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr brake_rpt_ptr_; + pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr shift_rpt_ptr_; + pacmod3_msgs::msg::GlobalRpt::ConstSharedPtr global_rpt_ptr_; + pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr turn_rpt_ptr_; + + // Diagnostic Updater + std::shared_ptr updater_ptr_; + + /* callbacks */ + 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 shift_rpt, + const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr turn_rpt, + const pacmod3_msgs::msg::GlobalRpt::ConstSharedPtr global_rpt); + + void callbackCan(const can_msgs::msg::Frame::ConstSharedPtr can); + + /* functions */ + void checkPacmodMsgs(diagnostic_updater::DiagnosticStatusWrapper & stat); + std::string addMsg(const std::string & original_msg, const std::string & additional_msg); + + bool isTimeoutCanMsgs(); + bool isTimeoutPacmodMsgs(); + bool receivedPacmodMsgs(); + bool isBrakeActuatorAccident(); + bool isBrakeWireAccident(); + bool isAccelAccident(); + bool isOtherAccident(); +}; + +#endif // PACMOD_INTERFACE__PACMOD_DIAG_PUBLISHER_HPP_ diff --git a/vehicle/pacmod_interface/include/pacmod_interface/pacmod_interface.hpp b/vehicle/pacmod_interface/include/pacmod_interface/pacmod_interface.hpp new file mode 100644 index 0000000000000..3a29e6748dd68 --- /dev/null +++ b/vehicle/pacmod_interface/include/pacmod_interface/pacmod_interface.hpp @@ -0,0 +1,226 @@ +// Copyright 2017-2019 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PACMOD_INTERFACE__PACMOD_INTERFACE_HPP_ +#define PACMOD_INTERFACE__PACMOD_INTERFACE_HPP_ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +class PacmodInterface : public rclcpp::Node +{ +public: + using ActuationCommandStamped = autoware_vehicle_msgs::msg::ActuationCommandStamped; + using ActuationStatusStamped = autoware_vehicle_msgs::msg::ActuationStatusStamped; + using SteeringWheelStatusStamped = autoware_vehicle_msgs::msg::SteeringWheelStatusStamped; + 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 + control_cmd_sub_; + rclcpp::Subscription::SharedPtr gear_cmd_sub_; + rclcpp::Subscription::SharedPtr + turn_indicators_cmd_sub_; + rclcpp::Subscription::SharedPtr + hazard_lights_cmd_sub_; + rclcpp::Subscription::SharedPtr engage_cmd_sub_; + rclcpp::Subscription::SharedPtr actuation_cmd_sub_; + rclcpp::Subscription::SharedPtr + emergency_sub_; + + // From Pacmod + 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_; + + /* publishers */ + // To Pacmod + rclcpp::Publisher::SharedPtr accel_cmd_pub_; + rclcpp::Publisher::SharedPtr brake_cmd_pub_; + rclcpp::Publisher::SharedPtr steer_cmd_pub_; + rclcpp::Publisher::SharedPtr shift_cmd_pub_; + rclcpp::Publisher::SharedPtr turn_cmd_pub_; + rclcpp::Publisher::SharedPtr + raw_steer_cmd_pub_; // only for debug + + // To Autoware + rclcpp::Publisher::SharedPtr + control_mode_pub_; + rclcpp::Publisher::SharedPtr vehicle_twist_pub_; + rclcpp::Publisher::SharedPtr + steering_status_pub_; + rclcpp::Publisher::SharedPtr gear_status_pub_; + rclcpp::Publisher::SharedPtr + turn_indicators_status_pub_; + rclcpp::Publisher::SharedPtr + hazard_lights_status_pub_; + rclcpp::Publisher::SharedPtr actuation_status_pub_; + rclcpp::Publisher::SharedPtr steering_wheel_status_pub_; + + rclcpp::TimerBase::SharedPtr timer_; + + /* ros param */ + std::string base_frame_id_; + int command_timeout_ms_; // vehicle_cmd timeout [ms] + bool is_pacmod_rpt_received_ = false; + bool is_pacmod_enabled_ = false; + bool is_clear_override_needed_ = false; + bool prev_override_ = false; + double loop_rate_; // [Hz] + double tire_radius_; // [m] + double wheel_base_; // [m] + double steering_offset_; // [rad] def: measured = truth + offset + double vgr_coef_a_; // variable gear ratio coeffs + double vgr_coef_b_; // variable gear ratio coeffs + double vgr_coef_c_; // variable gear ratio coeffs + double accel_pedal_offset_; // offset of accel pedal value + double brake_pedal_offset_; // offset of brake pedal value + + double emergency_brake_; // brake command when emergency [m/s^2] + double max_throttle_; // max throttle [0~1] + double max_brake_; // max throttle [0~1] + double max_steering_wheel_; // max steering wheel angle [rad] + double max_steering_wheel_rate_; // [rad/s] + double min_steering_wheel_rate_; // [rad/s] + double steering_wheel_rate_low_vel_; // [rad/s] + double steering_wheel_rate_stopped_; // [rad/s] + double low_vel_thresh_; // [m/s] + + bool enable_steering_rate_control_; // use steering angle speed for command [rad/s] + + double hazard_thresh_time_; + int hazard_recover_count_ = 0; + const int hazard_recover_cmd_num_ = 5; + + vehicle_info_util::VehicleInfo vehicle_info_; + + /* input values */ + ActuationCommandStamped::ConstSharedPtr actuation_cmd_ptr_; + autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr control_cmd_ptr_; + autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand::ConstSharedPtr turn_indicators_cmd_ptr_; + autoware_auto_vehicle_msgs::msg::HazardLightsCommand::ConstSharedPtr hazard_lights_cmd_ptr_; + autoware_auto_vehicle_msgs::msg::GearCommand::ConstSharedPtr gear_cmd_ptr_; + + pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr steer_wheel_rpt_ptr_; // [rad] + pacmod3_msgs::msg::WheelSpeedRpt::ConstSharedPtr wheel_speed_rpt_ptr_; // [m/s] + pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr accel_rpt_ptr_; + pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr brake_rpt_ptr_; // [m/s] + pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr gear_cmd_rpt_ptr_; // [m/s] + pacmod3_msgs::msg::GlobalRpt::ConstSharedPtr global_rpt_ptr_; // [m/s] + pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr turn_rpt_ptr_; + pacmod3_msgs::msg::SteeringCmd prev_steer_cmd_; + + bool engage_cmd_{false}; + bool is_emergency_{false}; + rclcpp::Time control_command_received_time_; + rclcpp::Time actuation_command_received_time_; + rclcpp::Time last_shift_inout_matched_time_; + + /* callbacks */ + void callbackActuationCmd(const ActuationCommandStamped::ConstSharedPtr msg); + void callbackControlCmd( + const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg); + + void callbackEmergencyCmd( + const autoware_vehicle_msgs::msg::VehicleEmergencyStamped::ConstSharedPtr msg); + + void callbackGearCmd(const autoware_auto_vehicle_msgs::msg::GearCommand::ConstSharedPtr msg); + void callbackTurnIndicatorsCommand( + const autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand::ConstSharedPtr msg); + void callbackHazardLightsCommand( + const autoware_auto_vehicle_msgs::msg::HazardLightsCommand::ConstSharedPtr msg); + void callbackEngage(const autoware_auto_vehicle_msgs::msg::Engage::ConstSharedPtr msg); + 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); + + /* functions */ + void publishCommands(); + double calculateVehicleVelocity( + const pacmod3_msgs::msg::WheelSpeedRpt & wheel_speed_rpt, + const pacmod3_msgs::msg::SystemRptInt & shift_rpt); + double calculateVariableGearRatio(const double vel, const double steer_wheel); + double calcSteerWheelRateCmd(const double gear_ratio); + uint16_t toPacmodShiftCmd(const autoware_auto_vehicle_msgs::msg::GearCommand & gear_cmd); + uint16_t toPacmodTurnCmd( + const autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand & turn, + const autoware_auto_vehicle_msgs::msg::HazardLightsCommand & hazard); + uint16_t toPacmodTurnCmdWithHazardRecover( + const autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand & turn, + const autoware_auto_vehicle_msgs::msg::HazardLightsCommand & hazard); + + std::optional toAutowareShiftReport(const pacmod3_msgs::msg::SystemRptInt & shift); + int32_t toAutowareTurnIndicatorsReport(const pacmod3_msgs::msg::SystemRptInt & turn); + int32_t toAutowareHazardLightsReport(const pacmod3_msgs::msg::SystemRptInt & turn); + double steerWheelRateLimiter( + const double current_steer_cmd, const double prev_steer_cmd, + const rclcpp::Time & current_steer_time, const rclcpp::Time & prev_steer_time, + const double steer_rate, const double current_steer_output, const bool engage); +}; + +#endif // PACMOD_INTERFACE__PACMOD_INTERFACE_HPP_ diff --git a/vehicle/pacmod_interface/launch/pacmod_additional_debug_publisher.launch.xml b/vehicle/pacmod_interface/launch/pacmod_additional_debug_publisher.launch.xml new file mode 100644 index 0000000000000..1912c1a1ec967 --- /dev/null +++ b/vehicle/pacmod_interface/launch/pacmod_additional_debug_publisher.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/vehicle/pacmod_interface/launch/pacmod_interface.launch.xml b/vehicle/pacmod_interface/launch/pacmod_interface.launch.xml new file mode 100644 index 0000000000000..d8fc978cc8cfe --- /dev/null +++ b/vehicle/pacmod_interface/launch/pacmod_interface.launch.xml @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/vehicle/pacmod_interface/package.xml b/vehicle/pacmod_interface/package.xml new file mode 100644 index 0000000000000..f718e614ca71e --- /dev/null +++ b/vehicle/pacmod_interface/package.xml @@ -0,0 +1,32 @@ + + + + pacmod_interface + 0.1.0 + AutonomouStuff pacmod interface as a ROS 2 node + Akihito Ohsato + Apache License 2.0 + + Akihito Ohsato + T.Ando + Horibe Takamasa + + autoware_auto_control_msgs + autoware_auto_vehicle_msgs + autoware_debug_msgs + autoware_vehicle_msgs + can_msgs + diagnostic_updater + message_filters + pacmod3_msgs + rclcpp + std_msgs + vehicle_info_util + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/vehicle/pacmod_interface/src/pacmod_additional_debug_publisher/pacmod_additional_debug_publisher_main.cpp b/vehicle/pacmod_interface/src/pacmod_additional_debug_publisher/pacmod_additional_debug_publisher_main.cpp new file mode 100644 index 0000000000000..eca9b0120bf40 --- /dev/null +++ b/vehicle/pacmod_interface/src/pacmod_additional_debug_publisher/pacmod_additional_debug_publisher_main.cpp @@ -0,0 +1,28 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/vehicle/pacmod_interface/src/pacmod_additional_debug_publisher/pacmod_additional_debug_publisher_node.cpp b/vehicle/pacmod_interface/src/pacmod_additional_debug_publisher/pacmod_additional_debug_publisher_node.cpp new file mode 100644 index 0000000000000..9c3a0da046bf2 --- /dev/null +++ b/vehicle/pacmod_interface/src/pacmod_additional_debug_publisher/pacmod_additional_debug_publisher_node.cpp @@ -0,0 +1,160 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +namespace +{ +bool isTargetId(uint32_t id) +{ + return id == 0x32C || id == 0x451 || id == 0x452 || id == 0x453 || id == 0x454 || id == 0x455 || + id == 0x456 || id == 0x457 || id == 0x790 || id == 0x791 || id == 0x792; +} +} // namespace + +PacmodAdditionalDebugPublisherNode::PacmodAdditionalDebugPublisherNode() +: Node("pacmod_additional_debug_publisher") +{ + using std::placeholders::_1; + + debug_pub_ = create_publisher( + "output/debug", rclcpp::QoS{1}); + sub_ = create_subscription( + "input/can_tx", rclcpp::QoS{1}, + std::bind(&PacmodAdditionalDebugPublisherNode::canTxCallback, this, _1)); + debug_value_.data.resize(17); + calibration_active_ = this->declare_parameter("calibration_active", false); + if (calibration_active_) { + accel_cal_rpt_pub_ = create_publisher( + "output/accel_cal_rpt", 1); + brake_cal_rpt_pub_ = create_publisher( + "output/brake_cal_rpt", 1); + steer_cal_rpt_pub_ = create_publisher( + "output/steer_cal_rpt", 1); + accel_cal_rpt_.data.resize(3); + brake_cal_rpt_.data.resize(5); + steer_cal_rpt_.data.resize(3); + } +} + +void PacmodAdditionalDebugPublisherNode::canTxCallback( + const can_msgs::msg::Frame::ConstSharedPtr msg) +{ + if (isTargetId(msg->id)) { + float debug1 = 0.0; + float debug2 = 0.0; + float debug3 = 0.0; + float debug4 = 0.0; + if (msg->id == 0x790) { + int16_t temp = 0; + temp = (static_cast(msg->data[0]) << 8) | msg->data[1]; + accel_cal_rpt_.data.at(0) = static_cast(temp / 1000.0); // accel_a_volt + temp = (static_cast(msg->data[2]) << 8) | msg->data[3]; + accel_cal_rpt_.data.at(1) = static_cast(temp / 1000.0); // accel_b_volt + temp = (static_cast(msg->data[4]) << 8) | msg->data[5]; + accel_cal_rpt_.data.at(2) = static_cast(temp / 1000.0); // output + } else if (msg->id == 0x791) { + int16_t temp = 0; + int8_t temp1 = 0; + temp = (static_cast(msg->data[0]) << 8) | msg->data[1]; + brake_cal_rpt_.data.at(0) = static_cast(temp / 1000.0); // sks1_volt + temp = (static_cast(msg->data[2]) << 8) | msg->data[3]; + brake_cal_rpt_.data.at(1) = static_cast(temp / 1000.0); // sks2_volt + temp1 = static_cast(msg->data[4]); + brake_cal_rpt_.data.at(2) = static_cast(temp1 / 100.0); // pedal_position + temp1 = static_cast(msg->data[5]); + brake_cal_rpt_.data.at(3) = static_cast(temp1 / 100.0); // brake_cmd + temp = (static_cast(msg->data[6]) << 8) | msg->data[7]; + brake_cal_rpt_.data.at(4) = static_cast(temp / 1000.0); // globe_position + } else if (msg->id == 0x792) { + int16_t temp = 0; + temp = (static_cast(msg->data[0]) << 8) | msg->data[1]; + steer_cal_rpt_.data.at(0) = static_cast(temp / 1000.0); // trq1_volt + temp = (static_cast(msg->data[2]) << 8) | msg->data[3]; + steer_cal_rpt_.data.at(1) = static_cast(temp / 1000.0); // trq2_volt + temp = (static_cast(msg->data[4]) << 8) | msg->data[5]; + steer_cal_rpt_.data.at(2) = static_cast(temp / 1000.0); // position + } else if (msg->id == 0x32C) { + int16_t temp = 0; + temp = (static_cast(msg->data[0]) << 8) | msg->data[1]; + debug1 = temp / 1000.0; + temp = (static_cast(msg->data[2]) << 8) | msg->data[3]; + debug2 = temp / 100.0; + temp = (static_cast(msg->data[4]) << 8) | msg->data[5]; + debug3 = temp / 1000.0; + temp = (static_cast(msg->data[6]) << 8) | msg->data[7]; + debug4 = temp / 100.0; + } else { + union Data { + uint32_t uint32_value; + float float_value; + } temp; + temp.uint32_value = (static_cast(msg->data[3]) << 24) | + (static_cast(msg->data[2]) << 16) | + (static_cast(msg->data[1]) << 8) | msg->data[0]; + debug1 = temp.float_value; + temp.uint32_value = (static_cast(msg->data[7]) << 24) | + (static_cast(msg->data[6]) << 16) | + (static_cast(msg->data[5]) << 8) | msg->data[4]; + debug2 = temp.float_value; + } + switch (msg->id) { + case 0x32C: + debug_value_.data.at(0) = debug1; // steering pos + debug_value_.data.at(1) = debug2; // steering_eps_assist + debug_value_.data.at(2) = debug3; // steering rate + debug_value_.data.at(3) = debug4; // steering eps input + break; + case 0x451: + debug_value_.data.at(4) = debug1; // pid command + debug_value_.data.at(5) = debug2; // pid output + break; + case 0x452: + debug_value_.data.at(6) = debug1; // pid_error + debug_value_.data.at(7) = debug2; // pid_output + break; + case 0x453: + debug_value_.data.at(8) = debug1; // pid_p_term + debug_value_.data.at(9) = debug2; // pid_i_term + break; + case 0x454: + debug_value_.data.at(10) = debug1; // pid_d_term + debug_value_.data.at(11) = debug2; // pid_filtered_rate + break; + case 0x455: + debug_value_.data.at(12) = debug1; // lugre + debug_value_.data.at(13) = debug2; // rtz + break; + case 0x456: + debug_value_.data.at(14) = debug1; // lugre_rtz_filtered_rate + debug_value_.data.at(15) = debug2; // ctrl_dt + break; + case 0x457: + debug_value_.data.at(16) = debug1; // rpt_dt + break; + default: + break; + } + debug_value_.stamp = this->now(); + debug_pub_->publish(debug_value_); + if (calibration_active_) { + accel_cal_rpt_.stamp = this->now(); + brake_cal_rpt_.stamp = this->now(); + steer_cal_rpt_.stamp = this->now(); + accel_cal_rpt_pub_->publish(accel_cal_rpt_); + brake_cal_rpt_pub_->publish(brake_cal_rpt_); + steer_cal_rpt_pub_->publish(steer_cal_rpt_); + } + } +} diff --git a/vehicle/pacmod_interface/src/pacmod_dynamic_parameter_changer/pacmod_dynamic_parameter_changer_main.cpp b/vehicle/pacmod_interface/src/pacmod_dynamic_parameter_changer/pacmod_dynamic_parameter_changer_main.cpp new file mode 100644 index 0000000000000..c0f0ad100d90a --- /dev/null +++ b/vehicle/pacmod_interface/src/pacmod_dynamic_parameter_changer/pacmod_dynamic_parameter_changer_main.cpp @@ -0,0 +1,26 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/vehicle/pacmod_interface/src/pacmod_dynamic_parameter_changer/pacmod_dynamic_parameter_changer_node.cpp b/vehicle/pacmod_interface/src/pacmod_dynamic_parameter_changer/pacmod_dynamic_parameter_changer_node.cpp new file mode 100644 index 0000000000000..aa54680772cae --- /dev/null +++ b/vehicle/pacmod_interface/src/pacmod_dynamic_parameter_changer/pacmod_dynamic_parameter_changer_node.cpp @@ -0,0 +1,166 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +PacmodDynamicParameterChangerNode::PacmodDynamicParameterChangerNode() +: Node("pacmod_dynamic_parameter_changer") +{ + straight_course_param_.p_gain = declare_parameter("straight_p_gain", 0.8); + straight_course_param_.i_gain = declare_parameter("straight_i_gain", 0.002); + straight_course_param_.lugre_fc = declare_parameter("straight_lugre_fc", 0.03); + straight_course_param_.rtz_offset = declare_parameter("straight_rtz_offset", 0.085); + curve_course_param_.p_gain = declare_parameter("curve_p_gain", 0.3); + curve_course_param_.i_gain = declare_parameter("curve_i_gain", 0.002); + curve_course_param_.lugre_fc = declare_parameter("curve_lugre_fc", 0.01); + curve_course_param_.rtz_offset = declare_parameter("curve_rtz_offset", 0.085); + min_steer_thresh_ = declare_parameter("min_steer_thresh", 0.2); + max_steer_thresh_ = declare_parameter("max_steer_thresh", 0.5); + param_max_rate_.p_gain = declare_parameter("p_gain_rate_max", 5.0); + param_max_rate_.i_gain = declare_parameter("i_gain_rate_max", 0.05); + param_max_rate_.lugre_fc = declare_parameter("lugre_fc_rate_max", 0.1); + param_max_rate_.rtz_offset = declare_parameter("rtz_offset_rate_max", 0.5); + param_min_rate_.p_gain = declare_parameter("p_gain_rate_min", -1.5); + param_min_rate_.i_gain = declare_parameter("i_gain_rate_min", -0.05); + param_min_rate_.lugre_fc = declare_parameter("lugre_fc_rate_min", -0.1); + param_min_rate_.rtz_offset = declare_parameter("rtz_offset_rate_min", -0.5); + + current_param_list_ = curve_course_param_; + + can_pub_ = create_publisher("~/output/can", rclcpp::QoS{1}); + debug_pub_ = + create_publisher("~/debug/parameter", rclcpp::QoS{1}); + + using std::placeholders::_1; + steer_rpt_sub_ = create_subscription( + "~/input/steer_rpt", rclcpp::QoS{1}, + std::bind(&PacmodDynamicParameterChangerNode::subSteerRpt, this, _1)); +} + +void PacmodDynamicParameterChangerNode::subSteerRpt( + const pacmod3_msgs::msg::SystemRptFloat::ConstSharedPtr msg) +{ + current_param_list_ = calculateParam(std::abs(msg->command), std::abs(msg->output), msg->enabled); + current_param_time_ = now(); + sendCanMsg(current_param_list_); + sendDebugMsg(current_param_list_); +} + +ParamList PacmodDynamicParameterChangerNode::calculateParam( + const double current_steer_cmd, const double current_steer, const bool enabled) +{ + if (!enabled) { + // return curve (default) param + return curve_course_param_; + } + + const double steer = std::max(current_steer, current_steer_cmd); + // run straight course (steer is low) -> return straight-course-param + // run curve course (steer is high) -> return curve-course-param + + if (steer >= max_steer_thresh_) { + return curve_course_param_; + } else if (steer <= min_steer_thresh_) { + return straight_course_param_; + } + + const double straight_rate = + (max_steer_thresh_ - steer) / (max_steer_thresh_ - min_steer_thresh_); + + const auto interpolate_param = + interpolateParam(straight_course_param_, curve_course_param_, straight_rate); + return rateLimit(interpolate_param, current_param_list_); +} + +void PacmodDynamicParameterChangerNode::sendDebugMsg(const ParamList param_list) +{ + // publish debug msg + std_msgs::msg::Float32MultiArray msg; + msg.data.resize(4); + msg.data.at(0) = param_list.p_gain; + msg.data.at(1) = param_list.i_gain; + msg.data.at(2) = param_list.lugre_fc; + msg.data.at(3) = param_list.rtz_offset; + debug_pub_->publish(msg); +} + +void PacmodDynamicParameterChangerNode::sendCanMsg(const ParamList param_list) +{ + // encoding + can_msgs::msg::Frame frame; + frame.header.stamp = now(); + frame.id = 0x13C; + frame.is_rtr = false; + frame.is_extended = false; + frame.is_error = false; + frame.dlc = 8; + uint16_t kp_u = static_cast(1000.0 * param_list.p_gain); + uint16_t ki_u = static_cast(1000.0 * param_list.i_gain); + uint16_t fc_u = static_cast(1000.0 * param_list.lugre_fc); + uint16_t rtz_u = static_cast(1000.0 * param_list.rtz_offset); + frame.data[0] = (kp_u & 0xFF00) >> 8; + frame.data[1] = ki_u & 0x00FF; + frame.data[2] = (ki_u & 0xFF00) >> 8; + frame.data[3] = ki_u & 0x00FF; + frame.data[4] = (fc_u & 0xFF00) >> 8; + frame.data[5] = fc_u & 0x00FF; + frame.data[6] = (rtz_u & 0xFF00) >> 8; + frame.data[7] = rtz_u & 0x00FF; + + // publish can msg + can_pub_->publish(frame); +} + +ParamList PacmodDynamicParameterChangerNode::interpolateParam( + const ParamList p1, const ParamList p2, const double p1_rate) +{ + const double p2_rate = 1.0 - p1_rate; + ParamList p; + p.p_gain = p1.p_gain * p1_rate + p2.p_gain * p2_rate; + p.i_gain = p1.i_gain * p1_rate + p2.i_gain * p2_rate; + p.lugre_fc = p1.lugre_fc * p1_rate + p2.lugre_fc * p2_rate; + p.rtz_offset = p1.rtz_offset * p1_rate + p2.rtz_offset * p2_rate; + return p; +} + +ParamList PacmodDynamicParameterChangerNode::rateLimit( + const ParamList new_param, const ParamList current_param) +{ + ParamList limited_param; + const double dt = (now() - current_param_time_).seconds(); + + // apply rate limit + limited_param.p_gain = rateLimit( + new_param.p_gain, current_param.p_gain, dt, param_max_rate_.p_gain, param_min_rate_.p_gain); + limited_param.i_gain = rateLimit( + new_param.i_gain, current_param.i_gain, dt, param_max_rate_.i_gain, param_min_rate_.i_gain); + limited_param.lugre_fc = rateLimit( + new_param.lugre_fc, current_param.lugre_fc, dt, param_max_rate_.lugre_fc, + param_min_rate_.lugre_fc); + limited_param.rtz_offset = rateLimit( + new_param.rtz_offset, current_param.rtz_offset, dt, param_max_rate_.rtz_offset, + param_min_rate_.rtz_offset); + return limited_param; +} + +double PacmodDynamicParameterChangerNode::rateLimit( + const double new_value, const double current_value, const double delta_time, + const double max_rate, const double min_rate) +{ + const double dval = new_value - current_value; + const double limit_dval = std::min(delta_time * max_rate, std::max(dval, delta_time * min_rate)); + return current_value + limit_dval; +} diff --git a/vehicle/pacmod_interface/src/pacmod_interface/pacmod_diag_publisher.cpp b/vehicle/pacmod_interface/src/pacmod_interface/pacmod_diag_publisher.cpp new file mode 100644 index 0000000000000..049375d51c732 --- /dev/null +++ b/vehicle/pacmod_interface/src/pacmod_interface/pacmod_diag_publisher.cpp @@ -0,0 +1,186 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include + +PacmodDiagPublisher::PacmodDiagPublisher() +: Node("pacmod_diag_publisher"), + last_can_received_time_(this->now()), + last_pacmod3_msgs_received_time_(this->now()) +{ + /* ros parameters */ + can_timeout_sec_ = declare_parameter("can_timeout_sec", 10.0); + pacmod3_msgs_timeout_sec_ = declare_parameter("pacmod_msg_timeout_sec", 10.0); + const double update_rate = declare_parameter("update_rate", 10.0); + + /* Diagnostic Updater */ + updater_ptr_ = std::make_shared(this, 1.0 / update_rate); + updater_ptr_->setHardwareID("pacmod_checker"); + updater_ptr_->add("pacmod_checker", this, &PacmodDiagPublisher::checkPacmodMsgs); + + /* register subscribers */ + can_sub_ = create_subscription( + "/pacmod/can_tx", 1, std::bind(&PacmodDiagPublisher::callbackCan, this, std::placeholders::_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( + &PacmodDiagPublisher::callbackPacmodRpt, this, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, + std::placeholders::_7)); +} + +void PacmodDiagPublisher::callbackCan( + [[maybe_unused]] const can_msgs::msg::Frame::ConstSharedPtr can) +{ + last_can_received_time_ = this->now(); +} + +void PacmodDiagPublisher::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) +{ + last_pacmod3_msgs_received_time_ = this->now(); + steer_wheel_rpt_ptr_ = steer_wheel_rpt; + wheel_speed_rpt_ptr_ = wheel_speed_rpt; + accel_rpt_ptr_ = accel_rpt; + brake_rpt_ptr_ = brake_rpt; + shift_rpt_ptr_ = shift_rpt; + global_rpt_ptr_ = global_rpt; + turn_rpt_ptr_ = turn_rpt; + is_pacmod_rpt_received_ = true; +} + +void PacmodDiagPublisher::checkPacmodMsgs(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + using DiagStatus = diagnostic_msgs::msg::DiagnosticStatus; + int8_t level = DiagStatus::OK; + std::string msg = ""; + + if (isTimeoutCanMsgs()) { + level = DiagStatus::ERROR; + msg = addMsg(msg, "CAN reception timeout."); + } + + if (isTimeoutPacmodMsgs()) { + level = DiagStatus::ERROR; + msg = addMsg(msg, "Pacmod msgs reception timeout."); + } + + if (!receivedPacmodMsgs()) { + if (level == DiagStatus::OK) { + msg = "OK"; + } + stat.summary(level, msg); + // do not receive pacmod msgs yet. + return; + } + + if (isBrakeActuatorAccident()) { + level = DiagStatus::ERROR; + msg = addMsg(msg, "Brake actuator failure."); + } + + if (isBrakeWireAccident()) { + level = DiagStatus::ERROR; + msg = addMsg(msg, "Brake wire failure."); + } + + if (isAccelAccident()) { + level = DiagStatus::ERROR; + msg = addMsg(msg, "Accel module failure."); + } + + if (level == DiagStatus::OK && isOtherAccident()) { + level = DiagStatus::ERROR; + msg = addMsg(msg, "Unknown Pacmod Error."); + } + + if (level == DiagStatus::OK) { + msg = "OK"; + } + stat.summary(level, msg); +} + +std::string PacmodDiagPublisher::addMsg( + const std::string & original_msg, const std::string & additional_msg) +{ + if (original_msg == "") { + return additional_msg; + } + + return original_msg + " ; " + additional_msg; +} + +bool PacmodDiagPublisher::isTimeoutCanMsgs() +{ + const double dt = (this->now() - last_can_received_time_).seconds(); + return dt > can_timeout_sec_; +} + +bool PacmodDiagPublisher::isTimeoutPacmodMsgs() +{ + const double dt = (this->now() - last_pacmod3_msgs_received_time_).seconds(); + return dt > pacmod3_msgs_timeout_sec_; +} + +bool PacmodDiagPublisher::receivedPacmodMsgs() { return is_pacmod_rpt_received_; } + +bool PacmodDiagPublisher::isBrakeActuatorAccident() +{ + return global_rpt_ptr_->pacmod_sys_fault_active && brake_rpt_ptr_->pacmod_fault; +} + +bool PacmodDiagPublisher::isBrakeWireAccident() +{ + return global_rpt_ptr_->pacmod_sys_fault_active && brake_rpt_ptr_->command_output_fault; +} + +bool PacmodDiagPublisher::isAccelAccident() +{ + return global_rpt_ptr_->pacmod_sys_fault_active && accel_rpt_ptr_->input_output_fault; +} + +bool PacmodDiagPublisher::isOtherAccident() { return global_rpt_ptr_->pacmod_sys_fault_active; } diff --git a/vehicle/pacmod_interface/src/pacmod_interface/pacmod_diag_publisher_node.cpp b/vehicle/pacmod_interface/src/pacmod_interface/pacmod_diag_publisher_node.cpp new file mode 100644 index 0000000000000..c79f003d4bea9 --- /dev/null +++ b/vehicle/pacmod_interface/src/pacmod_interface/pacmod_diag_publisher_node.cpp @@ -0,0 +1,27 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/vehicle/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp b/vehicle/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp new file mode 100644 index 0000000000000..55ddc4f1980a0 --- /dev/null +++ b/vehicle/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp @@ -0,0 +1,688 @@ +// Copyright 2017-2019 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include + +PacmodInterface::PacmodInterface() +: Node("pacmod_interface"), + vehicle_info_(vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo()) +{ + /* setup parameters */ + base_frame_id_ = declare_parameter("base_frame_id", "base_link"); + command_timeout_ms_ = declare_parameter("command_timeout_ms", 1000); + loop_rate_ = declare_parameter("loop_rate", 30.0); + + /* parameters for vehicle specifications */ + tire_radius_ = vehicle_info_.wheel_radius_m; + wheel_base_ = vehicle_info_.wheel_base_m; + + steering_offset_ = declare_parameter("steering_offset", 0.0); + enable_steering_rate_control_ = declare_parameter("enable_steering_rate_control", false); + + /* parameters for emergency stop */ + emergency_brake_ = declare_parameter("emergency_brake", 0.7); + + /* vehicle parameters */ + vgr_coef_a_ = declare_parameter("vgr_coef_a", 15.713); + vgr_coef_b_ = declare_parameter("vgr_coef_b", 0.053); + vgr_coef_c_ = declare_parameter("vgr_coef_c", 0.042); + accel_pedal_offset_ = declare_parameter("accel_pedal_offset", 0.0); + brake_pedal_offset_ = declare_parameter("brake_pedal_offset", 0.0); + + /* parameters for limitter */ + max_throttle_ = declare_parameter("max_throttle", 0.2); + max_brake_ = declare_parameter("max_brake", 0.8); + max_steering_wheel_ = declare_parameter("max_steering_wheel", 2.7 * M_PI); + max_steering_wheel_rate_ = declare_parameter("max_steering_wheel_rate", 6.6); + min_steering_wheel_rate_ = declare_parameter("min_steering_wheel_rate", 0.5); + steering_wheel_rate_low_vel_ = declare_parameter("steering_wheel_rate_low_vel", 5.0); + steering_wheel_rate_stopped_ = declare_parameter("steering_wheel_rate_stopped", 5.0); + low_vel_thresh_ = declare_parameter("low_vel_thresh", 1.389); // 5.0kmh + + /* parameters for turn signal recovery */ + hazard_thresh_time_ = declare_parameter("hazard_thresh_time", 0.20); // s + /* initialize */ + prev_steer_cmd_.header.stamp = this->now(); + prev_steer_cmd_.command = 0.0; + + /* subscribers */ + using std::placeholders::_1; + + // From autoware + control_cmd_sub_ = create_subscription( + "/control/command/control_cmd", 1, std::bind(&PacmodInterface::callbackControlCmd, this, _1)); + gear_cmd_sub_ = create_subscription( + "/control/command/gear_cmd", 1, std::bind(&PacmodInterface::callbackGearCmd, this, _1)); + turn_indicators_cmd_sub_ = + create_subscription( + "/control/command/turn_indicators_cmd", rclcpp::QoS{1}, + std::bind(&PacmodInterface::callbackTurnIndicatorsCommand, this, _1)); + hazard_lights_cmd_sub_ = + create_subscription( + "/control/command/hazard_lights_cmd", rclcpp::QoS{1}, + std::bind(&PacmodInterface::callbackHazardLightsCommand, this, _1)); + engage_cmd_sub_ = create_subscription( + "/vehicle/engage", rclcpp::QoS{1}, std::bind(&PacmodInterface::callbackEngage, this, _1)); + actuation_cmd_sub_ = create_subscription( + "/control/command/actuation_cmd", 1, + std::bind(&PacmodInterface::callbackActuationCmd, this, _1)); + emergency_sub_ = create_subscription( + "/control/command/emergency_cmd", 1, + std::bind(&PacmodInterface::callbackEmergencyCmd, this, _1)); + + // From pacmod + + 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)); + + /* publisher */ + // To pacmod + accel_cmd_pub_ = + create_publisher("/pacmod/accel_cmd", rclcpp::QoS{1}); + brake_cmd_pub_ = + create_publisher("/pacmod/brake_cmd", rclcpp::QoS{1}); + steer_cmd_pub_ = + create_publisher("/pacmod/steering_cmd", rclcpp::QoS{1}); + shift_cmd_pub_ = + create_publisher("/pacmod/shift_cmd", rclcpp::QoS{1}); + turn_cmd_pub_ = + create_publisher("/pacmod/turn_cmd", rclcpp::QoS{1}); + raw_steer_cmd_pub_ = create_publisher( + "/pacmod/raw_steer_cmd", rclcpp::QoS{1}); // only for debug + + // To Autoware + control_mode_pub_ = create_publisher( + "/vehicle/status/control_mode", rclcpp::QoS{1}); + vehicle_twist_pub_ = create_publisher( + "/vehicle/status/velocity_status", rclcpp::QoS{1}); + steering_status_pub_ = create_publisher( + "/vehicle/status/steering_status", rclcpp::QoS{1}); + gear_status_pub_ = create_publisher( + "/vehicle/status/gear_status", rclcpp::QoS{1}); + turn_indicators_status_pub_ = + create_publisher( + "/vehicle/status/turn_indicators_status", rclcpp::QoS{1}); + hazard_lights_status_pub_ = create_publisher( + "/vehicle/status/hazard_lights_status", rclcpp::QoS{1}); + actuation_status_pub_ = + create_publisher("/vehicle/status/actuation_status", 1); + steering_wheel_status_pub_ = + create_publisher("/vehicle/status/steering_wheel_status", 1); + + // Timer + auto timer_callback = std::bind(&PacmodInterface::publishCommands, this); + auto period = std::chrono::duration_cast( + std::chrono::duration(1.0 / loop_rate_)); + timer_ = std::make_shared>( + this->get_clock(), period, std::move(timer_callback), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(timer_, nullptr); +} + +void PacmodInterface::callbackActuationCmd(const ActuationCommandStamped::ConstSharedPtr msg) +{ + actuation_command_received_time_ = this->now(); + actuation_cmd_ptr_ = msg; +} + +void PacmodInterface::callbackEmergencyCmd( + const autoware_vehicle_msgs::msg::VehicleEmergencyStamped::ConstSharedPtr msg) +{ + is_emergency_ = msg->emergency; +} + +void PacmodInterface::callbackControlCmd( + const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg) +{ + control_command_received_time_ = this->now(); + control_cmd_ptr_ = msg; +} + +void PacmodInterface::callbackGearCmd( + const autoware_auto_vehicle_msgs::msg::GearCommand::ConstSharedPtr msg) +{ + gear_cmd_ptr_ = msg; +} + +void PacmodInterface::callbackTurnIndicatorsCommand( + const autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand::ConstSharedPtr msg) +{ + turn_indicators_cmd_ptr_ = msg; +} + +void PacmodInterface::callbackHazardLightsCommand( + const autoware_auto_vehicle_msgs::msg::HazardLightsCommand::ConstSharedPtr msg) +{ + hazard_lights_cmd_ptr_ = msg; +} + +void PacmodInterface::callbackEngage( + const autoware_auto_vehicle_msgs::msg::Engage::ConstSharedPtr msg) +{ + engage_cmd_ = msg->engage; + is_clear_override_needed_ = true; +} + +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) +{ + is_pacmod_rpt_received_ = true; + steer_wheel_rpt_ptr_ = steer_wheel_rpt; + wheel_speed_rpt_ptr_ = wheel_speed_rpt; + accel_rpt_ptr_ = accel_rpt; + brake_rpt_ptr_ = brake_rpt; + gear_cmd_rpt_ptr_ = shift_rpt; + global_rpt_ptr_ = global_rpt; + turn_rpt_ptr_ = turn_rpt; + + is_pacmod_enabled_ = + steer_wheel_rpt_ptr_->enabled && accel_rpt_ptr_->enabled && brake_rpt_ptr_->enabled; + RCLCPP_DEBUG( + get_logger(), + "enabled: is_pacmod_enabled_ %d, steer %d, accel %d, brake %d, shift %d, " + "global %d", + is_pacmod_enabled_, steer_wheel_rpt_ptr_->enabled, accel_rpt_ptr_->enabled, + brake_rpt_ptr_->enabled, gear_cmd_rpt_ptr_->enabled, global_rpt_ptr_->enabled); + + const double current_velocity = calculateVehicleVelocity( + *wheel_speed_rpt_ptr_, *gear_cmd_rpt_ptr_); // current vehicle speed > 0 [m/s] + const double current_steer_wheel = + steer_wheel_rpt_ptr_->output; // current vehicle steering wheel angle [rad] + const double adaptive_gear_ratio = + calculateVariableGearRatio(current_velocity, current_steer_wheel); + const double current_steer = current_steer_wheel / adaptive_gear_ratio - steering_offset_; + + std_msgs::msg::Header header; + header.frame_id = base_frame_id_; + header.stamp = get_clock()->now(); + + /* publish steering wheel status */ + { + SteeringWheelStatusStamped steering_wheel_status_msg; + steering_wheel_status_msg.stamp = header.stamp; + steering_wheel_status_msg.data = current_steer_wheel; + steering_wheel_status_pub_->publish(steering_wheel_status_msg); + } + + /* publish vehicle status control_mode */ + { + autoware_auto_vehicle_msgs::msg::ControlModeReport control_mode_msg; + control_mode_msg.stamp = header.stamp; + + if (global_rpt->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; + } + + control_mode_pub_->publish(control_mode_msg); + } + + /* publish vehicle status twist */ + { + autoware_auto_vehicle_msgs::msg::VelocityReport twist; + twist.header = header; + twist.longitudinal_velocity = current_velocity; // [m/s] + twist.heading_rate = current_velocity * std::tan(current_steer) / wheel_base_; // [rad/s] + vehicle_twist_pub_->publish(twist); + } + + /* publish current shift */ + { + autoware_auto_vehicle_msgs::msg::GearReport gear_report_msg; + gear_report_msg.stamp = header.stamp; + const auto opt_gear_report = toAutowareShiftReport(*gear_cmd_rpt_ptr_); + if (opt_gear_report) { + gear_report_msg.report = *opt_gear_report; + gear_status_pub_->publish(gear_report_msg); + } + } + + /* publish current status */ + { + autoware_auto_vehicle_msgs::msg::SteeringReport steer_msg; + steer_msg.stamp = header.stamp; + steer_msg.steering_tire_angle = current_steer; + steering_status_pub_->publish(steer_msg); + } + + /* publish control status */ + { + ActuationStatusStamped actuation_status; + actuation_status.header = header; + actuation_status.status.accel_status = accel_rpt_ptr_->output; + actuation_status.status.brake_status = brake_rpt_ptr_->output; + actuation_status.status.steer_status = current_steer; + actuation_status_pub_->publish(actuation_status); + } + + /* publish current turn signal */ + { + autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport turn_msg; + turn_msg.stamp = header.stamp; + turn_msg.report = toAutowareTurnIndicatorsReport(*turn_rpt); + 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_lights_status_pub_->publish(hazard_msg); + } +} + +void PacmodInterface::publishCommands() +{ + /* guard */ + if (!actuation_cmd_ptr_ || !control_cmd_ptr_ || !is_pacmod_rpt_received_ || !gear_cmd_ptr_) { + RCLCPP_INFO_THROTTLE( + get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), + "vehicle_cmd = %d, pacmod3_msgs = %d", actuation_cmd_ptr_ != nullptr, + is_pacmod_rpt_received_); + return; + } + + const rclcpp::Time current_time = get_clock()->now(); + + double desired_throttle = actuation_cmd_ptr_->actuation.accel_cmd + accel_pedal_offset_; + double desired_brake = actuation_cmd_ptr_->actuation.brake_cmd + brake_pedal_offset_; + if (actuation_cmd_ptr_->actuation.brake_cmd <= std::numeric_limits::epsilon()) { + desired_brake = 0.0; + } + + /* check emergency and timeout */ + const double control_cmd_delta_time_ms = + (current_time - control_command_received_time_).seconds() * 1000.0; + const double actuation_cmd_delta_time_ms = + (current_time - actuation_command_received_time_).seconds() * 1000.0; + bool timeouted = false; + const int t_out = command_timeout_ms_; + if (t_out >= 0 && (control_cmd_delta_time_ms > t_out || actuation_cmd_delta_time_ms > t_out)) { + timeouted = true; + } + if (is_emergency_ || timeouted) { + RCLCPP_ERROR( + get_logger(), "Emergency Stopping, emergency = %d, timeouted = %d", is_emergency_, timeouted); + desired_throttle = 0.0; + desired_brake = emergency_brake_; + } + + const double current_velocity = + calculateVehicleVelocity(*wheel_speed_rpt_ptr_, *gear_cmd_rpt_ptr_); + const double current_steer_wheel = steer_wheel_rpt_ptr_->output; + + /* calculate desired steering wheel */ + double adaptive_gear_ratio = calculateVariableGearRatio(current_velocity, current_steer_wheel); + double desired_steer_wheel = + (control_cmd_ptr_->lateral.steering_tire_angle + steering_offset_) * adaptive_gear_ratio; + desired_steer_wheel = + std::min(std::max(desired_steer_wheel, -max_steering_wheel_), max_steering_wheel_); + + /* check clear flag */ + bool clear_override = false; + if (is_pacmod_enabled_ == true) { + is_clear_override_needed_ = false; + } else if (is_clear_override_needed_ == true) { + clear_override = true; + } + + /* make engage cmd false when a driver overrides vehicle control */ + if (!prev_override_ && global_rpt_ptr_->override_active) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), + "Pacmod is overridden, enable flag is back to false"); + engage_cmd_ = false; + } + prev_override_ = global_rpt_ptr_->override_active; + + /* make engage cmd false when vehicle report is timed out, e.g. E-stop is depressed */ + const bool report_timed_out = ((current_time - global_rpt_ptr_->header.stamp).seconds() > 1.0); + if (report_timed_out) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), + "Pacmod report is timed out, enable flag is back to false"); + engage_cmd_ = false; + } + + /* make engage cmd false when vehicle fault is active */ + if (global_rpt_ptr_->pacmod_sys_fault_active) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), + "Pacmod fault is active, enable flag is back to false"); + engage_cmd_ = false; + } + RCLCPP_DEBUG( + get_logger(), + "is_pacmod_enabled_ = %d, is_clear_override_needed_ = %d, clear_override = " + "%d", + is_pacmod_enabled_, is_clear_override_needed_, clear_override); + + /* check shift change */ + const double brake_for_shift_trans = 0.7; + uint16_t desired_shift = gear_cmd_rpt_ptr_->output; + if (std::fabs(current_velocity) < 0.1) { // velocity is low -> the shift can be changed + if (toPacmodShiftCmd(*gear_cmd_ptr_) != gear_cmd_rpt_ptr_->output) { // need shift + // change. + desired_throttle = 0.0; + desired_brake = brake_for_shift_trans; // set brake to change the shift + desired_shift = toPacmodShiftCmd(*gear_cmd_ptr_); + RCLCPP_DEBUG( + get_logger(), "Doing shift change. current = %d, desired = %d. set brake_cmd to %f", + gear_cmd_rpt_ptr_->output, toPacmodShiftCmd(*gear_cmd_ptr_), desired_brake); + } + } + + /* publish accel cmd */ + { + pacmod3_msgs::msg::SystemCmdFloat accel_cmd; + accel_cmd.header.frame_id = base_frame_id_; + accel_cmd.header.stamp = current_time; + accel_cmd.enable = engage_cmd_; + accel_cmd.ignore_overrides = false; + accel_cmd.clear_override = clear_override; + accel_cmd.command = std::max(0.0, std::min(desired_throttle, max_throttle_)); + accel_cmd_pub_->publish(accel_cmd); + } + + /* publish brake cmd */ + { + pacmod3_msgs::msg::SystemCmdFloat brake_cmd; + brake_cmd.header.frame_id = base_frame_id_; + brake_cmd.header.stamp = current_time; + brake_cmd.enable = engage_cmd_; + brake_cmd.ignore_overrides = false; + brake_cmd.clear_override = clear_override; + brake_cmd.command = std::max(0.0, std::min(desired_brake, max_brake_)); + brake_cmd_pub_->publish(brake_cmd); + } + + /* publish steering cmd */ + { + pacmod3_msgs::msg::SteeringCmd steer_cmd; + steer_cmd.header.frame_id = base_frame_id_; + steer_cmd.header.stamp = current_time; + steer_cmd.enable = engage_cmd_; + steer_cmd.ignore_overrides = false; + steer_cmd.clear_override = clear_override; + steer_cmd.rotation_rate = calcSteerWheelRateCmd(adaptive_gear_ratio); + steer_cmd.command = steerWheelRateLimiter( + desired_steer_wheel, prev_steer_cmd_.command, current_time, prev_steer_cmd_.header.stamp, + steer_cmd.rotation_rate, current_steer_wheel, engage_cmd_); + steer_cmd_pub_->publish(steer_cmd); + prev_steer_cmd_ = steer_cmd; + } + + /* publish raw steering cmd for debug */ + { + pacmod3_msgs::msg::SteeringCmd raw_steer_cmd; + raw_steer_cmd.header.frame_id = base_frame_id_; + raw_steer_cmd.header.stamp = current_time; + raw_steer_cmd.enable = engage_cmd_; + raw_steer_cmd.ignore_overrides = false; + raw_steer_cmd.clear_override = clear_override; + raw_steer_cmd.command = desired_steer_wheel; + raw_steer_cmd.rotation_rate = + control_cmd_ptr_->lateral.steering_tire_rotation_rate * adaptive_gear_ratio; + raw_steer_cmd_pub_->publish(raw_steer_cmd); + } + + /* publish shift cmd */ + { + pacmod3_msgs::msg::SystemCmdInt shift_cmd; + shift_cmd.header.frame_id = base_frame_id_; + shift_cmd.header.stamp = current_time; + shift_cmd.enable = engage_cmd_; + shift_cmd.ignore_overrides = false; + shift_cmd.clear_override = clear_override; + shift_cmd.command = desired_shift; + shift_cmd_pub_->publish(shift_cmd); + } + + if (turn_indicators_cmd_ptr_ && hazard_lights_cmd_ptr_) { + /* publish shift cmd */ + pacmod3_msgs::msg::SystemCmdInt turn_cmd; + turn_cmd.header.frame_id = base_frame_id_; + turn_cmd.header.stamp = current_time; + turn_cmd.enable = engage_cmd_; + turn_cmd.ignore_overrides = false; + turn_cmd.clear_override = clear_override; + turn_cmd.command = + toPacmodTurnCmdWithHazardRecover(*turn_indicators_cmd_ptr_, *hazard_lights_cmd_ptr_); + turn_cmd_pub_->publish(turn_cmd); + } +} + +double PacmodInterface::calcSteerWheelRateCmd(const double gear_ratio) +{ + const auto current_vel = + std::fabs(calculateVehicleVelocity(*wheel_speed_rpt_ptr_, *gear_cmd_rpt_ptr_)); + + // send low steer rate at low speed + if (current_vel < std::numeric_limits::epsilon()) { + return steering_wheel_rate_stopped_; + } else if (current_vel < low_vel_thresh_) { + return steering_wheel_rate_low_vel_; + } + + if (!enable_steering_rate_control_) { + return max_steering_wheel_rate_; + } + + constexpr double margin = 1.5; + const double rate = margin * control_cmd_ptr_->lateral.steering_tire_rotation_rate * gear_ratio; + return std::min(std::max(std::fabs(rate), min_steering_wheel_rate_), max_steering_wheel_rate_); +} + +double PacmodInterface::calculateVehicleVelocity( + const pacmod3_msgs::msg::WheelSpeedRpt & wheel_speed_rpt, + const pacmod3_msgs::msg::SystemRptInt & shift_rpt) +{ + const double sign = (shift_rpt.output == pacmod3_msgs::msg::SystemRptInt::SHIFT_REVERSE) ? -1 : 1; + const double vel = + (wheel_speed_rpt.rear_left_wheel_speed + wheel_speed_rpt.rear_right_wheel_speed) * 0.5 * + tire_radius_; + return sign * vel; +} + +double PacmodInterface::calculateVariableGearRatio(const double vel, const double steer_wheel) +{ + return std::max( + 1e-5, vgr_coef_a_ + vgr_coef_b_ * vel * vel - vgr_coef_c_ * std::fabs(steer_wheel)); +} + +uint16_t PacmodInterface::toPacmodShiftCmd( + const autoware_auto_vehicle_msgs::msg::GearCommand & gear_cmd) +{ + using pacmod3_msgs::msg::SystemCmdInt; + + if (gear_cmd.command == autoware_auto_vehicle_msgs::msg::GearCommand::PARK) { + return SystemCmdInt::SHIFT_PARK; + } + if (gear_cmd.command == autoware_auto_vehicle_msgs::msg::GearCommand::REVERSE) { + return SystemCmdInt::SHIFT_REVERSE; + } + if (gear_cmd.command == autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE) { + return SystemCmdInt::SHIFT_FORWARD; + } + if (gear_cmd.command == autoware_auto_vehicle_msgs::msg::GearCommand::LOW) { + return SystemCmdInt::SHIFT_LOW; + } + + return SystemCmdInt::SHIFT_NONE; +} + +std::optional PacmodInterface::toAutowareShiftReport( + const pacmod3_msgs::msg::SystemRptInt & shift) +{ + using autoware_auto_vehicle_msgs::msg::GearReport; + using pacmod3_msgs::msg::SystemRptInt; + + if (shift.output == SystemRptInt::SHIFT_PARK) { + return GearReport::PARK; + } + if (shift.output == SystemRptInt::SHIFT_REVERSE) { + return GearReport::REVERSE; + } + if (shift.output == SystemRptInt::SHIFT_FORWARD) { + return GearReport::DRIVE; + } + if (shift.output == SystemRptInt::SHIFT_LOW) { + return GearReport::LOW; + } + return {}; +} + +uint16_t PacmodInterface::toPacmodTurnCmd( + const autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand & turn, + const autoware_auto_vehicle_msgs::msg::HazardLightsCommand & hazard) +{ + using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; + using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; + using pacmod3_msgs::msg::SystemCmdInt; + + // NOTE: hazard lights command has a highest priority here. + if (hazard.command == HazardLightsCommand::ENABLE) { + return SystemCmdInt::TURN_HAZARDS; + } + if (turn.command == TurnIndicatorsCommand::ENABLE_LEFT) { + return SystemCmdInt::TURN_LEFT; + } + if (turn.command == TurnIndicatorsCommand::ENABLE_RIGHT) { + return SystemCmdInt::TURN_RIGHT; + } + return SystemCmdInt::TURN_NONE; +} + +uint16_t PacmodInterface::toPacmodTurnCmdWithHazardRecover( + const autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand & turn, + const autoware_auto_vehicle_msgs::msg::HazardLightsCommand & hazard) +{ + using pacmod3_msgs::msg::SystemRptInt; + + if (!engage_cmd_ || turn_rpt_ptr_->command == turn_rpt_ptr_->output) { + last_shift_inout_matched_time_ = this->now(); + return toPacmodTurnCmd(turn, hazard); + } + + if ((this->now() - last_shift_inout_matched_time_).seconds() < hazard_thresh_time_) { + return toPacmodTurnCmd(turn, hazard); + } + + // hazard recover mode + if (hazard_recover_count_ > hazard_recover_cmd_num_) { + last_shift_inout_matched_time_ = this->now(); + hazard_recover_count_ = 0; + } + hazard_recover_count_++; + + if ( + turn_rpt_ptr_->command != SystemRptInt::TURN_HAZARDS && + turn_rpt_ptr_->output == SystemRptInt::TURN_HAZARDS) { + // publish hazard commands for turning off the hazard lights + return SystemRptInt::TURN_HAZARDS; + } else if ( // NOLINT + turn_rpt_ptr_->command == SystemRptInt::TURN_HAZARDS && + turn_rpt_ptr_->output != SystemRptInt::TURN_HAZARDS) { + // publish none commands for turning on the hazard lights + return SystemRptInt::TURN_NONE; + } else { + // something wrong + RCLCPP_ERROR_STREAM( + get_logger(), "turn signal command and output do not match. " + << "COMMAND: " << turn_rpt_ptr_->command + << "; OUTPUT: " << turn_rpt_ptr_->output); + return toPacmodTurnCmd(turn, hazard); + } +} + +int32_t PacmodInterface::toAutowareTurnIndicatorsReport( + const pacmod3_msgs::msg::SystemRptInt & turn) +{ + using autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport; + using pacmod3_msgs::msg::SystemRptInt; + + if (turn.output == SystemRptInt::TURN_RIGHT) { + return TurnIndicatorsReport::ENABLE_RIGHT; + } else if (turn.output == SystemRptInt::TURN_LEFT) { + return TurnIndicatorsReport::ENABLE_LEFT; + } else if (turn.output == SystemRptInt::TURN_NONE) { + return TurnIndicatorsReport::DISABLE; + } + return TurnIndicatorsReport::DISABLE; +} + +int32_t PacmodInterface::toAutowareHazardLightsReport( + const pacmod3_msgs::msg::SystemRptInt & hazard) +{ + using autoware_auto_vehicle_msgs::msg::HazardLightsReport; + using pacmod3_msgs::msg::SystemRptInt; + + if (hazard.output == SystemRptInt::TURN_HAZARDS) { + return HazardLightsReport::ENABLE; + } + + return HazardLightsReport::DISABLE; +} + +double PacmodInterface::steerWheelRateLimiter( + const double current_steer_cmd, const double prev_steer_cmd, + const rclcpp::Time & current_steer_time, const rclcpp::Time & prev_steer_time, + const double steer_rate, const double current_steer_output, const bool engage) +{ + if (!engage) { + // return current steer as steer command ( do not apply steer rate filter ) + return current_steer_output; + } + + const double dsteer = current_steer_cmd - prev_steer_cmd; + const double dt = std::max(0.0, (current_steer_time - prev_steer_time).seconds()); + const double max_dsteer = std::fabs(steer_rate) * dt; + const double limited_steer_cmd = + prev_steer_cmd + std::min(std::max(-max_dsteer, dsteer), max_dsteer); + return limited_steer_cmd; +} diff --git a/vehicle/pacmod_interface/src/pacmod_interface/pacmod_interface_node.cpp b/vehicle/pacmod_interface/src/pacmod_interface/pacmod_interface_node.cpp new file mode 100644 index 0000000000000..514d839cbdf56 --- /dev/null +++ b/vehicle/pacmod_interface/src/pacmod_interface/pacmod_interface_node.cpp @@ -0,0 +1,27 @@ +// Copyright 2017-2019 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +}