Drone movement is too aggressive

[romaster93]

hello .

I am implementing drone avoidance using ego planner v2 and ardupilot and mavros controller.

Everything is excellent! I admire your research.

However, when the obstacle disappears from the drone's field of view, it changes the yaw direction of the drone too abruptly. Also, it twists too much on narrow roads.

Can I make a smoother path if I fix anything?

Please refer to my video.

https://youtu.be/mMYVuqe9s_o

https://youtu.be/c3ECgOdWexE

[bigsuperZZZX]

I noticed that in your first video the drone failed to map the nearby obstacles when approaching the wall before crashing.
It is abnormal.

[romaster93]

@bigsuperZZZX
Thank you for answer.
I think you're right.
However,
Often parts of the map through the depth filter seem to be lost. The data sent from the depth camera looks normal, but not on the grid map. Could you possibly improve it by modifying some parameters?

+)
And I'm setting up to fly safely while modifying the inflation and clearance values, what are the units for these?

ex) The obstacle_clearance value is set to 5.0. Is this flying 5 meters away?

[bigsuperZZZX]

the unit of inflation and clearance is the meter.
Do not set obstacle_clearance to such a large number. It should be about dozens of centimeters.

[romaster93]

@bigsuperZZZX

Doesn't the obstacle_inflation value inflate the size of the actual obstacle? Is the metric unit correct? For example, if inflation is 1.5, what will be the size of the obstacle?

[bigsuperZZZX]

The size of the real inflation is "RESOLUTION * ceil(INFLATION / RESOLUTION )"

[shubham-shahh]

Hi @bigsuperZZZX, what is the obstacle_clearance parameter responsible for?

[bigsuperZZZX]

A safe space that the planner will try to keep.

[shubham-shahh]

Hi @bigsuperZZZX,
What's the difference between obstacle_clearance and obstacle_clearance_soft parameters?

[bigsuperZZZX]

obstacle_clearance is more strict than obstacle_clearance_soft.

[shubham-shahh]

obstacle_clearance is more strict than obstacle_clearance_soft.

Hi,
thanks for the response, but if the mentioned obstacle clearance is not met, the final_cost rises, but since we don't have an upper bound of max cost of obstacle clearance, we execute the path, how can we make sure, that path that does not satisfy the constraints is not executed?

I was trying to add a simple check here
if (finelyCheckAndSetConstraintPoints(segments_nouse, jerkOpt_, false) == CHK_RET::OBS_FREE && final_cost<200)

but after adding this check, checkCollisioncallback is not working as expected, can you try recreating this and help me?
all your help is appreciated. if you need any more info to recreate it, I can tell you

thanks again

[bigsuperZZZX]

To be honest, I have no idea why adding this cost limit will interfere with checkCollisioncallback.

[shubham-shahh]

To be honest, I have no idea why adding this cost limit will interfere with checkCollisioncallback.

yes, you are right, even though I am not able to understand what's wrong, the problem I see is, that if a path is planned through an obstacle in the begnning, and if replan function is not able to generate a path with low cost, checkcollisioncallback is not able to detect the trajectory points in the collision and collides with the obstacle. can you please try recreating it? in some time, I'll also share a video here

thanks

[shubham-shahh]

To be honest, I have no idea why adding this cost limit will interfere with checkCollisioncallback.

also, according to you, is this the right approach to keep an upper bound on the cost?

[bigsuperZZZX]

Yes

[shubham-shahh]

Hi @bigsuperZZZX, as mentioned above, I am attaching a video demonstrating the problem mentioned above.
as seen in the video, the drone moves into the obstacle, and checkCollisionCallback was not able to call the emergency stop based on the emergency time check, I also checked that the checkcollisioncallback is called at the correct rate.

I have only modified the below check
if (finelyCheckAndSetConstraintPoints(segments_nouse, jerkOpt_, false) == CHK_RET::OBS_FREE && final_cost<200)

cost_function_test-2023-01-31_19.18.09.1.mp4

[bigsuperZZZX]

Try to set a larger final_cost threshold.

[shubham-shahh]

Try to set a larger final_cost threshold.

okay, I can do that, but I am willing to understand why this is happening, why checkcollisioncallback is not able to detect points in the collision. are you able to recreate the problem?

thanks

[shubham-shahh]

to isolate the problem, I tried to recreate the issue in the original implementation of main_ws. and I am seeing similar results
as seen in the image below, the drone made a path through an obstacle and passed through it

[bigsuperZZZX]

I think that the planner failed to plan a new trajectory when the current one ran into an obstacle because the cost of the newly planned trajectory exceeded your cost limit.
However, the emergency stop should have triggered, I don't know why it didn't work.

[shubham-shahh]

I think that the planner failed to plan a new trajectory when the current one ran into an obstacle because the cost of the newly planned trajectory exceeded your cost limit. However, the emergency stop should have triggered, I don't know why it didn't work.

yes, you are right, since there is a cost check, it's unable to find a low-cost path, but the collided points should be able to trigger the emergency stop. I am looking into the issue, and so far I saw that the points in the collision are not able to set the dangerous flag in the checkcollisioncallback, which is why there is no trigger. but I don't see any reason why this could happen, I am trying to find the cause, your efforts and help are much appreciated, please look into this issue if you can

thanks

[shubham-shahh]

Hi @bigsuperZZZX, any update on this?

[shubham-shahh]

Hi @bigsuperZZZX, earlier I thought the problem is caused because of the resolution, but I think that's not the case, since it's a discretized problem with consideration of the resolution, I don't think that it can cause this issue.
I tried to visualize the point that is currently checked by the checkCollisionCallback, in the video below, the red arrow illustrated the point 'p' that is considered by the checkcollisioncallback

Eigen::Vector3d p = pts_chk[i][j].second;

as seen in the video below, when the planner fails to find the trajectory that satisfies the cost constraints, the arrow doesn't move past a certain point, because of which the planner is unable to detect the points in the collision. I don't know what is causing this, your input is welcome

cost_function_test2-2023-02-06_14.52.25.mp4

[bigsuperZZZX]

It's really unclear to see the relative position between the trajectory and obstacles. Please modify the visualization parameters to make the obstacles more clear, like this:

[shubham-shahh]

It's really unclear to see the relative position between the trajectory and obstacles. Please modify the visualization parameters to make the obstacles more clear, like this:

Hi @bigsuperZZZX,
I think the cause of the problem is,
pts_chk contains only 2/3 points of the entire trajectory, as mentioned here

computePointsToCheck(traj, ConstraintPoints::two_thirds_id(cps, touch_goal), pts_to_check);

but, we send the entire traj to the traj server for execution here

polyTraj2ROSMsg()

I believe if we only send 2/3 part of traj in polyTraj2ROSMsg() to traj server, this problem can be solved
by making the following modification

int piece_num = (data->traj.getPieceNum())*(2.0/3.0);

please correct me if this is not the right approach. or if there are any drawbacks to this approach

thanks

I'll also upload a picture of the problem the way you have illustrated above for more clarity

[bigsuperZZZX]

The planner plans trajectories at a given frequency like 1Hz, so a new trajectory is always generated before the drone reaches the last 1/3 part of the previous trajectory.

[shubham-shahh]

The planner plans trajectories at a given frequency like 1Hz, so a new trajectory is always generated before the drone reaches the last 1/3 part of the previous trajectory.

yes, but in this case, since we are having a cost constraint

if (finelyCheckAndSetConstraintPoints(segments_nouse, jerkOpt_, false) == CHK_RET::OBS_FREE && final_cost<200)

if its unable to plan the path, cps_check is not updated and traj_server executes the given traj

[bigsuperZZZX]

If the planner is failed to generate the new trajectory before the 1/3 threshold, an emergency stop is recommended.

[shubham-shahh]

I have recorded a video, I tried to mimic the config you mentioned, I hope this video is more clear

cost_function_test4-2023-02-08_12.20.52.mp4

[bigsuperZZZX]

I think that the cost threshold is too low.

[shubham-shahh]

I think that the cost threshold is too low.

yes, you are right, but to test the worst-case scenario, I have put the cost too low.

[shubham-shahh]

If the planner is failed to generate the new trajectory before the 1/3 threshold, an emergency stop is recommended.

I see, that makes sense, but if I only pass 2/3 traj to traj server as mentioned above, is it okay to use this approach? that way, we always make sure that we only send the traj that is checked by checkCollisionCallback.
please share your thoughts about this approach and let me know if there are any problems with the above approach

[bigsuperZZZX]

I think that will be OK

[shubham-shahh]

I think that will be OK

I see, Thank you so much for your proactive response and all your help to solve this problem
to send only 2/3 part of the trajectory, I am doing the following modification

int piece_num = (data->traj.getPieceNum())*(2.0/3.0);
is it the correct way or is there a better way to do the same?

thanks once again for all the help, it is much appreciated

[bigsuperZZZX]

Yes