From 76f1228c87cfee1007a384456a38fb26cdb8c556 Mon Sep 17 00:00:00 2001
From: Hilario Tome <hilario.tome@pal-robotics.com>
Date: Mon, 1 Aug 2016 11:06:09 +0200
Subject: [PATCH] Added imu
---
tor_description/gazebo/gazebo.urdf.xacro | 21 ++-
.../robots/tor_lower_body.urdf.xacro | 8 +
.../urdf/sensors/back_camera.urdf.xacro | 76 ++++++++++
.../urdf/sensors/calibration.xacro | 31 ++++
.../urdf/sensors/ftsensor.gazebo.xacro | 29 ++++
.../urdf/sensors/ftsensor.urdf.xacro | 75 ++++++++++
tor_description/urdf/sensors/imu.urdf.xacro | 33 +++++
.../urdf/sensors/laser.gazebo.xacro | 50 +++++++
tor_description/urdf/sensors/laser.urdf.xacro | 47 ++++++
.../mvbluefox_back_camera.gazebo.xacro | 60 ++++++++
.../urdf/sensors/mvbluefox_camera.urdf.xacro | 41 ++++++
.../urdf/sensors/openni.gazebo.xacro | 79 ++++++++++
.../sensors/prosilicaGC655C_camera.urdf.xacro | 40 +++++
.../prosilica_stereo_camera.gazebo.xacro | 67 +++++++++
.../urdf/sensors/range.gazebo.xacro | 56 +++++++
tor_description/urdf/sensors/range.urdf.xacro | 36 +++++
.../urdf/sensors/stereo_camera.gazebo.xacro | 28 ++++
.../urdf/sensors/stereo_camera.urdf.xacro | 94 ++++++++++++
.../urdf/sensors/xtion_pro_live.urdf.xacro | 138 ++++++++++++++++++
19 files changed, 1008 insertions(+), 1 deletion(-)
create mode 100644 tor_description/urdf/sensors/back_camera.urdf.xacro
create mode 100644 tor_description/urdf/sensors/calibration.xacro
create mode 100644 tor_description/urdf/sensors/ftsensor.gazebo.xacro
create mode 100644 tor_description/urdf/sensors/ftsensor.urdf.xacro
create mode 100644 tor_description/urdf/sensors/imu.urdf.xacro
create mode 100644 tor_description/urdf/sensors/laser.gazebo.xacro
create mode 100644 tor_description/urdf/sensors/laser.urdf.xacro
create mode 100644 tor_description/urdf/sensors/mvbluefox_back_camera.gazebo.xacro
create mode 100644 tor_description/urdf/sensors/mvbluefox_camera.urdf.xacro
create mode 100644 tor_description/urdf/sensors/openni.gazebo.xacro
create mode 100644 tor_description/urdf/sensors/prosilicaGC655C_camera.urdf.xacro
create mode 100644 tor_description/urdf/sensors/prosilica_stereo_camera.gazebo.xacro
create mode 100644 tor_description/urdf/sensors/range.gazebo.xacro
create mode 100644 tor_description/urdf/sensors/range.urdf.xacro
create mode 100644 tor_description/urdf/sensors/stereo_camera.gazebo.xacro
create mode 100644 tor_description/urdf/sensors/stereo_camera.urdf.xacro
create mode 100644 tor_description/urdf/sensors/xtion_pro_live.urdf.xacro
diff --git a/tor_description/gazebo/gazebo.urdf.xacro b/tor_description/gazebo/gazebo.urdf.xacro
index 0ea93c7..da2d1a7 100644
--- a/tor_description/gazebo/gazebo.urdf.xacro
+++ b/tor_description/gazebo/gazebo.urdf.xacro
@@ -18,7 +18,7 @@
</gazebo>
- <gazebo reference="base_link">
+ <gazebo reference="imu_link">
<!-- this is expected to be reparented to pelvis with appropriate offset
when imu_link is reduced by fixed joint reduction -->
<!-- todo: this is working with gazebo 1.4, need to write a unit test -->
@@ -45,6 +45,8 @@
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>
@@ -57,6 +59,23 @@
<bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev>
</accel>
+
+
+ <!--
+ <rate>
+ <mean>0.0</mean>
+ <stddev>0</stddev>
+ <bias_mean>0.000000</bias_mean>
+ <bias_stddev>0.0000008</bias_stddev>
+ </rate>
+ <accel>
+ <mean>0.0</mean>
+ <stddev>0</stddev>
+ <bias_mean>0.0</bias_mean>
+ <bias_stddev>0.000</bias_stddev>
+ </accel>
+ -->
+
</noise>
</imu>
</sensor>
diff --git a/tor_description/robots/tor_lower_body.urdf.xacro b/tor_description/robots/tor_lower_body.urdf.xacro
index 28ca4a3..5cc159c 100644
--- a/tor_description/robots/tor_lower_body.urdf.xacro
+++ b/tor_description/robots/tor_lower_body.urdf.xacro
@@ -47,5 +47,13 @@
<xacro:include filename="$(find tor_description)/gazebo/gazebo.urdf.xacro" />
<!-- Materials for visualization -->
<xacro:include filename="$(find tor_description)/urdf/materials.urdf.xacro" />
+
+
+ <xacro:include filename="$(find tor_description)/urdf/sensors/imu.urdf.xacro" />
+
+ <!-- imu -->
+ <xacro:tor_imu name="imu" parent="base_link" update_rate="100.0">
+ <origin xyz="0 0 0" rpy="0 0 0"/>
+ </xacro:tor_imu>
</robot>
diff --git a/tor_description/urdf/sensors/back_camera.urdf.xacro b/tor_description/urdf/sensors/back_camera.urdf.xacro
new file mode 100644
index 0000000..39b7656
--- /dev/null
+++ b/tor_description/urdf/sensors/back_camera.urdf.xacro
@@ -0,0 +1,76 @@
+<?xml version="1.0"?>
+<!--
+
+ Copyright (c) 2013, PAL Robotics, S.L.
+ All rights reserved.
+
+ This work is licensed under the Creative Commons Attribution-NonCommercial-NoDerivs 3.0 Unported License.
+ 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">
+
+ <!-- back camera macro uses stingrayf033c_camera macros -->
+ <xacro:include filename="$(find reemc_description)/urdf/sensors/mvbluefox_back_camera.gazebo.xacro" />
+ <xacro:include filename="$(find reemc_description)/urdf/sensors/mvbluefox_camera.urdf.xacro" />
+
+ <!-- this macro is used for creating wide and narrow double back camera links -->
+ <xacro:macro name="reemc_back_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" />
+ <parent link="${parent}"/>
+ <child link="${name}_link"/>
+ </joint>
+ <!-- camera link is at center of the optical frame, but in x-forward, z-upwards and y-leftwards notation (required by gazebo to simulate a camera) -->
+ <link name="${name}_link">
+ <inertial>
+ <mass value="0.05" />
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" /> <!-- this inertia is made up for now. -->
+ </inertial>
+
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <box size="0.001 0.001 0.001" />
+ </geometry>
+
+ </visual>
+
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <box size="0.001 0.001 0.001" />
+ </geometry>
+ </collision>
+
+ </link>
+
+ <!-- attach optical frame to the camera link -->
+ <joint name="${name}_optical_frame_joint" type="fixed">
+ <origin xyz="0 0 0" rpy="${-M_PI/2} 0.0 ${-M_PI/2}" /> <!-- rotate frame from x-forward to z-forward camera coords -->
+ <parent link="${name}_link"/>
+ <child link="${name}_optical_frame"/>
+ </joint>
+ <!-- optical frame for the back camera, with z-forward notation, this is the frame back camera images users should refer to -->
+ <link name="${name}_optical_frame" type="camera">
+ <inertial>
+ <mass value="0.05" />
+ <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
+ </inertial>
+ </link>
+
+ <!-- back camera -->
+ <xacro:mvbluefox_camera name="${name}_gazebo_back_camera" parent="${name}_link">
+ <origin xyz="0 0 0" rpy="0.0 0.0 0.0" />
+ </xacro:mvbluefox_camera>
+
+ <!-- Back camera Gazebo simulation -->
+ <xacro:mvbluefox_back_camera_gazebo name="${name}" image_format="${image_format}"
+ hfov="${hfov}" focal_length="${focal_length}"
+ frame_id="${name}_optical_frame"
+ image_width="${image_width}" image_height="${image_height}"/>
+
+ </xacro:macro>
+</robot>
diff --git a/tor_description/urdf/sensors/calibration.xacro b/tor_description/urdf/sensors/calibration.xacro
new file mode 100644
index 0000000..884ff29
--- /dev/null
+++ b/tor_description/urdf/sensors/calibration.xacro
@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<!--
+
+ Copyright (c) 2013, PAL Robotics, S.L.
+ All rights reserved.
+
+ This work is licensed under the Creative Commons Attribution-NonCommercial-NoDerivs 3.0 Unported License.
+ 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.
+
+ Calibration values for sensors:
+
+-->
+<robot>
+
+ <property name="M_PI" value="3.1415926535897931" />
+
+ <!-- Head-mounted ASUS Xtion Pro Live -->
+ <!-- These calibration values affect 2 frames:
+ (name)_xtion_ir_optical_frame
+ (name)_xtion_rgb_optical_frame -->
+ <property name="cal_head_mount_xtion_x" value="0.0" />
+ <property name="cal_head_mount_xtion_y" value="0.0" />
+ <property name="cal_head_mount_xtion_z" value="0.0" />
+ <property name="cal_head_mount_xtion_roll" value="0.0" />
+ <property name="cal_head_mount_xtion_pitch" value="0.0" />
+ <property name="cal_head_mount_xtion_yaw" value="0.0" />
+
+</robot>
+
+
diff --git a/tor_description/urdf/sensors/ftsensor.gazebo.xacro b/tor_description/urdf/sensors/ftsensor.gazebo.xacro
new file mode 100644
index 0000000..6cafe21
--- /dev/null
+++ b/tor_description/urdf/sensors/ftsensor.gazebo.xacro
@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<!--
+ Copyright (c) 2011, PAL Robotics, S.L.
+ All rights reserved.
+
+ This work is licensed under the Creative Commons Attribution-NonCommercial-NoDerivs 3.0 Unported License.
+ 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 name="reemc" xmlns:xacro="http://ros.org/wiki/xacro">
+
+<xacro:macro name="reemc_force_torque_sensor" params="name update_rate">
+ <gazebo reference="${name}">
+ <sensor name="${name}_contact_sensor" type="contact">
+ <pose>0.105 0.071 0 0 0 0</pose>
+ <always_on>1</always_on>
+ <update_rate>${update_rate}</update_rate>
+ <contact>
+ <collision>${name}_collision</collision>
+ </contact>
+ </sensor>
+ </gazebo>
+ <gazebo reference="${name}">
+ <provideFeedback>1</provideFeedback>
+ </gazebo>
+
+</xacro:macro>
+</robot>
diff --git a/tor_description/urdf/sensors/ftsensor.urdf.xacro b/tor_description/urdf/sensors/ftsensor.urdf.xacro
new file mode 100644
index 0000000..cba9ab5
--- /dev/null
+++ b/tor_description/urdf/sensors/ftsensor.urdf.xacro
@@ -0,0 +1,75 @@
+<?xml version="1.0"?>
+<!--
+
+ Copyright (c) 2011-2012, PAL Robotics, S.L.
+ All rights reserved.
+
+ This work is licensed under the Creative Commons Attribution-NonCommercial-NoDerivs 3.0 Unported License.
+ 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">
+
+ <xacro:macro name="ft_sensor" params="name parent side reflect">
+ <!--************************-->
+ <!-- ft sensor -->
+ <!--************************-->
+ <link name="${name}_${side}_ft_link">
+ <inertial>
+ <mass value="0.1" />
+ <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <cylinder radius="${0.045*0.5}" length="0.0157"/>
+ </geometry>
+ <material name="LightGrey" />
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <cylinder radius="${0.045*0.5}" length="0.0157"/>
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="${name}_${side}_ft_joint" type="fixed">
+ <parent link="${parent}" />
+ <child link="${name}_${side}_ft_link" />
+ <origin xyz="${0.0157*0.5} 0 0" rpy="${90.0 * deg_to_rad} 0 ${90.0 * deg_to_rad}" />
+ </joint>
+
+ <!--***********************-->
+ <!-- FT TOOL -->
+ <!--***********************-->
+ <link name="${name}_${side}_ft_tool_link">
+ <inertial>
+ <mass value="0.1" />
+ <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0" />
+ </inertial>
+ <visual>
+ <origin xyz="0.0 0 0" rpy="0 ${90.0 * deg_to_rad} 0" />
+ <geometry>
+ <cylinder radius="${0.05*0.5}" length="0.00975"/>
+ </geometry>
+ <material name="FlatBlack" />
+ </visual>
+ <collision>
+ <origin xyz="0.0 0 0" rpy="0 ${90.0 * deg_to_rad} 0" />
+ <geometry>
+ <cylinder radius="${0.05*0.5}" length="0.00975"/>
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="${name}_${side}_tool_joint" type="fixed">
+ <parent link="${name}_${side}_ft_link" />
+ <child link="${name}_${side}_ft_tool_link" />
+ <origin xyz="0 0 ${0.0157*0.5 + 0.00975*0.5}" rpy="${-90.0 * deg_to_rad} ${-90.0 * deg_to_rad} 0" />
+ </joint>
+
+</xacro:macro>
+
+</robot>
diff --git a/tor_description/urdf/sensors/imu.urdf.xacro b/tor_description/urdf/sensors/imu.urdf.xacro
new file mode 100644
index 0000000..114bca4
--- /dev/null
+++ b/tor_description/urdf/sensors/imu.urdf.xacro
@@ -0,0 +1,33 @@
+<?xml version="1.0"?>
+<!--
+ Copyright (c) 2011-2012, PAL Robotics, S.L.
+ All rights reserved.
+
+ This work is licensed under the Creative Commons Attribution-NonCommercial-NoDerivs 3.0 Unported License.
+ 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 name="tor" xmlns:xacro="http://ros.org/wiki/xacro">
+
+ <xacro:macro name="tor_imu" params="name parent update_rate *origin">
+ <link name="${name}_link">
+ <inertial>
+ <mass value="0.01"/>
+ <origin xyz="0 0 0"/>
+ <inertia ixx="0.000001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.000001"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.01 0.01 0.01"/>
+ </geometry>
+ </visual>
+ </link>
+ <joint name="imu_joint" type="fixed">
+ <insert_block name="origin"/>
+ <parent link="${parent}"/>
+ <child link="${name}_link"/>
+ </joint>
+ </xacro:macro>
+
+</robot>
diff --git a/tor_description/urdf/sensors/laser.gazebo.xacro b/tor_description/urdf/sensors/laser.gazebo.xacro
new file mode 100644
index 0000000..7e06f1e
--- /dev/null
+++ b/tor_description/urdf/sensors/laser.gazebo.xacro
@@ -0,0 +1,50 @@
+<?xml version="1.0"?>
+<!--
+
+ Copyright (c) 2011, PAL Robotics, S.L.
+ All rights reserved.
+
+ This work is licensed under the Creative Commons Attribution-NonCommercial-NoDerivs 3.0 Unported License.
+ 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:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
+ xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
+ xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
+ xmlns:xacro="http://ros.org/wiki/xacro">
+
+<xacro:macro name="reemc_laser_gazebo" params="name ros_topic update_rate min_angle max_angle nrays">
+ <gazebo reference="${name}_link">
+ <sensor type="ray" name="${name}_sensor">
+ <pose>0 0 0 0 0 0</pose>
+ <visualize>false</visualize>
+ <update_rate>${update_rate}</update_rate>
+ <ray>
+ <scan>
+ <horizontal>
+ <samples>${nrays}</samples>
+ <resolution>1</resolution>
+ <min_angle>${min_angle}</min_angle>
+ <max_angle>${max_angle}</max_angle>
+ </horizontal>
+ </scan>
+ <range>
+ <min>0.02</min>
+ <max>5.6</max>
+ <resolution>0.01</resolution>
+ </range>
+ </ray>
+ <plugin filename="libgazebo_ros_laser.so" name="gazebo_ros_laser" >
+ <frameName>/${name}_link</frameName>
+ <topicName>${ros_topic}</topicName>
+ <gaussianNoise>0.03</gaussianNoise>
+ <hokuyoMinIntensity>101</hokuyoMinIntensity>
+ <updateRate>${update_rate}</updateRate>
+ <alwaysOn>true</alwaysOn>
+ </plugin>
+ </sensor>
+ </gazebo>
+</xacro:macro>
+
+
+</robot>
diff --git a/tor_description/urdf/sensors/laser.urdf.xacro b/tor_description/urdf/sensors/laser.urdf.xacro
new file mode 100644
index 0000000..67b639e
--- /dev/null
+++ b/tor_description/urdf/sensors/laser.urdf.xacro
@@ -0,0 +1,47 @@
+<?xml version="1.0"?>
+<!--
+
+ Copyright (c) 2011, PAL Robotics, S.L.
+ All rights reserved.
+
+ This work is licensed under the Creative Commons Attribution-NonCommercial-NoDerivs 3.0 Unported License.
+ 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">
+
+ <xacro:include filename="$(find reemc_description)/urdf/sensors/laser.gazebo.xacro" />
+
+ <xacro:macro name="reemc_laser" params="name parent *origin ros_topic update_rate min_angle max_angle nrays">
+
+ <link name="${name}_link">
+ <inertial>
+ <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
+ <mass value="0.001" />
+ <inertia iyy="4.2412E-05" ixy="4.9927E-08" iyz="-9.8165E-09" ixx="3.7174E-05" ixz="1.1015E-05" izz="4.167E-05" />
+ </inertial>
+ <!-- <visual>
+ <origin xyz="0.0 0 0.05" rpy="0 0 ${90.0*deg_to_rad}" />
+ <geometry>
+ <mesh filename="package://reemc_description/meshes/sensors/urg-04lx-ug01.stl" scale="0.001 0.001 0.001"/>
+ </geometry>
+ <material name="DarkGrey" />
+ </visual>
+ -->
+ </link>
+
+ <joint name="${name}_joint" type="fixed">
+ <axis xyz="0 1 0" />
+ <insert_block name="origin" />
+ <parent link="${parent}"/>
+ <child link="${name}_link"/>
+ <dynamics friction="1.0" damping="1"/>
+ <limit lower="0" upper="0" effort="60" velocity="20" />
+ </joint>
+
+ <!-- gazebo extensions -->
+ <xacro:reemc_laser_gazebo name="${name}" ros_topic="${ros_topic}" update_rate="${update_rate}" min_angle="${min_angle}" max_angle="${max_angle}" nrays="${nrays}"/>
+
+ </xacro:macro>
+
+</robot>
diff --git a/tor_description/urdf/sensors/mvbluefox_back_camera.gazebo.xacro b/tor_description/urdf/sensors/mvbluefox_back_camera.gazebo.xacro
new file mode 100644
index 0000000..df00ee7
--- /dev/null
+++ b/tor_description/urdf/sensors/mvbluefox_back_camera.gazebo.xacro
@@ -0,0 +1,60 @@
+<?xml version="1.0"?>
+<!--
+
+ Copyright (c) 2013, PAL Robotics, S.L.
+ All rights reserved.
+
+ This work is licensed under the Creative Commons Attribution-NonCommercial-NoDerivs 3.0 Unported License.
+ 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 name="reemc" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
+ xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
+ xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
+ xmlns:xacro="http://ros.org/wiki/xacro">
+
+ <xacro:macro name="mvbluefox_back_camera_gazebo" params="name image_format hfov focal_length frame_id image_width image_height">
+
+ <gazebo reference="${name}_link">
+
+ <sensor type="camera" name="gazebo_${name}_camera">
+ <update_rate>15.0</update_rate>
+ <camera>
+ <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>
+ <plugin name="monocular_camera_controller" filename="libgazebo_ros_camera.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>15.0</updateRate>
+ <cameraName>${name}</cameraName>
+ <imageTopicName>image</imageTopicName>
+ <cameraInfoTopicName>camera_info</cameraInfoTopicName>
+ <frameName>${frame_id}</frameName>
+ <hackBaseline>0</hackBaseline>
+ <CxPrime>${(image_width+1)/2}</CxPrime>
+ <Cx>${(image_width+1)/2}</Cx>
+ <Cy>${(image_height+1)/2}</Cy>
+ <focalLength>${focal_length}</focalLength> <!-- image_width / (2*tan(hfov_radian /2)) -->
+ <distortionK1>0.0</distortionK1>
+ <distortionK2>0.0</distortionK2>
+ <distortionK3>0.0</distortionK3>
+ <distortionT1>0.0</distortionT1>
+ <distortionT2>0.0</distortionT2>
+ </plugin>
+ </sensor>
+
+ </gazebo>
+ </xacro:macro>
+
+</robot>
+
+
+
diff --git a/tor_description/urdf/sensors/mvbluefox_camera.urdf.xacro b/tor_description/urdf/sensors/mvbluefox_camera.urdf.xacro
new file mode 100644
index 0000000..f9b1042
--- /dev/null
+++ b/tor_description/urdf/sensors/mvbluefox_camera.urdf.xacro
@@ -0,0 +1,41 @@
+<?xml version="1.0"?>
+<!--
+
+ Copyright (c) 2013, PAL Robotics, S.L.
+ All rights reserved.
+
+ This work is licensed under the Creative Commons Attribution-NonCommercial-NoDerivs 3.0 Unported License.
+ 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.
+-->
+<!-- XML namespaces -->
+<robot name="reemc" xmlns:xacro="http://ros.org/wiki/xacro">
+
+ <xacro:macro name="mvbluefox_camera" params="name parent *origin">
+
+ <joint name="${name}_frame_joint" type="fixed">
+ <insert_block name="origin" />
+ <parent link="${parent}"/>
+ <child link="${name}_link"/>
+ </joint>
+
+ <link name="${name}_link">
+ <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>
+ <visual>
+ <origin xyz="-0.01 0 0" rpy="0 ${deg_to_rad * 90} 0" />
+ <geometry>
+ <cylinder radius="0.01" length="0.02" />
+ </geometry>
+ <material name="Blue"/>
+ </visual>
+ </link>
+
+ </xacro:macro>
+
+</robot>
diff --git a/tor_description/urdf/sensors/openni.gazebo.xacro b/tor_description/urdf/sensors/openni.gazebo.xacro
new file mode 100644
index 0000000..b0f20b1
--- /dev/null
+++ b/tor_description/urdf/sensors/openni.gazebo.xacro
@@ -0,0 +1,79 @@
+<?xml version="1.0"?>
+<!--
+
+ Copyright (c) 2013, PAL Robotics, S.L.
+ All rights reserved.
+
+ This work is licensed under the Creative Commons Attribution-NonCommercial-NoDerivs 3.0 Unported License.
+ 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.
+
+ ROS Gazebo plugin for RGBD sensor (OpenNI/Kinect/ASUS)
+ * Publishes MONO8 format "depth" image & PointCloud
+ * Publishes RGB8 format color image & 2nd copy of PointCloud (for registered)
+
+ Sensor parameters are from PR2 description.
+-->
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+ <xacro:macro name="openni_ir_gazebo_v0" params="link_name frame_name camera_name">
+ <gazebo reference="${link_name}">
+ <sensor:camera name="${name}_ir_sensor">
+ <imageFormat>L8</imageFormat>
+ <imageSize>640 480</imageSize>
+ <hfov>57</hfov>
+ <nearClip>0.01</nearClip>
+ <farClip>5</farClip>
+ <updateRate>1.0</updateRate>
+ <baseline>0.2</baseline>
+ <controller:gazebo_ros_openni_kinect name="${link_name}_controller" plugin="libgazebo_ros_openni_kinect.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1.0</updateRate>
+ <cameraName>${camera_name}_ir</cameraName>
+ <imageTopicName>/${camera_name}/depth/image_raw</imageTopicName>
+ <cameraInfoTopicName>/${camera_name}/depth/camera_info</cameraInfoTopicName>
+ <depthImageTopicName>/${camera_name}/depth/image_raw</depthImageTopicName>
+ <depthImageInfoTopicName>/${camera_name}/depth/camera_info</depthImageInfoTopicName>
+ <pointCloudTopicName>/${camera_name}/depth/points</pointCloudTopicName>
+ <frameName>${frame_name}</frameName>
+ <pointCloudCutoff>0.5</pointCloudCutoff>
+ <distortionK1>0.00000001</distortionK1>
+ <distortionK2>0.00000001</distortionK2>
+ <distortionK3>0.00000001</distortionK3>
+ <distortionT1>0.00000001</distortionT1>
+ <distortionT2>0.00000001</distortionT2>
+ </controller:gazebo_ros_openni_kinect>
+ </sensor:camera>
+ </gazebo>
+ </xacro:macro>
+
+ <xacro:macro name="openni_rgb_gazebo_v0" params="link_name frame_name camera_name">
+ <gazebo reference="${link_name}">
+ <sensor:camera name="${name}_rgb_sensor">
+ <imageFormat>R8G8B8</imageFormat>
+ <imageSize>640 480</imageSize>
+ <hfov>57</hfov>
+ <nearClip>0.01</nearClip>
+ <farClip>5</farClip>
+ <updateRate>1.0</updateRate>
+ <baseline>0.2</baseline>
+ <controller:gazebo_ros_openni_kinect name="${link_name}_controller" plugin="libgazebo_ros_openni_kinect.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1.0</updateRate>
+ <cameraName>${camera_name}_rgb</cameraName>
+ <imageTopicName>/${camera_name}/rgb/image_raw</imageTopicName>
+ <cameraInfoTopicName>/${camera_name}/rgb/camera_info</cameraInfoTopicName>
+ <pointCloudTopicName>/${camera_name}/depth_registered/points</pointCloudTopicName>
+ <frameName>${frame_name}</frameName>
+ <pointCloudCutoff>0.5</pointCloudCutoff>
+ <distortionK1>0.00000001</distortionK1>
+ <distortionK2>0.00000001</distortionK2>
+ <distortionK3>0.00000001</distortionK3>
+ <distortionT1>0.00000001</distortionT1>
+ <distortionT2>0.00000001</distortionT2>
+ </controller:gazebo_ros_openni_kinect>
+ </sensor:camera>
+ </gazebo>
+ </xacro:macro>
+
+</robot>
diff --git a/tor_description/urdf/sensors/prosilicaGC655C_camera.urdf.xacro b/tor_description/urdf/sensors/prosilicaGC655C_camera.urdf.xacro
new file mode 100644
index 0000000..8134fa6
--- /dev/null
+++ b/tor_description/urdf/sensors/prosilicaGC655C_camera.urdf.xacro
@@ -0,0 +1,40 @@
+<?xml version="1.0"?>
+<!--
+
+ Copyright (c) 2013, PAL Robotics, S.L.
+ All rights reserved.
+
+ This work is licensed under the Creative Commons Attribution-NonCommercial-NoDerivs 3.0 Unported License.
+ 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.
+-->
+<!-- XML namespaces -->
+<robot name="reemc" xmlns:xacro="http://www.ros.org/wiki/xacro">
+ <xacro:macro name="prosilicaGC655C_camera" params="name parent *origin">
+
+ <joint name="${name}_frame_joint" type="fixed">
+ <insert_block name="origin" />
+ <parent link="${parent}"/>
+ <child link="${name}_link"/>
+ </joint>
+
+ <link name="${name}_link">
+ <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>
+ <visual>
+ <origin xyz="-0.01 0 0" rpy="0 ${deg_to_rad * 90} 0" />
+ <geometry>
+ <cylinder radius="0.01" length="0.018" />
+ </geometry>
+ <material name="Blue"/>
+ </visual>
+ </link>
+
+ </xacro:macro>
+
+</robot>
diff --git a/tor_description/urdf/sensors/prosilica_stereo_camera.gazebo.xacro b/tor_description/urdf/sensors/prosilica_stereo_camera.gazebo.xacro
new file mode 100644
index 0000000..3cf6c28
--- /dev/null
+++ b/tor_description/urdf/sensors/prosilica_stereo_camera.gazebo.xacro
@@ -0,0 +1,67 @@
+<?xml version="1.0"?>
+<!--
+
+ Copyright (c) 2013, PAL Robotics, S.L.
+ All rights reserved.
+
+ This work is licensed under the Creative Commons Attribution-NonCommercial-NoDerivs 3.0 Unported License.
+ 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 name="reemc" xmlns:xacro="http://ros.org/wiki/xacro">
+
+ <xacro:macro name="prosilica_stereo_camera_gazebo" params="name image_format hfov focal_length frame_id hack_baseline image_width image_height">
+ <gazebo reference="${name}_link">
+
+ <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>
+ <height> ${image_height}</height>
+ <format>${image_format}</format>
+ </image>
+ <clip>
+ <near>0.01</near>
+ <far>100</far>
+ </clip>
+ </camera>
+ <plugin name="stereo_camera_controller" filename="libgazebo_ros_multicamera.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>25.0</updateRate>
+ <cameraName>${name}</cameraName>
+ <imageTopicName>image</imageTopicName>
+ <cameraInfoTopicName>camera_info</cameraInfoTopicName>
+ <frameName>${frame_id}</frameName>
+ <hackBaseline>${hack_baseline}</hackBaseline>
+ <CxPrime>${(image_width+1)/2}</CxPrime>
+ <Cx>${(image_width+1)/2}</Cx>
+ <Cy>${(image_height+1)/2}</Cy>
+ <focalLength>${focal_length}</focalLength> <!-- image_width / (2*tan(hfov_radian /2)) -->
+ <distortionK1>0.0</distortionK1>
+ <distortionK2>0.0</distortionK2>
+ <distortionK3>0.0</distortionK3>
+ <distortionT1>0.0</distortionT1>
+ <distortionT2>0.0</distortionT2>
+ </plugin>
+ </sensor>
+ </gazebo>
+ </xacro:macro>
+</robot>
+
+
+
diff --git a/tor_description/urdf/sensors/range.gazebo.xacro b/tor_description/urdf/sensors/range.gazebo.xacro
new file mode 100644
index 0000000..73806ef
--- /dev/null
+++ b/tor_description/urdf/sensors/range.gazebo.xacro
@@ -0,0 +1,56 @@
+<?xml version="1.0"?>
+
+<!--
+ Copyright (c) 2011, PAL Robotics, S.L.
+ All rights reserved.
+
+ This work is licensed under the Creative Commons Attribution-NonCommercial-NoDerivs 3.0 Unported License.
+ 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">
+
+ <xacro:macro name="range_reem_gazebo" params="name ros_topic update_rate maxRange minRange fov radiation" >
+ <gazebo reference="${name}_link">
+ <sensor type="ray" name="${name}">
+ <pose>0 0 0 0 0 0</pose>
+ <update_rate>${update_rate}</update_rate>
+ <visualize>false</visualize>
+ <ray>
+ <scan>
+ <horizontal>
+ <samples>5</samples>
+ <resolution>1</resolution>
+ <min_angle>-${fov/2}</min_angle>
+ <max_angle>${fov/2}</max_angle>
+ </horizontal>
+ <vertical>
+ <samples>5</samples>
+ <resolution>1</resolution>
+ <min_angle>-${fov/2}</min_angle>
+ <max_angle>${fov/2}</max_angle>
+ </vertical>
+ </scan>
+ <range>
+ <min>${minRange}</min>
+ <max>${maxRange}</max>
+ <resolution>0.01</resolution>
+ </range>
+ </ray>
+ <plugin filename="libgazebo_ros_range.so" name="gazebo_ros_range">
+ <gaussianNoise>0.005</gaussianNoise>
+ <alwaysOn>true</alwaysOn>
+ <updateRate>${update_rate}</updateRate>
+ <topicName>${ros_topic}</topicName>
+ <frameName>${name}_link</frameName>
+ <minRange>${minRange}</minRange>
+ <maxRange>${maxRange}</maxRange>
+ <fov>${fov}</fov>
+ <radiation>${radiation}</radiation>
+ </plugin>
+ </sensor>
+ </gazebo>
+ </xacro:macro>
+
+</robot>
diff --git a/tor_description/urdf/sensors/range.urdf.xacro b/tor_description/urdf/sensors/range.urdf.xacro
new file mode 100644
index 0000000..c9177fc
--- /dev/null
+++ b/tor_description/urdf/sensors/range.urdf.xacro
@@ -0,0 +1,36 @@
+<?xml version="1.0"?>
+
+<!--
+ Copyright (c) 2011, PAL Robotics, S.L.
+ All rights reserved.
+
+ This work is licensed under the Creative Commons Attribution-NonCommercial-NoDerivs 3.0 Unported License.
+ 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">
+
+ <xacro:include filename="$(find reemc_description)/urdf/sensors/range.gazebo.xacro" />
+
+ <xacro:macro name="range_reem" params="name parent *origin ros_topic update_rate maxRange minRange fov radiation">
+
+ <link name="${name}_link">
+ <inertial>
+ <mass value="0.001" />
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.0001" />
+ </inertial>
+ </link>
+
+ <joint name="${name}_joint" type="fixed">
+ <insert_block name="origin" />
+ <axis xyz="0 0 1"/>
+ <parent link="${parent}_link"/>
+ <child link="${name}_link"/>
+ </joint>
+
+ <!-- gazebo extensions -->
+ <xacro:range_reem_gazebo name="${name}" ros_topic="${ros_topic}" update_rate="${update_rate}" maxRange="${maxRange}" minRange="${minRange}" fov="${fov}" radiation="${radiation}"/>
+ </xacro:macro>
+</robot>
diff --git a/tor_description/urdf/sensors/stereo_camera.gazebo.xacro b/tor_description/urdf/sensors/stereo_camera.gazebo.xacro
new file mode 100644
index 0000000..8e8b59d
--- /dev/null
+++ b/tor_description/urdf/sensors/stereo_camera.gazebo.xacro
@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<!--
+
+ Copyright (c) 2011, PAL Robotics, S.L.
+ All rights reserved.
+
+ This work is licensed under the Creative Commons Attribution-NonCommercial-NoDerivs 3.0 Unported License.
+ 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 name="reemc" xmlns:xacro="http://ros.org/wiki/xacro">
+
+ <!-- this macro is used for creating wide and narrow double stereo camera in simulation -->
+ <xacro:macro name="stereo_camera_gazebo_v0" params="name focal_length_left hfov_left focal_length_right hfov_right image_width image_height">
+
+ <gazebo reference="${name}_link">
+ <material value="White"/>
+ <gravity value="false"/>
+ </gazebo>
+
+ <gazebo reference="${name}_optical_frame">
+ <material value="White"/>
+ <gravity value="false"/>
+ </gazebo>
+
+ </xacro:macro>
+
+</robot>
diff --git a/tor_description/urdf/sensors/stereo_camera.urdf.xacro b/tor_description/urdf/sensors/stereo_camera.urdf.xacro
new file mode 100644
index 0000000..bfbb3ef
--- /dev/null
+++ b/tor_description/urdf/sensors/stereo_camera.urdf.xacro
@@ -0,0 +1,94 @@
+<?xml version="1.0"?>
+<!--
+
+ Copyright (c) 2014, PAL Robotics, S.L.
+ All rights reserved.
+
+ This work is licensed under the Creative Commons Attribution-NonCommercial-NoDerivs 3.0 Unported License.
+ 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 name="reemc" xmlns:xacro="http://ros.org/wiki/xacro">
+
+ <xacro:include filename="$(find reemc_description)/urdf/sensors/prosilica_stereo_camera.gazebo.xacro" />
+ <xacro:include filename="$(find reemc_description)/urdf/sensors/prosilicaGC655C_camera.urdf.xacro" />
+
+ <property name="stereo_dx" value="0.00" />
+ <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="reemc_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" />
+ <parent link="${parent}"/>
+ <child link="${name}_link"/>
+ </joint>
+ <!-- camera link is at center of the optical frame, but in x-forward, z-upwards and y-leftwards notation (required by gazebo to simulate a camera) -->
+ <link name="${name}_link">
+ <inertial>
+ <mass value="0.01" />
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <inertia ixx="0" ixy="0" ixz="0"
+ iyy="0" iyz="0"
+ izz="0" /> <!-- this inertia is made up for now. -->
+ </inertial>
+
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <box size="0.001 0.001 0.001" />
+ </geometry>
+ <material name="Blue"/>
+ </visual>
+
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <box size="0.001 0.001 0.001" />
+ </geometry>
+ </collision>
+
+ </link>
+
+ <!-- attach optical frame to the camera link -->
+ <joint name="${name}_optical_frame_joint" type="fixed">
+ <origin xyz="0 0 0" rpy="${-M_PI/2} 0.0 ${-M_PI/2}" /> <!-- rotate frame from x-forward to z-forward camera coords -->
+ <parent link="${name}_link"/>
+ <child link="${name}_optical_frame"/>
+ </joint>
+ <!-- optical frame for the stereo camera, with z-forward notation, this is the frame stereo camera images users should refer to -->
+ <link name="${name}_optical_frame" type="camera">
+ <inertial>
+ <mass value="0.01" />
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <inertia ixx="0" ixy="0" ixz="0"
+ iyy="0" iyz="0"
+ izz="0" /> <!-- this inertia is made up for now. -->
+ </inertial>
+ </link>
+
+ <!-- stereo left camera -->
+ <xacro:prosilicaGC655C_camera name="${name}_gazebo_left_camera" parent="${name}_link">
+ <origin xyz="0 0 0" rpy="0.0 0.0 0.0" />
+ </xacro:prosilicaGC655C_camera>
+
+ <!-- stereo right camera -->
+ <xacro:prosilicaGC655C_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:prosilicaGC655C_camera>
+
+
+ <!-- Stereo camera Gazebo simulation -->
+ <xacro:prosilica_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>
diff --git a/tor_description/urdf/sensors/xtion_pro_live.urdf.xacro b/tor_description/urdf/sensors/xtion_pro_live.urdf.xacro
new file mode 100644
index 0000000..1ac27d4
--- /dev/null
+++ b/tor_description/urdf/sensors/xtion_pro_live.urdf.xacro
@@ -0,0 +1,138 @@
+<?xml version="1.0"?>
+<!--
+
+ Copyright (c) 2013, PAL Robotics, S.L.
+ All rights reserved.
+
+ This work is licensed under the Creative Commons Attribution-NonCommercial-NoDerivs 3.0 Unported License.
+ 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.
+
+ ASUS Xtion Pro Live
+ - visual model, without base
+ - collision geometry
+ - TF frames
+ - loads Gazebo plugin
+
+ Note: There is a bug that Gazebo in ROS Fuerte ignores the roll/pitch/yaw in the visual tag,
+ therefore it needs to be in a separate link, with the orientation specified at the joint.
+
+ ToDo: Fix visual mesh, re-scale it, & the origin is not centered along the frontal axis.
+ Can simplify collision mesh.
+ Rotate origin in mesh, to match the orientation of the base link and bypass the bug.
+
+-->
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+ <xacro:include filename="$(find reemc)/urdf/sensors/calibration.xacro" />
+ <xacro:include filename="$(find reemc)/urdf/sensors/openni.gazebo.xacro" />
+
+ <property name="M_PI" value="3.1415926535897931" />
+
+ <!-- Macro -->
+ <xacro:macro name="xtion_pro_live" params="name parent *origin *calibration_origin">
+ <!-- base link -->
+ <joint name="${name}_joint" type="fixed">
+ <insert_block name="origin" />
+ <parent link="${parent}"/>
+ <child link="${name}_link"/>
+ </joint>
+ <link name="${name}_link">
+ <inertial>
+ <mass value="0.01" />
+ <origin xyz="0 0 0" rpy="0 0 0"/>
+ <inertia ixx="0.0" ixy="0.0" ixz="0.0"
+ iyy="0.0" iyz="0.0"
+ izz="0.0" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0"/>
+ <geometry>
+ <mesh filename="package://reemc/meshes/sensors/xtion_pro_live/xtion_pro_live.dae" />
+ </geometry>
+ <material name="Grey">
+ <color rgba="0.5 0.5 0.5 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin xyz="-0.02 0.0025 0" rpy="0 0 0"/> <!--TODO: Tune x coordinate. In Gazebo 1.0.2 collision geom conflicts with RGBD sensor, WTF?!-->
+ <geometry>
+ <box size="0.04 0.185 0.03"/>
+ </geometry>
+ </collision>
+ </link>
+
+ <!-- Xtion IR sensor physical attachment -->
+ <joint name="${name}_ir_joint" type="fixed">
+ <!-- This relates the IR sensor to the Xtion base link -->
+ <!-- 4.8 cm because sensors are offset to right, from center -->
+ <origin xyz="0 0.048 0" rpy="0 0 0"/>
+ <parent link="${name}_link"/>
+ <child link="${name}_ir_link"/>
+ </joint>
+ <link name="${name}_ir_link">
+ <!-- This link is the reference for the IR sensor in Gazebo, so it has inertia properties -->
+ <inertial>
+ <mass value="0.01" />
+ <origin xyz="0 0 0" rpy="0 0 0"/>
+ <inertia ixx="0.0" ixy="0.0" ixz="0.0"
+ iyy="0.0" iyz="0.0"
+ izz="0.0" />
+ </inertial>
+ </link>
+ <!-- Xtion IR sensor frame -->
+ <joint name="${name}_ir_optical_frame_joint" type="fixed">
+ <insert_block name="calibration_origin" /> <!-- macro argument -->
+ <parent link="${name}_ir_link"/>
+ <child link="${name}_ir_optical_frame"/>
+ </joint>
+ <link name="${name}_ir_optical_frame">
+ <inertial>
+ <mass value="0.01" />
+ <origin xyz="0 0 0" rpy="0 0 0"/>
+ <inertia ixx="0.0" ixy="0.0" ixz="0.0"
+ iyy="0.0" iyz="0.0"
+ izz="0.0" />
+ </inertial>
+ </link>
+ <!-- OpenNI IR sensor Gazebo extensions -->
+ <xacro:openni_ir_gazebo_v0 link_name="${name}_ir_link" frame_name="${name}_ir_optical_frame" camera_name="${name}" />
+
+ <!-- Xtion RGB sensor physical attachment -->
+ <joint name="${name}_rgb_joint" type="fixed">
+ <!-- The measured distance from RGB lens back to IR lens, approx 2.5cm, but PR2 is using 3.0cm -->
+ <origin xyz="0 -0.025 0" rpy="0 0 0"/>
+ <parent link="${name}_ir_link"/>
+ <child link="${name}_rgb_link"/>
+ </joint>
+ <link name="${name}_rgb_link" >
+ <!-- This link is the reference for the RGB sensor in Gazebo, so it has inertia properties -->
+ <inertial>
+ <mass value="0.01" />
+ <origin xyz="0 0 0" rpy="0 0 0"/>
+ <inertia ixx="0.0" ixy="0.0" ixz="0.0"
+ iyy="0.0" iyz="0.0"
+ izz="0.0" />
+ </inertial>
+ </link>
+ <!-- Xtion RGB sensor frame -->
+ <joint name="${name}_rgb_optical_frame_joint" type="fixed">
+ <insert_block name="calibration_origin" /> <!-- macro argument -->
+ <parent link="${name}_rgb_link"/>
+ <child link="${name}_rgb_optical_frame"/>
+ </joint>
+ <link name="${name}_rgb_optical_frame">
+ <inertial>
+ <mass value="0.01" />
+ <origin xyz="0 0 0" rpy="0 0 0"/>
+ <inertia ixx="0.0" ixy="0.0" ixz="0.0"
+ iyy="0.0" iyz="0.0"
+ izz="0.0" />
+ </inertial>
+ </link>
+ <!-- OpenNI RGB sensor Gazebo extensions -->
+ <xacro:openni_rgb_gazebo_v0 link_name="${name}_rgb_link" frame_name="${name}_rgb_optical_frame" camera_name="${name}" />
+
+ </xacro:macro>
+
+</robot>
--
GitLab