Gazebo 1.5: libgazebo_openni_kinect.so: pointcloud data is rotated and flipped?

Question

Hi there!

I set up a gazebo simulation with the Clearpath Husky A200 robot, on which I mounted a kinect camera at the front bumper and a hokuyo laserscan on top of the rail.

Here is the situation: gazebo situation http://robotics.fh-aachen.de/pictures/kinect_bug/situation.png

For the hokuyo laser scan I use the "libgazebo_ros_laser.so" plugin. It works fine and provides the scan data on the "/scan" topic.

For the kinnect camera I am using the "libgazebo_openni_kinect.so", which provides "/kinect/image_raw", "/kinect/image_depth" and "/kinect/pointcloud" topics.

The camera can see this (screenshots taken from RVIZ):

"/kinect/image_raw": image raw http://robotics.fh-aachen.de/pictures/kinect_bug/depth_camera_image_raw.png

"/kinect/image_depth": image depth http://robotics.fh-aachen.de/pictures/kinect_bug/depth_camera_image_depth.png

So far, everything is looking good. So here comes the thing, when I tell RVIZ to show the "pointcloud" topic, which comes from the same camera, the siuation looks like this.

RIVZ pointcloud:

rviz http://robotics.fh-aachen.de/pictures/kinect_bug/rviz.png

The red points indicate the edges of the trash dumpster, which is recognized correctly by the hokuyo laser scan.

But why the hell is the point cloud data coming from the kinect flipped and projected into the sky? I would have expected it to be overlaying with the red laserscan data... I tried the whole day rotating the data but could not find a solution. Does anyone know what to do?

Best regards psei

---

Originally posted by psei on Gazebo Answers with karma: 166 on 2013-09-05

Post score: 2

Answer 1

I solved my problem. Anyone who is experiencing the same problem can try this:

Update your "libgazebo_ros_depth_camera.cpp" or "libgazebo_ros_openni_kinnect.cpp" as following:

#include "pcl/common/eigen.h"
#include "pcl/common/transforms.h"

then, look for the line "pcl::toROSMsg(point_cloud, point_cloud_msg);" in function

bool GazeboRosDepthCamera::FillPointCloudHelper( sensor_msgs::PointCloud2 &point_cloud_msg, uint32_t rows_arg, uint32_t cols_arg, uint32_t step_arg, void* data_arg)

right before that, transform the cloud as following:

pcl::PointCloudpcl::PointXYZRGB transformed_cloud;
double rotx = 0.0;
double roty = M_PI/2.0;
double rotz = -M_PI/2.0;
Eigen::Matrix4f rotMatrixX;
Eigen::Matrix4f rotMatrixY;
Eigen::Matrix4f rotMatrixZ;

rotMatrixX <<
1.0, 0.0, 0.0, 0.0,
0.0, cos(rotx), -sin(rotx), 0.0,
0.0, sin(rotx), cos(rotx), 0.0,
0.0, 0.0, 0.0, 1.0;

rotMatrixY <<
cos(roty), 0.0, sin(roty), 0.0,
0.0, 1.0, 0.0, 0.0,
-sin(roty), 0.0, cos(roty), 0.0,
0.0, 0.0, 0.0, 1.0;

rotMatrixZ <<
cos(rotz), -sin(rotz), 0.0, 0.0,
sin(rotz), cos(rotz), 0.0, 0.0,
0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 1.0;

pcl::transformPointCloud(point_cloud,transformed_cloud, rotMatrixX * rotMatrixY * rotMatrixZ);
pcl::toROSMsg(transformed_cloud, point_cloud_msg);

---

Originally posted by psei with karma: 166 on 2013-09-09

This answer was ACCEPTED on the original site

Post score: 0

---

Original comments

Comment by barcelosandre on 2013-09-09:
There is an other solution, using TF from ROS. you can publish a rotation transform to kinect_frame.

Comment by MadEgg on 2013-09-12:
I am experiencing the same issue. However, since I installed Gazebo using the Ubuntu Groovy repository, I do not have the source of the plugins. I checked it out and tried to compile, but then I enter dependency hell.

Could you care to post a link to your recompiled libgazebo_ros_openni_kinect.so so that I can use your binary instead of rolling my own? Thanks!

Comment by psei on 2013-09-13:
I uploaded them here for you:

https://robotics.fh-aachen.de/gazebo/libgazebo\_ros\_depth\_camera.so

https://robotics.fh-aachen.de/gazebo/libgazebo\_ros\_openni\_kinect.so

Comment by M.Ahmed on 2016-01-22:
hi i need correct libgazebo_ros_openni_kinect.so file but the these files not available at above links. can u give me these files cz i don't find its cpp file in gazebo plugins. Thnaks

Comment by Filippo Sanfilippo on 2016-05-07:
Hi, I also need your compiled files. Could you please repost them, they are not available anymore. Thank you! Also, could you please tell us the location folder of the ones we need to replace?

Comment by Filippo Sanfilippo on 2016-05-13:
Any updates on this? Could you please repost your .so files?

Comment by felixvd on 2018-07-11:
I cannot imagine that this is the correct way to deal with this. A similar question is posted here: https://answers.ros.org/question/53784/simulated-kinect-rotation-around-x-bug/?answer=55968#post-id-55968

Comment by user90 on 2018-07-20:
Hey all, I cannot find the line pcl::toROSMsg(point_cloud, point_cloud_msg); in both plugins and I have the same issue. Can someone please upload the updated version of these plugins, so that the problem is solved because the two links are not working. Thank you

Comment by vib2810 on 2021-01-11:
There is an amazing solution by barcelosandre and you only need to update the urdf file you have. Instead of setting kinect_link to be connected directly to the sensor, set an additional kinect_optical_link whose joint is rotated in RPY terms by (-1.57, 0, -1.57). The code can be found here https://paste.ubuntu.com/p/bFJPHVDNVQ/