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