From 3b19671181013a55648af976ccbbfe28c2074dfa Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Wed, 16 Feb 2022 17:34:27 +0900 Subject: [PATCH] refactor: move pacmod interface (#392) Signed-off-by: Takagi, Isamu --- vehicle/pacmod_interface/CMakeLists.txt | 45 -- vehicle/pacmod_interface/README.md | 81 --- .../pacmod_interface/config/pacmod.param.yaml | 19 - .../config/pacmod_extra.param.yaml | 20 - ...pacmod_additional_debug_publisher_node.hpp | 44 -- .../pacmod_dynamic_parameter_changer_node.hpp | 64 -- .../pacmod_diag_publisher.hpp | 112 --- .../pacmod_interface/pacmod_interface.hpp | 226 ------ ...cmod_additional_debug_publisher.launch.xml | 16 - .../launch/pacmod_interface.launch.xml | 27 - vehicle/pacmod_interface/package.xml | 32 - ...pacmod_additional_debug_publisher_main.cpp | 28 - ...pacmod_additional_debug_publisher_node.cpp | 160 ---- .../pacmod_dynamic_parameter_changer_main.cpp | 26 - .../pacmod_dynamic_parameter_changer_node.cpp | 166 ----- .../pacmod_diag_publisher.cpp | 186 ----- .../pacmod_diag_publisher_node.cpp | 27 - .../src/pacmod_interface/pacmod_interface.cpp | 685 ------------------ .../pacmod_interface_node.cpp | 27 - 19 files changed, 1991 deletions(-) delete mode 100644 vehicle/pacmod_interface/CMakeLists.txt delete mode 100644 vehicle/pacmod_interface/README.md delete mode 100644 vehicle/pacmod_interface/config/pacmod.param.yaml delete mode 100644 vehicle/pacmod_interface/config/pacmod_extra.param.yaml delete mode 100644 vehicle/pacmod_interface/include/pacmod_additional_debug_publisher/pacmod_additional_debug_publisher_node.hpp delete mode 100644 vehicle/pacmod_interface/include/pacmod_dynamic_parameter_changer/pacmod_dynamic_parameter_changer_node.hpp delete mode 100644 vehicle/pacmod_interface/include/pacmod_interface/pacmod_diag_publisher.hpp delete mode 100644 vehicle/pacmod_interface/include/pacmod_interface/pacmod_interface.hpp delete mode 100644 vehicle/pacmod_interface/launch/pacmod_additional_debug_publisher.launch.xml delete mode 100644 vehicle/pacmod_interface/launch/pacmod_interface.launch.xml delete mode 100644 vehicle/pacmod_interface/package.xml delete mode 100644 vehicle/pacmod_interface/src/pacmod_additional_debug_publisher/pacmod_additional_debug_publisher_main.cpp delete mode 100644 vehicle/pacmod_interface/src/pacmod_additional_debug_publisher/pacmod_additional_debug_publisher_node.cpp delete mode 100644 vehicle/pacmod_interface/src/pacmod_dynamic_parameter_changer/pacmod_dynamic_parameter_changer_main.cpp delete mode 100644 vehicle/pacmod_interface/src/pacmod_dynamic_parameter_changer/pacmod_dynamic_parameter_changer_node.cpp delete mode 100644 vehicle/pacmod_interface/src/pacmod_interface/pacmod_diag_publisher.cpp delete mode 100644 vehicle/pacmod_interface/src/pacmod_interface/pacmod_diag_publisher_node.cpp delete mode 100644 vehicle/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp delete mode 100644 vehicle/pacmod_interface/src/pacmod_interface/pacmod_interface_node.cpp diff --git a/vehicle/pacmod_interface/CMakeLists.txt b/vehicle/pacmod_interface/CMakeLists.txt deleted file mode 100644 index 177e506806ab6..0000000000000 --- a/vehicle/pacmod_interface/CMakeLists.txt +++ /dev/null @@ -1,45 +0,0 @@ -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 deleted file mode 100644 index 1f9c7fda886db..0000000000000 --- a/vehicle/pacmod_interface/README.md +++ /dev/null @@ -1,81 +0,0 @@ -# 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` | tier4_vehicle_msgs::msg::ActuationCommandStamped | actuation (accel/brake pedal, steering wheel) command | - | `/control/command/emergency_cmd` | tier4_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 deleted file mode 100644 index 11b4aca96f77e..0000000000000 --- a/vehicle/pacmod_interface/config/pacmod.param.yaml +++ /dev/null @@ -1,19 +0,0 @@ -/**: - 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 deleted file mode 100644 index f20548d196bbd..0000000000000 --- a/vehicle/pacmod_interface/config/pacmod_extra.param.yaml +++ /dev/null @@ -1,20 +0,0 @@ -/**: - 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 deleted file mode 100644 index c3166ff95cb5d..0000000000000 --- a/vehicle/pacmod_interface/include/pacmod_additional_debug_publisher/pacmod_additional_debug_publisher_node.hpp +++ /dev/null @@ -1,44 +0,0 @@ -// 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_; - tier4_debug_msgs::msg::Float32MultiArrayStamped debug_value_; - tier4_debug_msgs::msg::Float32MultiArrayStamped accel_cal_rpt_; - tier4_debug_msgs::msg::Float32MultiArrayStamped brake_cal_rpt_; - tier4_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 deleted file mode 100644 index 40b52b54b16b9..0000000000000 --- a/vehicle/pacmod_interface/include/pacmod_dynamic_parameter_changer/pacmod_dynamic_parameter_changer_node.hpp +++ /dev/null @@ -1,64 +0,0 @@ -// 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 deleted file mode 100644 index a499864462397..0000000000000 --- a/vehicle/pacmod_interface/include/pacmod_interface/pacmod_diag_publisher.hpp +++ /dev/null @@ -1,112 +0,0 @@ -// 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 deleted file mode 100644 index f9fb82e3a4dc8..0000000000000 --- a/vehicle/pacmod_interface/include/pacmod_interface/pacmod_interface.hpp +++ /dev/null @@ -1,226 +0,0 @@ -// 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 = tier4_vehicle_msgs::msg::ActuationCommandStamped; - using ActuationStatusStamped = tier4_vehicle_msgs::msg::ActuationStatusStamped; - using SteeringWheelStatusStamped = tier4_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 tire_radius_scale_factor_; // scale factor of tire radius - - 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 tier4_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 deleted file mode 100644 index 1912c1a1ec967..0000000000000 --- a/vehicle/pacmod_interface/launch/pacmod_additional_debug_publisher.launch.xml +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/vehicle/pacmod_interface/launch/pacmod_interface.launch.xml b/vehicle/pacmod_interface/launch/pacmod_interface.launch.xml deleted file mode 100644 index a1ae8e0617c1c..0000000000000 --- a/vehicle/pacmod_interface/launch/pacmod_interface.launch.xml +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/vehicle/pacmod_interface/package.xml b/vehicle/pacmod_interface/package.xml deleted file mode 100644 index d1fa093b90536..0000000000000 --- a/vehicle/pacmod_interface/package.xml +++ /dev/null @@ -1,32 +0,0 @@ - - - - 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 - can_msgs - diagnostic_updater - message_filters - pacmod3_msgs - rclcpp - std_msgs - tier4_debug_msgs - tier4_vehicle_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 deleted file mode 100644 index eca9b0120bf40..0000000000000 --- a/vehicle/pacmod_interface/src/pacmod_additional_debug_publisher/pacmod_additional_debug_publisher_main.cpp +++ /dev/null @@ -1,28 +0,0 @@ -// 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 deleted file mode 100644 index 26ae019372f6f..0000000000000 --- a/vehicle/pacmod_interface/src/pacmod_additional_debug_publisher/pacmod_additional_debug_publisher_node.cpp +++ /dev/null @@ -1,160 +0,0 @@ -// 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 deleted file mode 100644 index c0f0ad100d90a..0000000000000 --- a/vehicle/pacmod_interface/src/pacmod_dynamic_parameter_changer/pacmod_dynamic_parameter_changer_main.cpp +++ /dev/null @@ -1,26 +0,0 @@ -// 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 deleted file mode 100644 index aa54680772cae..0000000000000 --- a/vehicle/pacmod_interface/src/pacmod_dynamic_parameter_changer/pacmod_dynamic_parameter_changer_node.cpp +++ /dev/null @@ -1,166 +0,0 @@ -// 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 deleted file mode 100644 index 049375d51c732..0000000000000 --- a/vehicle/pacmod_interface/src/pacmod_interface/pacmod_diag_publisher.cpp +++ /dev/null @@ -1,186 +0,0 @@ -// 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 deleted file mode 100644 index c79f003d4bea9..0000000000000 --- a/vehicle/pacmod_interface/src/pacmod_interface/pacmod_diag_publisher_node.cpp +++ /dev/null @@ -1,27 +0,0 @@ -// 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 deleted file mode 100644 index d833cc97fbf75..0000000000000 --- a/vehicle/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp +++ /dev/null @@ -1,685 +0,0 @@ -// 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); - tire_radius_scale_factor_ = declare_parameter("tire_radius_scale_factor", 1.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 - const auto period_ns = rclcpp::Rate(loop_rate_).period(); - timer_ = rclcpp::create_timer( - this, get_clock(), period_ns, std::bind(&PacmodInterface::publishCommands, this)); -} - -void PacmodInterface::callbackActuationCmd(const ActuationCommandStamped::ConstSharedPtr msg) -{ - actuation_command_received_time_ = this->now(); - actuation_cmd_ptr_ = msg; -} - -void PacmodInterface::callbackEmergencyCmd( - const tier4_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_ * tire_radius_scale_factor_; - 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 deleted file mode 100644 index 514d839cbdf56..0000000000000 --- a/vehicle/pacmod_interface/src/pacmod_interface/pacmod_interface_node.cpp +++ /dev/null @@ -1,27 +0,0 @@ -// 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; -}