From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Thu, 7 Jan 2021 06:35:45 +0000 (-0300)
Subject: Add parameter to conditionally d435 3D camera.
X-Git-Tag: raulTCC~7^2
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=26759289081d4c273822564c8e61cba0ee8a8a6f;p=twil.git

Add parameter to conditionally d435 3D camera.
---

diff --git a/twil_2dnav/launch/gazebo.launch b/twil_2dnav/launch/gazebo.launch
index 0b1538f..4da76ba 100644
--- a/twil_2dnav/launch/gazebo.launch
+++ b/twil_2dnav/launch/gazebo.launch
@@ -15,6 +15,7 @@
 		<arg name="headless" value="$(arg headless)"/>
 		<arg name="use_sim_time" value="$(arg use_sim_time)"/>
 		<arg name="wam" value="$(arg wam)"/>
+		<arg name="d435" value="true"/>
 		<arg name="world" value="$(arg world)" />
 	</include>
 
diff --git a/twil_description/launch/display.launch b/twil_description/launch/display.launch
index 17abacf..bd41ead 100644
--- a/twil_description/launch/display.launch
+++ b/twil_description/launch/display.launch
@@ -1,16 +1,18 @@
 <launch>
 	<arg name="wam" default="false"/>
 	<arg name="use_gui" default="false"/>
+	<arg name="d435" default="false"/>
 
 	<include file="$(find twil_description)/launch/twil.launch" >
 		<arg name="wam" value="$(arg wam)" />
+		<arg name="d435" value="$(arg d435)" />
 	</include>
 
 	<node pkg="tf2_ros" type="static_transform_publisher" name="twil_origin_publisher" args="0 0 0 0 0 0 1 map twil_origin" />
 
 	<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" args="_use_gui:=$(arg use_gui)" />
 
-	<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
 
 	<node name="rviz" pkg="rviz" type="rviz" args="-d $(find twil_description)/rviz/display.rviz" required="true"/>
 </launch>
diff --git a/twil_description/launch/gazebo.launch b/twil_description/launch/gazebo.launch
index ee15c7f..307df51 100644
--- a/twil_description/launch/gazebo.launch
+++ b/twil_description/launch/gazebo.launch
@@ -3,6 +3,7 @@
 	<arg name="headless" default="false"/>
 	<arg name="use_sim_time" default="true"/>
 	<arg name="wam" default="false"/>
+	<arg name="d435" default="false"/>
 	<arg name="world" default ="worlds/empty_sky.world"/>
 
 	<include file="$(find gazebo_ros)/launch/empty_world.launch">
@@ -14,6 +15,7 @@
 
 	<include file="$(find twil_description)/launch/twil.launch" >
 		<arg name="wam" value="$(arg wam)" />
+		<arg name="d435" value="$(arg d435)" />
 	</include>
 
 	<node name="twil_spawner" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model twil" respawn="false" output="screen" />
diff --git a/twil_description/launch/twil.launch b/twil_description/launch/twil.launch
index 605bffe..6f25266 100644
--- a/twil_description/launch/twil.launch
+++ b/twil_description/launch/twil.launch
@@ -1,6 +1,7 @@
 <launch>
 	<arg name="wam" default="false"/>
+	<arg name="d435" default="false"/>
 
-	<param unless="$(arg wam)" name="robot_description" command="$(find xacro)/xacro '$(find twil_description)/xacro/twil.urdf.xacro'" />
-	<param if="$(arg wam)" name="robot_description" command="$(find xacro)/xacro '$(find twil_description)/xacro/twil_wam.urdf.xacro'" />
+	<param unless="$(arg wam)" name="robot_description" command="$(find xacro)/xacro '$(find twil_description)/xacro/twil.urdf.xacro' d435:=$(arg d435) add_plug:=true" />
+	<param if="$(arg wam)" name="robot_description" command="$(find xacro)/xacro '$(find twil_description)/xacro/twil_wam.urdf.xacro' d435:=$(arg d435) add_plug:=true" />
 </launch>
diff --git a/twil_description/xacro/twil.urdf.xacro b/twil_description/xacro/twil.urdf.xacro
index 4d17d25..0350bac 100644
--- a/twil_description/xacro/twil.urdf.xacro
+++ b/twil_description/xacro/twil.urdf.xacro
@@ -1,6 +1,8 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="twil">
 
+  <xacro:arg name="d435" default="false" />
+
   <xacro:property name="M_PI" value="3.1415926535897931" />  
 
   <xacro:include filename="$(find twil_description)/xacro/chassis.urdf.xacro" />
@@ -16,8 +18,9 @@
   <xacro:include filename="$(find twil_description)/xacro/step_motor.urdf.xacro" />
   <xacro:include filename="$(find twil_description)/xacro/sonar.urdf.xacro" />
 
-  <!--> Added camera description <-->
-  <xacro:include filename="$(find realsense2_description)/urdf/_d435.urdf.xacro" />
+  <xacro:if value="$(arg d435)">
+    <xacro:include filename="$(find realsense2_description)/urdf/_d435.urdf.xacro" />
+  </xacro:if>
 
   <link name="twil_origin" />
 
@@ -117,18 +120,6 @@
     <origin xyz="0.0 0.0 0.787" rpy="0.0 0.0 0.0" />
   </joint>
 
-  <xacro:sensor_d435 parent="tower">
-    <origin xyz="0 0 0.876" rpy="0.0 0.0 0.0" />
-  </xacro:sensor_d435>
-
-  <link name="plugin_camera_link" />
-
-  <joint name="plugin_camera_joint" type="fixed">
-    <parent link="camera_link"/>
-    <child link="plugin_camera_link" />
-    <origin xyz="0.0 0.0 0.0" rpy="${-M_PI/2} 0.0 ${-M_PI/2}" />
-  </joint>
-
   <gazebo>
     <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so" >
       <!-- <robotNamespace>/twil</robotNamespace> -->
@@ -140,55 +131,69 @@
     </plugin>
   </gazebo>
 
-  <gazebo reference="camera_link">
-    <sensor name="camera" type="depth">
-      <update_rate>15</update_rate> <!--or 6 or 30 -->
-      <camera>
-        <horizontal_fov>1.309</horizontal_fov> <!--75 deg at 480p  or 87 deg at 720p-->
-        <image>
-          <width>640</width> <!--or 1280 -->
-          <height>480</height> <!--or 720 -->
-          <format>R8G8B8</format>
-        </image>
-        <clip>
-          <near>0.05</near>
-          <far>10</far>
-        </clip>
-        <noise>
-          <type>gaussian</type>
-          <mean>0.0</mean>
-          <stddev>0.02</stddev>
-        </noise>
-      </camera>
-
-      <plugin name="camera_plugin" filename="libgazebo_ros_openni_kinect.so">
-        <baseline>0.05</baseline>  <!-- dist between sensors -->
-        <alwaysOn>true</alwaysOn>
-        <!-- Keep this zero, update_rate in the parent <sensor> tag
-          will control the frame rate. -->
-        <updateRate>0.0</updateRate>
-        <cameraName>camera</cameraName>
-        <imageTopicName>/camera/depth/image_raw</imageTopicName>
-        <cameraInfoTopicName>/camera/depth/camera_info</cameraInfoTopicName>
-        <depthImageTopicName>/camera/depth/image_raw</depthImageTopicName>
-        <depthImageInfoTopicName>/camera/depth/camera_info</depthImageInfoTopicName>
-        <pointCloudTopicName>/camera/depth/points</pointCloudTopicName>
-        <frameName>plugin_camera_link</frameName>
-        <pointCloudCutoff>0.175</pointCloudCutoff> <!-- or 0.28 at 720p -->
-	<pointCloudCutoffMax>10.0</pointCloudCutoffMax>
-        <distortionK1>0</distortionK1>
-        <distortionK2>0</distortionK2>
-        <distortionK3>0</distortionK3>
-        <distortionT1>0</distortionT1>
-        <distortionT2>0</distortionT2>
-        <CxPrime>0</CxPrime>
-        <Cx>0</Cx>
-        <Cy>0</Cy>
-        <focalLength>0</focalLength>
-        <hackBaseline>0</hackBaseline>
-      </plugin>
-    </sensor>
-  </gazebo>
-
+  <xacro:if value="$(arg d435)">
+    <xacro:sensor_d435 parent="tower">
+      <origin xyz="0 0 0.876" rpy="0.0 0.0 0.0" />
+    </xacro:sensor_d435>
+
+    <link name="plugin_camera_link" />
+
+    <joint name="plugin_camera_joint" type="fixed">
+      <parent link="camera_link"/>
+      <child link="plugin_camera_link" />
+      <origin xyz="0.0 0.0 0.0" rpy="${-M_PI/2} 0.0 ${-M_PI/2}" />
+    </joint>
+
+
+    <gazebo reference="camera_link">
+      <sensor name="camera" type="depth">
+        <update_rate>15</update_rate> <!--or 6 or 30 -->
+        <camera>
+          <horizontal_fov>1.309</horizontal_fov> <!--75 deg at 480p  or 87 deg at 720p-->
+          <image>
+            <width>640</width> <!--or 1280 -->
+            <height>480</height> <!--or 720 -->
+            <format>R8G8B8</format>
+          </image>
+          <clip>
+            <near>0.05</near>
+            <far>10</far>
+          </clip>
+          <noise>
+            <type>gaussian</type>
+            <mean>0.0</mean>
+            <stddev>0.02</stddev>
+          </noise>
+        </camera>
+
+        <plugin name="camera_plugin" filename="libgazebo_ros_openni_kinect.so">
+          <baseline>0.05</baseline>  <!-- dist between sensors -->
+          <alwaysOn>true</alwaysOn>
+          <!-- Keep this zero, update_rate in the parent <sensor> tag
+            will control the frame rate. -->
+          <updateRate>0.0</updateRate>
+          <cameraName>camera</cameraName>
+          <imageTopicName>/camera/depth/image_raw</imageTopicName>
+          <cameraInfoTopicName>/camera/depth/camera_info</cameraInfoTopicName>
+          <depthImageTopicName>/camera/depth/image_raw</depthImageTopicName>
+          <depthImageInfoTopicName>/camera/depth/camera_info</depthImageInfoTopicName>
+          <pointCloudTopicName>/camera/depth/points</pointCloudTopicName>
+          <frameName>plugin_camera_link</frameName>
+          <pointCloudCutoff>0.175</pointCloudCutoff> <!-- or 0.28 at 720p -->
+	  <pointCloudCutoffMax>10.0</pointCloudCutoffMax>
+          <distortionK1>0</distortionK1>
+          <distortionK2>0</distortionK2>
+          <distortionK3>0</distortionK3>
+          <distortionT1>0</distortionT1>
+          <distortionT2>0</distortionT2>
+          <CxPrime>0</CxPrime>
+          <Cx>0</Cx>
+          <Cy>0</Cy>
+          <focalLength>0</focalLength>
+          <hackBaseline>0</hackBaseline>
+        </plugin>
+      </sensor>
+    </gazebo>
+  </xacro:if>
 
 </robot>