From 262d8f848600a9405676e5a2f705ac1f3e26a102 Mon Sep 17 00:00:00 2001
From: grpetry <gabriel.petry26@gmail.com>
Date: Wed, 12 Jun 2019 14:37:39 -0300
Subject: [PATCH] Fixed launch and added camera

---
 twil_bringup/launch/gazebo.launch      |  2 +-
 twil_description/xacro/twil.urdf.xacro | 57 ++++++++++++++++++++++++++++++++++
 2 files changed, 58 insertions(+), 1 deletion(-)

diff --git a/twil_bringup/launch/gazebo.launch b/twil_bringup/launch/gazebo.launch
index 8b24b80..a3065c6 100644
--- a/twil_bringup/launch/gazebo.launch
+++ b/twil_bringup/launch/gazebo.launch
@@ -5,7 +5,7 @@
 	<arg name="wam" default="false"/>
 	
 	<arg name="controller" default="joint_effort_controller"/>
-        <arg name="config" default="$(find wam_bringup)/config/$(arg controller).yaml"/>
+        <arg name="config" default="$(find twil_bringup)/config/$(arg controller).yaml"/>
 
 	<include file="$(find twil_description)/launch/gazebo.launch" >
 		<arg name="paused" value="$(arg paused)"/>
diff --git a/twil_description/xacro/twil.urdf.xacro b/twil_description/xacro/twil.urdf.xacro
index 00e592c..7ee5cd3 100644
--- a/twil_description/xacro/twil.urdf.xacro
+++ b/twil_description/xacro/twil.urdf.xacro
@@ -13,6 +13,9 @@
   <xacro:include filename="$(find twil_description)/xacro/cpu.urdf.xacro" />
   <xacro:include filename="$(find twil_description)/xacro/eurocard.urdf.xacro" />
 
+  <!--> Added camera description <-->
+  <xacro:include filename="$(find realsense2_description)/urdf/_d435.urdf.xacro" />
+
   <link name="twil_origin" />
 
   <xacro:chassis name="chassis" parent="twil_origin">
@@ -95,6 +98,10 @@
     <origin xyz="0.0 0.0 0.787" rpy="0.0 0.0 0.0" />
   </joint>
 
+  <xacro:sensor_d435 parent="chassis_top">
+    <origin xyz="0 0 0" rpy="0.0 0.0 0.0" />
+  </xacro:sensor_d435>
+
   <gazebo>
     <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so" >
       <!-- <robotNamespace>/twil</robotNamespace> -->
@@ -106,5 +113,55 @@
     </plugin>
   </gazebo>
 
+  <gazebo reference="camera_link">
+    <sensor type="depth" name="camera">
+      <update_rate>30.0</update_rate>
+      <!-- math.atan(320 / 687.8065795898438) * 2 -->
+      <camera name="camera">
+        <horizontal_fov>0.8709216071359963</horizontal_fov>
+        <image>
+          <width>1280</width>
+          <height>720</height>
+          <format>B8G8R8</format>
+        </image>
+        <clip>
+          <near>1</near>
+          <far>20</far>
+        </clip>
+        <noise>
+          <type>gaussian</type>
+          <mean>0.0</mean>
+          <stddev>0.007</stddev>
+        </noise>
+      </camera>
+
+      <plugin name="camera_plugin" filename="libgazebo_ros_openni_kinect.so">
+        <baseline>0.2</baseline>
+        <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>camera_link</frameName>
+        <pointCloudCutoff>0.05</pointCloudCutoff>
+        <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>
+
 
 </robot>
-- 
2.12.0