Added dynamic Replanning #18
Conversation
|
Thoda drift hai still, I think due to the motion in the idle time when the path is recomputed. |
traj_follower_replan.py
Outdated
| start_drift = 100 | ||
|
|
||
| print("Computing Path ...") | ||
| path_o, vel = get_trajectory(waypoints, gate_length, gate_height, pos0, ts, 4) |
There was a problem hiding this comment.
Replan also considering that initial velocity and acc is not zero
|
|
||
| drift = (pos0[0]-px)**2 + (pos0[1]-py)**2 + (pos0[2]-pz)**2 | ||
| if drift > max(start_drift, 10) and i > 5: | ||
| print("Drift Increasing, Replan!") |
There was a problem hiding this comment.
It might be better to have replan and drift monitoring in a different thread like a watchdog.
There was a problem hiding this comment.
Can you explain more
There was a problem hiding this comment.
Low priority. Basically, monitor drift in a different thread and replan without blocking initial loop kinda.
|
Also, remove unneeded CEM files? |
| # Initial | ||
|
|
||
| waypoints, gate_length, gate_height = get_dummy_waypoints(client) | ||
| waypoints = get_fixed_points.get_points_modif(waypoints) |
There was a problem hiding this comment.
Point need to be recalculated too as the initial position of drone is now different.
There was a problem hiding this comment.
waypoints are getting updated in the loop
|
It doesn't work really well right now. Too many changes needed. |
|
Check how this version works on the simulator and revert. |
|
Drift calculation is a bit weird. It sometime doesn't replan till a lot of drift and sometimes replan with very little drift. Also, why is replanning so explicit? The vehicle stops and then restarts kinda. Need to fix somehow. Maybe when replanning, consider velocity and position sometime in future as initial? A better solution would be to replan in a parallel thread so that movement isn't affected. A temporary solution is to set simpause when replanning. |
|
Adding noise in velocity commands to simulate drift for first 1-2 iters.
(May have to modify threshold drift to suit simulation)