From 9c20a644404dc81e72d695955ebbc244b3c19556 Mon Sep 17 00:00:00 2001
From: Tan Pin Da <tan.pin.da.1996@gmail.com>
Date: Mon, 7 Sep 2020 13:05:40 +0800
Subject: [PATCH] added scout description to scout_base launch

---
 scout_base/description/scout_v2.xacro         | 94 +++++++++++++++++++
 .../sub_elements/scout_wheel_type1.xacro      | 59 ++++++++++++
 .../sub_elements/scout_wheel_type2.xacro      | 59 ++++++++++++
 scout_base/launch/scout_base.launch           |  4 +
 4 files changed, 216 insertions(+)
 create mode 100644 scout_base/description/scout_v2.xacro
 create mode 100644 scout_base/description/sub_elements/scout_wheel_type1.xacro
 create mode 100644 scout_base/description/sub_elements/scout_wheel_type2.xacro

diff --git a/scout_base/description/scout_v2.xacro b/scout_base/description/scout_v2.xacro
new file mode 100644
index 0000000..a3b96ab
--- /dev/null
+++ b/scout_base/description/scout_v2.xacro
@@ -0,0 +1,94 @@
+<?xml version="1.0"?>
+
+<robot name="scout_v2" 
+    xmlns:xacro="http://ros.org/wiki/xacro">
+
+    <xacro:arg name="robot_namespace" default="/" />
+    <xacro:arg name="onboard_sensors" default="false" />
+
+    <xacro:include filename="$(find scout_base)/description/sub_elements/scout_wheel_type1.xacro" />
+    <xacro:include filename="$(find scout_base)/description/sub_elements/scout_wheel_type2.xacro" />
+
+    <!-- Variables -->
+    <xacro:property name="M_PI" value="3.14159"/>
+
+    <!-- Vehicle Geometries -->
+    <xacro:property name="base_x_size" value="0.9250000" />
+    <xacro:property name="base_y_size" value="0.380000" />
+    <xacro:property name="base_z_size" value="0.210000" />
+
+    <xacro:property name="wheelbase" value="0.498" />
+    <xacro:property name="track" value="0.58306" />
+    <xacro:property name="wheel_vertical_offset" value="-0.0702" />
+
+    <xacro:property name="wheel_length" value="1.1653e-01" />
+    <xacro:property name="wheel_radius" value="1.6459e-01" />
+
+    <!-- Base link -->
+    <link name="base_link">
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://scout_description/meshes/base_link.dae" />
+            </geometry>
+        </visual>
+        <collision>
+            <origin xyz="0 0 0.008" rpy="0 0 0" />
+            <geometry>
+                <box size="${base_x_size} ${base_y_size} ${base_z_size}"/>
+            </geometry>
+        </collision>
+        <collision>
+            <origin xyz="0 0 ${base_z_size/6}" rpy="0 0 0" />
+            <geometry>
+                <box size="${base_x_size/6} ${base_y_size*1.65} ${base_z_size/3}"/>
+            </geometry>
+        </collision>
+    </link>
+
+    <link name="base_footprint"/>
+
+    <joint name="base_footprint_joint" type="fixed">
+        <origin xyz="0 0 ${wheel_vertical_offset - wheel_radius}" rpy="0 0 0" />
+        <parent link="base_link" />
+        <child link="base_footprint" />
+    </joint>
+
+    <link name="inertial_link">
+        <inertial>
+            <mass value="40" />
+            <origin xyz="0.0 0.0 0.0" />
+            <inertia ixx="2.288641" ixy="0" ixz="0" iyy="5.103976" iyz="0" izz="3.431465" />
+        </inertial>
+    </link>
+
+    <joint name="inertial_joint" type="fixed">
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <parent link="base_link" />
+        <child link="inertial_link" />
+    </joint>
+
+    <!-- Scout wheel macros -->
+    <!-- wheel labeled from 0 to 3, conter-clockwise, starting from front right wheel -->
+    <!-- motor 1 and 2 (left side) are mechanically installed in a reversed direction -->
+    <xacro:scout_wheel_type1 wheel_prefix="front_right">
+        <origin xyz="${wheelbase/2} ${-track/2} ${wheel_vertical_offset}" rpy="3.14 0 0" />
+    </xacro:scout_wheel_type1>
+    <xacro:scout_wheel_type2 wheel_prefix="front_left">
+        <origin xyz="${wheelbase/2} ${track/2} ${wheel_vertical_offset}" rpy="0 0 0" />
+    </xacro:scout_wheel_type2>
+    <xacro:scout_wheel_type1 wheel_prefix="rear_left">
+        <origin xyz="${-wheelbase/2} ${track/2} ${wheel_vertical_offset}" rpy="0 0 0" />
+    </xacro:scout_wheel_type1>
+    <xacro:scout_wheel_type2 wheel_prefix="rear_right">
+        <origin xyz="${-wheelbase/2} ${-track/2} ${wheel_vertical_offset}" rpy="3.14 0 0" />
+    </xacro:scout_wheel_type2>
+
+    <!-- Onboard sensors -->
+    <xacro:if value="$(arg onboard_sensors)">
+        <xacro:include filename="$(find scout_description)/urdf/onboard_sensors.urdf" />
+    </xacro:if>
+
+    <!-- Gazebo definitions  -->
+    <xacro:include filename="$(find scout_description)/urdf/scout_v2.gazebo" />
+</robot>
diff --git a/scout_base/description/sub_elements/scout_wheel_type1.xacro b/scout_base/description/sub_elements/scout_wheel_type1.xacro
new file mode 100644
index 0000000..960acf6
--- /dev/null
+++ b/scout_base/description/sub_elements/scout_wheel_type1.xacro
@@ -0,0 +1,59 @@
+<?xml version="1.0"?>
+<!--
+Software License Agreement (BSD)
+
+\file      scout_wheel.xacro (adapted from wheel.urdf.xacro in husky_description)
+\authors   Paul Bovbel <pbovbel@clearpathrobotics.com>
+           Ruixiang Du <ruixiang.du@westonrobot.com>
+\copyright Copyright (c) 2015, Clearpath Robotics, Inc., All rights reserved.
+\copyright Copyright (c) 2020, Weston Robot Pte. Ltd., All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification, are permitted provided that
+the following conditions are met:
+ * Redistributions of source code must retain the above copyright notice, this list of conditions and the
+   following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
+   following disclaimer in the documentation and/or other materials provided with the distribution.
+ * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote
+   products derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR-
+RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN-
+DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
+OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+-->
+<robot xmlns:xacro="http://ros.org/wiki/xacro" name="scout_wheel">
+
+    <xacro:macro name="scout_wheel_type1" params="wheel_prefix *joint_pose">
+        <link name="${wheel_prefix}_wheel_link">
+            <inertial>
+                <mass value="3" />
+                <origin xyz="0 0 0" />
+                <inertia ixx="0.7171" ixy="0" ixz="0" iyy="0.1361" iyz="0" izz="0.7171" />
+            </inertial>
+            <visual>
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <geometry>
+                    <mesh filename="package://scout_description/meshes/wheel_type1.dae" />
+                </geometry>
+            </visual>
+            <collision>
+                <origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
+                <geometry>
+                    <cylinder length="${wheel_length}" radius="${wheel_radius}" />
+                </geometry>
+            </collision>
+        </link>
+
+        <joint name="${wheel_prefix}_wheel" type="continuous">
+            <parent link="base_link"/>
+            <child link="${wheel_prefix}_wheel_link"/>
+            <xacro:insert_block name="joint_pose"/>
+            <axis xyz="0 -1 0" rpy="0 0 0" />
+        </joint>
+    </xacro:macro>
+    
+</robot>
diff --git a/scout_base/description/sub_elements/scout_wheel_type2.xacro b/scout_base/description/sub_elements/scout_wheel_type2.xacro
new file mode 100644
index 0000000..2435b5b
--- /dev/null
+++ b/scout_base/description/sub_elements/scout_wheel_type2.xacro
@@ -0,0 +1,59 @@
+<?xml version="1.0"?>
+<!--
+Software License Agreement (BSD)
+
+\file      scout_wheel.xacro (adapted from wheel.urdf.xacro in husky_description)
+\authors   Paul Bovbel <pbovbel@clearpathrobotics.com>
+           Ruixiang Du <ruixiang.du@westonrobot.com>
+\copyright Copyright (c) 2015, Clearpath Robotics, Inc., All rights reserved.
+\copyright Copyright (c) 2020, Weston Robot Pte. Ltd., All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification, are permitted provided that
+the following conditions are met:
+ * Redistributions of source code must retain the above copyright notice, this list of conditions and the
+   following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
+   following disclaimer in the documentation and/or other materials provided with the distribution.
+ * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote
+   products derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR-
+RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN-
+DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
+OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+-->
+<robot xmlns:xacro="http://ros.org/wiki/xacro" name="scout_wheel">
+
+    <xacro:macro name="scout_wheel_type2" params="wheel_prefix *joint_pose">
+        <link name="${wheel_prefix}_wheel_link">
+            <inertial>
+                <mass value="3" />
+                <origin xyz="0 0 0" />
+                <inertia ixx="0.7171" ixy="0" ixz="0" iyy="0.1361" iyz="0" izz="0.7171" />
+            </inertial>
+            <visual>
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <geometry>
+                    <mesh filename="package://scout_description/meshes/wheel_type2.dae" />
+                </geometry>
+            </visual>
+            <collision>
+                <origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
+                <geometry>
+                    <cylinder length="${wheel_length}" radius="${wheel_radius}" />
+                </geometry>
+            </collision>
+        </link>
+
+        <joint name="${wheel_prefix}_wheel" type="continuous">
+            <parent link="base_link"/>
+            <child link="${wheel_prefix}_wheel_link"/>
+            <xacro:insert_block name="joint_pose"/>
+            <axis xyz="0 -1 0" rpy="0 0 0" />
+        </joint>
+    </xacro:macro>
+
+</robot>
diff --git a/scout_base/launch/scout_base.launch b/scout_base/launch/scout_base.launch
index c2889c0..a860415 100644
--- a/scout_base/launch/scout_base.launch
+++ b/scout_base/launch/scout_base.launch
@@ -11,6 +11,7 @@
     -->
     <arg name="port_name" default="can0" />
     <arg name="simulated_robot" default="false" />
+    <arg name = "model_xacro" default = "$(find scout_base)/description/scout_v2.xacro" />>
 
     <node name="scout_base_node" pkg="scout_base" type="scout_base_node" output="screen">
         <param name="port_name" type="string" value="$(arg port_name)" />
@@ -19,4 +20,7 @@
         <param name="odom_frame" type="string" value="odom" />
         <param name="base_frame" type="string" value="base_link" />
     </node>
+    <param name="robot_description" command="$(find xacro)/xacro $(arg model_xacro)"/>
+    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node>
+    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
 </launch>