Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

refactor(raw_vehicle_cmd_converter): replace interpolate implementation with common library #418

Merged
merged 1 commit into from
Feb 21, 2022
Merged

refactor(raw_vehicle_cmd_converter): replace interpolate implementation with common library #418

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 0 additions & 1 deletion vehicle/raw_vehicle_cmd_converter/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,6 @@ ament_auto_add_library(actuation_map_converter SHARED
src/accel_map.cpp
src/brake_map.cpp
src/csv_loader.cpp
src/interpolate.cpp
src/pid.cpp
src/steer_converter.cpp
)
Expand Down
1 change: 0 additions & 1 deletion vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/accel_map.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,6 @@
#define RAW_VEHICLE_CMD_CONVERTER__ACCEL_MAP_HPP_

#include "raw_vehicle_cmd_converter/csv_loader.hpp"
#include "raw_vehicle_cmd_converter/interpolate.hpp"

#include <rclcpp/rclcpp.hpp>

Expand Down
1 change: 0 additions & 1 deletion vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/brake_map.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,6 @@
#define RAW_VEHICLE_CMD_CONVERTER__BRAKE_MAP_HPP_

#include "raw_vehicle_cmd_converter/csv_loader.hpp"
#include "raw_vehicle_cmd_converter/interpolate.hpp"

#include <rclcpp/rclcpp.hpp>

Expand Down
33 changes: 0 additions & 33 deletions vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/interpolate.hpp
View file
Open in desktop

This file was deleted.

1 change: 0 additions & 1 deletion vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/steer_converter.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,6 @@
#define RAW_VEHICLE_CMD_CONVERTER__STEER_CONVERTER_HPP_

#include "raw_vehicle_cmd_converter/csv_loader.hpp"
#include "raw_vehicle_cmd_converter/interpolate.hpp"
#include "raw_vehicle_cmd_converter/pid.hpp"

#include <rclcpp/rclcpp.hpp>
Expand Down
1 change: 1 addition & 0 deletions vehicle/raw_vehicle_cmd_converter/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
<depend>autoware_auto_control_msgs</depend>
<depend>autoware_auto_vehicle_msgs</depend>
<depend>geometry_msgs</depend>
<depend>interpolation</depend>
<depend>nav_msgs</depend>
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
Expand Down
16 changes: 6 additions & 10 deletions vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,8 @@

#include "raw_vehicle_cmd_converter/accel_map.hpp"

#include "interpolation/linear_interpolation.hpp"

#include <algorithm>
#include <chrono>
#include <string>
Expand Down Expand Up @@ -62,7 +64,6 @@ bool AccelMap::readAccelMapFromCSV(std::string csv_path)

bool AccelMap::getThrottle(double acc, double vel, double & throttle)
{
LinearInterpolate linear_interp;
std::vector<double> accs_interpolated;

if (vel < vel_index_.front()) {
Expand All @@ -83,9 +84,7 @@ bool AccelMap::getThrottle(double acc, double vel, double & throttle)

// (throttle, vel, acc) map => (throttle, acc) map by fixing vel
for (std::vector<double> accs : accel_map_) {
double acc_interpolated;
linear_interp.interpolate(vel_index_, accs, vel, acc_interpolated);
accs_interpolated.push_back(acc_interpolated);
accs_interpolated.push_back(interpolation::lerp(vel_index_, accs, vel));
}

// calculate throttle
Expand All @@ -97,14 +96,13 @@ bool AccelMap::getThrottle(double acc, double vel, double & throttle)
throttle = throttle_index_.back();
return true;
}
linear_interp.interpolate(accs_interpolated, throttle_index_, acc, throttle);
throttle = interpolation::lerp(accs_interpolated, throttle_index_, acc);

return true;
}

bool AccelMap::getAcceleration(double throttle, double vel, double & acc)
{
LinearInterpolate linear_interp;
std::vector<double> accs_interpolated;

if (vel < vel_index_.front()) {
Expand All @@ -125,9 +123,7 @@ bool AccelMap::getAcceleration(double throttle, double vel, double & acc)

// (throttle, vel, acc) map => (throttle, acc) map by fixing vel
for (std::vector<double> accs : accel_map_) {
double acc_interpolated;
linear_interp.interpolate(vel_index_, accs, vel, acc_interpolated);
accs_interpolated.push_back(acc_interpolated);
accs_interpolated.push_back(interpolation::lerp(vel_index_, accs, vel));
}

// calculate throttle
Expand All @@ -141,7 +137,7 @@ bool AccelMap::getAcceleration(double throttle, double vel, double & acc)
throttle = std::min(std::max(throttle, min_throttle), max_throttle);
}

linear_interp.interpolate(throttle_index_, accs_interpolated, throttle, acc);
acc = interpolation::lerp(throttle_index_, accs_interpolated, throttle);

return true;
}
Expand Down
16 changes: 6 additions & 10 deletions vehicle/raw_vehicle_cmd_converter/src/brake_map.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,8 @@

#include "raw_vehicle_cmd_converter/brake_map.hpp"

#include "interpolation/linear_interpolation.hpp"

#include <algorithm>
#include <string>
#include <vector>
Expand Down Expand Up @@ -63,7 +65,6 @@ bool BrakeMap::readBrakeMapFromCSV(std::string csv_path)

bool BrakeMap::getBrake(double acc, double vel, double & brake)
{
LinearInterpolate linear_interp;
std::vector<double> accs_interpolated;

if (vel < vel_index_.front()) {
Expand All @@ -84,9 +85,7 @@ bool BrakeMap::getBrake(double acc, double vel, double & brake)

// (throttle, vel, acc) map => (throttle, acc) map by fixing vel
for (std::vector<double> accs : brake_map_) {
double acc_interpolated;
linear_interp.interpolate(vel_index_, accs, vel, acc_interpolated);
accs_interpolated.push_back(acc_interpolated);
accs_interpolated.push_back(interpolation::lerp(vel_index_, accs, vel));
}

// calculate brake
Expand All @@ -106,14 +105,13 @@ bool BrakeMap::getBrake(double acc, double vel, double & brake)
}

std::reverse(std::begin(accs_interpolated), std::end(accs_interpolated));
linear_interp.interpolate(accs_interpolated, brake_index_rev_, acc, brake);
brake = interpolation::lerp(accs_interpolated, brake_index_rev_, acc);

return true;
}

bool BrakeMap::getAcceleration(double brake, double vel, double & acc)
{
LinearInterpolate linear_interp;
std::vector<double> accs_interpolated;

if (vel < vel_index_.front()) {
Expand All @@ -134,9 +132,7 @@ bool BrakeMap::getAcceleration(double brake, double vel, double & acc)

// (throttle, vel, acc) map => (throttle, acc) map by fixing vel
for (std::vector<double> accs : brake_map_) {
double acc_interpolated;
linear_interp.interpolate(vel_index_, accs, vel, acc_interpolated);
accs_interpolated.push_back(acc_interpolated);
accs_interpolated.push_back(interpolation::lerp(vel_index_, accs, vel));
}

// calculate brake
Expand All @@ -150,7 +146,7 @@ bool BrakeMap::getAcceleration(double brake, double vel, double & acc)
brake = std::min(std::max(brake, min_brake), max_brake);
}

linear_interp.interpolate(brake_index_, accs_interpolated, brake, acc);
acc = interpolation::lerp(brake_index_, accs_interpolated, brake);

return true;
}
Expand Down
102 changes: 0 additions & 102 deletions vehicle/raw_vehicle_cmd_converter/src/interpolate.cpp
View file
Open in desktop

This file was deleted.

11 changes: 5 additions & 6 deletions vehicle/raw_vehicle_cmd_converter/src/steer_converter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,8 @@

#include "raw_vehicle_cmd_converter/steer_converter.hpp"

#include "interpolation/linear_interpolation.hpp"

#include <string>
#include <vector>

Expand Down Expand Up @@ -134,7 +136,6 @@ void SteerConverter::calcFFMap(double steer_vel, double vehicle_vel, double & ou
{
rclcpp::Clock clock{RCL_ROS_TIME};

LinearInterpolate linear_interp;
std::vector<double> steer_angle_velocities_interp;

if (vehicle_vel < vel_index_.front()) {
Expand All @@ -154,16 +155,14 @@ void SteerConverter::calcFFMap(double steer_vel, double vehicle_vel, double & ou
}

for (std::vector<double> steer_angle_velocities : steer_map_) {
double steer_angle_vel_interp;
linear_interp.interpolate(
vel_index_, steer_angle_velocities, vehicle_vel, steer_angle_vel_interp);
steer_angle_velocities_interp.push_back(steer_angle_vel_interp);
steer_angle_velocities_interp.push_back(
interpolation::lerp(vel_index_, steer_angle_velocities, vehicle_vel));
}
if (steer_vel < steer_angle_velocities_interp.front()) {
steer_vel = steer_angle_velocities_interp.front();
} else if (steer_angle_velocities_interp.back() < steer_vel) {
steer_vel = steer_angle_velocities_interp.back();
}
linear_interp.interpolate(steer_angle_velocities_interp, output_index_, steer_vel, output);
output = interpolation::lerp(steer_angle_velocities_interp, output_index_, steer_vel);
}
} // namespace raw_vehicle_cmd_converter