Skip to content

Commit

Permalink
Merge pull request pal-robotics#6 from jordi-pages/update-stereo-urdf
Browse files Browse the repository at this point in the history 
Use the gazebo_ros_multicamera plugin for stereo
  • Loading branch information
@jordi-pages
jordi-pages committed Jan 27, 2014
2 parents e75080c + 0cecf02 commit 0dd4804
Show file tree
Hide file tree
Showing 4 changed files with 52 additions and 79 deletions.
4 changes: 2 additions & 2 deletions reem_description/robots/reem_full.urdf.xacro
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,10 +52,10 @@

<!-- stereo camera -->
<!-- IMPORTANT: focal_length = image_width/(2tan(hfov/2)) -->
<xacro:stereo_camera_v0 parent="head_2_link" name="stereo" focal_length_left="459.840874" hfov_left="${71.0*deg_to_rad}" focal_length_right="459.840874" hfov_right="${71.0*deg_to_rad}" image_format="B8G8R8" image_width="656" image_height="492">
<xacro:reem_stereo_camera parent="head_2_link" name="stereo" focal_length="459.840874" hfov="${71.0*deg_to_rad}" image_format="B8G8R8" image_width="656" image_height="492">
<!-- The following transformations expresses the pose of the stereo_link (X pointing forward, Z upwards and Y leftwards, contrary to the stereo_optical_link which has Z pointing forward) wrt head_2_link. -->
<origin xyz="${cal_stereo_x} ${cal_stereo_y} ${cal_stereo_z}" rpy="${cal_stereo_roll} ${cal_stereo_pitch} ${cal_stereo_yaw}" />
</xacro:stereo_camera_v0>
</xacro:reem_stereo_camera>

<!-- back camera -->
<xacro:reem_back_camera parent="torso_2_link" name="back_camera" focal_length="376.002379" hfov="${89.999451423*deg_to_rad}" image_format="B8G8R8" image_width="752" image_height="480" >
Expand Down
34 changes: 17 additions & 17 deletions reem_description/urdf/sensors/stereo_camera.urdf.xacro
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,20 +8,20 @@
To view a copy of this license, visit http://creativecommons.org/licenses/by-nc-nd/3.0/ or send a letter to
Creative Commons, 444 Castro Street, Suite 900, Mountain View, California, 94041, USA.
-->
<robot xmlns:xacro="http://ros.org/wiki/xacro">
<robot name="reem" xmlns:xacro="http://ros.org/wiki/xacro">

<!-- stereo camera macro uses stingrayf033c_camera macros -->
<xacro:include filename="$(find reem_description)/urdf/sensors/stingrayf033c_stereo_camera.gazebo.xacro" />
<xacro:include filename="$(find reem_description)/urdf/sensors/stingrayf033c_camera.urdf.xacro" />

<property name="stereo_dx" value="0.00" />
<property name="stereo_dy" value="-0.073" /> <!-- stereo baseline according to stereo_link which has X pointing forward, Y leftwards and Z upwards (required by Gazebo for a camera sensor) -->
<property name="stereo_dy" value="-0.073" /> <!-- stereo baseline according to base_link which has X pointing forward, Y leftwards and Z upwards (required by Gazebo for a camera sensor) -->
<property name="stereo_dz" value="0.00" />
<property name="stereo_rx" value="0.00" />
<property name="stereo_ry" value="0.00" />
<property name="stereo_rz" value="0.00" />

<!-- this macro is used for creating wide and narrow double stereo camera links -->
<xacro:macro name="stereo_camera_v0" params="name parent focal_length_left hfov_left focal_length_right hfov_right image_format image_width image_height *origin">
<xacro:macro name="reem_stereo_camera" params="name parent focal_length hfov image_format image_width image_height *origin">

<joint name="${name}_frame_joint" type="fixed">
<insert_block name="origin" />
Expand All @@ -43,7 +43,7 @@
<geometry>
<box size="0.001 0.001 0.001" />
</geometry>
<material name="Blue" />
<material name="Blue"/>
</visual>

<collision>
Expand Down Expand Up @@ -73,22 +73,22 @@
</link>

<!-- stereo left camera -->
<xacro:stingrayf033c_camera_v0 name="${name}_gazebo_l_stereo_camera" image_format="${image_format}" image_topic_name="/${name}/left/image"
camera_info_topic_name="/${name}/left/camera_info" parent="${name}_link"
hfov="${hfov_left}" focal_length="${focal_length_left}"
frame_id="${name}_optical_frame" hack_baseline="0"
image_width="${image_width}" image_height="${image_height}">
<xacro:stingrayf033c_camera name="${name}_gazebo_left_camera" parent="${name}_link">
<origin xyz="0 0 0" rpy="0.0 0.0 0.0" />
</xacro:stingrayf033c_camera_v0>
</xacro:stingrayf033c_camera>

<!-- stereo right camera -->
<xacro:stingrayf033c_camera_v0 name="${name}_gazebo_r_stereo_camera" image_format="${image_format}" image_topic_name="/${name}/right/image"
camera_info_topic_name="/${name}/right/camera_info" parent="${name}_gazebo_l_stereo_camera_frame"
hfov="${hfov_right}" focal_length="${focal_length_right}"
frame_id="${name}_optical_frame" hack_baseline="${-stereo_dy}"
image_width="${image_width}" image_height="${image_height}">
<xacro:stingrayf033c_camera name="${name}_gazebo_right_camera" parent="${name}_link">
<origin xyz="${stereo_dx} ${stereo_dy} ${stereo_dz}" rpy="${stereo_rx} ${stereo_ry} ${stereo_rz}" />
</xacro:stingrayf033c_camera_v0>
</xacro:stingrayf033c_camera>


<!-- Stereo camera Gazebo simulation -->
<xacro:stingrayf033c_stereo_camera_gazebo name="${name}" image_format="${image_format}"
hfov="${hfov}" focal_length="${focal_length}"
frame_id="${name}_optical_frame" hack_baseline="${-stereo_dy}"
image_width="${image_width}" image_height="${image_height}"/>


</xacro:macro>
</robot>
55 changes: 7 additions & 48 deletions reem_description/urdf/sensors/stingrayf033c_camera.urdf.xacro
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,5 @@
<?xml version="1.0"?>
<!--
Copyright (c) 2011-2014, PAL Robotics, S.L.
All rights reserved.
Expand All @@ -9,72 +8,32 @@
Creative Commons, 444 Castro Street, Suite 900, Mountain View, California, 94041, USA.
-->
<!-- XML namespaces -->
<robot xmlns:xacro="http://ros.org/wiki/xacro">

<xacro:include filename="$(find reem_description)/urdf/sensors/stingrayf033c_camera.gazebo.xacro" />

<!-- hack_baseline is used to simulate right stereo camera projection matrix translation to left stereo camera frame, currently on the hardware,
custom left stereo camera frame_id is passed to right stereo stingrayf033c camera node at launch time, baseline is stored on the camera. -->
<xacro:macro name="stingrayf033c_camera_v0" params="name image_format image_topic_name camera_info_topic_name parent hfov focal_length frame_id hack_baseline image_width image_height *origin">
<robot name="reem" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="stingrayf033c_camera" params="name parent *origin">

<joint name="${name}_frame_joint" type="fixed">
<insert_block name="origin" />
<parent link="${parent}"/>
<child link="${name}_frame"/>
<child link="${name}_link"/>
</joint>
<link name="${name}_frame">

<link name="${name}_link">
<inertial>
<mass value="0.01" />
<mass value="0.1" />
<origin xyz="0 0 0" />
<inertia ixx="0" ixy="0.0" ixz="0.0"
iyy="0" iyz="0.0"
izz="0" />
</inertial>

<visual>
<origin xyz="-0.01 0 0" rpy="0 ${deg_to_rad * 90} 0" />
<geometry>
<cylinder radius="0.01" length="0.02" />
<cylinder radius="0.01" length="0.018" />
</geometry>
<material name="Blue" />
<material name="Blue"/>
</visual>

<!--
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.01 0.01 0.01" />
</geometry>
</collision>
-->
</link>

<joint name="${name}_optical_frame_joint" type="fixed">
<origin xyz="0 0 0" rpy="${-M_PI/2} 0.0 ${-M_PI/2}" />
<parent link="${name}_frame" />
<child link="${name}_optical_frame"/>
</joint>

<link name="${name}_optical_frame">
<inertial>
<mass value="0.1" />
<origin xyz="0 0 0" />
<inertia ixx="0" ixy="0.0" ixz="0.0"
iyy="0" iyz="0.0"
izz="0" />
</inertial>
</link>


<!-- extensions -->
<xacro:stingrayf033c_camera_gazebo_v0 name="${name}" image_format="${image_format}" image_topic_name="${image_topic_name}"
camera_info_topic_name="${camera_info_topic_name}"
hfov="${hfov}" focal_length="${focal_length}"
frame_id="${frame_id}" hack_baseline="${hack_baseline}"
image_width="${image_width}" image_height="${image_height}"/>


</xacro:macro>

</robot>
38 changes: 26 additions & 12 deletions ...sensors/stingrayf033c_camera.gazebo.xacro → .../stingrayf033c_stereo_camera.gazebo.xacro
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,15 +8,27 @@
To view a copy of this license, visit http://creativecommons.org/licenses/by-nc-nd/3.0/ or send a letter to
Creative Commons, 444 Castro Street, Suite 900, Mountain View, California, 94041, USA.
-->
<robot xmlns:xacro="http://ros.org/wiki/xacro">
<robot name="reem" xmlns:xacro="http://ros.org/wiki/xacro">

<xacro:macro name="stingrayf033c_camera_gazebo_v0" params="name image_format image_topic_name camera_info_topic_name hfov focal_length frame_id hack_baseline image_width image_height">
<gazebo reference="${name}_frame">
<sensor type="camera" name="${name}_sensor">
<update_rate>15.0</update_rate>
<xacro:macro name="stingrayf033c_stereo_camera_gazebo" params="name image_format hfov focal_length frame_id hack_baseline image_width image_height">
<gazebo reference="${name}_link">

<camera name="${name}">
<pose>0 -${hack_baseline} 0 0 0 0</pose>
<sensor type="multicamera" name="gazebo_${name}_camera">
<update_rate>25.0</update_rate>
<camera name="left">
<horizontal_fov>${ hfov }</horizontal_fov>
<image>
<width>${image_width} </width>
<height> ${image_height}</height>
<format>${image_format}</format>
</image>
<clip>
<near>0.01</near>
<far>100</far>
</clip>
</camera>
<camera name="right">
<pose>0 -${hack_baseline} 0 0 0 0</pose> <!-- the pose is wrt /stereo_link which is the parent of stereo_optical_frame -->
<horizontal_fov>${ hfov }</horizontal_fov>
<image>
<width>${image_width} </width>
Expand All @@ -28,13 +40,12 @@
<far>100</far>
</clip>
</camera>

<plugin name="${name}_controller" filename="libgazebo_ros_camera.so">
<plugin name="stereo_camera_controller" filename="libgazebo_ros_multicamera.so">
<alwaysOn>true</alwaysOn>
<updateRate>15.0</updateRate>
<updateRate>25.0</updateRate>
<cameraName>${name}</cameraName>
<imageTopicName>${image_topic_name}</imageTopicName>
<cameraInfoTopicName>${camera_info_topic_name}</cameraInfoTopicName>
<imageTopicName>image</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>${frame_id}</frameName>
<hackBaseline>${hack_baseline}</hackBaseline>
<CxPrime>${(image_width+1)/2}</CxPrime>
Expand All @@ -51,3 +62,6 @@
</gazebo>
</xacro:macro>
</robot>



0 comments on commit 0dd4804

Please sign in to comment.