Arc 78 trajectory follower integration #26
Conversation
…ub.com/usdot-fhwa-stol/autoware.auto into arc-78-trajectory-follower-integration
…ub.com/usdot-fhwa-stol/autoware.auto into arc-78-trajectory-follower-integration
There was a problem hiding this comment.
Looks like 2 nodes added were from autoware, so I really didn't look at them.
Please let me know if there are specific lines that you added that need review.
It looks like we may be missing last commit because debug prints are still here, so I can look at it after they are removed.
| @@ -329,17 +329,21 @@ int64_t calcNearestIndex( | |||
| if (traj.points.empty()) { | |||
| return -1; | |||
| } | |||
| const float64_t my_yaw = tf2::getYaw(self_pose.orientation); | |||
| const float64_t my_yaw = 0.0; //tf2::getYaw(self_pose.orientation); | |||
There was a problem hiding this comment.
Is this correct? or did we forget to push commit.
If it is actually correct, let's explain why it needs to be this way.
| int64_t nearest_idx = -1; | ||
| float64_t min_dist_squared = std::numeric_limits<float64_t>::max(); | ||
| for (size_t i = 0; i < traj.points.size(); ++i) { | ||
| const float64_t dx = self_pose.position.x - static_cast<float64_t>(traj.points.at(i).x); | ||
| const float64_t dy = self_pose.position.y - static_cast<float64_t>(traj.points.at(i).y); | ||
| const float64_t dist_squared = dx * dx + dy * dy; | ||
| std::cerr<< "dist_squared: "<<dist_squared<<std::endl; |
There was a problem hiding this comment.
after the integration is done, it is probably better to remove these cerr prints, because I imagine they will be printing at 30hz which is too much.
| @@ -163,6 +163,12 @@ LateralController::LateralController(const rclcpp::NodeOptions & node_options) | |||
| // TODO(Frederik.Beaujean) ctor is too long, should factor out parameter declarations | |||
| declareMPCparameters(); | |||
|
|
|||
| rcutils_ret_t ret = rcutils_logging_set_logger_level("trajectory_follower", RCUTILS_LOG_SEVERITY_DEBUG); | |||
There was a problem hiding this comment.
debug prints should be removed
I only added one node (longitudinal_controller), there rest already existed. Also haven't made any logic changes, just adding some debug logs which were left and removed now. You can review again. |
| rcutils_ret_t ret = rcutils_logging_set_logger_level("trajectory_follower", RCUTILS_LOG_SEVERITY_DEBUG); | ||
| if (ret != RCUTILS_RET_OK) { | ||
| RCLCPP_ERROR(get_logger(), "Could not set log level for traj follower lib"); | ||
|
|
||
| } |
There was a problem hiding this comment.
we should remove this debugging because it is forcing "trajectory_follower" logger to be DEBUG.
There was a problem hiding this comment.
Looks good to me. CI is failing due to not enough memory available.
soedinglab/hh-suite#280
We can probably merge this.
PR Details
Description
This PR is part of the integration testing of the Trajectory Follower Wrapper on a physical vehicle.
Related PR:
usdot-fhwa-stol/carma-platform#2419
Related GitHub Issue
Related Jira Key
https://usdot-carma.atlassian.net/browse/ARC-78
Motivation and Context
Integrate the Trajectory Follower controller of Autoware, that has a MPC based lateral controller, to improve vehicle's performance.
How Has This Been Tested?
Integration tested on Blue Lexus
Types of changes
Checklist: