diff --git a/vehicle/autoware_external_cmd_converter/CMakeLists.txt b/vehicle/autoware_external_cmd_converter/CMakeLists.txt index 27a256ea775c5..01e82c99a9c4d 100644 --- a/vehicle/autoware_external_cmd_converter/CMakeLists.txt +++ b/vehicle/autoware_external_cmd_converter/CMakeLists.txt @@ -13,6 +13,16 @@ rclcpp_components_register_node(autoware_external_cmd_converter EXECUTABLE external_cmd_converter_node ) +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + tests/test_external_cmd_converter.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + autoware_external_cmd_converter + ) +endif() + + ament_auto_package(INSTALL_TO_SHARE launch config diff --git a/vehicle/autoware_external_cmd_converter/config/test_external_cmd_converter.param.yaml b/vehicle/autoware_external_cmd_converter/config/test_external_cmd_converter.param.yaml new file mode 100644 index 0000000000000..caba3777feeaf --- /dev/null +++ b/vehicle/autoware_external_cmd_converter/config/test_external_cmd_converter.param.yaml @@ -0,0 +1,9 @@ +/**: + ros__parameters: + csv_path_accel_map: $(var csv_path_accel_map) + csv_path_brake_map: $(var csv_path_brake_map) + ref_vel_gain: 0.2 + timer_rate: 0.5 + wait_for_first_topic: false + control_command_timeout: 2.0 + emergency_stop_timeout: 2.0 diff --git a/vehicle/autoware_external_cmd_converter/include/autoware_external_cmd_converter/node.hpp b/vehicle/autoware_external_cmd_converter/include/autoware_external_cmd_converter/node.hpp index c219c162d6b37..8d045279fc281 100644 --- a/vehicle/autoware_external_cmd_converter/include/autoware_external_cmd_converter/node.hpp +++ b/vehicle/autoware_external_cmd_converter/include/autoware_external_cmd_converter/node.hpp @@ -32,6 +32,8 @@ #include #include +class TestExternalCmdConverter; + namespace autoware::external_cmd_converter { using GearCommand = autoware_vehicle_msgs::msg::GearCommand; @@ -103,6 +105,8 @@ class ExternalCmdConverterNode : public rclcpp::Node double calculate_acc(const ExternalControlCommand & cmd, const double vel); double get_shift_velocity_sign(const GearCommand & cmd); + + friend class ::TestExternalCmdConverter; }; } // namespace autoware::external_cmd_converter diff --git a/vehicle/autoware_external_cmd_converter/package.xml b/vehicle/autoware_external_cmd_converter/package.xml index f98c14f4fd2c3..59e2077d45cc5 100644 --- a/vehicle/autoware_external_cmd_converter/package.xml +++ b/vehicle/autoware_external_cmd_converter/package.xml @@ -15,6 +15,7 @@ autoware_control_msgs autoware_raw_vehicle_cmd_converter + autoware_test_utils autoware_vehicle_msgs diagnostic_updater geometry_msgs @@ -26,6 +27,7 @@ tier4_external_api_msgs tier4_system_msgs + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/vehicle/autoware_external_cmd_converter/src/node.cpp b/vehicle/autoware_external_cmd_converter/src/node.cpp index 22fc1c319a4e3..0f7166c71fbff 100644 --- a/vehicle/autoware_external_cmd_converter/src/node.cpp +++ b/vehicle/autoware_external_cmd_converter/src/node.cpp @@ -15,6 +15,7 @@ #include "autoware_external_cmd_converter/node.hpp" #include +#include #include #include #include @@ -147,6 +148,14 @@ double ExternalCmdConverterNode::calculate_acc(const ExternalControlCommand & cm { const double desired_throttle = cmd.control.throttle; const double desired_brake = cmd.control.brake; + if ( + std::isnan(desired_throttle) || std::isnan(desired_brake) || std::isinf(desired_throttle) || + std::isinf(desired_brake)) { + std::cerr << "Input brake or throttle is out of range. returning 0.0 acceleration." + << std::endl; + return 0.0; + } + const double desired_pedal = desired_throttle - desired_brake; double ref_acceleration = 0.0; diff --git a/vehicle/autoware_external_cmd_converter/tests/test_external_cmd_converter.cpp b/vehicle/autoware_external_cmd_converter/tests/test_external_cmd_converter.cpp new file mode 100644 index 0000000000000..a508359af60e8 --- /dev/null +++ b/vehicle/autoware_external_cmd_converter/tests/test_external_cmd_converter.cpp @@ -0,0 +1,284 @@ +// Copyright 2024 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 "autoware_external_cmd_converter/node.hpp" + +#include +#include +#include +#include +#include + +#include + +#include +#include +using autoware::external_cmd_converter::ExternalCmdConverterNode; +using nav_msgs::msg::Odometry; +using tier4_control_msgs::msg::GateMode; +using GearCommand = autoware_vehicle_msgs::msg::GearCommand; +using autoware_control_msgs::msg::Control; +using ExternalControlCommand = tier4_external_api_msgs::msg::ControlCommandStamped; +using autoware::raw_vehicle_cmd_converter::AccelMap; +using autoware::raw_vehicle_cmd_converter::BrakeMap; + +class TestExternalCmdConverter : public ::testing::Test +{ +public: + void SetUp() override + { + rclcpp::init(0, nullptr); + set_up_node(); + } + + void set_up_node() + { + auto node_options = rclcpp::NodeOptions{}; + const auto external_cmd_converter_dir = + ament_index_cpp::get_package_share_directory("autoware_external_cmd_converter"); + node_options.arguments( + {"--ros-args", "--params-file", + external_cmd_converter_dir + "/config/test_external_cmd_converter.param.yaml"}); + external_cmd_converter_ = std::make_shared(node_options); + } + + void TearDown() override + { + external_cmd_converter_ = nullptr; + rclcpp::shutdown(); + } + + /** + * @brief Test the external command converter's check_remote_topic_rate method under + * different circumstances: + * @param received_data wether to simulate if the emergency heartbeat and gate mode messages have + * been received or not + * @param is_external set the gate mode to Auto or External + * @param time_has_passed wether to sleep for a certain time to make sure the timeout period of + * the function has expired or not. + */ + bool test_check_remote_topic_rate(bool received_data, bool is_auto, bool time_has_passed) + { + if (!received_data) { + return external_cmd_converter_->check_remote_topic_rate(); + } + GateMode gate; + gate.data = (is_auto) ? tier4_control_msgs::msg::GateMode::AUTO + : tier4_control_msgs::msg::GateMode::EXTERNAL; + external_cmd_converter_->current_gate_mode_ = std::make_shared(gate); + external_cmd_converter_->latest_cmd_received_time_ = + std::make_shared(external_cmd_converter_->now()); + + if (!time_has_passed) { + return external_cmd_converter_->check_remote_topic_rate(); + } + const int sleep_time = static_cast(external_cmd_converter_->control_command_timeout_) + 1; + std::this_thread::sleep_for(std::chrono::seconds(sleep_time)); + return external_cmd_converter_->check_remote_topic_rate(); + } + + /** + * @brief Test the external command converter's check_emergency_stop_topic_timeout method under + * different circumstances: + * @param received_data wether to simulate if the emergency heartbeat and gate mode messages have + * been received or not + * @param is_auto set the gate mode to Auto or External + * @param time_has_passed wether to sleep for a certain time to make sure the timeout period of + * the function has expired or not. + */ + bool test_check_emergency_stop_topic_timeout( + bool received_data, bool is_auto, bool time_has_passed) + { + if (!received_data) { + return external_cmd_converter_->check_emergency_stop_topic_timeout(); + } + tier4_external_api_msgs::msg::Heartbeat::ConstSharedPtr msg; + external_cmd_converter_->on_emergency_stop_heartbeat(msg); + GateMode gate; + gate.data = (is_auto) ? tier4_control_msgs::msg::GateMode::AUTO + : tier4_control_msgs::msg::GateMode::EXTERNAL; + external_cmd_converter_->current_gate_mode_ = std::make_shared(gate); + + const int sleep_time = static_cast(external_cmd_converter_->emergency_stop_timeout_) + 1; + if (time_has_passed) std::this_thread::sleep_for(std::chrono::seconds(sleep_time)); + return external_cmd_converter_->check_emergency_stop_topic_timeout(); + } + + /** + * @brief Test the external command converter's get_shift_velocity_sign. This function is a + * wrapper to call the converter's function during the test and does nothing but pass the command + * to the function. + * @param cmd the command to be passed to the get_shift_velocity_sign member function. + */ + double test_get_shift_velocity_sign(const GearCommand & cmd) + { + return external_cmd_converter_->get_shift_velocity_sign(cmd); + } + + /** + * @brief Helper function to initialize the accel and brake maps for testing. + */ + void initialize_maps(const std::string & accel_map, const std::string & brake_map) + { + external_cmd_converter_->accel_map_.readAccelMapFromCSV(accel_map); + external_cmd_converter_->brake_map_.readBrakeMapFromCSV(brake_map); + external_cmd_converter_->acc_map_initialized_ = true; + } + + /** + * @brief Test the external command converter's calculate_acc. This function is a + * wrapper to call the converter's function during the test and does nothing but pass the command + * to the function. + * @param cmd the command to be passed to the calculate_acc member function. + * @param vel the velocity to be passed to the calculate_acc member function. + */ + double test_calculate_acc( + + const ExternalControlCommand & cmd, const double vel) + { + return external_cmd_converter_->calculate_acc(cmd, vel); + } + + std::shared_ptr external_cmd_converter_; +}; + +/** + * @brief Test the external command converter's check_emergency_stop_topic_timeout method under + * different circumstances by setting different combinations of conditions for the function + * execution. the test_check_emergency_stop_topic_timeout() function's brief should be read for + * further details. + */ +TEST_F(TestExternalCmdConverter, testCheckEmergencyStopTimeout) +{ + EXPECT_TRUE(test_check_emergency_stop_topic_timeout(false, false, false)); + EXPECT_FALSE(test_check_emergency_stop_topic_timeout(true, true, false)); + EXPECT_TRUE(test_check_emergency_stop_topic_timeout(true, false, false)); + EXPECT_FALSE(test_check_emergency_stop_topic_timeout(true, false, true)); +} + +/** + * @brief Test the external command converter's check_remote_topic_rate method under + * different circumstances by setting different combinations of conditions for the function + * execution. the test_check_remote_topic_rate() function's brief should be read for further + * details. + */ +TEST_F(TestExternalCmdConverter, testRemoteTopicRate) +{ + EXPECT_TRUE(test_check_remote_topic_rate(false, false, false)); + EXPECT_TRUE(test_check_remote_topic_rate(true, true, false)); + EXPECT_TRUE(test_check_remote_topic_rate(true, true, true)); + EXPECT_FALSE(test_check_remote_topic_rate(true, false, true)); +} + +/** + * @brief Test the external command converter's get_shift_velocity_sign function. It tests different + * combinations of input commands for the functions. + */ +TEST_F(TestExternalCmdConverter, testGetShiftVelocitySign) +{ + GearCommand cmd; + cmd.command = GearCommand::DRIVE; + EXPECT_DOUBLE_EQ(1.0, test_get_shift_velocity_sign(cmd)); + cmd.command = GearCommand::LOW; + EXPECT_DOUBLE_EQ(1.0, test_get_shift_velocity_sign(cmd)); + cmd.command = GearCommand::REVERSE; + EXPECT_DOUBLE_EQ(-1.0, test_get_shift_velocity_sign(cmd)); + cmd.command = GearCommand::LOW_2; + EXPECT_DOUBLE_EQ(0.0, test_get_shift_velocity_sign(cmd)); +} + +/** + * @brief Test the external command converter's calculate_acc function. It tests different + * combinations of input commands and velocities for the functions. + */ +TEST_F(TestExternalCmdConverter, testCalculateAcc) +{ + const auto map_dir = + ament_index_cpp::get_package_share_directory("autoware_raw_vehicle_cmd_converter"); + const auto accel_map_path = map_dir + "/data/default/accel_map.csv"; + const auto brake_map_path = map_dir + "/data/default/brake_map.csv"; + initialize_maps(accel_map_path, brake_map_path); + + // test standard cases + { + ExternalControlCommand cmd; + cmd.control.throttle = 1.0; + cmd.control.brake = 0.0; + double vel = 0.0; + EXPECT_TRUE(test_calculate_acc(cmd, vel) > 0.0); + cmd.control.throttle = 0.0; + cmd.control.brake = 1.0; + EXPECT_TRUE(test_calculate_acc(cmd, vel) < 0.0); + + cmd.control.throttle = 1.0; + cmd.control.brake = 0.5; + vel = 1.0; + EXPECT_TRUE(test_calculate_acc(cmd, vel) > 0.0); + cmd.control.throttle = 0.5; + cmd.control.brake = 1.0; + EXPECT_TRUE(test_calculate_acc(cmd, vel) < 0.0); + } + // test border cases + { + // input brake or throttle are infinite. finite velocity. + ExternalControlCommand cmd; + cmd.control.throttle = std::numeric_limits::infinity(); + cmd.control.brake = 0.0; + double vel = 0.0; + double acc = test_calculate_acc(cmd, vel); + EXPECT_DOUBLE_EQ(acc, 0.0); + cmd.control.throttle = 0.0; + cmd.control.brake = std::numeric_limits::infinity(); + acc = test_calculate_acc(cmd, vel); + EXPECT_DOUBLE_EQ(acc, 0.0); + + // input brake or throttle are infinite. velocity is infinite. + // Check that acc is not NaN and not infinite + + vel = std::numeric_limits::infinity(); + cmd.control.throttle = std::numeric_limits::infinity(); + cmd.control.brake = 0.0; + acc = test_calculate_acc(cmd, vel); + EXPECT_FALSE(std::isnan(acc)); + EXPECT_FALSE(std::isinf(acc)); + EXPECT_DOUBLE_EQ(acc, 0.0); + cmd.control.throttle = 0.0; + cmd.control.brake = std::numeric_limits::infinity(); + acc = test_calculate_acc(cmd, vel); + EXPECT_FALSE(std::isnan(acc)); + EXPECT_FALSE(std::isinf(acc)); + EXPECT_DOUBLE_EQ(acc, 0.0); + + cmd.control.throttle = std::numeric_limits::infinity(); + cmd.control.brake = std::numeric_limits::infinity(); + acc = test_calculate_acc(cmd, vel); + EXPECT_FALSE(std::isnan(acc)); + EXPECT_FALSE(std::isinf(acc)); + EXPECT_DOUBLE_EQ(acc, 0.0); + + cmd.control.throttle = std::numeric_limits::quiet_NaN(); + cmd.control.brake = std::numeric_limits::quiet_NaN(); + acc = test_calculate_acc(cmd, vel); + EXPECT_FALSE(std::isnan(acc)); + EXPECT_FALSE(std::isinf(acc)); + EXPECT_DOUBLE_EQ(acc, 0.0); + + cmd.control.throttle = std::numeric_limits::signaling_NaN(); + cmd.control.brake = std::numeric_limits::signaling_NaN(); + acc = test_calculate_acc(cmd, vel); + EXPECT_FALSE(std::isnan(acc)); + EXPECT_FALSE(std::isinf(acc)); + EXPECT_DOUBLE_EQ(acc, 0.0); + } +}