[Feature] Improve Hybrid A* planner

[aohsato]

Status

DEVELOPMENT

Description

I merged logics in astar_navi and obstacle_avoid and created astar_search library package.
I fixed some problems on Hybrid A* algorithm in present Autoware, like the coordinate system, performance, and so on.
Finally, I re-created astar_navi node for free space planning and astar_avoid node for avoiding obstacles on waypoints with the internal state transition.

README

• Freespace planner
  https://github.com/CPFL/Autoware/blob/feature/improve_astar_planner/ros/src/computing/planning/mission/packages/freespace_planner/README.md

• Waypoint planner (old name: Astar planner)
  https://github.com/CPFL/Autoware/blob/feature/improve_astar_planner/ros/src/computing/planning/motion/packages/waypoint_planner/README.md

Related branch

depended on feature/rebuild_decision_maker
-> This PR is independent now

Todos

• Tests
  • astar_search/astar_util (Feature/test astar planner #1753 To be merged)
  • astar_search/astar_search (Feature/test astar planner #1753 To be merged)
  • freespace_planner
  • waypoint_planner/astar_avoid
• Documentation

Steps to Test or Reproduce

Freespace planning

1. Launch Autoware and localization on real vehicle or wf_simulator
2. Launch points2costmap and grid_map_filter
3. Launch astar_navi and publish 2D Nav Goal on Rviz
4. lane_waypoints_array is published and you can see it

Obstacle avoidance

1. Launch Autoware and localization, waypoint_loader and other planners
2. Launch points2costmap and grid_map_filter
3. Launch astar_avoid with Enable Avoidance config
4. Launch velocity_set, pure_pursuit, twist_filter for motion planning
5. When find obstacle and stop (by velocity_set behavior), the car start to plan and avoidance behavior

NOTE: If you want to use wf_simulator instead of real vehicle, please use lidar_fake_perception on sim_base_link frame to simulate pointcloud and you need to publish localizer_pose for velocity_set.

[kennedywai]

Hello Aohsato, thanks for trying to imporve astar_navi! BTW I have a few questions about how this planner works:

1. Which branch should I use to perform the astar planner + obstacle avoidance?

2. Is it not required to use either vector map and waypoint saver methods?

3. Is it the finall output that we set up a goal wherever on the free areas on the grid map and it will publish a global path and a velocity to the robot until it reaches the goal? Just like a normal 2D slam on ROS Navigation stack, we dont need to create a lane on vector map.

4. I tried to follow the instructions above but I could not get it working, the astar path wasn't out on Rviz, is that ok that you could upload your testing video on Youtube and share the link here?

Thank you!

[aohsato]

@kennedywai Thanks for your comments! Can we continue to discuss on my issue post?

[kfunaoka]

@kennedywai Would you create an issue and show details including how to reproduce the problem and where the process dies? I assume any issue can be categorized into environment problem, procedure problem, and/or code problem.

[sgermanserrano]

@kennedywai although the branch works the unit tests are needed to improve Autoware's code base. Let me know if you'd be happy to contribute to adding some unit tests as you have some experience with the modifications on this PR.

[aohsato]

@sgermanserrano Thanks and sorry for my late. About calcDiffOfRadian, I fixed that with fmod in this commit.
1bb1fc7
But actually, it's more simple to handle angle diff between -pi~pi by using atan2(sin(t),cos(t)) technique, so I'll fix that in my next chance. Thanks.

[aohsato]

@kennedywai I fixed process died problem in above changes, thanks for your comment.

[aohsato]

Finally, we already tested on our real car. Plz check. :)
https://www.dropbox.com/s/ista3yrnxc2sbdo/hybrid-astar-avoididance-moricoro-01-x3.mp4?dl=0

[kennedywai]

@aohsato Thank you Aohsato! I had been testing this in Jan on my robot and it worked fine except for the process died problem. I am now currently busy with other things but I will be soon testing the new code on my robot!! Thank you once again!

[aohsato]

@kennedywai Wow sounds great. Hopefully, let me know in Github, Autoware slack, and so on. Thanks too!

[hamkuchan]

Hi, guys, I'm wondering why we use the node "grip_map_filter", because the output topic of "grip_map_filter" goes nowhere. And where comes the topic "/realtime_cost_map" from if I just using 3D Lidar? And I followed the guide above, but no costmap out, can you please show me which nodes and which topic I should use in the latest version 1.11.0@aohsato@sgermanserrano

[gbiggs]

Thanks for using Autoware and for your question. However the issue tracker is for posting confirmed bugs and feature requests.

We ask that you please ask questions at the ROS Answers website following our support guidelines: https://github.com/autowarefoundation/autoware/wiki/Support-guidelines#ros-answers. Please pay particular attention to the information we ask you to provide.