Simulating a Kinect using Gazebo (without ROS)
I'm trying to simulate a Kinect sensor in Gazebo (6.1, standalone without ROS). I tried loading the Kinect model in ".gazebo/models/kinect" using the GUI: Insert->Kinect. I tried checking up the published topics using Ctrl+T, but could not see any image or point cloud topics pertaining to the Kinect.
I have a completely fresh install of Gazebo 6 on Ubuntu 14.04. Trying to load the PR2 model also results in no kinect related topics (other cameras seem to work - the stereo pair publishes ImageStamped messages).
Are there any extra libraries that need to be installed to get the kinect sensor to work? For example, do I need an openni driver (like in ros where you may have to run the openni launcher) for the kinect sensor to start publishing topics?
Asked by Arun on 2015-11-15 23:37:58 UTC
Answers
The Kinect model uses the DepthCamera sensor that I believe it's not finished. If you look at the source code in sensors/DepthCameraSensor.cc, you will see that there's no topic publication. I think these will be the set of steps to make it work:
Change the model.sdf to include a `depth_camera> section:
<sensor name="camera" type="depth"> <update_rate>20</update_rate> <visualize>true</visualize> <always_on>1</always_on> <camera> <horizontal_fov>1.047198</horizontal_fov> <image> <width>640</width> <height>480</height> <format>R8G8B8</format> </image> <clip> <near>0.05</near> <far>3</far> </clip> <depth_camera> <output>points</output> </depth_camera> </camera> </sensor>
Add to the DepthCameraSensor.cc the topic publication. You can use as a template the code in CameraSensor.cc. In the DepthCameraSensor case, the size of each pixel would be 4 bytes corresponding to a float. Also, we would access the depth information with this->camera->GetDepthData().
if (this->imagePub && this->imagePub->HasConnections()) { msgs::ImageStamped msg; msgs::Set(msg.mutable_time(), this->scene->GetSimTime()); msg.mutable_image()->set_width(this->camera->GetImageWidth()); msg.mutable_image()->set_height(this->camera->GetImageHeight()); msg.mutable_image()->set_pixel_format(13); msg.mutable_image()->set_step(this->camera->GetImageWidth() * 4); msg.mutable_image()->set_data(this->camera->GetDepthData(), msg.image().width() * 4 * msg.image().height()); this->imagePub->Publish(msg); }
I'm assuming that the code in rendering:DepthCamera works and the depth image is properly rendered. If you don't want to use gazebo_ros_pkgs, you would have to modify the Gazebo source code, include those changes and probably debug it until it works. It will be fun and a nice contribution if you make it work.
Asked by Carlos Agüero on 2015-11-18 13:55:27 UTC
Comments
Thanks for the reply Carlos. I just updated the code in DepthCameraSensor.cc & .hh to add an image publisher & followed the steps you had pointed to above. When I load the kinect now, I am able to see an ImageStamped topic. When I click on that topic to display the image, gazebo crashes (the image is about half copied I think). With gdb, I got the following error:
Program received signal SIGSEGV, Segmentation fault. __memcpy_sse2_unaligned () at ../sysdeps/x86_64/multiarch/memcpy-sse2-unaligned.S:33 33 ../sysdeps/x86_64/multiarch/memcpy-sse2-unaligned.S: No such file or directory. (gdb) bt
%0 __memcpy_sse2_unaligned () at ../sysdeps/x86_64/multiarch/memcpy-sse2-unaligned.S:33
%1 0x00007ffff7b9095b in std::string::_M_replace_safe(unsigned long, unsigned long, char const*, unsigned long) () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
%2 0x00007ffff5bde8a5 in gazebo::msgs::Image::set_data (this=0x393b380, value=0x0, size=1228800)
at gazebo/msgs/image.pb.h:303
%3 0x00007ffff5bf0f2e in gazebo::sensors::DepthCameraSensor::UpdateImpl (this=0x7fff682bb050)
at gazebo/sensors/DepthCameraSensor.cc:223
%4 0x00007ffff5c33837 in gazebo::sensors::Sensor::Update (this=0x7fff682bb050, _force=false) at gazebo/sensors/Sensor.cc:221
%5 0x00007ffff5c41f0d in gazebo::sensors::SensorManager::SensorContainer::Update (this=0xc0a730, _force=false)
at gazebo/sensors/SensorManager.cc:541
%6 0x00007ffff5c426e6 in gazebo::sensors::SensorManager::ImageSensorContainer::Update (this=0xc0a730, _force=false)
at gazebo/sensors/SensorManager.cc:658
%7 0x00007ffff5c40216 in gazebo::sensors::SensorManager::Update (this=0x7ffff63debc0 <SingletonT<gazebo::sensors::SensorManager>::GetInstance()::t>, _force=false)
at gazebo/sensors/SensorManager.cc:159
%8 0x00007ffff5c3168c in gazebo::sensors::run_once (_force=false) at gazebo/sensors/SensorsIface.cc:118
%9 0x00007ffff786c9b0 in gazebo::Server::Run (this=0x6f2d50) at gazebo/Server.cc:516
%10 0x000000000402bd0 in main (argc=1, argv=0x7fffffffd478) at /home/barun/Projects/ExternalPackages/gazebo/gazebo/server_main.cc:40
which occurs when it tries to copy the data from the depth sensor (line 223 of DepthCameraSensor.cc):
msg.mutable_image()->set_data(this->camera->GetDepthData(),
msg.image().width() * 4 *
msg.image().height()); // NOTE: We need to get the depth data, not image data! ImageDepth = 4 bytes (float)
My ImageStamped msg format is
gazebo::common::Image::PixelFormat::R_FLOAT32
Any idea what's causing this issue? I've attached the source files.
DepthCameraSensor.cc DepthCameraSensor.hh
Asked by Arun on 2015-11-19 00:19:28 UTC
Comments