diff --git a/control/vehicle_cmd_gate/CMakeLists.txt b/control/vehicle_cmd_gate/CMakeLists.txt new file mode 100644 index 000000000000..f51e273ada16 --- /dev/null +++ b/control/vehicle_cmd_gate/CMakeLists.txt @@ -0,0 +1,50 @@ +cmake_minimum_required(VERSION 3.5) +project(vehicle_cmd_gate) + +### Compile options +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 -Werror) +endif() + +### Dependencies +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +# Generate exe file +ament_auto_add_library(vehicle_cmd_gate_node SHARED + src/vehicle_cmd_gate.cpp + src/vehicle_cmd_filter.cpp +) + +rclcpp_components_register_node(vehicle_cmd_gate_node + PLUGIN "VehicleCmdGate" + EXECUTABLE vehicle_cmd_gate +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + ament_add_gtest(test_vehicle_cmd_gate + test/src/test_main.cpp + test/src/test_vehicle_cmd_filter.cpp + ) + ament_target_dependencies(test_vehicle_cmd_gate + rclcpp + autoware_control_msgs + ) + target_link_libraries(test_vehicle_cmd_gate + vehicle_cmd_gate_node + ) +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/control/vehicle_cmd_gate/README.md b/control/vehicle_cmd_gate/README.md new file mode 100644 index 000000000000..1c03259bf01c --- /dev/null +++ b/control/vehicle_cmd_gate/README.md @@ -0,0 +1,62 @@ +# vehicle_cmd_gate + +## Purpose + +`vehicle_cmd_gate` is the package to get information from emergency handler, planning module, external controller, and send a msg to vehicle. + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ------------------------------------------- | ---------------------------------------------------------- | -------------------------------------------------------------------- | +| `~/input/steering` | `autoware_auto_vehicle_msgs::msg::SteeringReport` | steering status | +| `~/input/auto/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | command for lateral and longitudinal velocity from planning module | +| `~/input/auto/turn_indicators_cmd` | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand` | turn indicators command from planning module | +| `~/input/auto/hazard_lights_cmd` | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command from planning module | +| `~/input/auto/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | gear command from planning module | +| `~/input/external/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | command for lateral and longitudinal velocity from external | +| `~/input/external/turn_indicators_cmd` | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand` | turn indicators command from external | +| `~/input/external/hazard_lights_cmd` | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command from external | +| `~/input/external/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | gear command from external | +| `~/input/external_emergency_stop_heartbeat` | `autoware_external_api_msgs::msg::Heartbeat` | heartbeat | +| `~/input/gate_mode` | `autoware_control_msgs::msg::GateMode` | gate mode (AUTO or EXTERNAL) | +| `~/input/emergency_state` | `autoware_auto_system_msgs::msg::EmergencyState` | used to detect the emergency situation of the vehicle | +| `~/input/emergency/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | command for lateral and longitudinal velocity from emergency handler | +| `~/input/emergency/hazard_lights_cmd` | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command from emergency handler | +| `~/input/emergency/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | gear command from emergency handler | +| `~/input/engage` | `autoware_auto_vehicle_msgs::msg::Engage` | engage signal | + +### Output + +| Name | Type | Description | +| -------------------------------------- | ---------------------------------------------------------- | -------------------------------------------------------- | +| `~/output/vehicle_cmd_emergency` | `autoware_auto_system_msgs::msg::EmergencyState` | emergency state which was originally in vehicle command | +| `~/output/command/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | command for lateral and longitudinal velocity to vehicle | +| `~/output/command/turn_indicators_cmd` | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand` | turn indicators command to vehicle | +| `~/output/command/hazard_lights_cmd` | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command to vehicle | +| `~/output/command/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | gear command to vehicle | +| `~/output/gate_mode` | `autoware_control_msgs::msg::GateMode` | gate mode (AUTO or EXTERNAL) | +| `~/output/engage` | `autoware_auto_vehicle_msgs::msg::Engage` | engage signal | +| `~/output/external_emergency` | `autoware_external_api_msgs::msg::Emergency` | external emergency signal | + +## Parameters + +| Parameter | Type | Description | +| ------------------------------------------- | ------ | -------------------------------------------------------------- | +| `update_period` | double | update period | +| `use_emergency_handling` | bool | true when emergency handler is used | +| `use_external_emergency_stop` | bool | true when external emergency stop information is used | +| `system_emergency_heartbeat_timeout` | double | timeout for system emergency | +| `external_emergency_stop_heartbeat_timeout` | double | timeout for external emergency | +| `stop_hold_acceleration` | double | longitudinal acceleration cmd when vehicle should stop | +| `emergency_acceleration` | double | longitudinal acceleration cmd when vehicle stop with emergency | +| `vel_lim` | double | limit of longitudinal velocity | +| `lon_acc_lim` | double | limit of longitudinal acceleration | +| `lon_jerk_lim` | double | limit of longitudinal jerk | +| `lat_acc_lim` | double | limit of lateral acceleration | +| `lat_jerk_lim` | double | limit of lateral jerk | + +## Assumptions / Known limits + +TBD. diff --git a/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml b/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml new file mode 100644 index 000000000000..94b682a6b4c5 --- /dev/null +++ b/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml @@ -0,0 +1,12 @@ +/**: + ros__parameters: + update_rate: 10.0 + system_emergency_heartbeat_timeout: 0.5 + external_emergency_stop_heartbeat_timeout: 0.0 + stop_hold_acceleration: -1.5 + emergency_acceleration: -2.4 + vel_lim: 25.0 + lon_acc_lim: 5.0 + lon_jerk_lim: 5.0 + lat_acc_lim: 5.0 + lat_jerk_lim: 5.0 diff --git a/control/vehicle_cmd_gate/include/vehicle_cmd_gate/vehicle_cmd_filter.hpp b/control/vehicle_cmd_gate/include/vehicle_cmd_gate/vehicle_cmd_filter.hpp new file mode 100644 index 000000000000..b72dff42572f --- /dev/null +++ b/control/vehicle_cmd_gate/include/vehicle_cmd_gate/vehicle_cmd_filter.hpp @@ -0,0 +1,62 @@ +// Copyright 2015-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 VEHICLE_CMD_GATE__VEHICLE_CMD_FILTER_HPP_ +#define VEHICLE_CMD_GATE__VEHICLE_CMD_FILTER_HPP_ + +#include + +class VehicleCmdFilter +{ +public: + VehicleCmdFilter(); + ~VehicleCmdFilter() = default; + + void setWheelBase(double v) { wheel_base_ = v; } + void setVelLim(double v) { vel_lim_ = v; } + void setLonAccLim(double v) { lon_acc_lim_ = v; } + void setLonJerkLim(double v) { lon_jerk_lim_ = v; } + void setLatAccLim(double v) { lat_acc_lim_ = v; } + void setLatJerkLim(double v) { lat_jerk_lim_ = v; } + void setPrevCmd(const autoware_auto_control_msgs::msg::AckermannControlCommand & v) + { + prev_cmd_ = v; + } + + void limitLongitudinalWithVel( + autoware_auto_control_msgs::msg::AckermannControlCommand & input) const; + void limitLongitudinalWithAcc( + const double dt, autoware_auto_control_msgs::msg::AckermannControlCommand & input) const; + void limitLongitudinalWithJerk( + const double dt, autoware_auto_control_msgs::msg::AckermannControlCommand & input) const; + void limitLateralWithLatAcc( + const double dt, autoware_auto_control_msgs::msg::AckermannControlCommand & input) const; + void limitLateralWithLatJerk( + const double dt, autoware_auto_control_msgs::msg::AckermannControlCommand & input) const; + +private: + double wheel_base_; + double vel_lim_; + double lon_acc_lim_; + double lon_jerk_lim_; + double lat_acc_lim_; + double lat_jerk_lim_; + autoware_auto_control_msgs::msg::AckermannControlCommand prev_cmd_; + + double calcLatAcc(const autoware_auto_control_msgs::msg::AckermannControlCommand & cmd) const; + double calcSteerFromLatacc(const double v, const double latacc) const; + double limitDiff(const double curr, const double prev, const double diff_lim) const; +}; + +#endif // VEHICLE_CMD_GATE__VEHICLE_CMD_FILTER_HPP_ diff --git a/control/vehicle_cmd_gate/include/vehicle_cmd_gate/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/include/vehicle_cmd_gate/vehicle_cmd_gate.hpp new file mode 100644 index 000000000000..b9c970ecc22f --- /dev/null +++ b/control/vehicle_cmd_gate/include/vehicle_cmd_gate/vehicle_cmd_gate.hpp @@ -0,0 +1,244 @@ +// Copyright 2015-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 VEHICLE_CMD_GATE__VEHICLE_CMD_GATE_HPP_ +#define VEHICLE_CMD_GATE__VEHICLE_CMD_GATE_HPP_ + +#include "vehicle_cmd_gate/vehicle_cmd_filter.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +struct Commands +{ + autoware_auto_control_msgs::msg::AckermannControlCommand control; + autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand turn_indicator; + autoware_auto_vehicle_msgs::msg::HazardLightsCommand hazard_light; + autoware_auto_vehicle_msgs::msg::GearCommand gear; + explicit Commands( + const uint8_t & default_gear = autoware_auto_vehicle_msgs::msg::GearCommand::PARK) + { + gear.command = default_gear; + } +}; + +class VehicleCmdGate : public rclcpp::Node +{ + using VehicleEmergencyStamped = autoware_vehicle_msgs::msg::VehicleEmergencyStamped; + +public: + explicit VehicleCmdGate(const rclcpp::NodeOptions & node_options); + +private: + // Publisher + rclcpp::Publisher::SharedPtr vehicle_cmd_emergency_pub_; + rclcpp::Publisher::SharedPtr + control_cmd_pub_; + rclcpp::Publisher::SharedPtr gear_cmd_pub_; + rclcpp::Publisher::SharedPtr + turn_indicator_cmd_pub_; + rclcpp::Publisher::SharedPtr + hazard_light_cmd_pub_; + rclcpp::Publisher::SharedPtr gate_mode_pub_; + rclcpp::Publisher::SharedPtr engage_pub_; + + // Subscription + rclcpp::Subscription::SharedPtr + emergency_state_sub_; + rclcpp::Subscription::SharedPtr + external_emergency_stop_heartbeat_sub_; + rclcpp::Subscription::SharedPtr gate_mode_sub_; + rclcpp::Subscription::SharedPtr steer_sub_; + + void onGateMode(autoware_control_msgs::msg::GateMode::ConstSharedPtr msg); + void onEmergencyState(autoware_auto_system_msgs::msg::EmergencyState::ConstSharedPtr msg); + void onExternalEmergencyStopHeartbeat( + autoware_external_api_msgs::msg::Heartbeat::ConstSharedPtr msg); + void onSteering(autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg); + + bool is_engaged_; + bool is_system_emergency_ = false; + bool is_external_emergency_stop_ = false; + double current_steer_ = 0; + autoware_control_msgs::msg::GateMode current_gate_mode_; + + // Heartbeat + std::shared_ptr emergency_state_heartbeat_received_time_; + bool is_emergency_state_heartbeat_timeout_ = false; + std::shared_ptr external_emergency_stop_heartbeat_received_time_; + bool is_external_emergency_stop_heartbeat_timeout_ = false; + bool isHeartbeatTimeout( + const std::shared_ptr & heartbeat_received_time, const double timeout); + + // Subscriber for auto + Commands auto_commands_; + rclcpp::Subscription::SharedPtr + auto_control_cmd_sub_; + rclcpp::Subscription::SharedPtr + auto_turn_indicator_cmd_sub_; + rclcpp::Subscription::SharedPtr + auto_hazard_light_cmd_sub_; + rclcpp::Subscription::SharedPtr auto_gear_cmd_sub_; + void onAutoCtrlCmd(autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg); + void onAutoTurnIndicatorsCmd( + autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand::ConstSharedPtr msg); + void onAutoHazardLightsCmd( + autoware_auto_vehicle_msgs::msg::HazardLightsCommand::ConstSharedPtr msg); + void onAutoShiftCmd(autoware_auto_vehicle_msgs::msg::GearCommand::ConstSharedPtr msg); + + // Subscription for external + Commands remote_commands_; + rclcpp::Subscription::SharedPtr + remote_control_cmd_sub_; + rclcpp::Subscription::SharedPtr + remote_turn_indicator_cmd_sub_; + rclcpp::Subscription::SharedPtr + remote_hazard_light_cmd_sub_; + rclcpp::Subscription::SharedPtr + remote_gear_cmd_sub_; + void onRemoteCtrlCmd( + autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg); + void onRemoteTurnIndicatorsCmd( + autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand::ConstSharedPtr msg); + void onRemoteHazardLightsCmd( + autoware_auto_vehicle_msgs::msg::HazardLightsCommand::ConstSharedPtr msg); + void onRemoteShiftCmd(autoware_auto_vehicle_msgs::msg::GearCommand::ConstSharedPtr msg); + + // Subscription for emergency + Commands emergency_commands_; + rclcpp::Subscription::SharedPtr + emergency_control_cmd_sub_; + rclcpp::Subscription::SharedPtr + emergency_hazard_light_cmd_sub_; + rclcpp::Subscription::SharedPtr + emergency_gear_cmd_sub_; + void onEmergencyCtrlCmd( + autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg); + void onEmergencyTurnIndicatorsCmd( + autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand::ConstSharedPtr msg); + void onEmergencyHazardLightsCmd( + autoware_auto_vehicle_msgs::msg::HazardLightsCommand::ConstSharedPtr msg); + void onEmergencyShiftCmd(autoware_auto_vehicle_msgs::msg::GearCommand::ConstSharedPtr msg); + + // Parameter + double update_period_; + bool use_emergency_handling_; + bool use_external_emergency_stop_; + double system_emergency_heartbeat_timeout_; + double external_emergency_stop_heartbeat_timeout_; + double stop_hold_acceleration_; + double emergency_acceleration_; + + // Service + rclcpp::Service::SharedPtr srv_engage_; + rclcpp::Service::SharedPtr srv_external_emergency_; + rclcpp::Publisher::SharedPtr pub_external_emergency_; + void onEngageService( + const autoware_external_api_msgs::srv::Engage::Request::SharedPtr request, + const autoware_external_api_msgs::srv::Engage::Response::SharedPtr response); + void onExternalEmergencyStopService( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + // TODO(Takagi, Isamu): deprecated + rclcpp::Subscription::SharedPtr engage_sub_; + rclcpp::Service::SharedPtr srv_external_emergency_stop_; + rclcpp::Service::SharedPtr srv_clear_external_emergency_stop_; + void onEngage(autoware_auto_vehicle_msgs::msg::Engage::ConstSharedPtr msg); + bool onSetExternalEmergencyStopService( + const std::shared_ptr req_header, + const std::shared_ptr req, + const std::shared_ptr res); + bool onClearExternalEmergencyStopService( + const std::shared_ptr req_header, + const std::shared_ptr req, + const std::shared_ptr res); + + // Timer / Event + rclcpp::TimerBase::SharedPtr timer_; + + void onTimer(); + void publishControlCommands(const Commands & input_msg); + void publishEmergencyStopControlCommands(); + + // Diagnostics Updater + diagnostic_updater::Updater updater_; + + void checkExternalEmergencyStop(diagnostic_updater::DiagnosticStatusWrapper & stat); + + // Algorithm + autoware_auto_control_msgs::msg::AckermannControlCommand prev_control_cmd_; + autoware_auto_control_msgs::msg::AckermannControlCommand createStopControlCmd() const; + autoware_auto_control_msgs::msg::AckermannControlCommand createEmergencyStopControlCmd() const; + + std::shared_ptr prev_time_; + double getDt(); + + VehicleCmdFilter filter_; + autoware_auto_control_msgs::msg::AckermannControlCommand filterControlCommand( + const autoware_auto_control_msgs::msg::AckermannControlCommand & msg); + + // Start request service + struct StartRequest + { + private: + static constexpr double eps = 1e-3; + using ControlCommandStamped = autoware_auto_control_msgs::msg::AckermannControlCommand; + + public: + StartRequest(rclcpp::Node * node, bool use_start_request); + bool isAccepted(); + void publishStartAccepted(); + void checkStopped(const ControlCommandStamped & control); + void checkStartRequest(const ControlCommandStamped & control); + + private: + bool use_start_request_; + bool is_start_requesting_; + bool is_start_accepted_; + bool is_start_cancelled_; + nav_msgs::msg::Odometry current_twist_; + + rclcpp::Node * node_; + rclcpp::Client::SharedPtr request_start_cli_; + rclcpp::Publisher::SharedPtr request_start_pub_; + rclcpp::Subscription::SharedPtr current_twist_sub_; + void onCurrentTwist(nav_msgs::msg::Odometry::ConstSharedPtr msg); + }; + + std::unique_ptr start_request_; +}; + +#endif // VEHICLE_CMD_GATE__VEHICLE_CMD_GATE_HPP_ diff --git a/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml b/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml new file mode 100644 index 000000000000..8658ea51d84c --- /dev/null +++ b/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml @@ -0,0 +1,50 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/control/vehicle_cmd_gate/package.xml b/control/vehicle_cmd_gate/package.xml new file mode 100644 index 000000000000..1cc2c1f46aa3 --- /dev/null +++ b/control/vehicle_cmd_gate/package.xml @@ -0,0 +1,33 @@ + + + vehicle_cmd_gate + 0.1.0 + The vehicle_cmd_gate package + h_ohta + Apache License 2.0 + + ament_cmake + + autoware_api_utils + autoware_auto_control_msgs + autoware_auto_system_msgs + autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_debug_msgs + autoware_external_api_msgs + diagnostic_updater + geometry_msgs + rclcpp + rclcpp_components + std_srvs + vehicle_info_util +　autoware_vehicle_msgs + + ament_cmake_gtest + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp new file mode 100644 index 000000000000..42aa3111caef --- /dev/null +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp @@ -0,0 +1,91 @@ +// Copyright 2015-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 "vehicle_cmd_gate/vehicle_cmd_filter.hpp" + +#include +#include + +VehicleCmdFilter::VehicleCmdFilter() {} + +void VehicleCmdFilter::limitLongitudinalWithVel( + autoware_auto_control_msgs::msg::AckermannControlCommand & input) const +{ + input.longitudinal.speed = + std::max(std::min(static_cast(input.longitudinal.speed), vel_lim_), -vel_lim_); +} + +void VehicleCmdFilter::limitLongitudinalWithAcc( + const double dt, autoware_auto_control_msgs::msg::AckermannControlCommand & input) const +{ + input.longitudinal.acceleration = std::max( + std::min(static_cast(input.longitudinal.acceleration), lon_acc_lim_), -lon_acc_lim_); + input.longitudinal.speed = + limitDiff(input.longitudinal.speed, prev_cmd_.longitudinal.speed, lon_acc_lim_ * dt); +} + +void VehicleCmdFilter::VehicleCmdFilter::limitLongitudinalWithJerk( + const double dt, autoware_auto_control_msgs::msg::AckermannControlCommand & input) const +{ + input.longitudinal.acceleration = limitDiff( + input.longitudinal.acceleration, prev_cmd_.longitudinal.acceleration, lon_jerk_lim_ * dt); +} + +void VehicleCmdFilter::limitLateralWithLatAcc( + [[maybe_unused]] const double dt, + autoware_auto_control_msgs::msg::AckermannControlCommand & input) const +{ + double latacc = calcLatAcc(input); + if (std::fabs(latacc) > lat_acc_lim_) { + double v_sq = + std::max(static_cast(input.longitudinal.speed * input.longitudinal.speed), 0.001); + double steer_lim = std::atan(lat_acc_lim_ * wheel_base_ / v_sq); + input.lateral.steering_tire_angle = latacc > 0.0 ? steer_lim : -steer_lim; + } +} + +void VehicleCmdFilter::limitLateralWithLatJerk( + const double dt, autoware_auto_control_msgs::msg::AckermannControlCommand & input) const +{ + double curr_latacc = calcLatAcc(input); + double prev_latacc = calcLatAcc(prev_cmd_); + + const double latacc_max = prev_latacc + lat_jerk_lim_ * dt; + const double latacc_min = prev_latacc - lat_jerk_lim_ * dt; + + if (curr_latacc > latacc_max) { + input.lateral.steering_tire_angle = calcSteerFromLatacc(input.longitudinal.speed, latacc_max); + } else if (curr_latacc < latacc_min) { + input.lateral.steering_tire_angle = calcSteerFromLatacc(input.longitudinal.speed, latacc_min); + } +} + +double VehicleCmdFilter::calcSteerFromLatacc(const double v, const double latacc) const +{ + const double v_sq = std::max(v * v, 0.001); + return std::atan(latacc * wheel_base_ / v_sq); +} + +double VehicleCmdFilter::calcLatAcc( + const autoware_auto_control_msgs::msg::AckermannControlCommand & cmd) const +{ + double v = cmd.longitudinal.speed; + return v * v * std::tan(cmd.lateral.steering_tire_angle) / wheel_base_; +} + +double VehicleCmdFilter::limitDiff( + const double curr, const double prev, const double diff_lim) const +{ + double diff = std::max(std::min(curr - prev, diff_lim), -diff_lim); + return prev + diff; +} diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp new file mode 100644 index 000000000000..c4a147baf780 --- /dev/null +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -0,0 +1,767 @@ +// Copyright 2015-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 "vehicle_cmd_gate/vehicle_cmd_gate.hpp" + +#include +#include + +#include +#include +#include +#include +#include + +namespace +{ +const char * getGateModeName(const autoware_control_msgs::msg::GateMode::_data_type & gate_mode) +{ + using autoware_control_msgs::msg::GateMode; + + if (gate_mode == GateMode::AUTO) { + return "AUTO"; + } + if (gate_mode == GateMode::EXTERNAL) { + return "EXTERNAL"; + } + return "NOT_SUPPORTED"; +} + +} // namespace + +VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) +: Node("vehicle_cmd_gate", node_options), is_engaged_(false), updater_(this) +{ + using std::placeholders::_1; + using std::placeholders::_2; + using std::placeholders::_3; + + rclcpp::QoS durable_qos{1}; + durable_qos.transient_local(); + + // Publisher + vehicle_cmd_emergency_pub_ = + this->create_publisher("output/vehicle_cmd_emergency", durable_qos); + control_cmd_pub_ = + this->create_publisher( + "output/control_cmd", durable_qos); + gear_cmd_pub_ = this->create_publisher( + "output/gear_cmd", durable_qos); + turn_indicator_cmd_pub_ = + this->create_publisher( + "output/turn_indicators_cmd", durable_qos); + hazard_light_cmd_pub_ = + this->create_publisher( + "output/hazard_lights_cmd", durable_qos); + + gate_mode_pub_ = + this->create_publisher("output/gate_mode", durable_qos); + engage_pub_ = + this->create_publisher("output/engage", durable_qos); + pub_external_emergency_ = this->create_publisher( + "output/external_emergency", durable_qos); + + // Subscriber + emergency_state_sub_ = this->create_subscription( + "input/emergency_state", 1, std::bind(&VehicleCmdGate::onEmergencyState, this, _1)); + external_emergency_stop_heartbeat_sub_ = + this->create_subscription( + "input/external_emergency_stop_heartbeat", 1, + std::bind(&VehicleCmdGate::onExternalEmergencyStopHeartbeat, this, _1)); + gate_mode_sub_ = this->create_subscription( + "input/gate_mode", 1, std::bind(&VehicleCmdGate::onGateMode, this, _1)); + engage_sub_ = this->create_subscription( + "input/engage", 1, std::bind(&VehicleCmdGate::onEngage, this, _1)); + steer_sub_ = this->create_subscription( + "input/steering", 1, std::bind(&VehicleCmdGate::onSteering, this, _1)); + + // Subscriber for auto + auto_control_cmd_sub_ = + this->create_subscription( + "input/auto/control_cmd", 1, std::bind(&VehicleCmdGate::onAutoCtrlCmd, this, _1)); + + auto_turn_indicator_cmd_sub_ = + this->create_subscription( + "input/auto/turn_indicators_cmd", 1, + std::bind(&VehicleCmdGate::onAutoTurnIndicatorsCmd, this, _1)); + + auto_hazard_light_cmd_sub_ = + this->create_subscription( + "input/auto/hazard_lights_cmd", 1, + std::bind(&VehicleCmdGate::onAutoHazardLightsCmd, this, _1)); + + auto_gear_cmd_sub_ = this->create_subscription( + "input/auto/gear_cmd", 1, std::bind(&VehicleCmdGate::onAutoShiftCmd, this, _1)); + + // Subscriber for external + remote_control_cmd_sub_ = + this->create_subscription( + "input/external/control_cmd", 1, std::bind(&VehicleCmdGate::onRemoteCtrlCmd, this, _1)); + + remote_turn_indicator_cmd_sub_ = + this->create_subscription( + "input/external/turn_indicators_cmd", 1, + std::bind(&VehicleCmdGate::onRemoteTurnIndicatorsCmd, this, _1)); + + remote_hazard_light_cmd_sub_ = + this->create_subscription( + "input/external/hazard_lights_cmd", 1, + std::bind(&VehicleCmdGate::onRemoteHazardLightsCmd, this, _1)); + + remote_gear_cmd_sub_ = this->create_subscription( + "input/external/gear_cmd", 1, std::bind(&VehicleCmdGate::onRemoteShiftCmd, this, _1)); + + // Subscriber for emergency + emergency_control_cmd_sub_ = + this->create_subscription( + "input/emergency/control_cmd", 1, std::bind(&VehicleCmdGate::onEmergencyCtrlCmd, this, _1)); + + emergency_hazard_light_cmd_sub_ = + this->create_subscription( + "input/emergency/hazard_lights_cmd", 1, + std::bind(&VehicleCmdGate::onEmergencyHazardLightsCmd, this, _1)); + + emergency_gear_cmd_sub_ = this->create_subscription( + "input/emergency/gear_cmd", 1, std::bind(&VehicleCmdGate::onEmergencyShiftCmd, this, _1)); + + // Parameter + update_period_ = 1.0 / declare_parameter("update_rate", 10.0); + use_emergency_handling_ = declare_parameter("use_emergency_handling", false); + use_external_emergency_stop_ = declare_parameter("use_external_emergency_stop", false); + system_emergency_heartbeat_timeout_ = + declare_parameter("system_emergency_heartbeat_timeout", 0.5); + external_emergency_stop_heartbeat_timeout_ = + declare_parameter("external_emergency_stop_heartbeat_timeout", 0.5); + stop_hold_acceleration_ = declare_parameter("stop_hold_acceleration", -1.5); + emergency_acceleration_ = declare_parameter("emergency_acceleration", -2.4); + + // Vehicle Parameter + const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + double wheel_base = vehicle_info.wheel_base_m; + double vel_lim = declare_parameter("vel_lim", 25.0); + double lon_acc_lim = declare_parameter("lon_acc_lim", 5.0); + double lon_jerk_lim = declare_parameter("lon_jerk_lim", 5.0); + double lat_acc_lim = declare_parameter("lat_acc_lim", 5.0); + double lat_jerk_lim = declare_parameter("lat_jerk_lim", 5.0); + filter_.setWheelBase(wheel_base); + filter_.setVelLim(vel_lim); + filter_.setLonAccLim(lon_acc_lim); + filter_.setLonJerkLim(lon_jerk_lim); + filter_.setLatAccLim(lat_acc_lim); + filter_.setLatJerkLim(lat_jerk_lim); + + // Set default value + current_gate_mode_.data = autoware_control_msgs::msg::GateMode::AUTO; + + // Service + srv_engage_ = create_service( + "~/service/engage", std::bind(&VehicleCmdGate::onEngageService, this, _1, _2)); + srv_external_emergency_ = create_service( + "~/service/external_emergency", + std::bind(&VehicleCmdGate::onExternalEmergencyStopService, this, _1, _2, _3)); + srv_external_emergency_stop_ = this->create_service( + "~/service/external_emergency_stop", + std::bind(&VehicleCmdGate::onSetExternalEmergencyStopService, this, _1, _2, _3)); + srv_clear_external_emergency_stop_ = this->create_service( + "~/service/clear_external_emergency_stop", + std::bind(&VehicleCmdGate::onClearExternalEmergencyStopService, this, _1, _2, _3)); + + // Diagnostics Updater + updater_.setHardwareID("vehicle_cmd_gate"); + updater_.add("heartbeat", [](auto & stat) { + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Alive"); + }); + updater_.add("emergency_stop_operation", this, &VehicleCmdGate::checkExternalEmergencyStop); + + // Start Request + const auto use_start_request = declare_parameter("use_start_request", false); + start_request_ = std::make_unique(this, use_start_request); + + // Timer + auto timer_callback = std::bind(&VehicleCmdGate::onTimer, this); + auto period = std::chrono::duration_cast( + std::chrono::duration(update_period_)); + 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); +} + +bool VehicleCmdGate::isHeartbeatTimeout( + const std::shared_ptr & heartbeat_received_time, const double timeout) +{ + if (timeout == 0.0) { + return false; + } + + if (!heartbeat_received_time) { + return true; + } + + const auto time_from_heartbeat = this->now() - *heartbeat_received_time; + + return time_from_heartbeat.seconds() > timeout; +} + +// for auto +void VehicleCmdGate::onAutoCtrlCmd( + autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg) +{ + auto_commands_.control = *msg; + + if (current_gate_mode_.data == autoware_control_msgs::msg::GateMode::AUTO) { + publishControlCommands(auto_commands_); + } +} + +void VehicleCmdGate::onAutoTurnIndicatorsCmd( + autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand::ConstSharedPtr msg) +{ + auto_commands_.turn_indicator = *msg; +} + +void VehicleCmdGate::onAutoHazardLightsCmd( + autoware_auto_vehicle_msgs::msg::HazardLightsCommand::ConstSharedPtr msg) +{ + auto_commands_.hazard_light = *msg; +} + +void VehicleCmdGate::onAutoShiftCmd( + autoware_auto_vehicle_msgs::msg::GearCommand::ConstSharedPtr msg) +{ + auto_commands_.gear = *msg; +} + +// for remote +void VehicleCmdGate::onRemoteCtrlCmd( + autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg) +{ + remote_commands_.control = *msg; + + if (current_gate_mode_.data == autoware_control_msgs::msg::GateMode::EXTERNAL) { + publishControlCommands(remote_commands_); + } +} + +void VehicleCmdGate::onRemoteTurnIndicatorsCmd( + autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand::ConstSharedPtr msg) +{ + remote_commands_.turn_indicator = *msg; +} + +void VehicleCmdGate::onRemoteHazardLightsCmd( + autoware_auto_vehicle_msgs::msg::HazardLightsCommand::ConstSharedPtr msg) +{ + remote_commands_.hazard_light = *msg; +} + +void VehicleCmdGate::onRemoteShiftCmd( + autoware_auto_vehicle_msgs::msg::GearCommand::ConstSharedPtr msg) +{ + remote_commands_.gear = *msg; +} + +// for emergency +void VehicleCmdGate::onEmergencyCtrlCmd( + autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg) +{ + emergency_commands_.control = *msg; + + if (use_emergency_handling_ && is_system_emergency_) { + publishControlCommands(emergency_commands_); + } +} +void VehicleCmdGate::onEmergencyHazardLightsCmd( + autoware_auto_vehicle_msgs::msg::HazardLightsCommand::ConstSharedPtr msg) +{ + emergency_commands_.hazard_light = *msg; +} +void VehicleCmdGate::onEmergencyShiftCmd( + autoware_auto_vehicle_msgs::msg::GearCommand::ConstSharedPtr msg) +{ + emergency_commands_.gear = *msg; +} + +void VehicleCmdGate::onTimer() +{ + updater_.force_update(); + + // Check system emergency heartbeat + if (use_emergency_handling_) { + is_emergency_state_heartbeat_timeout_ = isHeartbeatTimeout( + emergency_state_heartbeat_received_time_, system_emergency_heartbeat_timeout_); + + if (is_emergency_state_heartbeat_timeout_) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 1000 /*ms*/, "system_emergency heartbeat is timeout."); + publishEmergencyStopControlCommands(); + return; + } + } + + // Check external emergency stop heartbeat + if (use_external_emergency_stop_) { + is_external_emergency_stop_heartbeat_timeout_ = isHeartbeatTimeout( + external_emergency_stop_heartbeat_received_time_, external_emergency_stop_heartbeat_timeout_); + + if (is_external_emergency_stop_heartbeat_timeout_) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 1000 /*ms*/, "external_emergency_stop heartbeat is timeout."); + is_external_emergency_stop_ = true; + } + } + + // Check external emergency stop + if (is_external_emergency_stop_) { + if (!is_external_emergency_stop_heartbeat_timeout_) { + RCLCPP_INFO_THROTTLE( + get_logger(), *get_clock(), 1000 /*ms*/, + "Please call `clear_external_emergency_stop` service to clear state."); + } + + publishEmergencyStopControlCommands(); + return; + } + + // Select commands + autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand turn_indicator; + autoware_auto_vehicle_msgs::msg::HazardLightsCommand hazard_light; + autoware_auto_vehicle_msgs::msg::GearCommand gear; + if (use_emergency_handling_ && is_system_emergency_) { + turn_indicator = emergency_commands_.turn_indicator; + hazard_light = emergency_commands_.hazard_light; + gear = emergency_commands_.gear; + } else { + if (current_gate_mode_.data == autoware_control_msgs::msg::GateMode::AUTO) { + turn_indicator = auto_commands_.turn_indicator; + hazard_light = emergency_commands_.hazard_light; + gear = auto_commands_.gear; + + // Don't send turn signal when autoware is not engaged + if (!is_engaged_) { + turn_indicator.command = autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand::NO_COMMAND; + hazard_light.command = autoware_auto_vehicle_msgs::msg::HazardLightsCommand::NO_COMMAND; + } + } else if (current_gate_mode_.data == autoware_control_msgs::msg::GateMode::EXTERNAL) { + turn_indicator = remote_commands_.turn_indicator; + hazard_light = remote_commands_.hazard_light; + gear = remote_commands_.gear; + } else { + throw std::runtime_error("invalid mode"); + } + } + + // Engage + autoware_auto_vehicle_msgs::msg::Engage autoware_engage; + autoware_engage.stamp = this->now(); + autoware_engage.engage = is_engaged_; + + // External emergency + autoware_external_api_msgs::msg::Emergency external_emergency; + external_emergency.stamp = this->now(); + external_emergency.emergency = is_external_emergency_stop_; + + // Publish topics + gate_mode_pub_->publish(current_gate_mode_); + turn_indicator_cmd_pub_->publish(turn_indicator); + hazard_light_cmd_pub_->publish(hazard_light); + gear_cmd_pub_->publish(gear); + engage_pub_->publish(autoware_engage); + pub_external_emergency_->publish(external_emergency); + + // Publish start request + start_request_->publishStartAccepted(); +} + +void VehicleCmdGate::publishControlCommands(const Commands & commands) +{ + // Check system emergency + if (use_emergency_handling_ && is_emergency_state_heartbeat_timeout_) { + return; + } + + // Check external emergency stop + if (is_external_emergency_stop_) { + return; + } + + Commands filtered_commands; + + // Set default commands + { + filtered_commands.control = commands.control; + filtered_commands.gear = commands.gear; // tmp + } + + // Check emergency + if (use_emergency_handling_ && is_system_emergency_) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), "Emergency!"); + filtered_commands.control = emergency_commands_.control; + filtered_commands.gear = emergency_commands_.gear; // tmp + } + + // Check start after applying all gates except engage + if (is_engaged_) { + start_request_->checkStartRequest(filtered_commands.control); + } + + // Check engage + if (!is_engaged_ || !start_request_->isAccepted()) { + filtered_commands.control = createStopControlCmd(); + } + + // Check stopped after applying all gates + start_request_->checkStopped(filtered_commands.control); + + // Apply limit filtering + filtered_commands.control = filterControlCommand(filtered_commands.control); + + // tmp: Publish vehicle emergency status + VehicleEmergencyStamped vehicle_cmd_emergency; + vehicle_cmd_emergency.emergency = (use_emergency_handling_ && is_system_emergency_); + vehicle_cmd_emergency.stamp = filtered_commands.control.stamp; + + // Publish commands + vehicle_cmd_emergency_pub_->publish(vehicle_cmd_emergency); + control_cmd_pub_->publish(filtered_commands.control); + + // Save ControlCmd to steering angle when disengaged + prev_control_cmd_ = filtered_commands.control; +} + +void VehicleCmdGate::publishEmergencyStopControlCommands() +{ + const auto stamp = this->now(); + + // ControlCommand + autoware_auto_control_msgs::msg::AckermannControlCommand control_cmd; + control_cmd.stamp = stamp; + control_cmd = createEmergencyStopControlCmd(); + + // Check stopped after applying all gates + start_request_->checkStopped(control_cmd); + + // gear + autoware_auto_vehicle_msgs::msg::GearCommand gear; + gear.stamp = stamp; + // default value is 0 + + // TurnSignal + autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand turn_indicator; + turn_indicator.stamp = stamp; + turn_indicator.command = autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand::NO_COMMAND; + + // Hazard + autoware_auto_vehicle_msgs::msg::HazardLightsCommand hazard_light; + hazard_light.stamp = stamp; + hazard_light.command = autoware_auto_vehicle_msgs::msg::HazardLightsCommand::ENABLE; + + // VehicleCommand emergency; + VehicleEmergencyStamped vehicle_cmd_emergency; + vehicle_cmd_emergency.stamp = stamp; + vehicle_cmd_emergency.emergency = true; + + // Engage + autoware_auto_vehicle_msgs::msg::Engage autoware_engage; + autoware_engage.stamp = stamp; + autoware_engage.engage = is_engaged_; + + // External emergency + autoware_external_api_msgs::msg::Emergency external_emergency; + external_emergency.stamp = stamp; + external_emergency.emergency = is_external_emergency_stop_; + + // Publish topics + vehicle_cmd_emergency_pub_->publish(vehicle_cmd_emergency); + control_cmd_pub_->publish(control_cmd); + gate_mode_pub_->publish(current_gate_mode_); + turn_indicator_cmd_pub_->publish(turn_indicator); + hazard_light_cmd_pub_->publish(hazard_light); + gear_cmd_pub_->publish(gear); + engage_pub_->publish(autoware_engage); + pub_external_emergency_->publish(external_emergency); + + // Publish start request + start_request_->publishStartAccepted(); +} + +autoware_auto_control_msgs::msg::AckermannControlCommand VehicleCmdGate::filterControlCommand( + const autoware_auto_control_msgs::msg::AckermannControlCommand & in) +{ + autoware_auto_control_msgs::msg::AckermannControlCommand out = in; + const double dt = getDt(); + + filter_.limitLongitudinalWithVel(out); + filter_.limitLongitudinalWithAcc(dt, out); + filter_.limitLongitudinalWithJerk(dt, out); + filter_.limitLateralWithLatAcc(dt, out); + filter_.limitLateralWithLatJerk(dt, out); + filter_.setPrevCmd(out); + + return out; +} + +autoware_auto_control_msgs::msg::AckermannControlCommand VehicleCmdGate::createStopControlCmd() + const +{ + autoware_auto_control_msgs::msg::AckermannControlCommand cmd; + + cmd.lateral.steering_tire_angle = current_steer_; + cmd.lateral.steering_tire_rotation_rate = 0.0; + cmd.longitudinal.speed = 0.0; + cmd.longitudinal.acceleration = stop_hold_acceleration_; + + return cmd; +} + +autoware_auto_control_msgs::msg::AckermannControlCommand +VehicleCmdGate::createEmergencyStopControlCmd() const +{ + autoware_auto_control_msgs::msg::AckermannControlCommand cmd; + + cmd.lateral.steering_tire_angle = prev_control_cmd_.lateral.steering_tire_angle; + cmd.lateral.steering_tire_rotation_rate = prev_control_cmd_.lateral.steering_tire_rotation_rate; + cmd.longitudinal.speed = 0.0; + cmd.longitudinal.acceleration = emergency_acceleration_; + + return cmd; +} + +void VehicleCmdGate::onEmergencyState( + autoware_auto_system_msgs::msg::EmergencyState::ConstSharedPtr msg) +{ + using autoware_auto_system_msgs::msg::EmergencyState; + is_system_emergency_ = (msg->state == EmergencyState::MRM_OPERATING) || + (msg->state == EmergencyState::MRM_SUCCEEDED) || + (msg->state == EmergencyState::MRM_FAILED); + emergency_state_heartbeat_received_time_ = std::make_shared(this->now()); +} + +void VehicleCmdGate::onExternalEmergencyStopHeartbeat( + [[maybe_unused]] autoware_external_api_msgs::msg::Heartbeat::ConstSharedPtr msg) +{ + external_emergency_stop_heartbeat_received_time_ = std::make_shared(this->now()); +} + +void VehicleCmdGate::onGateMode(autoware_control_msgs::msg::GateMode::ConstSharedPtr msg) +{ + const auto prev_gate_mode = current_gate_mode_; + current_gate_mode_ = *msg; + + if (current_gate_mode_.data != prev_gate_mode.data) { + RCLCPP_INFO( + get_logger(), "GateMode changed: %s -> %s", getGateModeName(prev_gate_mode.data), + getGateModeName(current_gate_mode_.data)); + } +} + +void VehicleCmdGate::onEngage(autoware_auto_vehicle_msgs::msg::Engage::ConstSharedPtr msg) +{ + is_engaged_ = msg->engage; +} + +void VehicleCmdGate::onEngageService( + const autoware_external_api_msgs::srv::Engage::Request::SharedPtr request, + const autoware_external_api_msgs::srv::Engage::Response::SharedPtr response) +{ + is_engaged_ = request->engage; + response->status = autoware_api_utils::response_success(); +} + +void VehicleCmdGate::onSteering(autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg) +{ + current_steer_ = msg->steering_tire_angle; +} + +double VehicleCmdGate::getDt() +{ + if (!prev_time_) { + prev_time_ = std::make_shared(this->now()); + return 0.0; + } + + const auto current_time = this->now(); + const auto dt = (current_time - *prev_time_).seconds(); + *prev_time_ = current_time; + + return dt; +} + +void VehicleCmdGate::onExternalEmergencyStopService( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + auto req = std::make_shared(); + auto res = std::make_shared(); + if (request->emergency) { + onSetExternalEmergencyStopService(request_header, req, res); + } else { + onClearExternalEmergencyStopService(request_header, req, res); + } + + if (res->success) { + response->status = autoware_api_utils::response_success(res->message); + } else { + response->status = autoware_api_utils::response_error(res->message); + } +} + +bool VehicleCmdGate::onSetExternalEmergencyStopService( + [[maybe_unused]] const std::shared_ptr req_header, + [[maybe_unused]] const std::shared_ptr req, + const std::shared_ptr res) +{ + is_external_emergency_stop_ = true; + res->success = true; + res->message = "external_emergency_stop requested was accepted."; + + return true; +} + +bool VehicleCmdGate::onClearExternalEmergencyStopService( + [[maybe_unused]] const std::shared_ptr req_header, + [[maybe_unused]] const std::shared_ptr req, + const std::shared_ptr res) +{ + if (is_external_emergency_stop_) { + if (!is_external_emergency_stop_heartbeat_timeout_) { + is_external_emergency_stop_ = false; + res->success = true; + res->message = "external_emergency_stop state was cleared."; + } else { + res->success = false; + res->message = "Couldn't clear external_emergency_stop state because heartbeat is timeout."; + } + } else { + res->success = false; + res->message = "Not in external_emergency_stop state."; + } + + return true; +} + +void VehicleCmdGate::checkExternalEmergencyStop(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + using diagnostic_msgs::msg::DiagnosticStatus; + + DiagnosticStatus status; + if (is_external_emergency_stop_heartbeat_timeout_) { + status.level = DiagnosticStatus::ERROR; + status.message = "external_emergency_stop heartbeat is timeout."; + } else if (is_external_emergency_stop_) { + status.level = DiagnosticStatus::ERROR; + status.message = + "external_emergency_stop is required. Please call `clear_external_emergency_stop` service to " + "clear state."; + } else { + status.level = DiagnosticStatus::OK; + } + + stat.summary(status.level, status.message); +} + +VehicleCmdGate::StartRequest::StartRequest(rclcpp::Node * node, bool use_start_request) +{ + using std::placeholders::_1; + + node_ = node; + use_start_request_ = use_start_request; + is_start_requesting_ = false; + is_start_accepted_ = false; + is_start_cancelled_ = false; + + if (!use_start_request_) { + return; + } + + request_start_cli_ = + node_->create_client("/api/autoware/set/start_request"); + request_start_pub_ = node_->create_publisher( + "/api/autoware/get/start_accepted", rclcpp::QoS(1)); + current_twist_sub_ = node_->create_subscription( + "/localization/twist", rclcpp::QoS(1), + std::bind(&VehicleCmdGate::StartRequest::onCurrentTwist, this, _1)); +} + +void VehicleCmdGate::StartRequest::onCurrentTwist(nav_msgs::msg::Odometry::ConstSharedPtr msg) +{ + current_twist_ = *msg; +} + +bool VehicleCmdGate::StartRequest::isAccepted() +{ + return !use_start_request_ || is_start_accepted_; +} + +void VehicleCmdGate::StartRequest::publishStartAccepted() +{ + if (!use_start_request_) { + return; + } + + autoware_debug_msgs::msg::BoolStamped start_accepted; + start_accepted.stamp = node_->now(); + start_accepted.data = is_start_accepted_; + request_start_pub_->publish(start_accepted); +} + +void VehicleCmdGate::StartRequest::checkStopped(const ControlCommandStamped & control) +{ + if (!use_start_request_) { + return; + } + + if (is_start_accepted_) { + const auto control_velocity = std::abs(control.longitudinal.speed); + const auto current_velocity = std::abs(current_twist_.twist.twist.linear.x); + if (control_velocity < eps && current_velocity < eps) { + is_start_accepted_ = false; + is_start_cancelled_ = true; + RCLCPP_INFO(node_->get_logger(), "clear start request"); + } + } +} + +void VehicleCmdGate::StartRequest::checkStartRequest(const ControlCommandStamped & control) +{ + if (!use_start_request_) { + return; + } + + if (!is_start_accepted_ && !is_start_requesting_) { + const auto control_velocity = std::abs(control.longitudinal.speed); + if (eps < control_velocity) { + is_start_requesting_ = true; + is_start_cancelled_ = false; + request_start_cli_->async_send_request( + std::make_shared(), + [this](rclcpp::Client::SharedFuture future) { + const auto response = future.get(); + is_start_requesting_ = false; + if (!is_start_cancelled_) { + is_start_accepted_ = response->success; + RCLCPP_INFO(node_->get_logger(), "start request is updated"); + } else { + RCLCPP_INFO(node_->get_logger(), "start request is cancelled"); + } + }); + RCLCPP_INFO(node_->get_logger(), "call start request"); + } + } +} + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(VehicleCmdGate) diff --git a/control/vehicle_cmd_gate/test/src/test_main.cpp b/control/vehicle_cmd_gate/test/src/test_main.cpp new file mode 100644 index 000000000000..399bcff8f918 --- /dev/null +++ b/control/vehicle_cmd_gate/test/src/test_main.cpp @@ -0,0 +1,26 @@ +// 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 + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + bool result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp b/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp new file mode 100644 index 000000000000..382380ce82b9 --- /dev/null +++ b/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp @@ -0,0 +1,197 @@ +// 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 "vehicle_cmd_gate/vehicle_cmd_filter.hpp" + +#include + +#include +#include +#include + +#define THRESHOLD 1.0e-5 +#define ASSERT_LT_NEAR(x, y) ASSERT_LT(x, y + THRESHOLD) +#define ASSERT_GT_NEAR(x, y) ASSERT_GT(x, y - THRESHOLD) + +using autoware_auto_control_msgs::msg::AckermannControlCommand; + +constexpr double NOMINAL_INTERVAL = 1.0; + +void setFilterParams( + VehicleCmdFilter & f, double v, double a, double j, double lat_a, double lat_j, double wheelbase) +{ + f.setVelLim(v); + f.setLonAccLim(a); + f.setLonJerkLim(j); + f.setLatAccLim(lat_a); + f.setLatJerkLim(lat_j); + f.setWheelBase(wheelbase); +} + +AckermannControlCommand genCmd(double s, double sr, double v, double a) +{ + AckermannControlCommand cmd; + cmd.lateral.steering_tire_angle = s; + cmd.lateral.steering_tire_rotation_rate = sr; + cmd.longitudinal.speed = v; + cmd.longitudinal.acceleration = a; + return cmd; +} + +double calcLatAcc(const AckermannControlCommand & cmd, const double wheelbase) +{ + double v = cmd.longitudinal.speed; + return v * v * std::tan(cmd.lateral.steering_tire_angle) / wheelbase; +} + +void test_all( + double V_LIM, double A_LIM, double J_LIM, double LAT_A_LIM, double LAT_J_LIM, + const AckermannControlCommand & prev_cmd, const AckermannControlCommand & raw_cmd) +{ + const double WHEELBASE = 3.0; + const double DT = 0.1; // [s] + + VehicleCmdFilter filter; + setFilterParams(filter, V_LIM, A_LIM, J_LIM, LAT_A_LIM, LAT_J_LIM, WHEELBASE); + filter.setPrevCmd(prev_cmd); + + // velocity filter + { + auto filtered_cmd = raw_cmd; + filter.limitLongitudinalWithVel(filtered_cmd); + + // check if the filtered value does not exceed the limit. + ASSERT_LT_NEAR(filtered_cmd.longitudinal.speed, V_LIM); + + // check if the undesired filter is not applied. + if (std::abs(raw_cmd.longitudinal.speed) < V_LIM) { + ASSERT_NEAR(filtered_cmd.longitudinal.speed, raw_cmd.longitudinal.speed, THRESHOLD); + } + } + + // acceleration filter + { + auto filtered_cmd = raw_cmd; + filter.limitLongitudinalWithAcc(DT, filtered_cmd); + + // check if the filtered value does not exceed the limit. + ASSERT_LT_NEAR(filtered_cmd.longitudinal.acceleration, A_LIM); + ASSERT_GT_NEAR(filtered_cmd.longitudinal.acceleration, -A_LIM); + + // check if the undesired filter is not applied. + if ( + -A_LIM < filtered_cmd.longitudinal.acceleration && + filtered_cmd.longitudinal.acceleration < A_LIM) { + ASSERT_NEAR( + filtered_cmd.longitudinal.acceleration, raw_cmd.longitudinal.acceleration, THRESHOLD); + } + + // check if the filtered value does not exceed the limit. + const double v_max = prev_cmd.longitudinal.speed + A_LIM * DT; + const double v_min = prev_cmd.longitudinal.speed - A_LIM * DT; + ASSERT_LT_NEAR(filtered_cmd.longitudinal.speed, v_max); + ASSERT_GT_NEAR(filtered_cmd.longitudinal.speed, v_min); + + // check if the undesired filter is not applied. + if (v_min < raw_cmd.longitudinal.speed && raw_cmd.longitudinal.speed < v_max) { + ASSERT_NEAR(filtered_cmd.longitudinal.speed, raw_cmd.longitudinal.speed, THRESHOLD); + } + } + + // jerk filter + { + auto filtered_cmd = raw_cmd; + filter.limitLongitudinalWithJerk(DT, filtered_cmd); + const double acc_max = prev_cmd.longitudinal.acceleration + J_LIM * DT; + const double acc_min = prev_cmd.longitudinal.acceleration - J_LIM * DT; + + // check if the filtered value does not exceed the limit. + ASSERT_LT_NEAR(filtered_cmd.longitudinal.acceleration, acc_max); + ASSERT_GT_NEAR(filtered_cmd.longitudinal.acceleration, acc_min); + + // check if the undesired filter is not applied. + if ( + acc_min < raw_cmd.longitudinal.acceleration && raw_cmd.longitudinal.acceleration < acc_max) { + ASSERT_NEAR( + filtered_cmd.longitudinal.acceleration, raw_cmd.longitudinal.acceleration, THRESHOLD); + } + } + + // lateral acceleration filter + { + auto filtered_cmd = raw_cmd; + filter.limitLateralWithLatAcc(DT, filtered_cmd); + const double filtered_latacc = calcLatAcc(filtered_cmd, WHEELBASE); + const double raw_latacc = calcLatAcc(raw_cmd, WHEELBASE); + + // check if the filtered value does not exceed the limit. + ASSERT_LT_NEAR(std::abs(filtered_latacc), LAT_A_LIM); + + // check if the undesired filter is not applied. + if (std::abs(raw_latacc) < LAT_A_LIM) { + ASSERT_NEAR(filtered_latacc, raw_latacc, THRESHOLD); + } + } + + // lateral jerk filter + { + auto filtered_cmd = raw_cmd; + filter.limitLateralWithLatJerk(DT, filtered_cmd); + const double prev_lat_acc = calcLatAcc(prev_cmd, WHEELBASE); + const double filtered_lat_acc = calcLatAcc(filtered_cmd, WHEELBASE); + const double raw_lat_acc = calcLatAcc(raw_cmd, WHEELBASE); + const double filtered_lateral_jerk = (filtered_lat_acc - prev_lat_acc) / DT; + + // check if the filtered value does not exceed the limit. + ASSERT_LT_NEAR(std::abs(filtered_lateral_jerk), LAT_J_LIM); + + // check if the undesired filter is not applied. + const double raw_lateral_jerk = (raw_lat_acc - prev_lat_acc) / DT; + if (std::abs(raw_lateral_jerk) < LAT_J_LIM) { + ASSERT_NEAR( + filtered_cmd.lateral.steering_tire_angle, raw_cmd.lateral.steering_tire_angle, THRESHOLD); + } + } +} + +TEST(VehicleCmdFilter, VehicleCmdFilter) +{ + const std::vector v_arr = {0.0, 1.0, 100.0}; + const std::vector a_arr = {0.0, 1.0, 100.0}; + const std::vector j_arr = {0.0, 0.1, 1.0}; + const std::vector lat_a_arr = {0.01, 1.0, 100.0}; + const std::vector lat_j_arr = {0.01, 1.0, 100.0}; + + const std::vector prev_cmd_arr = { + genCmd(0.0, 0.0, 0.0, 0.0), genCmd(1.0, 1.0, 1.0, 1.0)}; + + const std::vector raw_cmd_arr = { + genCmd(1.0, 1.0, 1.0, 1.0), genCmd(10.0, -1.0, -1.0, -1.0)}; + + for (const auto & v : v_arr) { + for (const auto & a : a_arr) { + for (const auto & j : j_arr) { + for (const auto & la : lat_a_arr) { + for (const auto & lj : lat_j_arr) { + for (const auto & prev_cmd : prev_cmd_arr) { + for (const auto & raw_cmd : raw_cmd_arr) { + test_all(v, a, j, la, lj, prev_cmd, raw_cmd); + } + } + } + } + } + } + } +}