From 85275d2c22be19101938ab1b22df74b14e3a14b3 Mon Sep 17 00:00:00 2001
From: jlurgo <jlurgo@savioke.com>
Date: Fri, 17 Jan 2020 14:50:28 -0300
Subject: [PATCH] Dock model created

---
 ca_description/launch/spawn_dock.launch    | 14 +++++++
 ca_description/urdf/dock/dock.xacro        | 48 ++++++++++++++++++++++
 ca_gazebo/launch/create_empty_world.launch |  5 +++
 3 files changed, 67 insertions(+)
 create mode 100644 ca_description/launch/spawn_dock.launch
 create mode 100644 ca_description/urdf/dock/dock.xacro

diff --git a/ca_description/launch/spawn_dock.launch b/ca_description/launch/spawn_dock.launch
new file mode 100644
index 000000000..524d86d6b
--- /dev/null
+++ b/ca_description/launch/spawn_dock.launch
@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="name" default="dock"/>
+  <!-- Load the URDF into the ROS Parameter Server -->
+  <param name="dock_description"
+    command="$(find xacro)/xacro --inorder '$(find ca_description)/urdf/dock/dock.xacro'" />
+
+  <!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
+  <node name="dock_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen" args="
+          -urdf
+          -model $(arg name)
+          -param dock_description
+          "/>
+</launch>
diff --git a/ca_description/urdf/dock/dock.xacro b/ca_description/urdf/dock/dock.xacro
new file mode 100644
index 000000000..172b7ab14
--- /dev/null
+++ b/ca_description/urdf/dock/dock.xacro
@@ -0,0 +1,48 @@
+<?xml version="1.0" ?>
+<robot name="dock" xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:include filename="$(find ca_description)/urdf/common_properties.xacro" />
+  <xacro:macro name="dock" params="width">
+    <link name='back'>
+      <visual name='visual'>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <box size="${width} ${width/3} ${width * 2/3}"/>
+        </geometry>
+      </visual>
+      <collision name='collision'>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <box size="${width} ${width/3} ${width * 2/3}"/>
+        </geometry>
+      </collision>
+      <xacro:inertial_cuboid_with_pose mass="0.15" x_length="${width}"
+          y_length="${width/3}" z_length="${width * 2/3}" >
+        <origin xyz="0 0 0" />
+      </xacro:inertial_cuboid_with_pose>
+    </link>
+    <link name='base'>
+      <visual name='visual'>
+        <origin xyz="0 ${width/2 - width/6} ${-(width * 2/3)/2}" rpy="0 0 0" />
+        <geometry>
+          <box size="${width} ${width} 0.01"/>
+        </geometry>
+      </visual>
+      <collision name='collision'>
+        <origin xyz="0 ${width/2 - width/6} ${-(width * 2/3)/2}" rpy="0 0 0" />
+        <geometry>
+          <box size="${width} ${width} 0.01"/>
+        </geometry>
+      </collision>
+      <xacro:inertial_cuboid_with_pose mass="0.05" x_length="${width}"
+          y_length="${width}" z_length="0.01" >
+        <origin xyz="0 ${width/2 - width/6} ${-(width * 2/3)/2}" rpy="0 0 0" />
+      </xacro:inertial_cuboid_with_pose>
+    </link>
+    <joint name='glue' type='fixed'>
+      <parent link="back" />
+      <child link="base" />
+    </joint>
+  </xacro:macro>
+  <xacro:dock width="0.15">
+  </xacro:dock>
+</robot>
diff --git a/ca_gazebo/launch/create_empty_world.launch b/ca_gazebo/launch/create_empty_world.launch
index 6e2eafb79..70ff9bdb9 100644
--- a/ca_gazebo/launch/create_empty_world.launch
+++ b/ca_gazebo/launch/create_empty_world.launch
@@ -31,6 +31,11 @@
 		<arg name="localization" value="$(arg localization)" />
 	</include>
 
+	<!-- Spawn dock -->
+	<include file="$(find ca_description)/launch/spawn_dock.launch">
+		<arg name="name" value="$(optenv DOCK_NAME dock)" />
+	</include>
+
   <!-- map_server -->
   <include if="$(eval str(arg('localization'))=='amcl')"
 			file="$(find ca_move_base)/launch/map_server.launch">