rviz crashes on drawMavTrajectory about mav_trajectory_generation HOT 13 CLOSED

@helenol @rikba

from mav_trajectory_generation.

Can you print out the markers actually getting sent to rviz? Are there any nans in there? Could also be that the marker array is too big; rviz also crashes in these cases.

from mav_trajectory_generation.

Hey @helenol , thanks for the reply! I printed out the markers and can't see any NaNs. However, the scale looks suspicious:

 header: 
      seq: 0
      stamp: 
        secs: 84
        nsecs: 970000000
      frame_id: world
    ns: pose
    id: 60
    type: 0
    action: 0
    pose: 
      position: 
        x: 0.0
        y: 0.0
        z: 0.0
      orientation: 
        x: 0.0
        y: 0.0
        z: 0.0
        w: 0.0
    scale: 
      x: 0.03
      y: 0.06
      z: 0.0
    color: 
      r: 1.0
      g: 0.0
      b: 0.0
      a: 1.0

0.0 in z? Is this normal?

I'm not sure if overflow of the issue considering the visualization was working for me on my old machine (and with mav_planning_utils, before migration to mav_trajectory_generation).

from mav_trajectory_generation.

Can you change this line:

from mav_trajectory_generation.

@helenol Still the same issue. I googled the problem and only found the same error for Gazebo with 16.04, not rviz.. :-(

from mav_trajectory_generation.

So then the z scale isn't the issue? Can you also try shorter trajectories? Also can you please upload a full printout of the marker messages, as the one above is an incomplete marker (no pose, no points).

from mav_trajectory_generation.

Hey @helenol . The path I'm trying is a 1m x 1m square. It looks like this:

Printout of marker messages:

markers.txt

I'm not sure how to interpret this, but something is definitely wrong:

1. Most messages do not seem to send more than 1-2 points in the array?
2. Message with e.g. ID 8 has insane values for the points in x-y-z!
3. Message with e.g. ID 35 does send an array of points, but the scale in y and z is still 0.0?

from mav_trajectory_generation.

Thanks, this helps! Yes I think around ID 8 is the issue. If you note the namespaces, then IDs 0 through 34 are just the pose axes, and velocity and acceleration arrows. So looks like the sampling is failing at some point and giving uninitilalized or just crazy values for the derivatives.
ID 35 is the actual path, as a line strip -- http://wiki.ros.org/rviz/DisplayTypes/Marker -- and only the x scale is used.

Can you send me the commands/etc. to reproduce this? I think it's an issue at that exact timestamp with the sampling.

from mav_trajectory_generation.

Thanks, makes sense. :-) In this case, then, it might be that there's a bug in my trajectory sampler. Here is what I use to run the example:

1. Start the simulation environment:

roslaunch mav_simulator_demos mav_gps_sim.launch

2. Start the waypoint_navigator node from mav_pathplanning and the trajectory_sampler node from mav_trajectory_generation:

roslaunch waypoint_navigator waypoint_navigator.launch

3. Call the service to visualize the path/publish the markers:

rosservice call /firefly/visualize_path

If you open rviz now and try to display the MarkerArray topic, /firefly/waypoint_navigator_polynomial_markers, it will crash with the above error.

I hope this helps in debugging and thanks so much for your time~ 👍

from mav_trajectory_generation.

Ok so tracked down the issue to the source.
You have 26 segments for 4 waypoints (????) and one of the segments has a very short time: 1e-5 and therefore CRAZY coefficients:

Segment # 8: t: 1.17491e-05
 coefficients for position:
-4.01103e-14
 -3.8659e-07
-1.98417e-08
 7.51594e-07
 1.27555e-06
 3.26023e+14
-7.96298e+19
 8.23957e+24
-4.02143e+29
 7.60102e+33
          -3
  -0.0588243
   0.0687131
  0.00793468
 -0.00562712
-1.03079e+14
 2.53642e+19
-2.63507e+24
 1.29055e+29
-2.43388e+33
           1
 2.08534e-10
-2.37627e-05
 4.40356e-06
 3.32468e-05
 6.87195e+10
 1.59635e+07
 4.72237e+21
-1.54743e+26
  2.5353e+30
     -1.5708
-1.54955e-06
  -0.0005193
 0.000212527
  0.00102224
 1.29564e+15
-3.16009e+20
 3.26742e+25
-1.59354e+30
 3.01093e+34

I think the sampling code is trying to sample this one slightly past its actual length, but either way, you should not allow any time allocation with this short of time. This, as you can see, causes huuuuge numerical issues.

from mav_trajectory_generation.

Hi! Cool repo!
Could I ask, are waypoint_navigator and mav_pathplanning mentioned here public available?
Want to try mav_trajectory_generation with some GUI.
Thanks!

from mav_trajectory_generation.

@frontw open another issue for a different question please.

from mav_trajectory_generation.

Hi @frontw ,

waypoint_navigator and mav_pathplanning are still closed source. But you can interface mav_trajectory_generation writing your own node that receives waypoints over messages or loads them from a parameter file and then generates trajectories. You can then display the trajectories using the visualization tools in mav_trajectory_generation. Good luck :)

from mav_trajectory_generation.