Octomap not visible and ardrone not following path

[boonflies]

Thanks, after the fix in issue #1, i was able to move ahead till I got to this place.

After launching the lsd_slam,I moved the ardrone till I received, "least-square successful".

Then, on pressing 'i' in Point cloud viewer, it returned a output in command window as,
resetted animation list!
made animation with 0 keyframes, spanning 0.000000 s!

Then on pressing, 'p' in point cloud viewer, returned an output in command window as,
Flushing Pointcloud to /home/dyana/ros/catkin_ws/src/lsd_slam/lsd_slam_viewer/pc_tmp.ply!
Done Flushing Pointcloud with 0 points!

I moved the ardrone a little bit more and then made the same attempt to publish point cloud
On pressing 'i' in Point cloud viewer, returned a output in command window as,
"made animation with 0 keyframes, spanning 0.000000 s!"

Then on pressing, 'p' in point cloud viewer, returned an output in command window as,
Flushing Pointcloud to /home/dyana/ros/catkin_ws/src/lsd_slam/lsd_slam_viewer/pc_tmp.ply!
Done flushing frame 980 (12321 points)!
Done flushing frame 1105 (10092 points)!
Done Flushing Pointcloud with 22413 points!

Then on launching,
$ roslaunch ar_drone_moveit demo.launch

I could not see any octomap. I tried to echo the point cloud as,
$ rostopic echo pointcloud2_scaled
but there was no output. Is there any other step to publish the point cloud in 'pc_tmp.ply'

I selected the motion planning plugin, and selected a start & goal and planned a path.

Then launched the ardrone controller,
$ rosrun hypharos_ardrone ardrone_controller

but I saw no motion of ardrone, even after pressing p and then s to let ardrone follow the path

How do I get the point cloud, the octomap and get the ardrone follow the computed path?

[axelfar]

Octomap is now visible for me.
I have modified the frame_id parameter in ar_drone_moveit/launch/octomap.launch
cloud_frame ---> map

[haochihlin]

Hi @axelfar
Thanks for your reply!
If possible, could you send a pull request that we can test and fix it?

[boonflies]

Hi,
After locking the point cloud, i get "made animation with 0 keyframes, spanning 0.000000 s!' and then I flush the point cloud to pc_tmp.ply, i tried to launch demo.launch and octomap.launch(with the changes given by @axelfar), but still I could not see the octomap(after adding the octomapgrid). When I echo the octomap and pointcloud topics, there is no data coming. Am i not properly publishing the point cloud topic?

should we be using any other point cloud publishing ros package like, Point Cloud IO? Please help me publish point cloud and the computed octomap in Rviz

[kungfrank]

Hello @boonflies @axelfar
Sorry, I didn't describe it clearly.
Actually, you should press "l" and "p" in conversion node to lock and publish point cloud. Not in point cloud viewer !!!
We had add ros publisher which publish point cloud to "pointcloud2_scale" topic in conversion.cpp. You can find detail in the code. By the way, you can use
$ rostopic echo pointcloud2_scaled
to check whether it works or not.

[boonflies]

Thanks, I tried pressing "l" and "p" in the conversion node and yes I got the following printed on the terminal,
l[ INFO] [1526447183.019686002, 1763.820000000]: Least-square successful !!
[ INFO] [1526447183.019802610, 1763.820000000]:
dx: 0.532765 0.195323 0.409987 -0.731137
dq: -0.0264334 -0.00479063 0.89328 0.448697
x_lsd: -0.147014 0.168876 -0.0156736
x_tum:0.354216 0.196355 0.387884
Time:1763.820000000
[ INFO] [1526447183.019876523, 1763.820000000]: Scale Lock!! (Enter 'r' to unlock)
p[ INFO] [1526447188.834868191, 1766.979000000]: Start to publish the PC2_scaled !!

But when I try to echo the pointcloud2_scaled topic, as
$ rostopic echo pointcloud2_scaled
I see no output and I get,
WARNING: no messages received and simulated time is active.
Is /clock being published?
and so I cannot visualize pointclouds in RViz

Is there any problem with my dx and dq convergence, that the point cloud is not being published?

[kungfrank]

@boonflies could you pose your rqt node graph in here ? Maybe it can help us to figure out what's happening.

[axelfar]

This is my process to see the octomap :

1)roslaunch tum_ardrone ardrone_driver.launch
2)roslaunch tum_ardrone tum_ardrone.launch
3)rosrun lsd_slam_viewer viewer
4)rosrun lsd_slam_core live_slam image:=/ardrone/front/image_raw camera_info:=/ardrone/front/camera_info
5)rosrun hypharos_ardrone conversion (press l and p)
6)roslaunch ar_drone_moveit demo.launch
7)rosrun hypharos_ardrone ardrone_controller
8)roslaunch ar_drone_moveit octomap.launch
Then I move my drone.

I just have a problem with my camera calibration and the conversion result.
You can see the result:

[boonflies]

I am not getting,
pointcloud_CB
publishing Scaled Point_Cloud2
in my conversion terminal as seen in the image posted by @axelfar in the comment above.

[kungfrank]

It seems that LSD slam viewer node didn't publish point cloud. Because we modified the code of original LSD slam. Please check that you are using lsd_slam in our repo.

[boonflies]

Yes, you are right.
when I try to "rostopic echo /pointcloud", nothing is published.

I have used lsd_slam package provided inside third parties folder and I have these two lines in lsd_slam_viewer/src/KeyFrameDisplay.cpp
Line Number: 38
cloud_pub = n.advertise<sensor_msgs::PointCloud>("pointcloud", 50);
Line Number: 39
cloud2_pub = n.advertise<sensor_msgs::PointCloud2>("pointcloud2", 50);

what else should i be checking to get the pointcloud data published ?

Thanks.

[kungfrank]

Maybe you could redownload our lsd_slam package.
There are my node graph at README file.

[boonflies]

Can, using /image_raw instead of /image_rect (in the absence of messages in /ardrone/front/camera_info) be the reason for /pointcloud not publishing?

How to start getting messages in /ardrone/front/camera_info so that /image_proc could publish messages, /ardrone/front/image_rect and /ardrone/front/image_mono topics?

[axelfar]

Yes you can use image_raw for a test but the resulting cloudpoints are bad.
Have you calibrate your camera with 'camera_calibration' and commit the result? It's important to get camera_info and image_rect

[boonflies]

I redownloaded the lsd_slam package.
when I tried to rosrun the lsd_slam_core node, I get segmentation fault,
rosrun lsd_slam_core live_slam /image:=/ardrone/fnt/image_rect camera_info:=/ardrone/front/camera_info
...
Segmentation fault (core dumped)
I found that the ardrone_front.yaml had wrong entries for image_height.
I updated the image_height to 352 (it was 360 earlier)
After that I used image_rect and when I run lsd_slam_core, i don't get segmentation fault but the debug window appears as follows, frozen and unresponsive as a black window and /viewer does not publish point clouds.

But when I use image_raw,
_rosrun lsd_slam_core live_slam /image:=/ardrone/fnt/image_rect camera_info:=/ardrone/front/camera_info
I get proper depth window

But in both the cases, /viewer was not publishing /pointclouds.

@axelfar - No, I have not calibrated my camera with 'camera_calibration'. Could that be the reason why debug window appears so while using image_rect and point cloud not being published?

[axelfar]

@boonflies Yes you need to calibrate your camera to create your calibration file in ~/.ros/camera_info/
Without that if you do 'rostopic echo /ardrone/front/camera_info' all matrix are equal to 0 and the image_rect is not published.

I resume:

1. change image size in video.h 360 to 352
2. Install and run 'camera_calibration' for monocular camera to calibrate your drone
3. when you have the result click Commit
   After that normaly you have a correct camera_info and image_rect

In my case i have this result -> Left : image_raw Right : image_rect

[boonflies]

@axelfar thanks,
i calibrated my camera, and then set the following values in video.h,
#define D2_STREAM_WIDTH 640
#define D2_STREAM_HEIGHT 352

Now, the debug window looks fine, and the ros node too,

but only concern now is, the /viewer is not publishing messages in /pointcloud topic (though i have the updated lsd_slam_viewer made available in this repository)
Will the /viewer automatically publish once we run,
rosrun lsd_slam_viewer viewer

rosrun lsd_slam_core live_slam image:=/ardrone/front/image_rect camera_info:=/ardrone/front/camera_info
or should we do something to get the viewer publish the pointcloud message?

[axelfar]

I have this node graph :

My viewer publish pointcloud and pointcloud2.
Do you run 'rosrun hypharos_ardrone ardrone_controller' ?

Look if you have those lines in KeyFrameDisplay.cpp

This is the definition of the publishers.
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

Publishing moment.

[boonflies]

@axelfar, thanks. My keyframedisplay.cpp looks the same.

I wondered, if there was some discrepancy in build, so I cleared the workspace, and with keeping only the hypharos related files, I tried to build, then I receive this error during build,
/home/dyana/ros/catkin_ws/src/hypharos_ardrone_navigation/third_parties/lsd_slam/lsd_slam_core/src/IOWrapper/ROS/ROSOutput3DWrapper.cpp:28:46: fatal error: lsd_slam_viewer/keyframeGraphMsg.h: No such file or directory
#include "lsd_slam_viewer/keyframeGraphMsg.h"
^
compilation terminated.
/home/dyana/ros/catkin_ws/src/hypharos_ardrone_navigation/hypharos_ardrone/src/ardrone_controller.cpp:26:38: fatal error: tum_ardrone/filter_state.h: No such file or directory
#include "tum_ardrone/filter_state.h"
^
compilation terminated.
make[2]: *** [hypharos_ardrone_navigation/hypharos_ardrone/CMakeFiles/ardrone_controller.dir/src/ardrone_controller.cpp.o] Error 1
make[1]: *** [hypharos_ardrone_navigation/hypharos_ardrone/CMakeFiles/ardrone_controller.dir/all] Error 2
make[1]: *** Waiting for unfinished jobs....
make[2]: *** [hypharos_ardrone_navigation/third_parties/lsd_slam/lsd_slam_core/CMakeFiles/lsdslam.dir/src/IOWrapper/ROS/ROSOutput3DWrapper.cpp.o] Error 1
make[2]: *** Waiting for unfinished jobs....
make[1]: *** [hypharos_ardrone_navigation/third_parties/lsd_slam/lsd_slam_core/CMakeFiles/lsdslam.dir/all] Error 2

I gave 'catkin_make' once again, now the error was changed to,
**/home/dyana/ros/catkin_ws/src/hypharos_ardrone_navigation/hypharos_ardrone/src/ardrone_controller.cpp:26:38: fatal error: tum_ardrone/filter_state.h: No such file or directory
#include "tum_ardrone/filter_state.h"

[ 32%] Building CXX object hypharos_ardrone_navigation/third_parties/lsd_slam/lsd_slam_viewer/CMakeFiles/viewer.dir/src/PointCloudViewer.o
make[2]: *** [hypharos_ardrone_navigation/hypharos_ardrone/CMakeFiles/ardrone_controller.dir/src/ardrone_controller.cpp.o] Error 1
make[1]: *** [hypharos_ardrone_navigation/hypharos_ardrone/CMakeFiles/ardrone_controller.dir/all] Error 2
make[1]: *** Waiting for unfinished jobs....**

I gave catkin_make once more, this time, it just threw a warning,
/home/dyana/ros/catkin_ws/src/hypharos_ardrone_navigation/third_parties/lsd_slam/lsd_slam_viewer/src/KeyFrameDisplay.cpp: In member function ‘void KeyFrameDisplay::setFrom(lsd_slam_viewer::keyframeMsgConstPtr)’:
/home/dyana/ros/catkin_ws/src/hypharos_ardrone_navigation/third_parties/lsd_slam/lsd_slam_viewer/src/KeyFrameDisplay.cpp:98:106: warning: format ‘%u’ expects argument of type ‘unsigned int’, but argument 3 has type ‘long unsigned int’ [-Wformat=]
msg->pointcloud.size(), sizeof(InputPointDense), width, height, widthheightsizeof(InputPointDense));
^
/home/dyana/ros/catkin_ws/src/hypharos_ardrone_navigation/third_parties/lsd_slam/lsd_slam_viewer/src/KeyFrameDisplay.cpp:98:106: warning: format ‘%u’ expects argument of type ‘unsigned int’, but argument 6 has type ‘long unsigned int’ [-Wformat=]

but at the end of catkin_make, i received "[100%] Built target ardrone_driver"

Have the modules been built correctly?
did you get anything like this?

[axelfar]

Yes I have the same errors when I clean and re-build catkin workspace.
You just need to do catkin_make again and again like you do.

[boonflies]

Thanks, after the cleanup and reinstall, now I am able to see the pointcloud being published and the octomap when i select motion planning in RViz.

Should we make any specific changes in octomap.launch file or RVIz? for, I see the quadrotor move through the octomap while creating the map?

[axelfar]

I discover that you don't need to launch octomap.launch to see the map in rviz.
Just : rosrun hypharos_ardrone conversion
roslaunch ar_drone_moveit demo.launch
rosrun hypharos_ardrone ardrone_controller

For me the Quadrotor move in RVIZ, you have the shadow and some axis.
But for me the path planning don't work for now.

[boonflies]

Quadrotor plans and execute in rviz but the real drone does not move accordingly.
What i do is,

1. rosrun hypharos_ardrone ardrone_controller
2. Press 'p' to Select the mode - path follow
3. Press 's' to select the path mode - start following the path
4. Plan in rviz
5. After planning is successful, the following is printed in ardrone_controller node,

How do we make the real quadrotor to follow the computed path?

[kungfrank]

Hello~~~ you should do path planning in RVIZ before you press 's' in ardrone_controller.

[boonflies]

This is the map that is created of a rectangular room with a small tables at two edges.

And all my planning fails. From the map, it seems as though, the quadrotor has no place to move, but it is not so.
I created the map by moving the quadrotor by hand, for the tracking is lost and diverges quickly, if I fly the quadrotor to capture the map. How much of processing speed (processor and ram configuration) is required and is the created map the reason for failed path planning?

[axelfar]

Sometime your octomap generation is tiny because you have a bad conversion with conversion.cpp. I have the same problem.
Try to plan the path with a better map generation, and look if you don't have the error 'No planning library loaded'.

[boonflies]

@kungfrank if I don't press s in contoller before pressing plan in Rviz, the following is printed in controller terminal,
"Get the Path, the total size of points is: 58
If I had pressed s before planning in Rviz, I can see the list of goto statements printed in the contoller terminal.

Though in both cases the drone does not move.

I was earlier using the joystick mode in tum ardrone to map, and could that be a reason that the drone does not move? Because even switching to autopilot manually does not work after the mapping process using joystick.

Should planning be done only when the drone is in hovering mode or can it be done even in landed mode?

[kungfrank]

@boonflies You should press 'p' in controller terminal after path planning in Moveit and press 's' when drone is already takeoff. After you press 's', controller will generate waypoint for tum_ardrone (check tum_ardrone in autopilot mode).

[zackerwong5505]

@kungfrank hi , wan to ask why am i getting a inverted map from lsd slam? is it the calib file parameters problem?
0.771557 1.368560 0.552779 0.444056 1.156010
640 360
crop
640 352
this is the value im using for my ardrone

Besides , i have change the value of ardrone_front.yaml and video.h to value 640 x 352 , but still not change the value , tats why i hv to use the calib file

[haakonelf]

Hey @zackerwong5505 ! Glad there is someone keeping this thread alive, Im struggeling with this now myself.

why am i getting a inverted map from lsd slam?

I think lsd slam viewer is a bit weird like that. The medium artcile made by hypha-ros explains that lsd-slam has an inverted z-axis, but what is important is that it should be fixed in the move-it world. Check if it is okey there, nothing else should matter. Just flip the lsd-slam viewer if you need to use it.
How did you manage to use a calib file for the live lsd slam? I never got that to work myself, so Im using "ros_imresize".
Have you gotten it to make the drone follow the path? Even though rviz shows the path, mine is just .. wobbling.

[zackerwong5505]

Hey @zackerwong5505 ! Glad there is someone keeping this thread alive, Im struggeling with this now myself.

why am i getting a inverted map from lsd slam?

I think lsd slam viewer is a bit weird like that. The medium artcile made by hypha-ros explains that lsd-slam has an inverted z-axis, but what is important is that it should be fixed in the move-it world. Check if it is okey there, nothing else should matter. Just flip the lsd-slam viewer if you need to use it.
How did you manage to use a calib file for the live lsd slam? I never got that to work myself, so Im using "ros_imresize".
Have you gotten it to make the drone follow the path? Even though rviz shows the path, mine is just .. wobbling.

hi @haakonelf
i just make a calib file and run tis command
rosrun lsd_slam_core live_slam /image:=/ardrone/front/image_raw _/calib:=/home/zack/test_ws/src/hypharos_ardrone_navigation/third_parties/lsd_slam/lsd_slam_core/calib/ardrone.cfg
then i able to use calib file for live lsd slam.
now i am trying to port into my moveit and the visualize map is so small , and i dunno how to do with it , do u face the same problem as i do?

[kungfrank]

Hello @zackerwong5505 , thank you for using this repo. The reason why your map in MoveIt is so small is due to Recursive Least Square algorithm in "Conversion" node haven't converge yet.
After you do

$ rosrun hypharos_ardrone conversion

you should fly Ardrone around (up, down, left, right, forward, backward, and slight rotation) until dq and dx values converge completely (with steady values) before you press "l" and "p" to publish point cloud to MoveIt.

[zackerwong5505]

@kungfrank
really thanks for your reply , because i am too new for ROS environment

after dq and dx converge completely , i still can unlock it with "r" to explore more map right?

and dunno why i still having a inverted map even in rviz
How can i stop publishing my map into rviz because after i get a roughly map, it will keep on duplicate the map untill i cant recognise it

Reali sry for my stupidity, but i reali nid your help

[haakonelf]

Did you figure it out? And do you know why you cant move the drone upwards?

[zackerwong5505]

Did you figure it out? And do you know why you cant move the drone upwards?

i thk is my camera calib problem , after i use reimsize there is no problem for my map ,
the drone can upward shld be moveit configuration problem , how to reconfigure the moveit setting ,
but when u run rviz there is a z_point and yaw point which allow u to move drone upwards or down in rviz shld can work .

wan to ask when following the path , drone able to move to the goal? because my drone keep moving to somewhere which is not my goal..