Feature Request: Cost-Regulated Angular Velocity Scaling for Orchard Navigation

[ra13o]

Hello,

First of all, thank you very much for developing such an effective and robust controller.

I am currently using the controller for an orchard navigation use case and greatly appreciate its functionality. However, I wanted to ask if it would be appropriate to implement a small enhancement to include a "cost_regulated_angular_velocity_scaling" feature.

The idea is to ensure that the vehicle can maintain a safe distance, for instance, between rows of trees in an orchard while dynamically offsetting itself from the planned path when this feature is enabled (set to "true").

In scenarios where the planned path comes too close to tree rows, a mechanism similar to the existing "cost_regulated_linear_velocity_scaling" could help adjust angular velocity accordingly, enhancing both safety and path-following performance.

Would this feature be a welcome contribution, or do you have any suggestions or guidelines for implementing this functionality?

Thank you again for your time and for maintaining this amazing project.

[ggupta9777]

@ra13o The angular velocity automatically gets scaled as a result of this line. is there something else that you're referring to?

[ra13o]

Hello @ggupta9777

Inside the function VectorPursuitController::applyConstraints, the linear velocity is constrained by min_distance_to_obstacle, which is derived from the cost at the robot’s current pose. This effectively scales down the linear velocity if an obstacle is within a roughly 1m circle around the robot—even when the obstacle is lateral (on the left or right) and not directly blocking the forward path.

I was expecting that if an obstacle is only to the side, the controller could allow higher forward velocity and possibly nudge the robot around (assuming collision checking prevents actually hitting the obstacle). Instead, the code reduces the linear speed as soon as a non-free cost is seen, regardless of obstacle position, and sometimes of Collision is Imminent the Robt stops, which it can avoid by nudging to the free lateral space around the obstacle. This may lead to overly conservative motion in narrow corridors when there is sufficient lateral clearance or the path is by mistake planned nearer to one side but the expected result is robot staying at the Center of the corridors.

I hope this clarifies my concern. Please let me know if further explanation or details are needed.

[exMachina316]

Hey @ra13o!!

Because of the idea of delegation of tasks, the responsibility of obstacle avoidance is given to the global planner and I feel is outside the scope of this project. Ideally you should be able to get decent results with a combination of the costmap inflation and path planner.

[ra13o]

@exMachina316
I completely agree with your approach, but for larger use cases like mine, such as navigating large orchards, I would prefer to follow a fixed global path while allowing the controller to handle smaller discrepancies dynamically, similar to how TEB or DWB operates. However, the drawback with those controllers lies in their compatibility with Ackermann steering vehicles.

I understand that the controller you’ve developed is designed to be generic, whereas my requirements are more specific to orchard or plantation use cases. If it’s not an issue, I’d like to use your controller as a base and customize it to better suit these scenarios. Specifically, I aim to incorporate costmaps to influence lateral adjustments to velocity, enabling the scaling of angular or lateral velocity as a configurable parameter that can be toggled on or off, with the gain also parameterized for finer control.