simulated stereo camera fails when remapping topics
Dear all, I'm trying to simulate the Multisense SL device in Gazebo. I've started by modifying the plugin available here: https://bitbucket.org/osrf/drcsim/src/194be8500fef81593f79607a21ee2badd9700a0e/drcsim_gazebo_ros_plugins/src/?at=default
The Gazebo-related URDF is the following:
<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:macro name="multisense_sl_sensor" params="prefix robot_name">
<gazebo reference="head_root">
<mu1>0.9</mu1>
<mu2>0.9</mu2>
</gazebo>
<gazebo reference="head">
<mu1>0.9</mu1>
<mu2>0.9</mu2>
</gazebo>
<gazebo reference="${prefix}/hokuyo_link">
<mu1>0.9</mu1>
<mu2>0.9</mu2>
</gazebo>
<gazebo reference="${prefix}/head_hokuyo_frame">
<mu1>0.9</mu1>
<mu2>0.9</mu2>
</gazebo>
<gazebo reference="${prefix}/motor_joint">
<implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
<gazebo reference="${prefix}/head_hokuyo_frame">
<sensor type="gpu_ray" name="head_hokuyo_sensor">
<pose>0 0 0 0 0 0</pose>
<visualize>false</visualize>
<update_rate>40</update_rate>
<ray>
<scan>
<horizontal>
<samples>1081</samples>
<resolution>1</resolution>
<min_angle>-2.356194</min_angle>
<max_angle>2.356194</max_angle>
</horizontal>
</scan>
<range>
<min>0.10</min>
<max>30.0</max>
<resolution>0.01</resolution>
</range>
<noise>
<type>gaussian</type>
<!-- Noise parameters based on published spec for Hokuyo laser
achieving "+-30mm" accuracy at range < 10m. A mean of 0.0m and
stddev of 0.01m will put 99.7% of samples within 0.03m of the true
reading. -->
<mean>0.0</mean>
<stddev>0.0</stddev>
</noise>
</ray>
<plugin name="${prefix}/head_hokuyo_controller" filename="libgazebo_ros_gpu_laser.so">
<topicName>${prefix}/lidar_scan</topicName>
<frameName>${prefix}/head_hokuyo_frame</frameName>
</plugin>
</sensor>
</gazebo>
<gazebo reference="${prefix}/left_camera_frame">
<sensor type="multicamera" name="${prefix}/stereo_camera">
<!-- see MultiSenseSLPlugin.h for available modes -->
<update_rate>10.0</update_rate>
<camera name="left">
<!-- Spec sheet says 80deg X 45deg @ 1024x544pix. Based on feedback
from teams, we're instead doing 80deg X 80deg @ 800x800pix. -->
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>1024</width>
<height>1024</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<!-- Noise is sampled independently per pixel on each frame.
That pixel's noise value is added to each of its color
channels, which at that point lie in the range [0,1].
The stddev value of 0.007 is based on experimental data
from a camera in a Sandia hand pointed at a static scene
in a couple of different lighting conditions. -->
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
<camera name="right">
<pose>0 -0.07 0 0 0 0</pose>
<!-- Spec sheet says 80deg X 45deg @ 1024x544pix. Based on feedback
from teams, we're instead doing 80deg X 80deg @ 800x800pix. -->
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>1024</width>
<height>1024</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<!-- Noise is sampled independently per pixel on each frame.
That pixel's noise value is added to each of its color
channels, which at that point lie in the range [0,1].
The stddev value of 0.007 is based on experimental data
from a camera in a Sandia hand pointed at a static scene
in a couple of different lighting conditions. -->
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
<plugin name="${prefix}/stereo_camera_controller" filename="libgazebo_ros_multicamera.so">
<alwaysOn>true</alwaysOn>
<updateRate>10.0</updateRate>
<cameraName>${prefix}/camera</cameraName>
<imageTopicName>image_color</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>${prefix}/left_camera_optical_frame</frameName>
<rightFrameName>${prefix}/right_camera_optical_frame</rightFrameName>
<hackBaseline>0.07</hackBaseline>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
</plugin>
</sensor>
</gazebo>
<gazebo reference="${prefix}/head_imu_link">
<!-- this is expected to be reparented to head with appropriate offset
when head_imu_link is reduced by fixed joint reduction -->
<!-- todo: this is working with gazebo 1.4, need to write a unit test -->
<sensor name="${prefix}/head_imu_sensor" type="imu">
<always_on>1</always_on>
<update_rate>1000.0</update_rate>
<imu>
<noise>
<type>gaussian</type>
<!-- Noise parameters from Boston Dynamics
(http://gazebosim.org/wiki/Sensor_noise):
rates (rad/s): mean=0, stddev=2e-4
accels (m/s/s): mean=0, stddev=1.7e-2
rate bias (rad/s): 5e-6 - 1e-5
accel bias (m/s/s): 1e-1
Experimentally, simulation provide rates with noise of
about 1e-3 rad/s and accels with noise of about 1e-1 m/s/s.
So we don't expect to see the noise unless number of inner iterations
are increased.
We will add bias. In this model, bias is sampled once for rates
and once for accels at startup; the sign (negative or positive)
of each bias is then switched with equal probability. Thereafter,
the biases are fixed additive offsets. We choose
bias means and stddevs to produce biases close to the provided
data. -->
<rate>
<mean>0.0</mean>
<stddev>2e-4</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</rate>
<accel>
<mean>0.0</mean>
<stddev>1.7e-2</stddev>
<bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev>
</accel>
</noise>
</imu>
</sensor>
</gazebo>
<gazebo>
<plugin filename="libMultiSenseSLPlugin.so" name="multisense_plugin">
<robot_name>${robot_name}</robot_name>
</plugin>
</gazebo>
</xacro:macro>
</robot>
To simulate the stereo, I need to run the stereo imageproc node from ROS, which needs a remap from the name "imageraw" (standard for the node) to "imagecolor" (standard name for Multisense real hardware).
The code is the following:
<node ns="multisense/camera" name="stereo_proc" pkg="stereo_image_proc" type="stereo_image_proc" respawn="false" output="log">
<param name="disparity_range" type="int" value="128"/>
<param name="approximate_sync" type="bool" value="false"/>
<remap from="left/image_raw" to="left/image_color" />
<remap from="right/image_raw" to="right/image_color" />
</node>
When I run the two, I get more than 600 Hz for the /multisense/camera/left/image_color
topic (despite the update rate is set at 10 Hz), while for the /multisense/camera/points2
I get 0.2 Hz.
If I put image_raw
in both files, I correctly get 10 Hz for both topics.
Any suggestions?
I'm running the simulation on Gazebo 2.2 with Indigo and Ubuntu 14.04.
Asked by mark_vision on 2017-06-07 03:42:10 UTC
Answers
I've just realized that the stereo processing node does not produce only the pointclouds but also many other topics, including image_color
.
The processing node was autofeeding himeself, causing an increment in frequency of the stereo pairs and making the stereo processing too slow to compute.
Asked by mark_vision on 2017-06-07 04:45:54 UTC
Comments