From 24fa52aa2fc120818bca1be0f76f7386b38cd1f0 Mon Sep 17 00:00:00 2001 From: taikitanaka3 Date: Sat, 15 Oct 2022 18:44:13 +0900 Subject: [PATCH 01/15] refactor: error message --- .../raw_vehicle_cmd_converter/csv_loader.hpp | 4 + .../steer_converter.hpp | 24 ++---- .../src/accel_map.cpp | 39 ++-------- .../src/brake_map.cpp | 39 ++-------- .../src/csv_loader.cpp | 27 +++++++ .../raw_vehicle_cmd_converter/src/node.cpp | 33 ++++---- .../src/steer_converter.cpp | 75 ++----------------- .../test/test_raw_vehicle_cmd_converter.cpp | 44 ++++++++++- 8 files changed, 114 insertions(+), 171 deletions(-) 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 index ff605ffa00279..6d2a144fb87d1 100644 --- 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 @@ -24,6 +24,7 @@ namespace raw_vehicle_cmd_converter { using Table = std::vector>; +using Map = std::vector>; class CSVLoader { public: @@ -31,8 +32,11 @@ class CSVLoader bool readCSV(Table & result, const char delim = ','); static bool validateData(const Table & table, const std::string & csv_path); + static Map getMap(const Table & table); static std::vector getRowIndex(const Table & table); static std::vector getColumnIndex(const Table & table); + static double clampValue( + const double val, const std::vector ranges, const std::string & name); private: std::string csv_path_; 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 index 3574850ac712d..bb7d5e6102eea 100644 --- 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 @@ -28,37 +28,23 @@ 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; + bool readSteerMapFromCSV(const std::string & csv_path); 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); } + void getSteer(double steer_vel, double steer, double & output); + bool fb_gains_initialized_{false}; + bool fb_limits_initialized_{false}; + PIDController pid_; 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 diff --git a/vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp b/vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp index 74c7f52e22085..2f6ce02631b72 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp @@ -37,28 +37,14 @@ bool AccelMap::readAccelMapFromCSV(std::string csv_path) vehicle_name_ = table[0][0]; vel_index_ = CSVLoader::getRowIndex(table); throttle_index_ = CSVLoader::getColumnIndex(table); - - for (unsigned int i = 1; i < table.size(); i++) { - 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); - } - + accel_map_ = CSVLoader::getMap(table); return true; } bool AccelMap::getThrottle(double acc, double vel, double & throttle) { std::vector accs_interpolated; - - if (vel < vel_index_.front() || vel_index_.back() < vel) { - RCLCPP_WARN_SKIPFIRST_THROTTLE( - logger_, clock_, 1000, "Exceeding the min:%f < current vel:%f < max:%f.", - vel_index_.front(), vel, vel_index_.back()); - vel = std::min(std::max(vel, vel_index_.front()), vel_index_.back()); - } + vel = CSVLoader::clampValue(vel, vel_index_, "throttle: vel"); // (throttle, vel, acc) map => (throttle, acc) map by fixing vel for (std::vector accs : accel_map_) { @@ -82,13 +68,7 @@ bool AccelMap::getThrottle(double acc, double vel, double & throttle) bool AccelMap::getAcceleration(double throttle, double vel, double & acc) { std::vector accs_interpolated; - - if (vel < vel_index_.front() || vel_index_.back() < vel) { - RCLCPP_WARN_SKIPFIRST_THROTTLE( - logger_, clock_, 1000, "Exceeding the min:%f < current vel:%f < max:%f.", - vel_index_.front(), vel, vel_index_.back()); - vel = std::min(std::max(vel, vel_index_.front()), vel_index_.back()); - } + vel = CSVLoader::clampValue(vel, vel_index_, "throttle: vel"); // (throttle, vel, acc) map => (throttle, acc) map by fixing vel for (std::vector accs : accel_map_) { @@ -96,16 +76,9 @@ bool AccelMap::getAcceleration(double throttle, double vel, double & acc) } // 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); - } - + // When the desired acceleration is smaller than the throttle area, return min acc + // When the desired acceleration is greater than the throttle area, return max acc + throttle = CSVLoader::clampValue(throttle, throttle_index_, "throttle: acc"); acc = interpolation::lerp(throttle_index_, accs_interpolated, throttle); return true; diff --git a/vehicle/raw_vehicle_cmd_converter/src/brake_map.cpp b/vehicle/raw_vehicle_cmd_converter/src/brake_map.cpp index 5aec005dd1a22..d2978ce872654 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/brake_map.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/brake_map.cpp @@ -34,15 +34,7 @@ bool BrakeMap::readBrakeMapFromCSV(std::string csv_path) vehicle_name_ = table[0][0]; vel_index_ = CSVLoader::getRowIndex(table); brake_index_ = CSVLoader::getColumnIndex(table); - - for (unsigned int i = 1; i < table.size(); i++) { - 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_map_ = CSVLoader::getMap(table); brake_index_rev_ = brake_index_; std::reverse(std::begin(brake_index_rev_), std::end(brake_index_rev_)); @@ -52,13 +44,8 @@ bool BrakeMap::readBrakeMapFromCSV(std::string csv_path) bool BrakeMap::getBrake(double acc, double vel, double & brake) { std::vector accs_interpolated; + vel = CSVLoader::clampValue(vel, vel_index_, "brake: vel"); - if (vel < vel_index_.front() || vel_index_.back() < vel) { - RCLCPP_WARN_SKIPFIRST_THROTTLE( - logger_, clock_, 1000, "Exceeding the min:%f < current vel:%f < max:%f.", - vel_index_.front(), vel, vel_index_.back()); - vel = std::min(std::max(vel, vel_index_.front()), vel_index_.back()); - } // (throttle, vel, acc) map => (throttle, acc) map by fixing vel for (std::vector accs : brake_map_) { accs_interpolated.push_back(interpolation::lerp(vel_index_, accs, vel)); @@ -89,14 +76,7 @@ bool BrakeMap::getBrake(double acc, double vel, double & brake) bool BrakeMap::getAcceleration(double brake, double vel, double & acc) { std::vector accs_interpolated; - - if (vel < vel_index_.front() || vel_index_.back() < vel) { - RCLCPP_WARN_SKIPFIRST_THROTTLE( - logger_, clock_, 1000, - "Exceeding the vel range. Current vel: %f < min or max < %f vel on map: %f.", vel, - vel_index_.front(), vel_index_.back()); - vel = std::min(std::max(vel, vel_index_.front()), vel_index_.back()); - } + vel = CSVLoader::clampValue(vel, vel_index_, "brake: vel"); // (throttle, vel, acc) map => (throttle, acc) map by fixing vel for (std::vector accs : brake_map_) { @@ -104,16 +84,9 @@ bool BrakeMap::getAcceleration(double brake, double vel, double & acc) } // 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); - } - + // When the desired acceleration is smaller than the brake area, return min acc + // When the desired acceleration is greater than the brake area, return min acc + brake = CSVLoader::clampValue(brake, brake_index_, "brake: acc"); acc = interpolation::lerp(brake_index_, accs_interpolated, brake); return true; diff --git a/vehicle/raw_vehicle_cmd_converter/src/csv_loader.cpp b/vehicle/raw_vehicle_cmd_converter/src/csv_loader.cpp index 2b3fb7b744f06..8b7490596f503 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/csv_loader.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/csv_loader.cpp @@ -66,6 +66,19 @@ bool CSVLoader::validateData(const Table & table, const std::string & csv_path) return true; } +Map CSVLoader::getMap(const Table & table) +{ + Map map = {}; + for (unsigned int i = 1; i < table.size(); i++) { + std::vector accs; + for (unsigned int j = 1; j < table[i].size(); j++) { + accs.push_back(std::stod(table[i][j])); + } + map.push_back(accs); + } + return map; +} + std::vector CSVLoader::getRowIndex(const Table & table) { std::vector index = {}; @@ -84,4 +97,18 @@ std::vector CSVLoader::getColumnIndex(const Table & table) return index; } +double CSVLoader::clampValue( + const double val, const std::vector ranges, const std::string & name) +{ + double ret = val; + const double max_value = ranges.back(); + const double min_value = ranges.front(); + if (val < min_value || max_value < val) { + std::cerr << "Input" << name << ": " << val << " is out off range. use closest value." + << std::endl; + ret = std::min(std::max(val, min_value), max_value); + } + return ret; +} + } // 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 index 20799155b86ad..6c778456c4f0d 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/node.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/node.cpp @@ -56,32 +56,25 @@ RawVehicleCommandConverterNode::RawVehicleCommandConverterNode( 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()); + steer_controller_.pid_.setDecay(invalid_integration_decay); + if (!steer_controller_.readSteerMapFromCSV(csv_path_steer_map)) { ff_map_initialized_ = false; } - steer_controller_.setFBGains(kp_steer, ki_steer, kd_steer); - steer_controller_.setFBLimits( + steer_controller_.pid_.setGains(kp_steer, ki_steer, kd_steer); + steer_controller_.fb_gains_initialized_ = true; + steer_controller_.pid_.setLimits( 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); + steer_controller_.fb_limits_initialized_ = true; } pub_actuation_cmd_ = create_publisher("~/output/actuation_cmd", 1); sub_control_cmd_ = create_subscription( @@ -161,12 +154,20 @@ double RawVehicleCommandConverterNode::calculateSteer( prev_time_steer_calculation_ = current_time; // feed-forward if (use_steer_ff_) { - ff_value = steer_controller_.calcFFSteer(steer_rate, *current_steer_ptr_); + if (!ff_map_initialized_) { + RCLCPP_WARN_EXPRESSION(get_logger(), is_debugging_, "FF map is not initialized!"); + } else { + steer_controller_.getSteer(steer_rate, *current_steer_ptr_, ff_value); + } } // feedback if (use_steer_fb_) { - fb_value = steer_controller_.calcFBSteer( - steering, dt, vel, *current_steer_ptr_, pid_contributions, pid_errors); + if (steer_controller_.fb_gains_initialized_ && steer_controller_.fb_limits_initialized_) { + RCLCPP_WARN_EXPRESSION(get_logger(), is_debugging_, "FF map is not initialized!"); + } else { + fb_value = steer_controller_.calcFBSteer( + steering, dt, vel, *current_steer_ptr_, pid_contributions, pid_errors); + } } steering_output = ff_value + fb_value; // for steer debugging diff --git a/vehicle/raw_vehicle_cmd_converter/src/steer_converter.cpp b/vehicle/raw_vehicle_cmd_converter/src/steer_converter.cpp index 453c8555185eb..335f9450b01b0 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/steer_converter.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/steer_converter.cpp @@ -21,45 +21,6 @@ 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, @@ -84,12 +45,8 @@ double SteerConverter::calcFBSteer( 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 +bool SteerConverter::readSteerMapFromCSV(const std::string & csv_path) { - rclcpp::Clock clock{RCL_ROS_TIME}; - CSVLoader csv(csv_path); std::vector> table; @@ -97,37 +54,21 @@ bool SteerConverter::readSteerMapFromCSV( return false; } - vehicle_name = table[0][0]; - vel_index = CSVLoader::getRowIndex(table); - output_index = CSVLoader::getColumnIndex(table); - - for (unsigned int i = 1; i < table.size(); ++i) { - 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); - } - + vehicle_name_ = table[0][0]; + vel_index_ = CSVLoader::getRowIndex(table); + output_index_ = CSVLoader::getColumnIndex(table); + steer_map_ = CSVLoader::getMap(table); return true; } -void SteerConverter::calcFFMap(double steer_vel, double vehicle_vel, double & output) const +void SteerConverter::getSteer(double steer_vel, double steer, double & output) { - rclcpp::Clock clock{RCL_ROS_TIME}; - std::vector steer_angle_velocities_interp; - - if (vehicle_vel < vel_index_.front() || vel_index_.back() < vehicle_vel) { - RCLCPP_WARN_SKIPFIRST_THROTTLE( - logger_, clock, 1000, "Exceeding the min:%f < current vel:%f < max:%f.", vel_index_.front(), - vehicle_vel, vel_index_.back()); - vehicle_vel = std::min(std::max(vehicle_vel, vel_index_.front()), vel_index_.back()); - } + steer = CSVLoader::clampValue(steer, vel_index_, "steer: steer"); for (std::vector steer_angle_velocities : steer_map_) { steer_angle_velocities_interp.push_back( - interpolation::lerp(vel_index_, steer_angle_velocities, vehicle_vel)); + interpolation::lerp(vel_index_, steer_angle_velocities, steer)); } if (steer_vel < steer_angle_velocities_interp.front()) { steer_vel = steer_angle_velocities_interp.front(); diff --git a/vehicle/raw_vehicle_cmd_converter/test/test_raw_vehicle_cmd_converter.cpp b/vehicle/raw_vehicle_cmd_converter/test/test_raw_vehicle_cmd_converter.cpp index 5c59aa4b2f09f..f503dca130763 100644 --- a/vehicle/raw_vehicle_cmd_converter/test/test_raw_vehicle_cmd_converter.cpp +++ b/vehicle/raw_vehicle_cmd_converter/test/test_raw_vehicle_cmd_converter.cpp @@ -66,7 +66,7 @@ bool loadBrakeMapData(BrakeMap & brake_map) bool loadSteerMapData(SteerConverter & steer_map) { - return steer_map.setFFMap(map_path + "test_steer_map.csv"); + return steer_map.readSteerMapFromCSV(map_path + "test_steer_map.csv"); } TEST(ConverterTests, LoadValidPath) @@ -83,7 +83,7 @@ TEST(ConverterTests, LoadValidPath) // for invalid path EXPECT_FALSE(accel_map.readAccelMapFromCSV("invalid.csv")); EXPECT_FALSE(brake_map.readBrakeMapFromCSV("invalid.csv")); - EXPECT_FALSE(steer_map.setFFMap("invalid.csv")); + EXPECT_FALSE(steer_map.readSteerMapFromCSV("invalid.csv")); } TEST(ConverterTests, AccelMapCalculation) @@ -111,6 +111,24 @@ TEST(ConverterTests, AccelMapCalculation) // case for interpolation EXPECT_DOUBLE_EQ(calcThrottle(9.0, 5.0), 0.25); + + const auto calcAcceleration = [&](double throttle, double vel) { + double output; + accel_map.getAcceleration(throttle, vel, output); + return output; + }; + + // case for min vel + EXPECT_DOUBLE_EQ(calcAcceleration(1.0, 0.0), 3.0); + + // case for min throttle + EXPECT_DOUBLE_EQ(calcAcceleration(0.0, 25.0), 21.0); + + // case for direct access + EXPECT_DOUBLE_EQ(calcAcceleration(0.5, 10.0), 22.0); + + // case for interpolation + EXPECT_DOUBLE_EQ(calcAcceleration(0.75, 5.0), 15.0); } TEST(ConverterTests, BrakeMapCalculation) @@ -138,6 +156,24 @@ TEST(ConverterTests, BrakeMapCalculation) // case for interpolation EXPECT_DOUBLE_EQ(calcBrake(-9.0, 5.0), 0.25); + + const auto calcAcceleration = [&](double brake, double vel) { + double output; + brake_map.getAcceleration(brake, vel, output); + return output; + }; + + // case for min vel + EXPECT_DOUBLE_EQ(calcAcceleration(1.0, 0.0), -3.0); + + // case for min throttle + EXPECT_DOUBLE_EQ(calcAcceleration(0.0, 25.0), -21.0); + + // case for direct access + EXPECT_DOUBLE_EQ(calcAcceleration(0.5, 10.0), -22.0); + + // case for interpolation + EXPECT_DOUBLE_EQ(calcAcceleration(0.75, 5.0), -15.0); } TEST(ConverterTests, SteerMapCalculation) @@ -145,7 +181,9 @@ TEST(ConverterTests, SteerMapCalculation) SteerConverter steer_map; loadSteerMapData(steer_map); const auto calcSteer = [&](double steer_vel, double steer) { - return steer_map.calcFFSteer(steer_vel, steer); + double output; + steer_map.getSteer(steer_vel, steer, output); + return output; }; // case for min steer From 8001ef641af310ddbe9f32014e98d511d9842c6c Mon Sep 17 00:00:00 2001 From: taikitanaka3 Date: Sat, 15 Oct 2022 19:44:13 +0900 Subject: [PATCH 02/15] refactor: remove pid depends from steer converter this will make pid implementation easiter to accel/brake Signed-off-by: taikitanaka3 --- .../raw_vehicle_cmd_converter/node.hpp | 4 +- .../include/raw_vehicle_cmd_converter/pid.hpp | 6 +++ .../steer_converter.hpp | 9 +--- .../raw_vehicle_cmd_converter/src/node.cpp | 44 +++++++++---------- vehicle/raw_vehicle_cmd_converter/src/pid.cpp | 15 +++++++ .../src/steer_converter.cpp | 29 ++---------- 6 files changed, 50 insertions(+), 57 deletions(-) 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 index 068c6880b23e8..faa4450afa113 100644 --- 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 @@ -87,6 +87,9 @@ class RawVehicleCommandConverterNode : public rclcpp::Node AckermannControlCommand::ConstSharedPtr control_cmd_ptr_; AccelMap accel_map_; BrakeMap brake_map_; + SteerConverter steer_map_; + // TODO(tanaka): consider accel/brake pid too + PIDController steer_pid_; bool ff_map_initialized_; double max_accel_cmd_; double max_brake_cmd_; @@ -99,7 +102,6 @@ class RawVehicleCommandConverterNode : public rclcpp::Node 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( 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 index a2333e95453fa..df8dc7f678d80 100644 --- 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 @@ -24,6 +24,10 @@ namespace raw_vehicle_cmd_converter class PIDController { public: + double calculateFB( + const double target_value, const double dt, const double reset_trigger_value, + const double current_value, std::vector & pid_contributions, + std::vector & errors); double calculatePID( const double error, const double dt, const bool enable_integration, std::vector & pid_contributions, std::vector & errors, bool is_debugging); @@ -33,6 +37,8 @@ class PIDController 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; } + bool fb_gains_initialized_{false}; + bool fb_limits_initialized_{false}; private: // parameters 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 index bb7d5e6102eea..e63127ed7c40c 100644 --- 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 @@ -29,18 +29,11 @@ class SteerConverter { public: bool readSteerMapFromCSV(const std::string & csv_path); - 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 getSteer(double steer_vel, double steer, double & output); - bool fb_gains_initialized_{false}; - bool fb_limits_initialized_{false}; - PIDController pid_; private: std::string vehicle_name_; - std::vector vel_index_; + std::vector steer_index_; std::vector output_index_; std::vector> steer_map_; rclcpp::Logger logger_{ diff --git a/vehicle/raw_vehicle_cmd_converter/src/node.cpp b/vehicle/raw_vehicle_cmd_converter/src/node.cpp index 6c778456c4f0d..a1baf0452dc7b 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/node.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/node.cpp @@ -40,19 +40,6 @@ RawVehicleCommandConverterNode::RawVehicleCommandConverterNode( // 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)) { @@ -65,16 +52,29 @@ RawVehicleCommandConverterNode::RawVehicleCommandConverterNode( } } if (convert_steer_cmd_) { - steer_controller_.pid_.setDecay(invalid_integration_decay); - if (!steer_controller_.readSteerMapFromCSV(csv_path_steer_map)) { + if (!steer_map_.readSteerMapFromCSV(csv_path_steer_map)) { ff_map_initialized_ = false; } - steer_controller_.pid_.setGains(kp_steer, ki_steer, kd_steer); - steer_controller_.fb_gains_initialized_ = true; - steer_controller_.pid_.setLimits( + 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)}; + steer_pid_.setDecay(invalid_integration_decay); + steer_pid_.setGains(kp_steer, ki_steer, kd_steer); + steer_pid_.fb_gains_initialized_ = true; + steer_pid_.setLimits( 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); - steer_controller_.fb_limits_initialized_ = true; + steer_pid_.fb_limits_initialized_ = true; } pub_actuation_cmd_ = create_publisher("~/output/actuation_cmd", 1); sub_control_cmd_ = create_subscription( @@ -157,15 +157,15 @@ double RawVehicleCommandConverterNode::calculateSteer( if (!ff_map_initialized_) { RCLCPP_WARN_EXPRESSION(get_logger(), is_debugging_, "FF map is not initialized!"); } else { - steer_controller_.getSteer(steer_rate, *current_steer_ptr_, ff_value); + steer_map_.getSteer(steer_rate, *current_steer_ptr_, ff_value); } } // feedback if (use_steer_fb_) { - if (steer_controller_.fb_gains_initialized_ && steer_controller_.fb_limits_initialized_) { + if (steer_pid_.fb_gains_initialized_ && steer_pid_.fb_limits_initialized_) { RCLCPP_WARN_EXPRESSION(get_logger(), is_debugging_, "FF map is not initialized!"); } else { - fb_value = steer_controller_.calcFBSteer( + fb_value = steer_pid_.calculateFB( steering, dt, vel, *current_steer_ptr_, pid_contributions, pid_errors); } } diff --git a/vehicle/raw_vehicle_cmd_converter/src/pid.cpp b/vehicle/raw_vehicle_cmd_converter/src/pid.cpp index e2a2054fe6944..c01df23bd7743 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/pid.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/pid.cpp @@ -19,6 +19,21 @@ namespace raw_vehicle_cmd_converter { + +double PIDController::calculateFB( + const double target_value, const double dt, const double reset_trigger_value, + const double current_value, std::vector & pid_contributions, std::vector & errors) +{ + double fb_value = 0; + const double error = target_value - current_value; + bool enable_integration = true; + if (std::abs(reset_trigger_value) < 0.01) { + enable_integration = false; + } + fb_value = calculatePID(error, dt, enable_integration, pid_contributions, errors, false); + return fb_value; +} + double PIDController::calculatePID( const double error, const double dt, const bool enable_integration, std::vector & pid_contributions, std::vector & errors, bool is_debugging) diff --git a/vehicle/raw_vehicle_cmd_converter/src/steer_converter.cpp b/vehicle/raw_vehicle_cmd_converter/src/steer_converter.cpp index 335f9450b01b0..bba9ebd05fbc8 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/steer_converter.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/steer_converter.cpp @@ -22,29 +22,6 @@ namespace raw_vehicle_cmd_converter { -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) { CSVLoader csv(csv_path); @@ -55,7 +32,7 @@ bool SteerConverter::readSteerMapFromCSV(const std::string & csv_path) } vehicle_name_ = table[0][0]; - vel_index_ = CSVLoader::getRowIndex(table); + steer_index_ = CSVLoader::getRowIndex(table); output_index_ = CSVLoader::getColumnIndex(table); steer_map_ = CSVLoader::getMap(table); return true; @@ -64,11 +41,11 @@ bool SteerConverter::readSteerMapFromCSV(const std::string & csv_path) void SteerConverter::getSteer(double steer_vel, double steer, double & output) { std::vector steer_angle_velocities_interp; - steer = CSVLoader::clampValue(steer, vel_index_, "steer: steer"); + steer = CSVLoader::clampValue(steer, steer_index_, "steer: steer"); for (std::vector steer_angle_velocities : steer_map_) { steer_angle_velocities_interp.push_back( - interpolation::lerp(vel_index_, steer_angle_velocities, steer)); + interpolation::lerp(steer_index_, steer_angle_velocities, steer)); } if (steer_vel < steer_angle_velocities_interp.front()) { steer_vel = steer_angle_velocities_interp.front(); From c0411a26ae9c09cf45db80cd8ce73ca821e31f7e Mon Sep 17 00:00:00 2001 From: taikitanaka3 Date: Sat, 15 Oct 2022 19:52:28 +0900 Subject: [PATCH 03/15] refactor: add getter setter method Signed-off-by: taikitanaka3 --- .../include/raw_vehicle_cmd_converter/pid.hpp | 5 +++-- vehicle/raw_vehicle_cmd_converter/src/node.cpp | 7 +++---- 2 files changed, 6 insertions(+), 6 deletions(-) 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 index df8dc7f678d80..c13de9b332fa3 100644 --- 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 @@ -37,8 +37,8 @@ class PIDController 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; } - bool fb_gains_initialized_{false}; - bool fb_limits_initialized_{false}; + void setInitialized() { is_initialized_ = true; } + bool getInitialized() { return is_initialized_; } private: // parameters @@ -58,6 +58,7 @@ class PIDController double prev_error_{0.0}; bool is_first_time_{true}; double invalid_integration_decay_{0.0}; + double is_initialized_{false}; }; } // 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 index a1baf0452dc7b..56b109a1c0c66 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/node.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/node.cpp @@ -70,11 +70,10 @@ RawVehicleCommandConverterNode::RawVehicleCommandConverterNode( declare_parameter("steer_pid.invalid_integration_decay", 0.97)}; steer_pid_.setDecay(invalid_integration_decay); steer_pid_.setGains(kp_steer, ki_steer, kd_steer); - steer_pid_.fb_gains_initialized_ = true; steer_pid_.setLimits( 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); - steer_pid_.fb_limits_initialized_ = true; + steer_pid_.setInitialized(); } pub_actuation_cmd_ = create_publisher("~/output/actuation_cmd", 1); sub_control_cmd_ = create_subscription( @@ -149,7 +148,7 @@ double RawVehicleCommandConverterNode::calculateSteer( 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; + dt = 1.0; } prev_time_steer_calculation_ = current_time; // feed-forward @@ -162,7 +161,7 @@ double RawVehicleCommandConverterNode::calculateSteer( } // feedback if (use_steer_fb_) { - if (steer_pid_.fb_gains_initialized_ && steer_pid_.fb_limits_initialized_) { + if (!steer_pid_.getInitialized()) { RCLCPP_WARN_EXPRESSION(get_logger(), is_debugging_, "FF map is not initialized!"); } else { fb_value = steer_pid_.calculateFB( From 86890fe7221b8754a27773dabbce0d97488aa5f5 Mon Sep 17 00:00:00 2001 From: taikitanaka3 Date: Sat, 15 Oct 2022 19:52:49 +0900 Subject: [PATCH 04/15] chore: add maintainer Signed-off-by: taikitanaka3 --- vehicle/raw_vehicle_cmd_converter/package.xml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/vehicle/raw_vehicle_cmd_converter/package.xml b/vehicle/raw_vehicle_cmd_converter/package.xml index cf1e55978bfb1..e2c85916d69b9 100644 --- a/vehicle/raw_vehicle_cmd_converter/package.xml +++ b/vehicle/raw_vehicle_cmd_converter/package.xml @@ -5,10 +5,13 @@ 0.1.0 The raw_vehicle_cmd_converter package Takamasa Horibe + Tanaka Taiki + Apache License 2.0 Makoto Kurihara Takamasa Horibe + Tanaka Taiki ament_cmake_auto From ab6a46fa8594fbf691b9023d7a15e9a677d0e20c Mon Sep 17 00:00:00 2001 From: taikitanaka3 Date: Sat, 15 Oct 2022 20:00:10 +0900 Subject: [PATCH 05/15] refactor: unite steer converter to same class name Signed-off-by: taikitanaka3 --- .../include/raw_vehicle_cmd_converter/node.hpp | 2 +- .../include/raw_vehicle_cmd_converter/steer_converter.hpp | 2 +- vehicle/raw_vehicle_cmd_converter/src/steer_converter.cpp | 4 ++-- .../test/test_raw_vehicle_cmd_converter.cpp | 8 ++++---- 4 files changed, 8 insertions(+), 8 deletions(-) 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 index faa4450afa113..5ed0729ad4f3a 100644 --- 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 @@ -87,7 +87,7 @@ class RawVehicleCommandConverterNode : public rclcpp::Node AckermannControlCommand::ConstSharedPtr control_cmd_ptr_; AccelMap accel_map_; BrakeMap brake_map_; - SteerConverter steer_map_; + SteerMap steer_map_; // TODO(tanaka): consider accel/brake pid too PIDController steer_pid_; bool ff_map_initialized_; 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 index e63127ed7c40c..4d357a80c0b12 100644 --- 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 @@ -25,7 +25,7 @@ namespace raw_vehicle_cmd_converter { -class SteerConverter +class SteerMap { public: bool readSteerMapFromCSV(const std::string & csv_path); diff --git a/vehicle/raw_vehicle_cmd_converter/src/steer_converter.cpp b/vehicle/raw_vehicle_cmd_converter/src/steer_converter.cpp index bba9ebd05fbc8..24b016e9a0098 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/steer_converter.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/steer_converter.cpp @@ -22,7 +22,7 @@ namespace raw_vehicle_cmd_converter { -bool SteerConverter::readSteerMapFromCSV(const std::string & csv_path) +bool SteerMap::readSteerMapFromCSV(const std::string & csv_path) { CSVLoader csv(csv_path); std::vector> table; @@ -38,7 +38,7 @@ bool SteerConverter::readSteerMapFromCSV(const std::string & csv_path) return true; } -void SteerConverter::getSteer(double steer_vel, double steer, double & output) +void SteerMap::getSteer(double steer_vel, double steer, double & output) { std::vector steer_angle_velocities_interp; steer = CSVLoader::clampValue(steer, steer_index_, "steer: steer"); diff --git a/vehicle/raw_vehicle_cmd_converter/test/test_raw_vehicle_cmd_converter.cpp b/vehicle/raw_vehicle_cmd_converter/test/test_raw_vehicle_cmd_converter.cpp index f503dca130763..86df0f5954c46 100644 --- a/vehicle/raw_vehicle_cmd_converter/test/test_raw_vehicle_cmd_converter.cpp +++ b/vehicle/raw_vehicle_cmd_converter/test/test_raw_vehicle_cmd_converter.cpp @@ -48,7 +48,7 @@ using raw_vehicle_cmd_converter::AccelMap; using raw_vehicle_cmd_converter::BrakeMap; -using raw_vehicle_cmd_converter::SteerConverter; +using raw_vehicle_cmd_converter::SteerMap; // may throw PackageNotFoundError exception for invalid package const auto map_path = @@ -64,7 +64,7 @@ bool loadBrakeMapData(BrakeMap & brake_map) return brake_map.readBrakeMapFromCSV(map_path + "test_brake_map.csv"); } -bool loadSteerMapData(SteerConverter & steer_map) +bool loadSteerMapData(SteerMap & steer_map) { return steer_map.readSteerMapFromCSV(map_path + "test_steer_map.csv"); } @@ -73,7 +73,7 @@ TEST(ConverterTests, LoadValidPath) { AccelMap accel_map; BrakeMap brake_map; - SteerConverter steer_map; + SteerMap steer_map; // for valid path EXPECT_TRUE(loadAccelMapData(accel_map)); @@ -178,7 +178,7 @@ TEST(ConverterTests, BrakeMapCalculation) TEST(ConverterTests, SteerMapCalculation) { - SteerConverter steer_map; + SteerMap steer_map; loadSteerMapData(steer_map); const auto calcSteer = [&](double steer_vel, double steer) { double output; From 769c3fbc72d312fc12f8ff7960d11daa59e16417 Mon Sep 17 00:00:00 2001 From: taikitanaka3 Date: Sat, 15 Oct 2022 20:05:48 +0900 Subject: [PATCH 06/15] chore: revert fix commit Signed-off-by: taikitanaka3 --- vehicle/raw_vehicle_cmd_converter/src/node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/vehicle/raw_vehicle_cmd_converter/src/node.cpp b/vehicle/raw_vehicle_cmd_converter/src/node.cpp index 56b109a1c0c66..46e3c39ee662c 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/node.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/node.cpp @@ -148,7 +148,7 @@ double RawVehicleCommandConverterNode::calculateSteer( 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 = 1.0; + dt = 0.0; } prev_time_steer_calculation_ = current_time; // feed-forward From 274939d5473bbbd8e6bc378ae6761569305c943a Mon Sep 17 00:00:00 2001 From: taikitanaka3 Date: Sun, 16 Oct 2022 01:24:38 +0900 Subject: [PATCH 07/15] refactor: use const Signed-off-by: taikitanaka3 --- .../raw_vehicle_cmd_converter/accel_map.hpp | 10 +++++----- .../raw_vehicle_cmd_converter/brake_map.hpp | 8 ++++---- .../steer_converter.hpp | 2 +- .../raw_vehicle_cmd_converter/src/accel_map.cpp | 4 ++-- .../raw_vehicle_cmd_converter/src/brake_map.cpp | 2 +- .../src/csv_loader.cpp | 15 +++++++-------- vehicle/raw_vehicle_cmd_converter/src/pid.cpp | 9 ++------- .../src/steer_converter.cpp | 17 ++++++++--------- 8 files changed, 30 insertions(+), 37 deletions(-) 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 index bb872db838f18..08d9deb0e1fe8 100644 --- 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 @@ -30,11 +30,11 @@ 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_; } + bool getThrottle(double acc, double vel, double & throttle) const; + bool getAcceleration(double throttle, double vel, double & acc) const; + std::vector getVelIdx() const { return vel_index_; } + std::vector getThrottleIdx() const { return throttle_index_; } + std::vector> getAccelMap() const { return accel_map_; } private: rclcpp::Logger logger_{rclcpp::get_logger("raw_vehicle_cmd_converter").get_child("accel_map")}; 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 index ba1fadfcd61c9..e6dddd85f225c 100644 --- 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 @@ -31,10 +31,10 @@ 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_; } + bool getAcceleration(double brake, double vel, double & acc) const; + std::vector getVelIdx() const { return vel_index_; } + std::vector getBrakeIdx() const { return brake_index_; } + std::vector> getBrakeMap() const { return brake_map_; } private: rclcpp::Logger logger_{rclcpp::get_logger("raw_vehicle_cmd_converter").get_child("accel_map")}; 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 index 4d357a80c0b12..2da19997fbee1 100644 --- 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 @@ -29,7 +29,7 @@ class SteerMap { public: bool readSteerMapFromCSV(const std::string & csv_path); - void getSteer(double steer_vel, double steer, double & output); + void getSteer(double steer_vel, double steer, double & output) const; private: std::string vehicle_name_; diff --git a/vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp b/vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp index 2f6ce02631b72..1c789e3c917f0 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp @@ -41,7 +41,7 @@ bool AccelMap::readAccelMapFromCSV(std::string csv_path) return true; } -bool AccelMap::getThrottle(double acc, double vel, double & throttle) +bool AccelMap::getThrottle(double acc, double vel, double & throttle) const { std::vector accs_interpolated; vel = CSVLoader::clampValue(vel, vel_index_, "throttle: vel"); @@ -65,7 +65,7 @@ bool AccelMap::getThrottle(double acc, double vel, double & throttle) return true; } -bool AccelMap::getAcceleration(double throttle, double vel, double & acc) +bool AccelMap::getAcceleration(double throttle, double vel, double & acc) const { std::vector accs_interpolated; vel = CSVLoader::clampValue(vel, vel_index_, "throttle: vel"); diff --git a/vehicle/raw_vehicle_cmd_converter/src/brake_map.cpp b/vehicle/raw_vehicle_cmd_converter/src/brake_map.cpp index d2978ce872654..0809854d61658 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/brake_map.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/brake_map.cpp @@ -73,7 +73,7 @@ bool BrakeMap::getBrake(double acc, double vel, double & brake) return true; } -bool BrakeMap::getAcceleration(double brake, double vel, double & acc) +bool BrakeMap::getAcceleration(double brake, double vel, double & acc) const { std::vector accs_interpolated; vel = CSVLoader::clampValue(vel, vel_index_, "brake: vel"); diff --git a/vehicle/raw_vehicle_cmd_converter/src/csv_loader.cpp b/vehicle/raw_vehicle_cmd_converter/src/csv_loader.cpp index 8b7490596f503..3b702d272feba 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/csv_loader.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/csv_loader.cpp @@ -56,7 +56,7 @@ bool CSVLoader::validateData(const Table & table, const std::string & csv_path) << std::endl; return false; } - for (unsigned int i = 1; i < table.size(); i++) { + for (size_t i = 1; i < table.size(); i++) { if (table[0].size() != table[i].size()) { std::cerr << "Cannot read " << csv_path.c_str() << ". Each row should have a same number of columns" << std::endl; @@ -69,9 +69,9 @@ bool CSVLoader::validateData(const Table & table, const std::string & csv_path) Map CSVLoader::getMap(const Table & table) { Map map = {}; - for (unsigned int i = 1; i < table.size(); i++) { + for (size_t i = 1; i < table.size(); i++) { std::vector accs; - for (unsigned int j = 1; j < table[i].size(); j++) { + for (size_t j = 1; j < table[i].size(); j++) { accs.push_back(std::stod(table[i][j])); } map.push_back(accs); @@ -82,7 +82,7 @@ Map CSVLoader::getMap(const Table & table) std::vector CSVLoader::getRowIndex(const Table & table) { std::vector index = {}; - for (unsigned int i = 1; i < table[0].size(); i++) { + for (size_t i = 1; i < table[0].size(); i++) { index.push_back(std::stod(table[0][i])); } return index; @@ -91,7 +91,7 @@ std::vector CSVLoader::getRowIndex(const Table & table) std::vector CSVLoader::getColumnIndex(const Table & table) { std::vector index = {}; - for (unsigned int i = 1; i < table[0].size(); i++) { + for (size_t i = 1; i < table[0].size(); i++) { index.push_back(std::stod(table[i][0])); } return index; @@ -100,15 +100,14 @@ std::vector CSVLoader::getColumnIndex(const Table & table) double CSVLoader::clampValue( const double val, const std::vector ranges, const std::string & name) { - double ret = val; const double max_value = ranges.back(); const double min_value = ranges.front(); if (val < min_value || max_value < val) { std::cerr << "Input" << name << ": " << val << " is out off range. use closest value." << std::endl; - ret = std::min(std::max(val, min_value), max_value); + return std::min(std::max(val, min_value), max_value); } - return ret; + return val; } } // namespace raw_vehicle_cmd_converter diff --git a/vehicle/raw_vehicle_cmd_converter/src/pid.cpp b/vehicle/raw_vehicle_cmd_converter/src/pid.cpp index c01df23bd7743..216382be3dc81 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/pid.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/pid.cpp @@ -24,14 +24,9 @@ double PIDController::calculateFB( const double target_value, const double dt, const double reset_trigger_value, const double current_value, std::vector & pid_contributions, std::vector & errors) { - double fb_value = 0; const double error = target_value - current_value; - bool enable_integration = true; - if (std::abs(reset_trigger_value) < 0.01) { - enable_integration = false; - } - fb_value = calculatePID(error, dt, enable_integration, pid_contributions, errors, false); - return fb_value; + const bool enable_integration = (std::abs(reset_trigger_value) < 0.01) ? false : true; + return calculatePID(error, dt, enable_integration, pid_contributions, errors, false); } double PIDController::calculatePID( diff --git a/vehicle/raw_vehicle_cmd_converter/src/steer_converter.cpp b/vehicle/raw_vehicle_cmd_converter/src/steer_converter.cpp index 24b016e9a0098..658ff76f4cede 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/steer_converter.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/steer_converter.cpp @@ -38,20 +38,19 @@ bool SteerMap::readSteerMapFromCSV(const std::string & csv_path) return true; } -void SteerMap::getSteer(double steer_vel, double steer, double & output) +void SteerMap::getSteer(double steer_vel, double steer, double & output) const { - std::vector steer_angle_velocities_interp; steer = CSVLoader::clampValue(steer, steer_index_, "steer: steer"); + std::vector steer_rate_interp = {}; for (std::vector steer_angle_velocities : steer_map_) { - steer_angle_velocities_interp.push_back( - interpolation::lerp(steer_index_, steer_angle_velocities, steer)); + steer_rate_interp.push_back(interpolation::lerp(steer_index_, steer_angle_velocities, steer)); } - 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(); + if (steer_vel < steer_rate_interp.front()) { + steer_vel = steer_rate_interp.front(); + } else if (steer_rate_interp.back() < steer_vel) { + steer_vel = steer_rate_interp.back(); } - output = interpolation::lerp(steer_angle_velocities_interp, output_index_, steer_vel); + output = interpolation::lerp(steer_rate_interp, output_index_, steer_vel); } } // namespace raw_vehicle_cmd_converter From 2129dc9689d813a20ed62649254b04b72c0da58e Mon Sep 17 00:00:00 2001 From: taikitanaka3 Date: Sun, 16 Oct 2022 01:32:53 +0900 Subject: [PATCH 08/15] refactor: add const left Signed-off-by: taikitanaka3 --- .../include/raw_vehicle_cmd_converter/accel_map.hpp | 4 ++-- .../include/raw_vehicle_cmd_converter/brake_map.hpp | 4 ++-- .../include/raw_vehicle_cmd_converter/csv_loader.hpp | 4 ++-- .../raw_vehicle_cmd_converter/steer_converter.hpp | 2 +- vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp | 4 ++-- vehicle/raw_vehicle_cmd_converter/src/brake_map.cpp | 4 ++-- vehicle/raw_vehicle_cmd_converter/src/csv_loader.cpp | 4 ++-- .../src/steer_converter.cpp | 12 ++++++------ 8 files changed, 19 insertions(+), 19 deletions(-) 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 index 08d9deb0e1fe8..76af7162d24fa 100644 --- 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 @@ -29,8 +29,8 @@ namespace raw_vehicle_cmd_converter class AccelMap { public: - bool readAccelMapFromCSV(std::string csv_path); - bool getThrottle(double acc, double vel, double & throttle) const; + bool readAccelMapFromCSV(const std::string & csv_path); + bool getThrottle(const double acc, double vel, double & throttle) const; bool getAcceleration(double throttle, double vel, double & acc) const; std::vector getVelIdx() const { return vel_index_; } std::vector getThrottleIdx() const { return throttle_index_; } 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 index e6dddd85f225c..3840f3e697cd6 100644 --- 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 @@ -29,8 +29,8 @@ namespace raw_vehicle_cmd_converter class BrakeMap { public: - bool readBrakeMapFromCSV(std::string csv_path); - bool getBrake(double acc, double vel, double & brake); + bool readBrakeMapFromCSV(const std::string & csv_path); + bool getBrake(const double acc, double vel, double & brake); bool getAcceleration(double brake, double vel, double & acc) const; std::vector getVelIdx() const { return vel_index_; } std::vector getBrakeIdx() const { return brake_index_; } 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 index 6d2a144fb87d1..1786050347362 100644 --- 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 @@ -28,7 +28,7 @@ using Map = std::vector>; class CSVLoader { public: - explicit CSVLoader(std::string csv_path); + explicit CSVLoader(const std::string & csv_path); bool readCSV(Table & result, const char delim = ','); static bool validateData(const Table & table, const std::string & csv_path); @@ -36,7 +36,7 @@ class CSVLoader static std::vector getRowIndex(const Table & table); static std::vector getColumnIndex(const Table & table); static double clampValue( - const double val, const std::vector ranges, const std::string & name); + const double val, const std::vector & ranges, const std::string & name); private: std::string csv_path_; 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 index 2da19997fbee1..5cb150943485d 100644 --- 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 @@ -29,7 +29,7 @@ class SteerMap { public: bool readSteerMapFromCSV(const std::string & csv_path); - void getSteer(double steer_vel, double steer, double & output) const; + void getSteer(double steer_rate, double steer, double & output) const; private: std::string vehicle_name_; diff --git a/vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp b/vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp index 1c789e3c917f0..af91984ff2fa6 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp @@ -25,7 +25,7 @@ using namespace std::literals::chrono_literals; namespace raw_vehicle_cmd_converter { -bool AccelMap::readAccelMapFromCSV(std::string csv_path) +bool AccelMap::readAccelMapFromCSV(const std::string & csv_path) { CSVLoader csv(csv_path); std::vector> table; @@ -41,7 +41,7 @@ bool AccelMap::readAccelMapFromCSV(std::string csv_path) return true; } -bool AccelMap::getThrottle(double acc, double vel, double & throttle) const +bool AccelMap::getThrottle(const double acc, double vel, double & throttle) const { std::vector accs_interpolated; vel = CSVLoader::clampValue(vel, vel_index_, "throttle: vel"); diff --git a/vehicle/raw_vehicle_cmd_converter/src/brake_map.cpp b/vehicle/raw_vehicle_cmd_converter/src/brake_map.cpp index 0809854d61658..422500a8be777 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/brake_map.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/brake_map.cpp @@ -22,7 +22,7 @@ namespace raw_vehicle_cmd_converter { -bool BrakeMap::readBrakeMapFromCSV(std::string csv_path) +bool BrakeMap::readBrakeMapFromCSV(const std::string & csv_path) { CSVLoader csv(csv_path); std::vector> table; @@ -41,7 +41,7 @@ bool BrakeMap::readBrakeMapFromCSV(std::string csv_path) return true; } -bool BrakeMap::getBrake(double acc, double vel, double & brake) +bool BrakeMap::getBrake(const double acc, double vel, double & brake) { std::vector accs_interpolated; vel = CSVLoader::clampValue(vel, vel_index_, "brake: vel"); diff --git a/vehicle/raw_vehicle_cmd_converter/src/csv_loader.cpp b/vehicle/raw_vehicle_cmd_converter/src/csv_loader.cpp index 3b702d272feba..768b202c22335 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/csv_loader.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/csv_loader.cpp @@ -19,7 +19,7 @@ namespace raw_vehicle_cmd_converter { -CSVLoader::CSVLoader(std::string csv_path) { csv_path_ = csv_path; } +CSVLoader::CSVLoader(const std::string & csv_path) { csv_path_ = csv_path; } bool CSVLoader::readCSV(Table & result, const char delim) { @@ -98,7 +98,7 @@ std::vector CSVLoader::getColumnIndex(const Table & table) } double CSVLoader::clampValue( - const double val, const std::vector ranges, const std::string & name) + const double val, const std::vector & ranges, const std::string & name) { const double max_value = ranges.back(); const double min_value = ranges.front(); diff --git a/vehicle/raw_vehicle_cmd_converter/src/steer_converter.cpp b/vehicle/raw_vehicle_cmd_converter/src/steer_converter.cpp index 658ff76f4cede..b57b2ff1a8ca4 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/steer_converter.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/steer_converter.cpp @@ -38,7 +38,7 @@ bool SteerMap::readSteerMapFromCSV(const std::string & csv_path) return true; } -void SteerMap::getSteer(double steer_vel, double steer, double & output) const +void SteerMap::getSteer(double steer_rate, double steer, double & output) const { steer = CSVLoader::clampValue(steer, steer_index_, "steer: steer"); @@ -46,11 +46,11 @@ void SteerMap::getSteer(double steer_vel, double steer, double & output) const for (std::vector steer_angle_velocities : steer_map_) { steer_rate_interp.push_back(interpolation::lerp(steer_index_, steer_angle_velocities, steer)); } - if (steer_vel < steer_rate_interp.front()) { - steer_vel = steer_rate_interp.front(); - } else if (steer_rate_interp.back() < steer_vel) { - steer_vel = steer_rate_interp.back(); + if (steer_rate < steer_rate_interp.front()) { + steer_rate = steer_rate_interp.front(); + } else if (steer_rate_interp.back() < steer_rate) { + steer_rate = steer_rate_interp.back(); } - output = interpolation::lerp(steer_rate_interp, output_index_, steer_vel); + output = interpolation::lerp(steer_rate_interp, output_index_, steer_rate); } } // namespace raw_vehicle_cmd_converter From f25bdb295f911a59958b974bf0f86592f15845f9 Mon Sep 17 00:00:00 2001 From: taikitanaka3 Date: Sun, 16 Oct 2022 11:06:40 +0900 Subject: [PATCH 09/15] style: spell check Signed-off-by: taikitanaka3 --- vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp | 8 ++++---- vehicle/raw_vehicle_cmd_converter/src/brake_map.cpp | 8 ++++---- vehicle/raw_vehicle_cmd_converter/src/csv_loader.cpp | 6 +++--- 3 files changed, 11 insertions(+), 11 deletions(-) diff --git a/vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp b/vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp index af91984ff2fa6..8471cd62ddc89 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp @@ -47,8 +47,8 @@ bool AccelMap::getThrottle(const double acc, double vel, double & throttle) cons vel = CSVLoader::clampValue(vel, vel_index_, "throttle: vel"); // (throttle, vel, acc) map => (throttle, acc) map by fixing vel - for (std::vector accs : accel_map_) { - accs_interpolated.push_back(interpolation::lerp(vel_index_, accs, vel)); + for (std::vector accelerations : accel_map_) { + accs_interpolated.push_back(interpolation::lerp(vel_index_, accelerations, vel)); } // calculate throttle @@ -71,8 +71,8 @@ bool AccelMap::getAcceleration(double throttle, double vel, double & acc) const vel = CSVLoader::clampValue(vel, vel_index_, "throttle: vel"); // (throttle, vel, acc) map => (throttle, acc) map by fixing vel - for (std::vector accs : accel_map_) { - accs_interpolated.push_back(interpolation::lerp(vel_index_, accs, vel)); + for (std::vector accelerations : accel_map_) { + accs_interpolated.push_back(interpolation::lerp(vel_index_, accelerations, vel)); } // calculate throttle diff --git a/vehicle/raw_vehicle_cmd_converter/src/brake_map.cpp b/vehicle/raw_vehicle_cmd_converter/src/brake_map.cpp index 422500a8be777..f88c2c95c2010 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/brake_map.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/brake_map.cpp @@ -47,8 +47,8 @@ bool BrakeMap::getBrake(const double acc, double vel, double & brake) vel = CSVLoader::clampValue(vel, vel_index_, "brake: vel"); // (throttle, vel, acc) map => (throttle, acc) map by fixing vel - for (std::vector accs : brake_map_) { - accs_interpolated.push_back(interpolation::lerp(vel_index_, accs, vel)); + for (std::vector accelerations : brake_map_) { + accs_interpolated.push_back(interpolation::lerp(vel_index_, accelerations, vel)); } // calculate brake @@ -79,8 +79,8 @@ bool BrakeMap::getAcceleration(double brake, double vel, double & acc) const vel = CSVLoader::clampValue(vel, vel_index_, "brake: vel"); // (throttle, vel, acc) map => (throttle, acc) map by fixing vel - for (std::vector accs : brake_map_) { - accs_interpolated.push_back(interpolation::lerp(vel_index_, accs, vel)); + for (std::vector accelerations : brake_map_) { + accs_interpolated.push_back(interpolation::lerp(vel_index_, accelerations, vel)); } // calculate brake diff --git a/vehicle/raw_vehicle_cmd_converter/src/csv_loader.cpp b/vehicle/raw_vehicle_cmd_converter/src/csv_loader.cpp index 768b202c22335..b60ae331cd55d 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/csv_loader.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/csv_loader.cpp @@ -70,11 +70,11 @@ Map CSVLoader::getMap(const Table & table) { Map map = {}; for (size_t i = 1; i < table.size(); i++) { - std::vector accs; + std::vector accelerations; for (size_t j = 1; j < table[i].size(); j++) { - accs.push_back(std::stod(table[i][j])); + accelerations.push_back(std::stod(table[i][j])); } - map.push_back(accs); + map.push_back(accelerations); } return map; } From 93e83ad20e112c25ce99e9961f7c58e88add6d79 Mon Sep 17 00:00:00 2001 From: taikitanaka3 Date: Sun, 16 Oct 2022 16:51:45 +0900 Subject: [PATCH 10/15] style: spell check Signed-off-by: taikitanaka3 --- .../src/accel_map.cpp | 16 +++++++-------- .../src/brake_map.cpp | 20 +++++++++---------- 2 files changed, 18 insertions(+), 18 deletions(-) diff --git a/vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp b/vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp index 8471cd62ddc89..4caa6a529f98a 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp @@ -43,43 +43,43 @@ bool AccelMap::readAccelMapFromCSV(const std::string & csv_path) bool AccelMap::getThrottle(const double acc, double vel, double & throttle) const { - std::vector accs_interpolated; + std::vector accelerations_interpolated; vel = CSVLoader::clampValue(vel, vel_index_, "throttle: vel"); // (throttle, vel, acc) map => (throttle, acc) map by fixing vel for (std::vector accelerations : accel_map_) { - accs_interpolated.push_back(interpolation::lerp(vel_index_, accelerations, vel)); + accelerations_interpolated.push_back(interpolation::lerp(vel_index_, accelerations, vel)); } // 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()) { + if (acc < accelerations_interpolated.front()) { return false; - } else if (accs_interpolated.back() < acc) { + } else if (accelerations_interpolated.back() < acc) { throttle = throttle_index_.back(); return true; } - throttle = interpolation::lerp(accs_interpolated, throttle_index_, acc); + throttle = interpolation::lerp(accelerations_interpolated, throttle_index_, acc); return true; } bool AccelMap::getAcceleration(double throttle, double vel, double & acc) const { - std::vector accs_interpolated; + std::vector accelerations_interpolated; vel = CSVLoader::clampValue(vel, vel_index_, "throttle: vel"); // (throttle, vel, acc) map => (throttle, acc) map by fixing vel for (std::vector accelerations : accel_map_) { - accs_interpolated.push_back(interpolation::lerp(vel_index_, accelerations, vel)); + accelerations_interpolated.push_back(interpolation::lerp(vel_index_, accelerations, vel)); } // calculate throttle // When the desired acceleration is smaller than the throttle area, return min acc // When the desired acceleration is greater than the throttle area, return max acc throttle = CSVLoader::clampValue(throttle, throttle_index_, "throttle: acc"); - acc = interpolation::lerp(throttle_index_, accs_interpolated, throttle); + acc = interpolation::lerp(throttle_index_, accelerations_interpolated, throttle); return true; } diff --git a/vehicle/raw_vehicle_cmd_converter/src/brake_map.cpp b/vehicle/raw_vehicle_cmd_converter/src/brake_map.cpp index f88c2c95c2010..6e7d372c6b342 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/brake_map.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/brake_map.cpp @@ -43,51 +43,51 @@ bool BrakeMap::readBrakeMapFromCSV(const std::string & csv_path) bool BrakeMap::getBrake(const double acc, double vel, double & brake) { - std::vector accs_interpolated; + std::vector accelerations_interpolated; vel = CSVLoader::clampValue(vel, vel_index_, "brake: vel"); // (throttle, vel, acc) map => (throttle, acc) map by fixing vel for (std::vector accelerations : brake_map_) { - accs_interpolated.push_back(interpolation::lerp(vel_index_, accelerations, vel)); + accelerations_interpolated.push_back(interpolation::lerp(vel_index_, accelerations, vel)); } // 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()) { + if (acc < accelerations_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()); + acc, accelerations_interpolated.back()); brake = brake_index_.back(); return true; - } else if (accs_interpolated.front() < acc) { + } else if (accelerations_interpolated.front() < acc) { brake = brake_index_.front(); return true; } - std::reverse(std::begin(accs_interpolated), std::end(accs_interpolated)); - brake = interpolation::lerp(accs_interpolated, brake_index_rev_, acc); + std::reverse(std::begin(accelerations_interpolated), std::end(accelerations_interpolated)); + brake = interpolation::lerp(accelerations_interpolated, brake_index_rev_, acc); return true; } bool BrakeMap::getAcceleration(double brake, double vel, double & acc) const { - std::vector accs_interpolated; + std::vector accelerations_interpolated; vel = CSVLoader::clampValue(vel, vel_index_, "brake: vel"); // (throttle, vel, acc) map => (throttle, acc) map by fixing vel for (std::vector accelerations : brake_map_) { - accs_interpolated.push_back(interpolation::lerp(vel_index_, accelerations, vel)); + accelerations_interpolated.push_back(interpolation::lerp(vel_index_, accelerations, vel)); } // calculate brake // When the desired acceleration is smaller than the brake area, return min acc // When the desired acceleration is greater than the brake area, return min acc brake = CSVLoader::clampValue(brake, brake_index_, "brake: acc"); - acc = interpolation::lerp(brake_index_, accs_interpolated, brake); + acc = interpolation::lerp(brake_index_, accelerations_interpolated, brake); return true; } From 2c8a5b15b99d50dff212638c261056ce2bd08955 Mon Sep 17 00:00:00 2001 From: tanaka3 Date: Mon, 17 Oct 2022 12:39:07 +0900 Subject: [PATCH 11/15] style: update const with suggestion Signed-off-by: tanaka3 --- .../raw_vehicle_cmd_converter/accel_map.hpp | 4 +-- .../raw_vehicle_cmd_converter/brake_map.hpp | 4 +-- .../steer_converter.hpp | 2 +- .../src/accel_map.cpp | 26 +++++++-------- .../src/brake_map.cpp | 32 +++++++++---------- .../src/steer_converter.cpp | 19 +++++------ 6 files changed, 42 insertions(+), 45 deletions(-) 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 index 76af7162d24fa..fba69efdbc921 100644 --- 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 @@ -30,8 +30,8 @@ class AccelMap { public: bool readAccelMapFromCSV(const std::string & csv_path); - bool getThrottle(const double acc, double vel, double & throttle) const; - bool getAcceleration(double throttle, double vel, double & acc) const; + bool getThrottle(const double acc, const double vel, double & throttle) const; + bool getAcceleration(const double throttle, const double vel, double & acc) const; std::vector getVelIdx() const { return vel_index_; } std::vector getThrottleIdx() const { return throttle_index_; } std::vector> getAccelMap() const { return accel_map_; } 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 index 3840f3e697cd6..8d4d35545254c 100644 --- 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 @@ -30,8 +30,8 @@ class BrakeMap { public: bool readBrakeMapFromCSV(const std::string & csv_path); - bool getBrake(const double acc, double vel, double & brake); - bool getAcceleration(double brake, double vel, double & acc) const; + bool getBrake(const double acc, const double vel, double & brake); + bool getAcceleration(const double brake, const double vel, double & acc) const; std::vector getVelIdx() const { return vel_index_; } std::vector getBrakeIdx() const { return brake_index_; } std::vector> getBrakeMap() const { return brake_map_; } 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 index 5cb150943485d..f59ceed5f4610 100644 --- 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 @@ -29,7 +29,7 @@ class SteerMap { public: bool readSteerMapFromCSV(const std::string & csv_path); - void getSteer(double steer_rate, double steer, double & output) const; + void getSteer(const double steer_rate, const double steer, double & output) const; private: std::string vehicle_name_; diff --git a/vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp b/vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp index 4caa6a529f98a..0075bcd5be50c 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp @@ -43,43 +43,43 @@ bool AccelMap::readAccelMapFromCSV(const std::string & csv_path) bool AccelMap::getThrottle(const double acc, double vel, double & throttle) const { - std::vector accelerations_interpolated; - vel = CSVLoader::clampValue(vel, vel_index_, "throttle: vel"); + std::vector interpolated_acc_vec; + const double clamped_vel = CSVLoader::clampValue(vel, vel_index_, "throttle: vel"); // (throttle, vel, acc) map => (throttle, acc) map by fixing vel for (std::vector accelerations : accel_map_) { - accelerations_interpolated.push_back(interpolation::lerp(vel_index_, accelerations, vel)); + interpolated_acc_vec.push_back(interpolation::lerp(vel_index_, accelerations, clamped_vel)); } // 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 < accelerations_interpolated.front()) { + if (acc < interpolated_acc_vec.front()) { return false; - } else if (accelerations_interpolated.back() < acc) { + } else if (interpolated_acc_vec.back() < acc) { throttle = throttle_index_.back(); return true; } - throttle = interpolation::lerp(accelerations_interpolated, throttle_index_, acc); + throttle = interpolation::lerp(interpolated_acc_vec, throttle_index_, acc); return true; } -bool AccelMap::getAcceleration(double throttle, double vel, double & acc) const +bool AccelMap::getAcceleration(const double throttle, const double vel, double & acc) const { - std::vector accelerations_interpolated; - vel = CSVLoader::clampValue(vel, vel_index_, "throttle: vel"); + std::vector interpolated_acc_vec; + const double clamed_vel = CSVLoader::clampValue(vel, vel_index_, "throttle: vel"); // (throttle, vel, acc) map => (throttle, acc) map by fixing vel - for (std::vector accelerations : accel_map_) { - accelerations_interpolated.push_back(interpolation::lerp(vel_index_, accelerations, vel)); + for (const auto & acc_vec : accel_map_) { + interpolated_acc_vec.push_back(interpolation::lerp(vel_index_, acc_vec, clamed_vel)); } // calculate throttle // When the desired acceleration is smaller than the throttle area, return min acc // When the desired acceleration is greater than the throttle area, return max acc - throttle = CSVLoader::clampValue(throttle, throttle_index_, "throttle: acc"); - acc = interpolation::lerp(throttle_index_, accelerations_interpolated, throttle); + const double clamped_throttle = CSVLoader::clampValue(throttle, throttle_index_, "throttle: acc"); + acc = interpolation::lerp(throttle_index_, interpolated_acc_vec, clamped_throttle); return true; } diff --git a/vehicle/raw_vehicle_cmd_converter/src/brake_map.cpp b/vehicle/raw_vehicle_cmd_converter/src/brake_map.cpp index 6e7d372c6b342..6f31b446614ce 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/brake_map.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/brake_map.cpp @@ -41,53 +41,53 @@ bool BrakeMap::readBrakeMapFromCSV(const std::string & csv_path) return true; } -bool BrakeMap::getBrake(const double acc, double vel, double & brake) +bool BrakeMap::getBrake(const double acc, const double vel, double & brake) { - std::vector accelerations_interpolated; - vel = CSVLoader::clampValue(vel, vel_index_, "brake: vel"); + std::vector interpolated_acc_vec; + const double clamped_vel = CSVLoader::clampValue(vel, vel_index_, "brake: vel"); // (throttle, vel, acc) map => (throttle, acc) map by fixing vel for (std::vector accelerations : brake_map_) { - accelerations_interpolated.push_back(interpolation::lerp(vel_index_, accelerations, vel)); + interpolated_acc_vec.push_back(interpolation::lerp(vel_index_, accelerations, clamped_vel)); } // 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 < accelerations_interpolated.back()) { + if (acc < interpolated_acc_vec.back()) { RCLCPP_WARN_SKIPFIRST_THROTTLE( logger_, clock_, 1000, "Exceeding the acc range. Desired acc: %f < min acc on map: %f. return max " "value.", - acc, accelerations_interpolated.back()); + acc, interpolated_acc_vec.back()); brake = brake_index_.back(); return true; - } else if (accelerations_interpolated.front() < acc) { + } else if (interpolated_acc_vec.front() < acc) { brake = brake_index_.front(); return true; } - std::reverse(std::begin(accelerations_interpolated), std::end(accelerations_interpolated)); - brake = interpolation::lerp(accelerations_interpolated, brake_index_rev_, acc); + std::reverse(std::begin(interpolated_acc_vec), std::end(interpolated_acc_vec)); + brake = interpolation::lerp(interpolated_acc_vec, brake_index_rev_, acc); return true; } -bool BrakeMap::getAcceleration(double brake, double vel, double & acc) const +bool BrakeMap::getAcceleration(const double brake, const double vel, double & acc) const { - std::vector accelerations_interpolated; - vel = CSVLoader::clampValue(vel, vel_index_, "brake: vel"); + std::vector interpolated_acc_vec; + const double clamped_vel = CSVLoader::clampValue(vel, vel_index_, "brake: vel"); // (throttle, vel, acc) map => (throttle, acc) map by fixing vel - for (std::vector accelerations : brake_map_) { - accelerations_interpolated.push_back(interpolation::lerp(vel_index_, accelerations, vel)); + for (const auto & acc_vec : brake_map_) { + interpolated_acc_vec.push_back(interpolation::lerp(vel_index_, acc_vec, clamped_vel)); } // calculate brake // When the desired acceleration is smaller than the brake area, return min acc // When the desired acceleration is greater than the brake area, return min acc - brake = CSVLoader::clampValue(brake, brake_index_, "brake: acc"); - acc = interpolation::lerp(brake_index_, accelerations_interpolated, brake); + const double clamped_brake = CSVLoader::clampValue(brake, brake_index_, "brake: acc"); + acc = interpolation::lerp(brake_index_, interpolated_acc_vec, clamped_brake); return true; } diff --git a/vehicle/raw_vehicle_cmd_converter/src/steer_converter.cpp b/vehicle/raw_vehicle_cmd_converter/src/steer_converter.cpp index b57b2ff1a8ca4..54f21e01d6679 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/steer_converter.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/steer_converter.cpp @@ -38,19 +38,16 @@ bool SteerMap::readSteerMapFromCSV(const std::string & csv_path) return true; } -void SteerMap::getSteer(double steer_rate, double steer, double & output) const +void SteerMap::getSteer(const double steer_rate, const double steer, double & output) const { - steer = CSVLoader::clampValue(steer, steer_index_, "steer: steer"); - + const double clamped_steer = CSVLoader::clampValue(steer, steer_index_, "steer: steer"); std::vector steer_rate_interp = {}; - for (std::vector steer_angle_velocities : steer_map_) { - steer_rate_interp.push_back(interpolation::lerp(steer_index_, steer_angle_velocities, steer)); - } - if (steer_rate < steer_rate_interp.front()) { - steer_rate = steer_rate_interp.front(); - } else if (steer_rate_interp.back() < steer_rate) { - steer_rate = steer_rate_interp.back(); + for (const auto & steer_rate_vec : steer_map_) { + steer_rate_interp.push_back(interpolation::lerp(steer_index_, steer_rate_vec, clamped_steer)); } - output = interpolation::lerp(steer_rate_interp, output_index_, steer_rate); + + const double clamped_steer_rate = + CSVLoader::clampValue(steer_rate, steer_rate_interp, "steer: steer_rate"); + output = interpolation::lerp(steer_rate_interp, output_index_, clamped_steer_rate); } } // namespace raw_vehicle_cmd_converter From bc5c817f3925c0d34f1354f66abba0dfeaa04dd9 Mon Sep 17 00:00:00 2001 From: tanaka3 Date: Mon, 17 Oct 2022 12:43:36 +0900 Subject: [PATCH 12/15] style: to steer map Signed-off-by: tanaka3 --- vehicle/raw_vehicle_cmd_converter/CMakeLists.txt | 2 +- .../include/raw_vehicle_cmd_converter/node.hpp | 2 +- .../{steer_converter.hpp => steer_map.hpp} | 8 ++++---- .../src/{steer_converter.cpp => steer_map.cpp} | 2 +- .../test/test_raw_vehicle_cmd_converter.cpp | 2 +- 5 files changed, 8 insertions(+), 8 deletions(-) rename vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/{steer_converter.hpp => steer_map.hpp} (87%) rename vehicle/raw_vehicle_cmd_converter/src/{steer_converter.cpp => steer_map.cpp} (96%) diff --git a/vehicle/raw_vehicle_cmd_converter/CMakeLists.txt b/vehicle/raw_vehicle_cmd_converter/CMakeLists.txt index 36b96a2a300c5..d253b2315465e 100644 --- a/vehicle/raw_vehicle_cmd_converter/CMakeLists.txt +++ b/vehicle/raw_vehicle_cmd_converter/CMakeLists.txt @@ -7,9 +7,9 @@ autoware_package() ament_auto_add_library(actuation_map_converter SHARED src/accel_map.cpp src/brake_map.cpp + src/steer_map.cpp src/csv_loader.cpp src/pid.cpp - src/steer_converter.cpp ) ament_auto_add_library(raw_vehicle_cmd_converter_node_component SHARED 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 index 5ed0729ad4f3a..27346e99b60fb 100644 --- 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 @@ -18,7 +18,7 @@ #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 "raw_vehicle_cmd_converter/steer_map.hpp" #include 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_map.hpp similarity index 87% rename from vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/steer_converter.hpp rename to vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/steer_map.hpp index f59ceed5f4610..767689146fdf8 100644 --- 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_map.hpp @@ -12,8 +12,8 @@ // 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_ +#ifndef RAW_VEHICLE_CMD_CONVERTER__STEER_MAP_HPP_ +#define RAW_VEHICLE_CMD_CONVERTER__STEER_MAP_HPP_ #include "raw_vehicle_cmd_converter/csv_loader.hpp" #include "raw_vehicle_cmd_converter/pid.hpp" @@ -37,8 +37,8 @@ class SteerMap std::vector output_index_; std::vector> steer_map_; rclcpp::Logger logger_{ - rclcpp::get_logger("raw_vehicle_cmd_converter").get_child("steer_converter")}; + rclcpp::get_logger("raw_vehicle_cmd_converter").get_child("steer_map")}; }; } // namespace raw_vehicle_cmd_converter -#endif // RAW_VEHICLE_CMD_CONVERTER__STEER_CONVERTER_HPP_ +#endif // RAW_VEHICLE_CMD_CONVERTER__STEER_MAP_HPP_ diff --git a/vehicle/raw_vehicle_cmd_converter/src/steer_converter.cpp b/vehicle/raw_vehicle_cmd_converter/src/steer_map.cpp similarity index 96% rename from vehicle/raw_vehicle_cmd_converter/src/steer_converter.cpp rename to vehicle/raw_vehicle_cmd_converter/src/steer_map.cpp index 54f21e01d6679..1c78e3c6ee17c 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/steer_converter.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/steer_map.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "raw_vehicle_cmd_converter/steer_converter.hpp" +#include "raw_vehicle_cmd_converter/steer_map.hpp" #include "interpolation/linear_interpolation.hpp" diff --git a/vehicle/raw_vehicle_cmd_converter/test/test_raw_vehicle_cmd_converter.cpp b/vehicle/raw_vehicle_cmd_converter/test/test_raw_vehicle_cmd_converter.cpp index 86df0f5954c46..8dda91e481f5b 100644 --- a/vehicle/raw_vehicle_cmd_converter/test/test_raw_vehicle_cmd_converter.cpp +++ b/vehicle/raw_vehicle_cmd_converter/test/test_raw_vehicle_cmd_converter.cpp @@ -16,7 +16,7 @@ #include "gtest/gtest.h" #include "raw_vehicle_cmd_converter/accel_map.hpp" #include "raw_vehicle_cmd_converter/brake_map.hpp" -#include "raw_vehicle_cmd_converter/steer_converter.hpp" +#include "raw_vehicle_cmd_converter/steer_map.hpp" #include From e563bb7b91e69d34d3f5fc81f36ba2c2072a8fd9 Mon Sep 17 00:00:00 2001 From: tanaka3 Date: Mon, 17 Oct 2022 13:38:22 +0900 Subject: [PATCH 13/15] style: spell check Signed-off-by: tanaka3 --- vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp b/vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp index 0075bcd5be50c..011830accc0e7 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp @@ -68,11 +68,11 @@ bool AccelMap::getThrottle(const double acc, double vel, double & throttle) cons bool AccelMap::getAcceleration(const double throttle, const double vel, double & acc) const { std::vector interpolated_acc_vec; - const double clamed_vel = CSVLoader::clampValue(vel, vel_index_, "throttle: vel"); + const double clamped_vel = CSVLoader::clampValue(vel, vel_index_, "throttle: vel"); // (throttle, vel, acc) map => (throttle, acc) map by fixing vel for (const auto & acc_vec : accel_map_) { - interpolated_acc_vec.push_back(interpolation::lerp(vel_index_, acc_vec, clamed_vel)); + interpolated_acc_vec.push_back(interpolation::lerp(vel_index_, acc_vec, clamped_vel)); } // calculate throttle From 465713e729cda425c0eba414505214ad78c901b4 Mon Sep 17 00:00:00 2001 From: tanaka3 Date: Mon, 17 Oct 2022 15:13:56 +0900 Subject: [PATCH 14/15] style: size_t Signed-off-by: tanaka3 --- vehicle/raw_vehicle_cmd_converter/src/csv_loader.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/vehicle/raw_vehicle_cmd_converter/src/csv_loader.cpp b/vehicle/raw_vehicle_cmd_converter/src/csv_loader.cpp index 18a69174940cf..30592c23e0f25 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/csv_loader.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/csv_loader.cpp @@ -91,7 +91,7 @@ std::vector CSVLoader::getRowIndex(const Table & table) std::vector CSVLoader::getColumnIndex(const Table & table) { std::vector index = {}; - for (unsigned int i = 1; i < table.size(); i++) { + for (size_t i = 1; i < table.size(); i++) { index.push_back(std::stod(table[i][0])); } return index; From f2eec0051ce45080cebf57583c6adcf2914d311a Mon Sep 17 00:00:00 2001 From: tanaka3 Date: Mon, 17 Oct 2022 16:25:11 +0900 Subject: [PATCH 15/15] style: pre-commit Signed-off-by: tanaka3 --- .../include/raw_vehicle_cmd_converter/steer_map.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/steer_map.hpp b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/steer_map.hpp index 767689146fdf8..5bdfdef639d20 100644 --- a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/steer_map.hpp +++ b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/steer_map.hpp @@ -36,8 +36,7 @@ class SteerMap std::vector steer_index_; std::vector output_index_; std::vector> steer_map_; - rclcpp::Logger logger_{ - rclcpp::get_logger("raw_vehicle_cmd_converter").get_child("steer_map")}; + rclcpp::Logger logger_{rclcpp::get_logger("raw_vehicle_cmd_converter").get_child("steer_map")}; }; } // namespace raw_vehicle_cmd_converter