diff --git a/vehicle/raw_vehicle_cmd_converter/CMakeLists.txt b/vehicle/raw_vehicle_cmd_converter/CMakeLists.txt new file mode 100644 index 0000000000000..89cf9abe24e49 --- /dev/null +++ b/vehicle/raw_vehicle_cmd_converter/CMakeLists.txt @@ -0,0 +1,51 @@ +cmake_minimum_required(VERSION 3.5) + +project(raw_vehicle_cmd_converter) + +### 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) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +ament_auto_add_library(actuation_map_converter SHARED + src/accel_map.cpp + src/brake_map.cpp + src/csv_loader.cpp + src/interpolate.cpp + src/pid.cpp + src/steer_converter.cpp +) + +ament_auto_add_library(raw_vehicle_cmd_converter_node_component SHARED + src/node.cpp +) + +target_link_libraries(raw_vehicle_cmd_converter_node_component actuation_map_converter) + +rclcpp_components_register_node(raw_vehicle_cmd_converter_node_component + PLUGIN "raw_vehicle_cmd_converter::RawVehicleCommandConverterNode" + EXECUTABLE raw_vehicle_cmd_converter_node +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package(INSTALL_TO_SHARE + config + data + launch +) + +install(PROGRAMS scripts/plot_accel_brake_map.py + DESTINATION lib/${PROJECT_NAME} +) diff --git a/vehicle/raw_vehicle_cmd_converter/README.md b/vehicle/raw_vehicle_cmd_converter/README.md new file mode 100644 index 0000000000000..e5a3002d437dd --- /dev/null +++ b/vehicle/raw_vehicle_cmd_converter/README.md @@ -0,0 +1,31 @@ +# raw_vehicle_cmd_converter + +`raw_vehicle_command_converter` is a node that converts desired acceleration and velocity to mechanical input by using feed forward + feed back control (optional). + +## Input topics + +| Name | Type | Description | +| --------------------- | -------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------ | +| `~/input/control_cmd` | autoware_auto_control_msgs::msg::AckermannControlCommand | target `velocity/acceleration/steering_angle/steering_angle_velocity` is necessary to calculate actuation command. | +| `~/input/steering"` | autoware_auto_vehicle_msgs::SteeringReport | current status of steering used for steering feed back control | +| `~/input/twist` | navigation_msgs::Odometry | twist topic in odometry is used. | + +## Output topics + +| Name | Type | Description | +| ------------------------ | --------------------------------------------------- | ------------------------------------------------------- | +| `~/output/actuation_cmd` | autoware_vehicle_msgs::msg::ActuationCommandStamped | actuation command for vehicle to apply mechanical input | + +## Parameters + +| Parameter | Type | Description | +| -------------------------- | ------ | ------------------------------------------------------------------------------- | +| `update_rate` | double | timer's update rate | +| `th_max_message_delay_sec` | double | threshold time of input messages' maximum delay | +| `th_arrived_distance_m` | double | threshold distance to check if vehicle has arrived at the trajectory's endpoint | +| `th_stopped_time_sec` | double | threshold time to check if vehicle is stopped | +| `th_stopped_velocity_mps` | double | threshold velocity to check if vehicle is stopped | + +## Limitation + +The current feed back implementation is only applied to steering control. diff --git a/vehicle/raw_vehicle_cmd_converter/config/converter.param.yaml b/vehicle/raw_vehicle_cmd_converter/config/converter.param.yaml new file mode 100644 index 0000000000000..3e98b794d8dc4 --- /dev/null +++ b/vehicle/raw_vehicle_cmd_converter/config/converter.param.yaml @@ -0,0 +1,18 @@ +/**: + ros__parameters: + use_steer_ff: true + use_steer_fb: true + is_debugging: false + steer_pid: + kp: 150.0 + ki: 15.0 + kd: 0.0 + max: 8.0 + min: -8.0 + max_p: 8.0 + min_p: -8.0 + max_i: 8.0 + min_i: -8.0 + max_d: 0.0 + min_d: 0.0 + invalid_integration_decay: 0.97 diff --git a/vehicle/raw_vehicle_cmd_converter/data/default/accel_map.csv b/vehicle/raw_vehicle_cmd_converter/data/default/accel_map.csv new file mode 100644 index 0000000000000..32e639cad3373 --- /dev/null +++ b/vehicle/raw_vehicle_cmd_converter/data/default/accel_map.csv @@ -0,0 +1,7 @@ +default,0.0, 1.39, 2.78, 4.17, 5.56, 6.94, 8.33, 9.72, 11.11, 12.50, 13.89 +0,0.3,-0.05,-0.3,-0.39,-0.4,-0.41,-0.42,-0.44,-0.46,-0.48,-0.5 +0.1,0.6,0.42,0.24,0.18,0.12,0.05,-0.08,-0.16,-0.2,-0.24,-0.28 +0.2,1.15,0.98,0.78,0.6,0.48,0.34,0.26,0.2,0.1,0.05,-0.03 +0.3,1.75,1.6,1.42,1.3,1.14,1,0.9,0.8,0.72,0.64,0.58 +0.4,2.65,2.48,2.3,2.13,1.95,1.75,1.58,1.45,1.32,1.2,1.1 +0.5,3.3,3.25,3.12,2.92,2.68,2.35,2.17,1.98,1.88,1.73,1.61 diff --git a/vehicle/raw_vehicle_cmd_converter/data/default/brake_map.csv b/vehicle/raw_vehicle_cmd_converter/data/default/brake_map.csv new file mode 100644 index 0000000000000..adf2c80950bb1 --- /dev/null +++ b/vehicle/raw_vehicle_cmd_converter/data/default/brake_map.csv @@ -0,0 +1,10 @@ +default,0.0, 1.39, 2.78, 4.17, 5.56, 6.94, 8.33, 9.72, 11.11, 12.50, 13.89 +0,0.3,-0.05,-0.3,-0.39,-0.4,-0.41,-0.42,-0.44,-0.46,-0.48,-0.5 +0.1,0.29,-0.06,-0.31,-0.4,-0.41,-0.42,-0.43,-0.45,-0.47,-0.49,-0.51 +0.2,-0.38,-0.4,-0.72,-0.8,-0.82,-0.85,-0.87,-0.89,-0.91,-0.94,-0.96 +0.3,-1,-1.04,-1.48,-1.55,-1.57,-1.59,-1.61,-1.63,-1.631,-1.632,-1.633 +0.4,-1.48,-1.5,-1.85,-2.05,-2.1,-2.101,-2.102,-2.103,-2.104,-2.105,-2.106 +0.5,-1.49,-1.51,-1.86,-2.06,-2.11,-2.111,-2.112,-2.113,-2.114,-2.115,-2.116 +0.6,-1.5,-1.52,-1.87,-2.07,-2.12,-2.121,-2.122,-2.123,-2.124,-2.125,-2.126 +0.7,-1.51,-1.53,-1.88,-2.08,-2.13,-2.131,-2.132,-2.133,-2.134,-2.135,-2.136 +0.8,-2.18,-2.2,-2.7,-2.8,-2.9,-2.95,-2.951,-2.952,-2.953,-2.954,-2.955 diff --git a/vehicle/raw_vehicle_cmd_converter/data/default/steer_map.csv b/vehicle/raw_vehicle_cmd_converter/data/default/steer_map.csv new file mode 100644 index 0000000000000..046d15b3c0699 --- /dev/null +++ b/vehicle/raw_vehicle_cmd_converter/data/default/steer_map.csv @@ -0,0 +1,4 @@ +default,-10,0,10 +-10,-10,-10,-10 +0,0,0,0 +10,10,10,10 diff --git a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/accel_map.hpp b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/accel_map.hpp new file mode 100644 index 0000000000000..4c7bedfed93b4 --- /dev/null +++ b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/accel_map.hpp @@ -0,0 +1,50 @@ +// 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 RAW_VEHICLE_CMD_CONVERTER__ACCEL_MAP_HPP_ +#define RAW_VEHICLE_CMD_CONVERTER__ACCEL_MAP_HPP_ + +#include "raw_vehicle_cmd_converter/csv_loader.hpp" +#include "raw_vehicle_cmd_converter/interpolate.hpp" + +#include + +#include +#include +#include +#include + +namespace raw_vehicle_cmd_converter +{ +class AccelMap +{ +public: + bool readAccelMapFromCSV(std::string csv_path); + bool getThrottle(double acc, double vel, double & throttle); + bool getAcceleration(double throttle, double vel, double & acc); + std::vector getVelIdx() { return vel_index_; } + std::vector getThrottleIdx() { return throttle_index_; } + std::vector> getAccelMap() { return accel_map_; } + +private: + rclcpp::Logger logger_{rclcpp::get_logger("raw_vehicle_cmd_converter").get_child("accel_map")}; + rclcpp::Clock clock_{RCL_ROS_TIME}; + std::string vehicle_name_; + std::vector vel_index_; + std::vector throttle_index_; + std::vector> accel_map_; +}; +} // namespace raw_vehicle_cmd_converter + +#endif // RAW_VEHICLE_CMD_CONVERTER__ACCEL_MAP_HPP_ diff --git a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/brake_map.hpp b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/brake_map.hpp new file mode 100644 index 0000000000000..a5065d244877b --- /dev/null +++ b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/brake_map.hpp @@ -0,0 +1,51 @@ +// 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 RAW_VEHICLE_CMD_CONVERTER__BRAKE_MAP_HPP_ +#define RAW_VEHICLE_CMD_CONVERTER__BRAKE_MAP_HPP_ + +#include "raw_vehicle_cmd_converter/csv_loader.hpp" +#include "raw_vehicle_cmd_converter/interpolate.hpp" + +#include + +#include +#include +#include +#include + +namespace raw_vehicle_cmd_converter +{ +class BrakeMap +{ +public: + bool readBrakeMapFromCSV(std::string csv_path); + bool getBrake(double acc, double vel, double & brake); + bool getAcceleration(double brake, double vel, double & acc); + std::vector getVelIdx() { return vel_index_; } + std::vector getBrakeIdx() { return brake_index_; } + std::vector> getBrakeMap() { return brake_map_; } + +private: + rclcpp::Logger logger_{rclcpp::get_logger("raw_vehicle_cmd_converter").get_child("accel_map")}; + rclcpp::Clock clock_{RCL_ROS_TIME}; + std::string vehicle_name_; + std::vector vel_index_; + std::vector brake_index_; + std::vector brake_index_rev_; + std::vector> brake_map_; +}; +} // namespace raw_vehicle_cmd_converter + +#endif // RAW_VEHICLE_CMD_CONVERTER__BRAKE_MAP_HPP_ diff --git a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/csv_loader.hpp b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/csv_loader.hpp new file mode 100644 index 0000000000000..6537615d5320c --- /dev/null +++ b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/csv_loader.hpp @@ -0,0 +1,38 @@ +// Copyright 2018-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 RAW_VEHICLE_CMD_CONVERTER__CSV_LOADER_HPP_ +#define RAW_VEHICLE_CMD_CONVERTER__CSV_LOADER_HPP_ + +#include +#include +#include +#include +#include + +namespace raw_vehicle_cmd_converter +{ +class CSVLoader +{ +public: + explicit CSVLoader(std::string csv_path); + + bool readCSV(std::vector> & result, const char delim = ','); + +private: + std::string csv_path_; +}; +} // namespace raw_vehicle_cmd_converter + +#endif // RAW_VEHICLE_CMD_CONVERTER__CSV_LOADER_HPP_ diff --git a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/interpolate.hpp b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/interpolate.hpp new file mode 100644 index 0000000000000..99ac24f71d5a1 --- /dev/null +++ b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/interpolate.hpp @@ -0,0 +1,33 @@ +// Copyright 2018-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 RAW_VEHICLE_CMD_CONVERTER__INTERPOLATE_HPP_ +#define RAW_VEHICLE_CMD_CONVERTER__INTERPOLATE_HPP_ + +#include +#include +#include + +namespace raw_vehicle_cmd_converter +{ +class LinearInterpolate +{ +public: + static bool interpolate( + const std::vector & base_index, const std::vector & base_value, + const double & return_index, double & return_value); +}; +} // namespace raw_vehicle_cmd_converter + +#endif // RAW_VEHICLE_CMD_CONVERTER__INTERPOLATE_HPP_ diff --git a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/node.hpp b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/node.hpp new file mode 100644 index 0000000000000..5b8bc75b7a7ed --- /dev/null +++ b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/node.hpp @@ -0,0 +1,119 @@ +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 RAW_VEHICLE_CMD_CONVERTER__NODE_HPP_ +#define RAW_VEHICLE_CMD_CONVERTER__NODE_HPP_ + +#include "raw_vehicle_cmd_converter/accel_map.hpp" +#include "raw_vehicle_cmd_converter/brake_map.hpp" +#include "raw_vehicle_cmd_converter/pid.hpp" +#include "raw_vehicle_cmd_converter/steer_converter.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace raw_vehicle_cmd_converter +{ +using AckermannControlCommand = autoware_auto_control_msgs::msg::AckermannControlCommand; +using autoware_debug_msgs::msg::Float32MultiArrayStamped; +using autoware_vehicle_msgs::msg::ActuationCommandStamped; +using TwistStamped = geometry_msgs::msg::TwistStamped; +using Odometry = nav_msgs::msg::Odometry; +using Steering = autoware_auto_vehicle_msgs::msg::SteeringReport; + +class DebugValues +{ +public: + enum class TYPE { + CURR_TIME = 0, + P = 1, + I = 2, + D = 3, + FF = 4, + FB = 5, + STEER = 6, + ERROR_P = 7, + ERROR_I = 8, + ERROR_D = 9, + SIZE // this is the number of enum elements + }; + std::array(TYPE::SIZE)> getValues() const { return values_; } + void setValues(TYPE type, double val) { values_.at(static_cast(type)) = val; } + void setValues(int type, double val) { values_.at(type) = val; } + +private: + std::array(TYPE::SIZE)> values_; +}; + +class RawVehicleCommandConverterNode : public rclcpp::Node +{ +public: + explicit RawVehicleCommandConverterNode(const rclcpp::NodeOptions & node_options); + + //!< @brief topic publisher for low level vehicle command + rclcpp::Publisher::SharedPtr pub_actuation_cmd_; + //!< @brief subscriber for current velocity + rclcpp::Subscription::SharedPtr sub_velocity_; + //!< @brief subscriber for vehicle command + rclcpp::Subscription::SharedPtr sub_control_cmd_; + //!< @brief subscriber for steering + rclcpp::Subscription::SharedPtr sub_steering_; + + rclcpp::TimerBase::SharedPtr timer_; + + std::unique_ptr current_twist_ptr_; // [m/s] + std::unique_ptr current_steer_ptr_; + AckermannControlCommand::ConstSharedPtr control_cmd_ptr_; + AccelMap accel_map_; + BrakeMap brake_map_; + bool ff_map_initialized_; + double max_accel_cmd_; + double max_brake_cmd_; + double max_steer_cmd_; + double min_steer_cmd_; + // ros parameter + bool use_steer_ff_; + bool use_steer_fb_; + bool is_debugging_; + bool convert_accel_cmd_; //!< @brief use accel or not + bool convert_brake_cmd_; //!< @brief use brake or not + bool convert_steer_cmd_; //!< @brief use steer or not + SteerConverter steer_controller_; + rclcpp::Time prev_time_steer_calculation_{0, 0, RCL_ROS_TIME}; + + double calculateAccelMap( + const double current_velocity, const double desired_acc, bool & accel_cmd_is_zero); + double calculateBrakeMap(const double current_velocity, const double desired_acc); + double calculateSteer(const double vel, const double steering, const double steer_rate); + void onSteering(const Steering::ConstSharedPtr msg); + void onControlCmd(const AckermannControlCommand::ConstSharedPtr msg); + void onVelocity(const Odometry::ConstSharedPtr msg); + void publishActuationCmd(); + // for debugging + rclcpp::Publisher::SharedPtr debug_pub_steer_pid_; + DebugValues debug_steer_; +}; +} // namespace raw_vehicle_cmd_converter + +#endif // RAW_VEHICLE_CMD_CONVERTER__NODE_HPP_ diff --git a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/pid.hpp b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/pid.hpp new file mode 100644 index 0000000000000..a2333e95453fa --- /dev/null +++ b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/pid.hpp @@ -0,0 +1,58 @@ +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 RAW_VEHICLE_CMD_CONVERTER__PID_HPP_ +#define RAW_VEHICLE_CMD_CONVERTER__PID_HPP_ + +#include +#include +#include + +namespace raw_vehicle_cmd_converter +{ +class PIDController +{ +public: + double calculatePID( + const double error, const double dt, const bool enable_integration, + std::vector & pid_contributions, std::vector & errors, bool is_debugging); + void setGains(const double kp, const double ki, const double kd); + void setLimits( + const double max_ret, const double min_ret, const double max_ret_p, const double min_ret_p, + const double max_ret_i, const double min_ret_i, const double max_ret_d, const double min_ret_d); + void reset(); + void setDecay(const double decay) { invalid_integration_decay_ = decay; } + +private: + // parameters + double kp_; + double ki_; + double kd_; + double max_ret_p_; + double min_ret_p_; + double max_ret_i_; + double min_ret_i_; + double max_ret_d_; + double min_ret_d_; + double max_ret_; + double min_ret_; + // states + double error_integral_{0.0}; + double prev_error_{0.0}; + bool is_first_time_{true}; + double invalid_integration_decay_{0.0}; +}; +} // namespace raw_vehicle_cmd_converter + +#endif // RAW_VEHICLE_CMD_CONVERTER__PID_HPP_ diff --git a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/steer_converter.hpp b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/steer_converter.hpp new file mode 100644 index 0000000000000..b476e4554bd43 --- /dev/null +++ b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/steer_converter.hpp @@ -0,0 +1,66 @@ +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 RAW_VEHICLE_CMD_CONVERTER__STEER_CONVERTER_HPP_ +#define RAW_VEHICLE_CMD_CONVERTER__STEER_CONVERTER_HPP_ + +#include "raw_vehicle_cmd_converter/csv_loader.hpp" +#include "raw_vehicle_cmd_converter/interpolate.hpp" +#include "raw_vehicle_cmd_converter/pid.hpp" + +#include + +#include +#include + +namespace raw_vehicle_cmd_converter +{ +class SteerConverter +{ +public: + bool setFFMap(const std::string & csv_path); + void setFBGains(const double kp, const double ki, const double kd); + bool setFBLimits( + const double max_ret, const double min_ret, const double max_ret_p, const double min_ret_p, + const double max_ret_i, const double min_ret_i, const double max_ret_d, const double min_ret_d); + double calcFFSteer( + const double target_steer_angle_velocity, const double current_steer_angle) const; + double calcFBSteer( + const double target_steer_angle, const double dt, const double current_velocity, + const double current_steer_angle, std::vector & pid_contributions, + std::vector & errors); + void setDecay(const double decay) { pid_.setDecay(decay); } + +private: + double kp_, ki_, kd_; + std::string vehicle_name_; + std::vector vel_index_; + std::vector output_index_; + std::vector> steer_map_; + PIDController pid_; + bool ff_map_initialized_{false}; + bool fb_gains_initialized_{false}; + bool fb_limits_initialized_{false}; + + rclcpp::Logger logger_{ + rclcpp::get_logger("raw_vehicle_cmd_converter").get_child("steer_converter")}; + + bool readSteerMapFromCSV( + const std::string & csv_path, std::string & vehicle_name, std::vector & vel_index, + std::vector & output_index, std::vector> & steer_map) const; + void calcFFMap(double steer_vel, double current_steer_val, double & output) const; +}; +} // namespace raw_vehicle_cmd_converter + +#endif // RAW_VEHICLE_CMD_CONVERTER__STEER_CONVERTER_HPP_ diff --git a/vehicle/raw_vehicle_cmd_converter/launch/raw_vehicle_converter.launch.xml b/vehicle/raw_vehicle_cmd_converter/launch/raw_vehicle_converter.launch.xml new file mode 100644 index 0000000000000..23e5d531d5e3e --- /dev/null +++ b/vehicle/raw_vehicle_cmd_converter/launch/raw_vehicle_converter.launch.xml @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/vehicle/raw_vehicle_cmd_converter/package.xml b/vehicle/raw_vehicle_cmd_converter/package.xml new file mode 100644 index 0000000000000..7100dc0b7bbdf --- /dev/null +++ b/vehicle/raw_vehicle_cmd_converter/package.xml @@ -0,0 +1,30 @@ + + + raw_vehicle_cmd_converter + 0.1.0 + The raw_vehicle_cmd_converter package + Horibe Takamasa + Makoto Kurihara + Horibe Takamasa + Apache License 2.0 + + ament_cmake_auto + + autoware_auto_control_msgs + autoware_auto_vehicle_msgs + autoware_debug_msgs + autoware_vehicle_msgs + geometry_msgs + nav_msgs + rclcpp + rclcpp_components + + rclpy + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/vehicle/raw_vehicle_cmd_converter/scripts/plot_accel_brake_map.py b/vehicle/raw_vehicle_cmd_converter/scripts/plot_accel_brake_map.py new file mode 100755 index 0000000000000..62979fce89bcf --- /dev/null +++ b/vehicle/raw_vehicle_cmd_converter/scripts/plot_accel_brake_map.py @@ -0,0 +1,99 @@ +#!/usr/bin/env python3 + +# Copyright 2021 Tier IV, Inc. All rights reserved. +# +# 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. + +import argparse + +from ament_index_python.packages import get_package_share_directory +import matplotlib.pyplot as plt +import numpy as np + + +def main(dimension, map_dir, accel_or_brake): + if map_dir is None: + script_dir = get_package_share_directory("raw_vehicle_cmd_converter") + csv_dir = script_dir + "/data/default" + else: + csv_dir = map_dir + + vel_list = [] + stroke_list = [] + acc_list = [] + try: + with open(csv_dir + "/{}_map.csv".format(accel_or_brake)) as f: + for l_idx, l in enumerate(f.readlines()): + w = l.split(",") + w[-1] = w[-1][:-1] + if l_idx == 0: + vel_list = [float(e) * 3600 / 1000 for e in w[1:]] + else: + stroke_list.append(float(w[0])) + acc_list.append([float(e) for e in w[1:]]) + + plt.rcParams["font.size"] = 12 + if plot_dimension == 2: + plt.title("{} map".format(accel_or_brake)) + plt.xlabel("stroke") + plt.ylabel("acceleration [m/s^2]") + + for vel_idx, vel in enumerate(vel_list): + plt.plot( + stroke_list, np.array(acc_list).T[vel_idx], label="vel={}[km/h]".format(vel) + ) + plt.legend() + else: + fig = plt.figure() + ax = fig.add_subplot(111, projection="3d") + ax.set_title("{} map".format(accel_or_brake)) + ax.set_xlabel("stroke") + ax.set_ylabel("vel [km/h]") + ax.set_zlabel("acceleration [m/s^2]") + + surf = ax.plot_surface( + np.array([stroke_list for e in vel_list]).T, + [vel_list for e in stroke_list], + np.array(acc_list), + cmap="bwr", + ) + fig.colorbar(surf, shrink=0.5, aspect=10) + + plt.show() + + except Exception: # noqa: B902 + print("No map file found in {}".format(map_dir)) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument( + "-p", "--plot-dimension", default=3, type=int, help="dimension of plotting map" + ) + parser.add_argument("-a", "--accel", action="store_true", help="show accel map") + parser.add_argument("-b", "--brake", action="store_true", help="show brake map") + parser.add_argument("-d", "--dir", default=None, help="map directory") + args = parser.parse_args() + + plot_dimension = args.plot_dimension + accel = args.accel + brake = args.brake + map_dir = args.dir + + if accel: + main(plot_dimension, map_dir, "accel") + elif brake: + main(plot_dimension, map_dir, "brake") + else: + print('Please add argument of "-a" or "-b"') + parser.print_help() diff --git a/vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp b/vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp new file mode 100644 index 0000000000000..7ee2955bc4ad2 --- /dev/null +++ b/vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp @@ -0,0 +1,148 @@ +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 "raw_vehicle_cmd_converter/accel_map.hpp" + +#include +#include +#include +#include + +using namespace std::literals::chrono_literals; + +namespace raw_vehicle_cmd_converter +{ +bool AccelMap::readAccelMapFromCSV(std::string csv_path) +{ + CSVLoader csv(csv_path); + std::vector> table; + + if (!csv.readCSV(table)) { + RCLCPP_ERROR(logger_, "Cannot open %s", csv_path.c_str()); + return false; + } + + if (table[0].size() < 2) { + RCLCPP_ERROR( + logger_, "Cannot read %s. CSV file should have at least 2 column", csv_path.c_str()); + return false; + } + vehicle_name_ = table[0][0]; + for (unsigned int i = 1; i < table[0].size(); i++) { + vel_index_.push_back(std::stod(table[0][i])); + } + + for (unsigned int i = 1; i < table.size(); i++) { + if (table[0].size() != table[i].size()) { + RCLCPP_ERROR( + logger_, "Cannot read %s. Each row should have a same number of columns", csv_path.c_str()); + return false; + } + throttle_index_.push_back(std::stod(table[i][0])); + std::vector accs; + for (unsigned int j = 1; j < table[i].size(); j++) { + accs.push_back(std::stod(table[i][j])); + } + accel_map_.push_back(accs); + } + + return true; +} + +bool AccelMap::getThrottle(double acc, double vel, double & throttle) +{ + LinearInterpolate linear_interp; + std::vector accs_interpolated; + + if (vel < vel_index_.front()) { + RCLCPP_WARN_SKIPFIRST_THROTTLE( + logger_, clock_, 1000, + "Exceeding the vel range. Current vel: %f < min vel on map: %f. Use min " + "velocity.", + vel, vel_index_.front()); + vel = vel_index_.front(); + } else if (vel_index_.back() < vel) { + RCLCPP_WARN_SKIPFIRST_THROTTLE( + logger_, clock_, 1000, + "Exceeding the vel range. Current vel: %f > max vel on map: %f. Use max " + "velocity.", + vel, vel_index_.back()); + vel = vel_index_.back(); + } + + // (throttle, vel, acc) map => (throttle, acc) map by fixing vel + for (std::vector accs : accel_map_) { + double acc_interpolated; + linear_interp.interpolate(vel_index_, accs, vel, acc_interpolated); + accs_interpolated.push_back(acc_interpolated); + } + + // calculate throttle + // When the desired acceleration is smaller than the throttle area, return false => brake sequence + // When the desired acceleration is greater than the throttle area, return max throttle + if (acc < accs_interpolated.front()) { + return false; + } else if (accs_interpolated.back() < acc) { + throttle = throttle_index_.back(); + return true; + } + linear_interp.interpolate(accs_interpolated, throttle_index_, acc, throttle); + + return true; +} + +bool AccelMap::getAcceleration(double throttle, double vel, double & acc) +{ + LinearInterpolate linear_interp; + std::vector accs_interpolated; + + if (vel < vel_index_.front()) { + RCLCPP_WARN_SKIPFIRST_THROTTLE( + logger_, clock_, 1000, + "Exceeding the vel range. Current vel: %f < min vel on map: %f. Use min " + "velocity.", + vel, vel_index_.front()); + vel = vel_index_.front(); + } else if (vel_index_.back() < vel) { + RCLCPP_WARN_SKIPFIRST_THROTTLE( + logger_, clock_, 1000, + "Exceeding the vel range. Current vel: %f > max vel on map: %f. Use max " + "velocity.", + vel, vel_index_.back()); + vel = vel_index_.back(); + } + + // (throttle, vel, acc) map => (throttle, acc) map by fixing vel + for (std::vector accs : accel_map_) { + double acc_interpolated; + linear_interp.interpolate(vel_index_, accs, vel, acc_interpolated); + accs_interpolated.push_back(acc_interpolated); + } + + // calculate throttle + // When the desired acceleration is smaller than the throttle area, return false => brake sequence + // When the desired acceleration is greater than the throttle area, return max throttle + const double max_throttle = throttle_index_.back(); + const double min_throttle = throttle_index_.front(); + if (throttle < min_throttle || max_throttle < throttle) { + RCLCPP_WARN_SKIPFIRST_THROTTLE( + logger_, clock_, 1000, "Input throttle: %f is out off range. use closest value.", throttle); + throttle = std::min(std::max(throttle, min_throttle), max_throttle); + } + + linear_interp.interpolate(throttle_index_, accs_interpolated, throttle, acc); + + return true; +} +} // namespace raw_vehicle_cmd_converter diff --git a/vehicle/raw_vehicle_cmd_converter/src/brake_map.cpp b/vehicle/raw_vehicle_cmd_converter/src/brake_map.cpp new file mode 100644 index 0000000000000..bae57dfcbd50f --- /dev/null +++ b/vehicle/raw_vehicle_cmd_converter/src/brake_map.cpp @@ -0,0 +1,157 @@ +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 "raw_vehicle_cmd_converter/brake_map.hpp" + +#include +#include +#include + +namespace raw_vehicle_cmd_converter +{ +bool BrakeMap::readBrakeMapFromCSV(std::string csv_path) +{ + CSVLoader csv(csv_path); + std::vector> table; + + if (!csv.readCSV(table)) { + RCLCPP_ERROR(logger_, "Cannot open %s", csv_path.c_str()); + return false; + } + + if (table[0].size() < 2) { + RCLCPP_ERROR( + logger_, "Cannot read %s. CSV file should have at least 2 column", csv_path.c_str()); + return false; + } + + vehicle_name_ = table[0][0]; + for (unsigned int i = 1; i < table[0].size(); i++) { + vel_index_.push_back(std::stod(table[0][i])); + } + + for (unsigned int i = 1; i < table.size(); i++) { + if (table[0].size() != table[i].size()) { + RCLCPP_ERROR( + logger_, "Cannot read %s. Each row should have a same number of columns", csv_path.c_str()); + return false; + } + brake_index_.push_back(std::stod(table[i][0])); + std::vector accs; + for (unsigned int j = 1; j < table[i].size(); j++) { + accs.push_back(std::stod(table[i][j])); + } + brake_map_.push_back(accs); + } + + brake_index_rev_ = brake_index_; + std::reverse(std::begin(brake_index_rev_), std::end(brake_index_rev_)); + + return true; +} + +bool BrakeMap::getBrake(double acc, double vel, double & brake) +{ + LinearInterpolate linear_interp; + std::vector accs_interpolated; + + if (vel < vel_index_.front()) { + RCLCPP_WARN_SKIPFIRST_THROTTLE( + logger_, clock_, 1000, + "Exceeding the vel range. Current vel: %f < min vel on map: %f. Use min " + "velocity.", + vel, vel_index_.front()); + vel = vel_index_.front(); + } else if (vel_index_.back() < vel) { + RCLCPP_WARN_SKIPFIRST_THROTTLE( + logger_, clock_, 1000, + "Exceeding the vel range. Current vel: %f > max vel on map: %f. Use max " + "velocity.", + vel, vel_index_.back()); + vel = vel_index_.back(); + } + + // (throttle, vel, acc) map => (throttle, acc) map by fixing vel + for (std::vector accs : brake_map_) { + double acc_interpolated; + linear_interp.interpolate(vel_index_, accs, vel, acc_interpolated); + accs_interpolated.push_back(acc_interpolated); + } + + // calculate brake + // When the desired acceleration is smaller than the brake area, return max brake on the map + // When the desired acceleration is greater than the brake area, return min brake on the map + if (acc < accs_interpolated.back()) { + RCLCPP_WARN_SKIPFIRST_THROTTLE( + logger_, clock_, 1000, + "Exceeding the acc range. Desired acc: %f < min acc on map: %f. return max " + "value.", + acc, accs_interpolated.back()); + brake = brake_index_.back(); + return true; + } else if (accs_interpolated.front() < acc) { + brake = brake_index_.front(); + return true; + } + + std::reverse(std::begin(accs_interpolated), std::end(accs_interpolated)); + linear_interp.interpolate(accs_interpolated, brake_index_rev_, acc, brake); + + return true; +} + +bool BrakeMap::getAcceleration(double brake, double vel, double & acc) +{ + LinearInterpolate linear_interp; + std::vector accs_interpolated; + + if (vel < vel_index_.front()) { + RCLCPP_WARN_SKIPFIRST_THROTTLE( + logger_, clock_, 1000, + "Exceeding the vel range. Current vel: %f < min vel on map: %f. Use min " + "velocity.", + vel, vel_index_.front()); + vel = vel_index_.front(); + } else if (vel_index_.back() < vel) { + RCLCPP_WARN_SKIPFIRST_THROTTLE( + logger_, clock_, 1000, + "Exceeding the vel range. Current vel: %f > max vel on map: %f. Use max " + "velocity.", + vel, vel_index_.back()); + vel = vel_index_.back(); + } + + // (throttle, vel, acc) map => (throttle, acc) map by fixing vel + for (std::vector accs : brake_map_) { + double acc_interpolated; + linear_interp.interpolate(vel_index_, accs, vel, acc_interpolated); + accs_interpolated.push_back(acc_interpolated); + } + + // calculate brake + // When the desired acceleration is smaller than the brake area, return max brake on the map + // When the desired acceleration is greater than the brake area, return min brake on the map + const double max_brake = brake_index_.back(); + const double min_brake = brake_index_.front(); + if (brake < min_brake || max_brake < brake) { + RCLCPP_WARN_SKIPFIRST_THROTTLE( + logger_, clock_, 1000, "Input brake: %f is out off range. use closest value.", brake); + brake = std::min(std::max(brake, min_brake), max_brake); + } + + linear_interp.interpolate(brake_index_, accs_interpolated, brake, acc); + + return true; +} +} // namespace raw_vehicle_cmd_converter diff --git a/vehicle/raw_vehicle_cmd_converter/src/csv_loader.cpp b/vehicle/raw_vehicle_cmd_converter/src/csv_loader.cpp new file mode 100644 index 0000000000000..42d2ac1dea4dc --- /dev/null +++ b/vehicle/raw_vehicle_cmd_converter/src/csv_loader.cpp @@ -0,0 +1,48 @@ +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 "raw_vehicle_cmd_converter/csv_loader.hpp" + +#include +#include + +namespace raw_vehicle_cmd_converter +{ +CSVLoader::CSVLoader(std::string csv_path) { csv_path_ = csv_path; } + +bool CSVLoader::readCSV(std::vector> & result, const char delim) +{ + std::ifstream ifs(csv_path_); + if (!ifs.is_open()) { + return false; + } + + std::string buf; + while (std::getline(ifs, buf)) { + std::vector tokens; + + std::istringstream iss(buf); + std::string token; + while (std::getline(iss, token, delim)) { + tokens.push_back(token); + } + + if (tokens.size() != 0) { + result.push_back(tokens); + } + } + + return true; +} +} // namespace raw_vehicle_cmd_converter diff --git a/vehicle/raw_vehicle_cmd_converter/src/interpolate.cpp b/vehicle/raw_vehicle_cmd_converter/src/interpolate.cpp new file mode 100644 index 0000000000000..a9dc90c2a0179 --- /dev/null +++ b/vehicle/raw_vehicle_cmd_converter/src/interpolate.cpp @@ -0,0 +1,102 @@ +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 "raw_vehicle_cmd_converter/interpolate.hpp" + +#include + +/* + * linear interpolation + */ + +namespace raw_vehicle_cmd_converter +{ +bool LinearInterpolate::interpolate( + const std::vector & base_index, const std::vector & base_value, + const double & return_index, double & return_value) +{ + auto isIncrease = [](const std::vector & x) { + for (size_t i = 0; i < x.size() - 1; ++i) { + if (x[i] > x[i + 1]) { + return false; + } + } + return true; + }; + + if (base_index.size() == 0 || base_value.size() == 0) { + printf( + "[interpolate] some vector size is zero: base_index.size() = %lu, base_value.size() = %lu", + base_index.size(), base_value.size()); + return false; + } + + // check if inputs are valid + if ( + !isIncrease(base_index) || return_index < base_index.front() || + base_index.back() < return_index || base_index.size() != base_value.size()) { + std::cerr << "[isIncrease] bad index, return false" << std::endl; + bool b1 = !isIncrease(base_index); + bool b3 = return_index < base_index.front(); + bool b4 = base_index.back() < return_index; + bool b5 = base_index.size() != base_value.size(); + printf("%d, %d, %d, %d\n", b1, b3, b4, b5); + printf("base_index = [%f, %f]\n", base_index.front(), base_index.back()); + printf( + "base_index.size() = %lu, base_value.size() = %lu\n", base_index.size(), base_value.size()); + printf("base_index: ["); + for (size_t i = 0; i < base_index.size(); ++i) { + printf("%f, ", base_index.at(i)); + } + printf("]\n"); + printf("base_value: ["); + for (size_t i = 0; i < base_value.size(); ++i) { + printf("%f, ", base_value.at(i)); + } + printf("]\n"); + printf("return_index = %f\n", return_index); + return false; + } + + // calculate linear interpolation + size_t i = 0; + if (base_index[i] == return_index) { + return_value = base_value[i]; + return true; + } + while (base_index[i] < return_index) { + ++i; + } + if (i <= 0 || base_index.size() - 1 < i) { + std::cerr << "? something wrong. skip this return_index." << std::endl; + return false; + } + + const double dist_base_return_index = base_index[i] - base_index[i - 1]; + const double dist_to_forward = base_index[i] - return_index; + const double dist_to_backward = return_index - base_index[i - 1]; + if (dist_to_forward < 0.0 || dist_to_backward < 0.0) { + std::cerr << "?? something wrong. skip this return_index. base_index[i - 1] = " + << base_index[i - 1] << ", return_index = " << return_index + << ", base_index[i] = " << base_index[i] << std::endl; + return false; + } + + const double value = (dist_to_backward * base_value[i] + dist_to_forward * base_value[i - 1]) / + dist_base_return_index; + return_value = value; + + return true; +} +} // namespace raw_vehicle_cmd_converter diff --git a/vehicle/raw_vehicle_cmd_converter/src/node.cpp b/vehicle/raw_vehicle_cmd_converter/src/node.cpp new file mode 100644 index 0000000000000..aefefd5b0ba0a --- /dev/null +++ b/vehicle/raw_vehicle_cmd_converter/src/node.cpp @@ -0,0 +1,235 @@ +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 "raw_vehicle_cmd_converter/node.hpp" + +#include +#include +#include + +namespace raw_vehicle_cmd_converter +{ +RawVehicleCommandConverterNode::RawVehicleCommandConverterNode( + const rclcpp::NodeOptions & node_options) +: Node("raw_vehicle_cmd_converter_node", node_options) +{ + using std::placeholders::_1; + /* parameters for accel/brake map */ + const auto csv_path_accel_map = declare_parameter("csv_path_accel_map", std::string("empty")); + const auto csv_path_brake_map = declare_parameter("csv_path_brake_map", std::string("empty")); + const auto csv_path_steer_map = declare_parameter("csv_path_steer_map", std::string("empty")); + convert_accel_cmd_ = declare_parameter("convert_accel_cmd", true); + convert_brake_cmd_ = declare_parameter("convert_brake_cmd", true); + convert_steer_cmd_ = declare_parameter("convert_steer_cmd", true); + max_accel_cmd_ = declare_parameter("max_throttle", 0.2); + max_brake_cmd_ = declare_parameter("max_brake", 0.8); + max_steer_cmd_ = declare_parameter("max_steer", 10.0); + min_steer_cmd_ = declare_parameter("min_steer", -10.0); + is_debugging_ = declare_parameter("is_debugging", false); + // for steering steer controller + use_steer_ff_ = declare_parameter("use_steer_ff", true); + use_steer_fb_ = declare_parameter("use_steer_fb", true); + const auto kp_steer{declare_parameter("steer_pid.kp", 150.0)}; + const auto ki_steer{declare_parameter("steer_pid.ki", 15.0)}; + const auto kd_steer{declare_parameter("steer_pid.kd", 0.0)}; + const auto max_ret_steer{declare_parameter("steer_pid.max", 8.0)}; + const auto min_ret_steer{declare_parameter("steer_pid.min", -8.0)}; + const auto max_ret_p_steer{declare_parameter("steer_pid.max_p", 8.0)}; + const auto min_ret_p_steer{declare_parameter("steer_pid.min_p", -8.0)}; + const auto max_ret_i_steer{declare_parameter("steer_pid.max_i", 8.0)}; + const auto min_ret_i_steer{declare_parameter("steer_pid.min_i", -8.0)}; + const auto max_ret_d_steer{declare_parameter("steer_pid.max_d", 0.0)}; + const auto min_ret_d_steer{declare_parameter("steer_pid.min_d", 0.0)}; + const auto invalid_integration_decay{ + declare_parameter("steer_pid.invalid_integration_decay", 0.97)}; + ff_map_initialized_ = true; + if (convert_accel_cmd_) { + if (!accel_map_.readAccelMapFromCSV(csv_path_accel_map)) { + RCLCPP_ERROR( + get_logger(), "Cannot read accelmap. csv path = %s. stop calculation.", + csv_path_accel_map.c_str()); + ff_map_initialized_ = false; + } + } + if (convert_brake_cmd_) { + if (!brake_map_.readBrakeMapFromCSV(csv_path_brake_map)) { + RCLCPP_ERROR( + get_logger(), "Cannot read brakemap. csv path = %s. stop calculation.", + csv_path_brake_map.c_str()); + ff_map_initialized_ = false; + } + } + if (convert_steer_cmd_) { + steer_controller_.setDecay(invalid_integration_decay); + if (!steer_controller_.setFFMap(csv_path_steer_map)) { + RCLCPP_ERROR( + get_logger(), "Cannot read steer map. csv path = %s. stop calculation.", + csv_path_steer_map.c_str()); + ff_map_initialized_ = false; + } + steer_controller_.setFBGains(kp_steer, ki_steer, kd_steer); + steer_controller_.setFBLimits( + max_ret_steer, min_ret_steer, max_ret_p_steer, min_ret_p_steer, max_ret_i_steer, + min_ret_i_steer, max_ret_d_steer, min_ret_d_steer); + } + pub_actuation_cmd_ = create_publisher("~/output/actuation_cmd", 1); + sub_control_cmd_ = create_subscription( + "~/input/control_cmd", 1, std::bind(&RawVehicleCommandConverterNode::onControlCmd, this, _1)); + sub_velocity_ = create_subscription( + "~/input/odometry", 1, std::bind(&RawVehicleCommandConverterNode::onVelocity, this, _1)); + sub_steering_ = create_subscription( + "~/input/steering", 1, std::bind(&RawVehicleCommandConverterNode::onSteering, this, _1)); + debug_pub_steer_pid_ = create_publisher( + "/vehicle/raw_vehicle_cmd_converter/debug/steer_pid", 1); +} + +void RawVehicleCommandConverterNode::publishActuationCmd() +{ + if (!ff_map_initialized_) { + RCLCPP_WARN_EXPRESSION(get_logger(), is_debugging_, "ff map is not initialized"); + return; + } + if (!current_twist_ptr_ || !control_cmd_ptr_ || !current_steer_ptr_) { + RCLCPP_WARN_EXPRESSION( + get_logger(), is_debugging_, "some of twist/control_cmd/steer pointer is null"); + return; + } + double desired_accel_cmd = 0.0; + double desired_brake_cmd = 0.0; + double desired_steer_cmd = 0.0; + ActuationCommandStamped actuation_cmd; + const double acc = control_cmd_ptr_->longitudinal.acceleration; + const double vel = current_twist_ptr_->twist.linear.x; + const double steer = control_cmd_ptr_->lateral.steering_tire_angle; + const double steer_rate = control_cmd_ptr_->lateral.steering_tire_rotation_rate; + bool accel_cmd_is_zero = true; + if (convert_accel_cmd_) { + desired_accel_cmd = calculateAccelMap(vel, acc, accel_cmd_is_zero); + } else { + // if conversion is disabled use acceleration as actuation cmd + desired_accel_cmd = (acc >= 0) ? acc : 0; + } + if (convert_brake_cmd_) { + if (accel_cmd_is_zero) { + desired_brake_cmd = calculateBrakeMap(vel, acc); + } + } else { + // if conversion is disabled use acceleration as brake cmd + desired_brake_cmd = (acc < 0) ? acc : 0; + } + if (convert_steer_cmd_) { + desired_steer_cmd = calculateSteer(vel, steer, steer_rate); + } else { + // if conversion is disabled use steering angle as steer cmd + desired_steer_cmd = steer; + } + actuation_cmd.header.frame_id = "base_link"; + actuation_cmd.header.stamp = control_cmd_ptr_->stamp; + actuation_cmd.actuation.accel_cmd = desired_accel_cmd; + actuation_cmd.actuation.brake_cmd = desired_brake_cmd; + actuation_cmd.actuation.steer_cmd = desired_steer_cmd; + pub_actuation_cmd_->publish(actuation_cmd); +} + +double RawVehicleCommandConverterNode::calculateSteer( + const double vel, const double steering, const double steer_rate) +{ + double steering_output = 0; + double ff_value = 0; + double fb_value = 0; + std::vector pid_contributions(3, 0.0); + std::vector pid_errors(3, 0.0); + rclcpp::Time current_time = this->now(); + double dt = (current_time - prev_time_steer_calculation_).seconds(); + if (std::abs(dt) > 1.0) { + RCLCPP_WARN_EXPRESSION(get_logger(), is_debugging_, "ignore old topic"); + dt = 0.0; + } + prev_time_steer_calculation_ = current_time; + // feed-forward + if (use_steer_ff_) { + ff_value = steer_controller_.calcFFSteer(steer_rate, *current_steer_ptr_); + } + // feedback + if (use_steer_fb_) { + fb_value = steer_controller_.calcFBSteer( + steering, dt, vel, *current_steer_ptr_, pid_contributions, pid_errors); + } + steering_output = ff_value + fb_value; + // for steer debugging + { + debug_steer_.setValues(DebugValues::TYPE::CURR_TIME, current_time.seconds()); + debug_steer_.setValues(DebugValues::TYPE::P, pid_contributions.at(0)); + debug_steer_.setValues(DebugValues::TYPE::I, pid_contributions.at(1)); + debug_steer_.setValues(DebugValues::TYPE::D, pid_contributions.at(2)); + debug_steer_.setValues(DebugValues::TYPE::FF, ff_value); + debug_steer_.setValues(DebugValues::TYPE::FB, fb_value); + debug_steer_.setValues(DebugValues::TYPE::STEER, steering_output); + debug_steer_.setValues(DebugValues::TYPE::ERROR_P, pid_errors.at(0)); + debug_steer_.setValues(DebugValues::TYPE::ERROR_I, pid_errors.at(1)); + debug_steer_.setValues(DebugValues::TYPE::ERROR_D, pid_errors.at(2)); + autoware_debug_msgs::msg::Float32MultiArrayStamped msg{}; + for (const auto & v : debug_steer_.getValues()) { + msg.data.push_back(v); + } + msg.stamp = this->now(); + debug_pub_steer_pid_->publish(msg); + } + steering_output = std::max(std::min(max_steer_cmd_, steering_output), min_steer_cmd_); + return steering_output; +} + +double RawVehicleCommandConverterNode::calculateAccelMap( + const double current_velocity, const double desired_acc, bool & accel_cmd_is_zero) +{ + double desired_accel_cmd = 0; + if (!accel_map_.getThrottle(desired_acc, std::abs(current_velocity), desired_accel_cmd)) { + desired_accel_cmd = 0; + } else { + accel_cmd_is_zero = false; + } + desired_accel_cmd = std::min(std::max(desired_accel_cmd, 0.0), max_accel_cmd_); + return desired_accel_cmd; +} + +double RawVehicleCommandConverterNode::calculateBrakeMap( + const double current_velocity, const double desired_acc) +{ + double desired_brake_cmd = 0; + brake_map_.getBrake(desired_acc, std::abs(current_velocity), desired_brake_cmd); + desired_brake_cmd = std::min(std::max(desired_brake_cmd, 0.0), max_brake_cmd_); + return desired_brake_cmd; +} + +void RawVehicleCommandConverterNode::onSteering(const Steering::ConstSharedPtr msg) +{ + current_steer_ptr_ = std::make_unique(msg->steering_tire_angle); +} + +void RawVehicleCommandConverterNode::onVelocity(const Odometry::ConstSharedPtr msg) +{ + current_twist_ptr_ = std::make_unique(); + current_twist_ptr_->header = msg->header; + current_twist_ptr_->twist = msg->twist.twist; +} + +void RawVehicleCommandConverterNode::onControlCmd(const AckermannControlCommand::ConstSharedPtr msg) +{ + control_cmd_ptr_ = msg; + publishActuationCmd(); +} +} // namespace raw_vehicle_cmd_converter + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(raw_vehicle_cmd_converter::RawVehicleCommandConverterNode) diff --git a/vehicle/raw_vehicle_cmd_converter/src/pid.cpp b/vehicle/raw_vehicle_cmd_converter/src/pid.cpp new file mode 100644 index 0000000000000..e2a2054fe6944 --- /dev/null +++ b/vehicle/raw_vehicle_cmd_converter/src/pid.cpp @@ -0,0 +1,93 @@ +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 "raw_vehicle_cmd_converter/pid.hpp" + +#include +#include + +namespace raw_vehicle_cmd_converter +{ +double PIDController::calculatePID( + const double error, const double dt, const bool enable_integration, + std::vector & pid_contributions, std::vector & errors, bool is_debugging) +{ + double ret_p = kp_ * error; + ret_p = std::min(std::max(ret_p, min_ret_p_), max_ret_p_); + + if (enable_integration) { + error_integral_ += error * dt; + error_integral_ = std::min(std::max(error_integral_, min_ret_i_ / ki_), max_ret_i_ / ki_); + if (is_debugging) { + std::cout << "error: " << error << ", dt: " << dt << ", integ_error: " << error_integral_ + << std::endl; + } + } else { + error_integral_ *= invalid_integration_decay_; + } + double ret_i = ki_ * error_integral_; + + double error_differential; + if (is_first_time_) { + error_differential = 0; + is_first_time_ = false; + } else { + error_differential = (error - prev_error_) / dt; + } + double ret_d = kd_ * error_differential; + ret_d = std::min(std::max(ret_d, min_ret_d_), max_ret_d_); + + prev_error_ = error; + + pid_contributions.at(0) = ret_p; + pid_contributions.at(1) = ret_i; + pid_contributions.at(2) = ret_d; + errors.at(0) = error; + errors.at(1) = error_integral_; + errors.at(2) = error_differential; + + double ret = ret_p + ret_i + ret_d; + ret = std::min(std::max(ret, min_ret_), max_ret_); + + return ret; +} + +void PIDController::setGains(const double kp, const double ki, const double kd) +{ + kp_ = kp; + ki_ = ki; + kd_ = kd; +} + +void PIDController::setLimits( + const double max_ret, const double min_ret, const double max_ret_p, const double min_ret_p, + const double max_ret_i, const double min_ret_i, const double max_ret_d, const double min_ret_d) +{ + max_ret_ = max_ret; + min_ret_ = min_ret; + max_ret_p_ = max_ret_p; + min_ret_p_ = min_ret_p; + max_ret_d_ = max_ret_d; + min_ret_d_ = min_ret_d; + max_ret_i_ = max_ret_i; + min_ret_i_ = min_ret_i; +} + +void PIDController::reset() +{ + error_integral_ = 0; + prev_error_ = 0; + is_first_time_ = true; +} +} // namespace raw_vehicle_cmd_converter diff --git a/vehicle/raw_vehicle_cmd_converter/src/steer_converter.cpp b/vehicle/raw_vehicle_cmd_converter/src/steer_converter.cpp new file mode 100644 index 0000000000000..0300ac61b19c9 --- /dev/null +++ b/vehicle/raw_vehicle_cmd_converter/src/steer_converter.cpp @@ -0,0 +1,169 @@ +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// 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 "raw_vehicle_cmd_converter/steer_converter.hpp" + +#include +#include + +namespace raw_vehicle_cmd_converter +{ +bool SteerConverter::setFFMap(const std::string & csv_path) +{ + if (!readSteerMapFromCSV(csv_path, vehicle_name_, vel_index_, output_index_, steer_map_)) { + return false; + } + ff_map_initialized_ = true; + return true; +} + +void SteerConverter::setFBGains(const double kp, const double ki, const double kd) +{ + pid_.setGains(kp, ki, kd); + fb_gains_initialized_ = true; +} + +bool SteerConverter::setFBLimits( + const double max_ret, const double min_ret, const double max_ret_p, const double min_ret_p, + const double max_ret_i, const double min_ret_i, const double max_ret_d, const double min_ret_d) +{ + pid_.setLimits( + max_ret, min_ret, max_ret_p, min_ret_p, max_ret_i, min_ret_i, max_ret_d, min_ret_d); + fb_limits_initialized_ = true; + return true; +} + +double SteerConverter::calcFFSteer( + const double target_steer_angle_velocity, const double current_steer_angle) const +{ + rclcpp::Clock clock{RCL_ROS_TIME}; + + if (!ff_map_initialized_) { + RCLCPP_WARN_THROTTLE(logger_, clock, 3000, "FF map is not initialized!"); + return 0; + } + + double ff_value = 0; + calcFFMap(target_steer_angle_velocity, current_steer_angle, ff_value); + return ff_value; +} + +double SteerConverter::calcFBSteer( + const double target_steer_angle, const double dt, const double current_velocity, + const double current_steer_angle, std::vector & pid_contributions, + std::vector & errors) +{ + rclcpp::Clock clock{RCL_ROS_TIME}; + + if (!fb_gains_initialized_ || !fb_limits_initialized_) { + RCLCPP_WARN_THROTTLE(logger_, clock, 3000, "FB params are not initialized!"); + return 0; + } + + double fb_value = 0; + const double error_steer_angle = target_steer_angle - current_steer_angle; + bool enable_integration = true; + if (std::abs(current_velocity) < 0.01) { + enable_integration = false; + } + fb_value = + pid_.calculatePID(error_steer_angle, dt, enable_integration, pid_contributions, errors, false); + return fb_value; +} + +bool SteerConverter::readSteerMapFromCSV( + const std::string & csv_path, std::string & vehicle_name, std::vector & vel_index, + std::vector & output_index, std::vector> & steer_map) const +{ + rclcpp::Clock clock{RCL_ROS_TIME}; + + CSVLoader csv(csv_path); + std::vector> table; + + if (!csv.readCSV(table)) { + RCLCPP_ERROR_THROTTLE(logger_, clock, 3000, "Cannot open %s", csv_path.c_str()); + return false; + } + + if (table[0].size() < 2) { + RCLCPP_ERROR_THROTTLE( + logger_, clock, 3000, + "Cannot read %s. CSV file should have " + "at least 2 column", + csv_path.c_str()); + return false; + } + + vehicle_name = table[0][0]; + for (unsigned int i = 1; i < table[0].size(); ++i) { + vel_index.push_back(std::stod(table[0][i])); + } + + for (unsigned int i = 1; i < table.size(); ++i) { + if (table[0].size() != table[i].size()) { + RCLCPP_ERROR_THROTTLE( + logger_, clock, 3000, + "Cannot read %s. Each row should have " + "a same number of columns", + csv_path.c_str()); + return false; + } + output_index.push_back(std::stod(table[i][0])); + std::vector steer_angle_velocities; + for (unsigned int j = 1; j < table[i].size(); ++j) { + steer_angle_velocities.push_back(std::stod(table[i][j])); + } + steer_map.push_back(steer_angle_velocities); + } + + return true; +} + +void SteerConverter::calcFFMap(double steer_vel, double vehicle_vel, double & output) const +{ + rclcpp::Clock clock{RCL_ROS_TIME}; + + LinearInterpolate linear_interp; + std::vector steer_angle_velocities_interp; + + if (vehicle_vel < vel_index_.front()) { + RCLCPP_WARN_THROTTLE( + logger_, clock, 1000, + "Exceeding the vel range. Current vel: " + "%f < min vel on map: %f. Use min velocity.", + vehicle_vel, vel_index_.front()); + vehicle_vel = vel_index_.front(); + } else if (vel_index_.back() < vehicle_vel) { + RCLCPP_WARN_THROTTLE( + logger_, clock, 1000, + "Exceeding the vel range. Current vel: " + "%f > max vel on map: %f. Use max velocity.", + vehicle_vel, vel_index_.back()); + vehicle_vel = vel_index_.back(); + } + + for (std::vector steer_angle_velocities : steer_map_) { + double steer_angle_vel_interp; + linear_interp.interpolate( + vel_index_, steer_angle_velocities, vehicle_vel, steer_angle_vel_interp); + steer_angle_velocities_interp.push_back(steer_angle_vel_interp); + } + if (steer_vel < steer_angle_velocities_interp.front()) { + steer_vel = steer_angle_velocities_interp.front(); + } else if (steer_angle_velocities_interp.back() < steer_vel) { + steer_vel = steer_angle_velocities_interp.back(); + } + linear_interp.interpolate(steer_angle_velocities_interp, output_index_, steer_vel, output); +} +} // namespace raw_vehicle_cmd_converter