Conversation
|
@The-Sole-King please resolve merge conflict before review. |
alyashour
left a comment
alyashour
left a comment
There was a problem hiding this comment.
Hi @The-Sole-King really good work! Need some changes pls.
| // send speed msg to ctrl | ||
| speed_profile_pub_->publish(create_speed_profile()); | ||
| // 1. Create route | ||
| auto route = create_route(); |
There was a problem hiding this comment.
This code creates the route twice, this is inefficient. Create a ref to the route instead when it's created and pass that into create_speed_profile.
| // 3. Publish current speed limit (for UI) | ||
| // Use the first waypoint's computed limit. | ||
| std_msgs::msg::Float64 limit_msg; | ||
| if (!speed_profile.speeds.empty()) { |
There was a problem hiding this comment.
Instead of combining the metrics into 1 and publishing every frame add a service that responds with:
- hardware speed limit (hard coded config)
- sector speed limit (straight ahead speed limit of the current sector. Sectors aren't implemented yet so just output -1 or infinity or something)
- the lateral and forward max acceleration (also hard coded in config)
While planning might need to calculate the car's lat/lon acceleration, there's no need to publish that info to Console.
| // ===================== SPEED PROFILE CONSTANTS ===================== // | ||
|
|
||
| constexpr double MAX_SPEED_MPS = 6.0; // Global max speed | ||
| constexpr double MAX_LAT_ACC_MPS2 = 1.5; // Lateral accel limit (turning) | ||
| constexpr double MAX_FWD_ACC_MPS2 = 2.0; // Forward accel limit | ||
| constexpr double MIN_TURN_RADIUS = 0.01; // Avoid division by zero | ||
|
|
There was a problem hiding this comment.
Load these values from a config file. This is an example of this from Control: https://github.com/WE-Autopilot/control/blob/6fe7ee518dd046b2ba135196c259269cc74542ed/src/ackermann_controller.cpp#L89-L105
Although apparently there is a way to do it through ROS - might be worth looking into.
| double dy2 = y3 - y2; | ||
|
|
||
| double cross = dx1 * dy2 - dy1 * dx2; | ||
| [[maybe_unused]] double dot = dx1 * dx2 + dy1 * dy2; |
There was a problem hiding this comment.
Why is this kept in if unused?
| // 2. Dynamic turn-based speed profiling | ||
| // ======================================================================= | ||
| double last_speed = this->speed_; // previously commanded speed | ||
| double dt = 0.1; // assume 10 Hz planning loop |
There was a problem hiding this comment.
No need to assume, see PlannerNode.rate_hz
2 participants
Adds MAX_SPEED_MPS, MAX_LAT_ACC_MPS2, forward limit
Adds curvature → turn radius → turn-speed
Clamps speed using both global + lateral constraints
Adds /ap1/planning/current_speed_limit publisher
Fully contained inside planning package
Does not affect other AP1 submodules