Mission Planning: way_planner and freespace_planner cannot be Deformed by Local Planners to Avoid Collision

[marcusvinicius178]

Bug report

Required information:

• Operating system and version:
  Ubuntu 20.04.2 LTS
• Autoware installation type:

-Installed From Docker: Case 3: Creating a Custom Autoware Docker Container: https://github.com/Autoware-AI/autoware.ai/wiki/Generic-x86-Docker
Steps:

$ git clone https://github.com/Autoware-AI/docker.git $ cd docker/generic $ ./run.sh -t local

Description of the bug

Tested countless time the way_planner global planner or mission planning: https://github.com/Autoware-AI/core_planning/tree/master/way_planner/launch

Afterwards I tried to use the Astar Avoid Local Planner to calculate the safety waypoints and final waypoints, recalculating the closest waypoint most suitable for the Pure Pursuit control follow it.
I have also tested other Local Planner: The Lattice.

Both of them fails to distort the way_planner global planner,in order to get this global planner deviating from Obstacle.

Another interesting point that caught my attention was the fact that both local planners: Astar and Lattice fails to send the vehicle trajectory. It is always needed to launch the Dp planner along with them to make them work:https://github.com/Autoware-AI/core_planning/tree/master/dp_planner

I have also tried the freespace_planner as mission_planner/global planner. This planner at least calculates a trajectory that "pseudoavoids" the obstacle. In fact if the obstacle is near the trajectory this planner also is not recalculated by Astar or Lattice local planners....

I imagine there is a bug that prevents the local planners distort the global planners, or that prevents them to change the closest waypoint and safety waypoints for the pure pursuit follows.

Steps to reproduce the bug

A) First Case: way_planner + waypoint_planner (astar+velocity_set) + dp_planner

1 - Launch the Runtime Manager

2- On Quick Start Tab Launch Map, Sensing and Localization modules from CubeTown map: https://github.com/lgsvl/autoware-data/tree/master/CubeTown/my_launch

3- Launch the Mission Planning/Global Planner from the terminal(docker):

A) way_planner (if launch this, you need to change the arg map_source with vim. Remove default = "1" and set value = "0" and then build the package:
`
colcon build

source install/setup.bash`:

Then launch it:
roslaunch way_planner way_planner.launch

OR

B)freespace_planner
roslaunch freespace_planner freespace_planner.launch

4- Activate the ray_ground filter on sensing Tab (Runtime_Manager)

5- Activate costmap_generator on Computing Tab with Lanelet2 box option checked otherwise you will face this issue: https://github.com/Autoware-AI/autoware.ai/issues/2379

6-Activate on Computing Tab the astar_avoid(with Enable_Avoidance check box checked) and velocity_set in waypoint_planner (motion_planning)

7- In Rviz send the Goal: With Nav_Goal Icon: Now the way_planner or freespace planner will be drawed

8- Launch the my_mission_planning, that in fact act as a behavior planner? (DP_Planner)
I have changed my_mission_planning.launch (https://github.com/lgsvl/autoware-data/blob/master/CubeTown/my_launch/my_mission_planning.launch) to the code below:

<launch>
  <arg name="map_source" default="0" /> <!-- Autoware=0, Vector Map Folder=1, kml file=2 -->
  
  <!-- dp_planner -->
  <include file="$(find dp_planner)/launch/dp_planner.launch">
    <arg name="mapSource" value="$(arg map_source)"/>
  </include>

</launch>

8- In SVL simulator change gear to reverse, pressing PageDown, To avoid the vehicle moves and crash. This is good to do to debug with RQT-Instrospection-Topics/Nodes Active

9-Launch my_motion_planning. I have changed this file code: https://github.com/lgsvl/autoware-data/blob/master/CubeTown/my_launch/my_motion_planning.launch to:

<launch>

  <!-- Vehicle Contorl -->
  <include file="$(find runtime_manager)/launch_files/vehicle_socket.launch"/>

  <!-- pure_pursuit -->
  <!-- Copied $(find pure_pursuit)/launch/pure_pursuit.launch to use respawn attribute. -->
  <arg name="is_linear_interpolation" default="True"/>
  <arg name="publishes_for_steering_robot" default="False"/>
  <arg name="add_virtual_end_waypoints" default="False"/>
  <arg name="const_lookahead_distance" default="4.0"/>
  <arg name="const_velocity" default="5.0"/>
  <arg name="lookahead_ratio" default="2.0"/>
  <arg name="minimum_lookahead_distance" default="6.0"/>

  <!-- 0 = waypoints, 1 = provided constant velocity -->
  <arg name="velocity_source" default="1"/>

  <!-- rosrun waypoint_follower pure_pursuit -->
  <node pkg="pure_pursuit" type="pure_pursuit" name="pure_pursuit" output="log" respawn="true" respawn_delay="10">
    <param name="is_linear_interpolation" value="$(arg is_linear_interpolation)"/>
    <param name="publishes_for_steering_robot" value="$(arg publishes_for_steering_robot)"/>
    <param name="add_virtual_end_waypoints" value="$(arg add_virtual_end_waypoints)"/>
    <param name="const_lookahead_distance" value="$(arg const_lookahead_distance)"/>
    <param name="const_velocity" value="$(arg const_velocity)"/>
    <param name="lookahead_ratio" value="$(arg lookahead_ratio)"/>
    <param name="minimum_lookahead_distance" value="$(arg minimum_lookahead_distance)"/>
    <param name="velocity_source" value="$(arg velocity_source)"/>
  </node>

  <!-- twist_filter -->
  <include file="$(find twist_filter)/launch/twist_filter.launch"/>

</launch>

10- Now after take a look into RQT, just click Page UP to switch on the gear forward again on SVL simulator.

Expected behavior

I expected the way_planner or Freespace planner to be deformed or at least the vehicle follow other waypoints, such as those supposed to be generated by waypoint_planner local planner (A*+velocity_set) or by Lattice local planner.

Actually I would like to see the final waypoints or some produced marker by velocity_set node*
or
see the output of Lattice planner: cubic_spline_viz topic data on marker plugin in rviz, which is never displayed too (just on RQT and rostopic echo it is possible see this data flow exist!)

Actual behavior

As I said the local planners (A* or Lattice) calculates the safety_waypoints send to the velocity_set that calculates the final_waypoints. However these waypoints are not used to distort the global planner and therefore the car follows the "old global planner waypoints" and crashes!
It is very curious for me to understand why these local planners just work with support of dp_planner. I thought that dp_planner was just another local planner, but it is always required to be used together with other local planners, otherwise the pure_pursuit node never receives the local trajectories (waypoints) calculated by local planners. And I am sending attached the rosgraph of this fact (local planners launched without and with the dp_planner)

Screenshots

I will send here the ROSGRAPHS from RQT:

CASE 1: way_planner global planner + A* local Planner:

CASE 2: way_planner global planner + A* local Planner + Dp_Planner:

CASE 3: freespace_planner global planner + A* local Planner:

CASE 4: freespace_planner global planner + A* local Planner + Dp_Planner:

Additional information

Previously my issue for not get local planner A* working was the costmap generation ( this was fixed launching the costmap_generator_lanelet2):
roslaunch costmap_generator costmap_generator_lanelet2.launch
The tutorials to launch the "raw" costmap_generator "raw" are wrong, and must be updated

Besides that I was using in docker the Autoware.Ai 1.14.0 release, which contains the deprecated, old and wrong A* code. I have checked the AutowareAI team fixedc this, but updated the A code just for the master*: autowarefoundation/autoware_ai_planning@d8dc678

Even with the A* fixed, it keeps failing to avoid collision (at least using the SVL simulator)

It is not a problem of system performance. I am running the Stack/Simulator in a:

64 GB RAM
16 CPUs Core High freq clock
P6000x2 NVIDIA Dedicated

Also runned with a notebook (Autoware) + LG (Desktop)

In both cases Autoware does not achieve the 70% of CPU consumption in each core and arrives below the 8 GB use of RAM...

###VIDEOS FROM ROSGRAPH CASES:

WAY_PLANNER + ASTAR
https://youtu.be/qKN0Pjl5CLQ

FREESPACE_PLANNER + LATTICE/ASTAR
https://youtu.be/3sR6dZUWyHc

I have more specific videos and info if needed.
I think from these videos is possible get the idea

Finally I wish to ask if some AutowareAI user has successfully used the local planners with SVL simulator at least one time?
Do these planners work with SVL? I have checked other videos that they seemed to work with internal simulator or kind of...

I will be grateful for some feedback of Autoware experts!

Thanks!