Robotics StackExchange | Archived questions

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

Comments

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