[mppi] color trajectories by cost #5637
Conversation
Color trajectories by costs by adding a red component which is inversely proportional ti the costs of the visualized trajectories (the lower the cost the more red the trajectory) Signed-off-by: Adi Vardi <adi.vardi@enway.ai>
adivardi
force-pushed
the
cost_trajectories
branch
2 times, most recently
from
d8ab5e8 to
c938564
Compare
Signed-off-by: Adi Vardi <adi.vardi@enway.ai>
adivardi
force-pushed
the
cost_trajectories
branch
from
c938564 to
9649454
Compare
There was a problem hiding this comment.
What is the change in the computational cost as a result of this? Can you uncomment https://github.com/ros-navigation/navigation2/blob/main/nav2_mppi_controller/src/controller.cpp#L20 and see what the difference is in runtime of the controller?
Also you have a test relating to this seeming to segfault https://app.circleci.com/pipelines/github/ros-navigation/navigation2/16553/workflows/d301d58c-d10b-463a-aced-020932420f54/jobs/48461/tests
| Eigen::ArrayXXf x; | ||
| Eigen::ArrayXXf y; | ||
| Eigen::ArrayXXf yaws; | ||
| Eigen::ArrayXf costs; |
There was a problem hiding this comment.
You could make this a reference that points to costs_ (just would need a constructor and pass it in). I didn't trace this completely to know if this is neat in terms of where Trajectories is constructed, but it would be preferable so that we didn't have that huge copy
There was a problem hiding this comment.
I'd also accept another solution that doesn't involve copying the costs vector around. Its quite large.
|
I just saw this right as I opened: #5643 |
3 participants
Basic Info
Description of contribution in a few bullet points
Color trajectories by costs by adding a red component which is inversely proportional to the costs of the visualized trajectories (the lower the cost the more red the trajectory)
Description of documentation updates required from your changes
Should this be noted in the documentation?
Description of how this change was tested
Simulation and actual robot. Visually inspect the trajectories and see the color becomes more red/orange for low cost trajectories. The optimal path is chosen from around the low cost trajectories
Future work that may be required in bullet points
For Maintainers:
backport-*.