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 fba69efdbc921..3bdb71a11f744 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,7 +29,7 @@ namespace raw_vehicle_cmd_converter class AccelMap { public: - bool readAccelMapFromCSV(const std::string & csv_path); + bool readAccelMapFromCSV(const std::string & csv_path, const bool validation = false); 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_; } 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 8d4d35545254c..6dd5ab94c5d06 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,7 +29,7 @@ namespace raw_vehicle_cmd_converter class BrakeMap { public: - bool readBrakeMapFromCSV(const std::string & csv_path); + bool readBrakeMapFromCSV(const std::string & csv_path, const bool validation = false); 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_; } 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 5bdfdef639d20..7e4a3084d0223 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 @@ -28,7 +28,7 @@ namespace raw_vehicle_cmd_converter class SteerMap { public: - bool readSteerMapFromCSV(const std::string & csv_path); + bool readSteerMapFromCSV(const std::string & csv_path, const bool validation = false); void getSteer(const double steer_rate, const double steer, double & output) const; private: diff --git a/vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp b/vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp index 9ebb4bdbb5df7..42c63b152e4f4 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(const std::string & csv_path) +bool AccelMap::readAccelMapFromCSV(const std::string & csv_path, const bool validation) { CSVLoader csv(csv_path); std::vector> table; @@ -38,7 +38,7 @@ bool AccelMap::readAccelMapFromCSV(const std::string & csv_path) vel_index_ = CSVLoader::getRowIndex(table); throttle_index_ = CSVLoader::getColumnIndex(table); accel_map_ = CSVLoader::getMap(table); - if (!CSVLoader::validateMap(accel_map_, true)) { + if (validation && !CSVLoader::validateMap(accel_map_, true)) { return false; } 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 154a0fc455a0a..68d89474f3ca6 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(const std::string & csv_path) +bool BrakeMap::readBrakeMapFromCSV(const std::string & csv_path, const bool validation) { CSVLoader csv(csv_path); std::vector> table; @@ -36,7 +36,7 @@ bool BrakeMap::readBrakeMapFromCSV(const std::string & csv_path) brake_index_ = CSVLoader::getColumnIndex(table); brake_map_ = CSVLoader::getMap(table); brake_index_rev_ = brake_index_; - if (!CSVLoader::validateMap(brake_map_, false)) { + if (validation && !CSVLoader::validateMap(brake_map_, false)) { return false; } std::reverse(std::begin(brake_index_rev_), std::end(brake_index_rev_)); diff --git a/vehicle/raw_vehicle_cmd_converter/src/node.cpp b/vehicle/raw_vehicle_cmd_converter/src/node.cpp index 007cce0b66cff..fb0bd00794c52 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/node.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/node.cpp @@ -42,17 +42,17 @@ RawVehicleCommandConverterNode::RawVehicleCommandConverterNode( use_steer_ff_ = declare_parameter("use_steer_ff", true); use_steer_fb_ = declare_parameter("use_steer_fb", true); if (convert_accel_cmd_) { - if (!accel_map_.readAccelMapFromCSV(csv_path_accel_map)) { + if (!accel_map_.readAccelMapFromCSV(csv_path_accel_map, true)) { throw std::invalid_argument("Accel map is invalid."); } } if (convert_brake_cmd_) { - if (!brake_map_.readBrakeMapFromCSV(csv_path_brake_map)) { + if (!brake_map_.readBrakeMapFromCSV(csv_path_brake_map, true)) { throw std::invalid_argument("Brake map is invalid."); } } if (convert_steer_cmd_) { - if (!steer_map_.readSteerMapFromCSV(csv_path_steer_map)) { + if (!steer_map_.readSteerMapFromCSV(csv_path_steer_map, true)) { throw std::invalid_argument("Steer map is invalid."); } const auto kp_steer{declare_parameter("steer_pid.kp", 150.0)}; diff --git a/vehicle/raw_vehicle_cmd_converter/src/steer_map.cpp b/vehicle/raw_vehicle_cmd_converter/src/steer_map.cpp index 73a6641dd424d..e050a1beba3ae 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/steer_map.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/steer_map.cpp @@ -22,7 +22,7 @@ namespace raw_vehicle_cmd_converter { -bool SteerMap::readSteerMapFromCSV(const std::string & csv_path) +bool SteerMap::readSteerMapFromCSV(const std::string & csv_path, const bool validation) { CSVLoader csv(csv_path); std::vector> table; @@ -35,7 +35,7 @@ bool SteerMap::readSteerMapFromCSV(const std::string & csv_path) steer_index_ = CSVLoader::getRowIndex(table); output_index_ = CSVLoader::getColumnIndex(table); steer_map_ = CSVLoader::getMap(table); - if (!CSVLoader::validateMap(steer_map_, true)) { + if (validation && !CSVLoader::validateMap(steer_map_, true)) { return false; } return true; 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 3c8eba3072126..9f035b303e1a9 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 @@ -95,9 +95,9 @@ TEST(ConverterTests, LoadExampleMap) const auto data_path = ament_index_cpp::get_package_share_directory("raw_vehicle_cmd_converter") + "/data/default/"; // for invalid path - EXPECT_TRUE(accel_map.readAccelMapFromCSV(data_path + "accel_map.csv")); - EXPECT_TRUE(brake_map.readBrakeMapFromCSV(data_path + "brake_map.csv")); - EXPECT_TRUE(steer_map.readSteerMapFromCSV(data_path + "steer_map.csv")); + EXPECT_TRUE(accel_map.readAccelMapFromCSV(data_path + "accel_map.csv", true)); + EXPECT_TRUE(brake_map.readBrakeMapFromCSV(data_path + "brake_map.csv", true)); + EXPECT_TRUE(steer_map.readSteerMapFromCSV(data_path + "steer_map.csv", true)); } TEST(ConverterTests, LoadValidPath) @@ -112,14 +112,14 @@ TEST(ConverterTests, LoadValidPath) EXPECT_TRUE(loadSteerMapData(steer_map)); // for invalid path - EXPECT_FALSE(accel_map.readAccelMapFromCSV("invalid.csv")); - EXPECT_FALSE(brake_map.readBrakeMapFromCSV("invalid.csv")); - EXPECT_FALSE(steer_map.readSteerMapFromCSV("invalid.csv")); + EXPECT_FALSE(accel_map.readAccelMapFromCSV("invalid.csv", true)); + EXPECT_FALSE(brake_map.readBrakeMapFromCSV("invalid.csv", true)); + EXPECT_FALSE(steer_map.readSteerMapFromCSV("invalid.csv", true)); // for invalid maps - EXPECT_FALSE(accel_map.readAccelMapFromCSV(map_path + "test_1col_map.csv")); - EXPECT_FALSE(accel_map.readAccelMapFromCSV(map_path + "test_inconsistent_rows_map.csv")); - EXPECT_FALSE(accel_map.readAccelMapFromCSV(map_path + "test_not_interpolatable.csv")); + EXPECT_FALSE(accel_map.readAccelMapFromCSV(map_path + "test_1col_map.csv", true)); + EXPECT_FALSE(accel_map.readAccelMapFromCSV(map_path + "test_inconsistent_rows_map.csv", true)); + EXPECT_FALSE(accel_map.readAccelMapFromCSV(map_path + "test_not_interpolatable.csv", true)); } TEST(ConverterTests, AccelMapCalculation)