80 crptg1prioc stanley and pp implementation #82
Conversation
| find_package(autoware_planning_msgs REQUIRED) | ||
| find_package(autoware_vehicle_msgs REQUIRED) | ||
| find_package(nav_msgs REQUIRED) | ||
| find_package(pacmod3_msgs REQUIRED) |
There was a problem hiding this comment.
why do we need pcamod3 at this stage?
| namespace apl | ||
| { | ||
| struct ControlInput{ | ||
| std::vector<double> path_x; |
There was a problem hiding this comment.
please use our way of noting m_ for member variables
| std::vector<double> path_x; | |
| std::vector<double> m_path_x; |
There was a problem hiding this comment.
I am not sure if this struct was never defined before... we are in crp::apl namespace, where ControlInputs/Output struct types may have been defined already... wouldn't this cause any troubles?
btw, for all controllers the same base class definition shall be done as we did for planners, right @AnonymDavid ?
There was a problem hiding this comment.
For controllers we don't have any interfaces defined yet. But yes it should have a base class and interfaces defined in crp_APL/interfaces. Should this be another task since the other controllers are missing these too?
There was a problem hiding this comment.
Ye, make it another task, pls @gfigneczi1
| }; | ||
| } // namespace apl | ||
| } | ||
| #endif // CRP_APL_CTRL_VEHICLE_LAT_COMPENSATORY_HPP |
There was a problem hiding this comment.
formality, but still...
| #endif // CRP_APL_CTRL_VEHICLE_LAT_COMPENSATORY_HPP | |
| #endif // CRP_APL_CTRL_VEHICLE_LAT_PURE_P_HPP |
| // calculate the steering angle | ||
| m_output.steeringAngleTarget = 0.0f; | ||
|
|
||
| float alpha = atan2(m_input.path_y[target_index], m_input.path_x[target_index]); |
There was a problem hiding this comment.
target_index shall be checked, if greater than zero (if trajectory is too small, or the look ahead distance is too high, than it remains 0 per line 60... probably it shall be set to last point by default? supposing the path contains any points
|
|
||
| float alpha = atan2(m_input.path_y[target_index], m_input.path_x[target_index]); | ||
|
|
||
| m_output.steeringAngleTarget = atan2(2.0f * m_params.wheelbase * sin(alpha) / distance_to_index, 1); |
|
|
||
| // control algorithm | ||
| if(m_input.path_x.size() > 0 && m_input.path_y.size() > 0) | ||
| pure_p_control(); |
There was a problem hiding this comment.
else zero steering angle target? otherwise system keeps the last value which is not the safe behavior
There was a problem hiding this comment.
else i would just drop the msg (not publish anything), for ex. when the steering wheel is at 20 deg and all of a sudden comes back 0, is not the best behavior in my op...
| { | ||
| m_input.path_x.clear(); | ||
| m_input.path_y.clear(); | ||
| m_input.path_theta.clear(); |
There was a problem hiding this comment.
not realy needed in the controller
There was a problem hiding this comment.
For controllers we don't have any interfaces defined yet. But yes it should have a base class and interfaces defined in crp_APL/interfaces. Should this be another task since the other controllers are missing these too?
|
|
||
| float alpha = atan2(m_input.path_y[target_index], m_input.path_x[target_index]); | ||
|
|
||
| m_output.steeringAngleTarget = atan2(2.0f * m_params.wheelbase * sin(alpha) / distance_to_index, 1); |
There was a problem hiding this comment.
If ego speed (m_input.vxEgo) is 0 than distance_to_index would be 0 too and this would give a zero division error, no? In that case there should be a check for that.
| executable="ctrl_vehicle_control_lat_stanley", | ||
| parameters=[ | ||
| {"/ctrl/k_gain": 0.5}, | ||
| {"/ctrl/wheelbase": 2.7}, |
There was a problem hiding this comment.
This should be a parameter. Probably in an other task we should sync vehicle params as global (as in crp repo) parameters in every package.
There was a problem hiding this comment.
yes should make another task for it
|
@AnonymDavid and @gfigneczi1 resolved most of the issues |
Add Pure Pursuit and Stanley Controller