Avoidng obstacles detected with by cv_tracker using dp_planner and pure_pusuit

[hadiTab]

I'm using trying to follow a global path set by way_planner using dp_planner. There are obstacles along the path that are detected by Yolo and their bounding boxes can be visualized in rviz. However, dp_planner does not adjust it's path and keeps on moving down the center trajectory rollout until it hits the obstacle.

Am I doing something worng?
Are there any intermediate nodes that need to be run?

[hatem-darweesh]

dp_planner uses the "/cloud_clusters" topic published by euclidean cluster , if you can somehow merge both or convert the yolo detection to point cloud it should work.

one idea is to project the yolo detection onto the LIDAR data, then pass the result to euclidean_cluster.
Regards,

[hadiTab]

Thanks for the response. Is there any built-in way to avoid the bounding box detected by cv_tracker? i.e. if I use op_trajectory_generator?

Also, I can visualize the clusters from euclidean_cluster but dp_planner is still not avoiding them. The clusters do tend to flicker a bit though. Could that be the problem? Do I need to use one of the tracker nodes as well? I tried using kf_counter_tracker but every time I launch it rviz crashes.

EDIT: In the tutorial vidoes for OpenPlanner simulations, the local planner will avoid obstacles that are manually placed on the map using the point publishing tool in rviz. Is there any way to get a similar effect outside of simulations with obstacles that are detected by the sensors? Even with the euclidean clustering, dp_planner does not attempt to avoid them for me.

[hatem-darweesh]

we have tested dp_planner , it works fine with sensor data (avoid obstacles).
sure if you play back the data it won't avoid , just change the current selected roll out (the pink line).
can you give details steps of how are you using dp_planner ?

Regards,

[hadiTab]

I found one issue I was having but there still is another one. I had the wrong output frame selected for euclidean cluster so the obstacles detected by dp_planner were being placed at the wrong point on the map. After fixing this dp_planner will avoid obstacles only if the obstacle isn't blocking all of the roll-out trajectories. For obstacles completely blocking the road the planner just seems to select the roll-out with the least cost and proceeds to crash into the obstacle.

1. Is there a way to have it stop if it cannot find a way around the obstacle?

2. Also, is dp_planner the only planner currently able to avoid detected obstacles?
   I haven't been able to get lattice planner to avoid obstacles and op_trajectory_generator seems to only generate trajectories but not evaluate them (at least the version in the master branch).

3. I'm having issues avoiding multiple obstacles with dp_planner. After avoiding the first obstacle it goes back to the center of the lane and won't go back to the side to to avoid the next obstacle even though the highlighted trajectory is the one on the side.

[hatem-darweesh]

Hello,

• Can you tell me what pure_pursuit parameter are you using ?
• for multiple obstacles you can solve the problem by increasing the value of "horizontalSafetyDistance" and "verticalSafetyDistance".

[hadiTab]

Hi Hatem,

For pure_pursuit I'm pretty much using the default parameters:

• Dialog
• Velocity = 5
• Lookahead Distance = 4
• displacement_threshold = 0
• relative_angle_threshold = 0

Changing the value of "horizontalSafetyDistance" and "verticalSafetyDistance" did not fix the problem with multiple obstacles. The obstacles are not close to each other. It just seems like after avoiding the first one dp_planner always selects the middle trajectory even if another one is highlighted in rviz.

[Mor-harry]

@hadiTab Do you use the VectorMap information?

[hadiTab]

@Mor-harry I've built a basic vector map. The vector map works when I use OpenPlanner in simulation mode (i.e. obstacles are avoided and it stops at stop lines).

[hatem-darweesh]

Sorry for the late reply,
for pure_pursuit to work with OpenPlanner properly you need to use Waypoint not Dialog.
also in the simulation mode when you use (wf_simulator) accel_rate should be the same as acceleration/deceleration set for the local planner (dp_planner) .

[hadiTab]

@hatem-darweesh I have no issues using OpenPlanner in simulation mode. I do not want to run it in simulation mode though, I'm trying to actually command the vehicle to move.

When I set pure_pursuit to Waypoint it does not send /twist_cmd at all so the vehicle will not move. It only moves when I use Dialog.

Can you list the nodes I need to have running to be able to use OpenPlanner (not in simulation mode)?

[hatem-darweesh]

In my experience, the only differences between Simulation mode and going live are:
Simulation Mode:
run node (vel_pose_connect) set (Simulation Mode) true
run node (pure_pursuit)
run node (twist_filter)
run node (wf_simulator)

Live Move:
run node (vel_pose_connect) set (Simulation Mode) false
run node (pure_pursuit)
run node (twist_filter)

twist_cmd should be published in both cases because it is published from twist_filter.

[Mor-harry]

@hadiTab
I also encountered your problem, in the real car can not avoid obstacles, the car directly hit the past, there is no obstacle avoidance response.

[Mor-harry]

@hadiTab Hello, what did you modify the Euclidean cluster, then you could avoid obstacles?
I now open all the nodes i need, and export the /cloud_clusters, but the real car still does not avoid obstacles.

[hadiTab]

@Mor-harry I still cannot avoid any obstacles. I haven't modified Euclidean cluster. dp_planner subscribes to the clusters topic, tracks them and calculates a boundary for them which it publishes for visualization in rviz. This part works on my side and I'm able to see the correct boundaries in rviz. I also usually avoids the first obstacle if it's early on in the path as long as it isn't completely blocking the lane. If it is blocking it completely it will just go ahead and hit it.

[hatem-darweesh]

@hadiTab I think this is a bug, I had the same result from other experiments, I will check the logs and try to find exactly why this behavior happens with live sensor data.

[hadiTab]

@hatem-darweesh have you been able to figure out why the problem occurs? Thanks.

[hatem-darweesh]

Hi @hadiTab ,
I tested with rosbag data, and it was working normally (state changes from Forward to Follow).
one thing you can use for debugging:
dp_planner writes the selected local trajectory to a file (home/user/SimuLogs/TrajectoriesLogs)
please check the speed column in the log file.

Regards,