Merge development into master (#2)

* init commit

* pillar driver init commit

* travis fix. init commit for arm bringup

* update travis.yml

* update travis.yml again

* added description files

* added swp

* base driver now uses third party wrapper, description files cleaned to some degree, gamepad controller support added

.gitignore

@@ -40,6 +40,8 @@ qtcreator-*
 
 *~
 
+*.swp
+
 # Emacs
 .#*
 

poli_launch/.travis.yml → .travis.yml

@@ -1,9 +1,6 @@
 sudo: required
 dist: trusty
-# Force travis to use its minimal image with default Python settings
 language: generic
-compiler:
-  - gcc
 env:
   global:
     - CATKIN_WS=~/catkin_ws
@@ -13,6 +10,7 @@ env:
 before_install:
   - git clone https://github.com/StanleyInnovation/segway_v3_robot.git
   - git clone https://github.com/StanleyInnovation/segway_v3
+  - git clone https://github.com/Kinovarobotics/kinova-ros.git
 install:
   - sudo sh -c 'echo "deb http://packages.ros.org/ros-shadow-fixed/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list'
   - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
@@ -33,4 +31,4 @@ script:
     catkin_make_isolated --install --cmake-args -DCMAKE_BUILD_TYPE=Release --catkin-make-args tests &&
     catkin_make_isolated --install --cmake-args -DCMAKE_BUILD_TYPE=Release --catkin-make-args run_tests
   # Check results
-- catkin_test_results ./build_isolated
+  - catkin_test_results ./build_isolated

poli_description/CMakeLists.txt

@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(poli_description)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+)
+
+catkin_package(
+)
+
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
+

poli_description/package.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<package>
+  <name>poli_description</name>
+  <version>0.0.1</version>
+  <description>The poli_description package</description>
+
+  <maintainer email="maxwell@cs.utexas.edu">Maxwell Svetlik</maintainer>
+  <license>BSD</license>
+
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>rospy</run_depend>
+
+
+  <export>
+  </export>
+</package>

poli_description/robots/poli.urdf.xacro

@@ -0,0 +1,7 @@
+<?xml version="1.0"?>
+
+<robot name="segway_robot" xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:include filename="$(find poli_description)/urdf/RMP_110/rmp.urdf.xacro" />
+  <xacro:segway_robot />
+
+</robot>

poli_description/urdf/RMP_110/rmp.urdf.xacro

@@ -0,0 +1,129 @@
+<?xml version="1.0"?>
+<!--
+Software License Agreement (BSD)
+
+\file      rmp.urdf.xacro
+\authors   Patrick Hussey
+\copyright Copyright (c) 2015, Stanley Innovation, Inc., 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 Stanley Innovation, Inc. 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 name="segway_robot" xmlns:xacro="http://ros.org/wiki/xacro">
+
+    <!-- Included URDF/XACRO Files -->
+    <xacro:include filename="$(find poli_description)/urdf/segway_components/battery.urdf.xacro" />
+    <xacro:include filename="$(find poli_description)/urdf/segway_components/i2_wheel.urdf.xacro" />
+    <xacro:include filename="$(find poli_description)/urdf/segway_components/caster_assembly_110.urdf.xacro" />
+    <xacro:include filename="$(find poli_description)/urdf/segway_components/powerbase_110.urdf.xacro" />
+    <xacro:include filename="$(find poli_description)/urdf/segway_components/hmi_box_110.urdf.xacro" />
+    <xacro:include filename="$(find poli_description)/urdf/segway_components/hmi_plate_110.urdf.xacro" />
+    <xacro:include filename="$(find poli_description)/urdf/accessories.urdf.xacro" />
+
+    <property name="M_PI" value="3.14159"/>
+
+    <!-- Wheel Mounting Positions -->
+    <xacro:property name="track" value="0.48122440" />
+    <xacro:property name="wheel_vertical_offset" value="0.08255015" />
+
+    <!-- Wheel Properties -->
+    <xacro:property name="wheel_length" value="0.09224900" />
+    <xacro:property name="wheel_radius" value="0.2405681" />
+
+    <xacro:macro name="segway_robot">  
+        <gazebo> 
+          <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so"> 
+              <robotNamespace>/</robotNamespace>
+          </plugin> 
+        </gazebo>
+
+        <!-- Base link is the center of the robot's chassis between the motors projected on the ground -->
+        <link name="base_link"/>
+
+        <!--Chassis -->
+        <xacro:powerbase_110 chassis_prefix="base" parent="base_link">
+            <origin xyz="0 0 ${-wheel_vertical_offset + wheel_radius}" rpy="0 0 0" />
+        </xacro:powerbase_110>
+
+        <link name="inertial_collision_link">
+			<inertial>
+				<mass value="29.756375" />
+				<origin xyz="0.001751 0.000008 0.018544" />
+				<inertia ixx="0.696269"  ixy="0.016133" ixz="0.003232"
+                         iyx="0.016133"  iyy="0.299110" iyz="-0.000004"
+                         izx="-0.077476" izy="-0.000004" izz="0.864394" />
+			</inertial>
+            <collision>
+				<geometry>
+					<mesh filename="package://poli_description/meshes/collision/110_powerbase_collision.stl" />
+				</geometry>
+			</collision>
+		</link>
+
+		<joint name="inertial_collision_joint" type="fixed" >
+		    <parent link="base_chassis_link" />
+		    <child link="inertial_collision_link" />
+		</joint>                    
+
+
+        <!-- Propulsion Batteries -->
+        <xacro:battery battery_prefix="mcu_1" parent="base_chassis_link">
+            <origin xyz="-0.09298 0 0" rpy="0 0 0" />
+        </xacro:battery>
+
+        <!-- Aux Batteries -->
+        <xacro:battery battery_prefix="aux" parent="base_chassis_link">
+            <origin xyz="0.09298 0 0" rpy="0 0 ${M_PI}" />
+        </xacro:battery>
+
+
+        <!-- HMI Box -->
+        <xacro:hmi_box_110 parent="base_chassis_link">
+            <origin xyz="0 0 0.09504494" rpy="0 0 0" />
+        </xacro:hmi_box_110>
+
+        <xacro:hmi_plate_110 parent="hmi_link">
+            <origin xyz="0 0 0" rpy="0 0 0" />
+        </xacro:hmi_plate_110>
+
+        <!-- Segway wheel macros -->
+        <xacro:i2_wheel wheel_prefix="left" parent="base_chassis_link" visual_rotation="0.0">
+            <origin xyz="0 ${track/2} ${wheel_vertical_offset}" rpy="0 0 0" />
+        </xacro:i2_wheel>
+        <xacro:i2_wheel wheel_prefix="right" parent="base_chassis_link" visual_rotation="${M_PI}">
+            <origin xyz="0 ${-track/2} ${wheel_vertical_offset}" rpy="0 0 0" />
+        </xacro:i2_wheel>
+
+        <!-- Segway Caster Assembly-->
+        <xacro:if value="$(optenv SEGWAY_HAS_REAR_CASTER false)">
+            <xacro:caster_assembly_110 caster_prefix="rear" parent="base_chassis_link">
+                <origin xyz="-0.32784550 0 0.03326933" rpy="0 0 ${M_PI}" />
+            </xacro:caster_assembly_110>
+        </xacro:if>
+
+        <xacro:if value="$(optenv SEGWAY_HAS_FRONT_CASTER false)">
+            <xacro:caster_assembly_110 caster_prefix="front" parent="base_chassis_link">
+                <origin xyz="0.32784550 0 0.03326933" rpy="0 0 0" />
+            </xacro:caster_assembly_110>
+        </xacro:if>
+
+        <!-- Setup any accessories defined in the environment -->
+        <xacro:segway_accessories />
+
+    </xacro:macro>
+
+</robot>

poli_description/urdf/accessories.urdf.xacro

@@ -0,0 +1,46 @@
+<?xml version="1.0"?>
+<!--
+Software License Agreement (BSD)
+
+\file      accessories.urdf.xacro
+\authors   Patrick Hussey
+\copyright Copyright (c) 2015, Stanley Innovation, Inc., 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 Stanley Innovation, Inc. 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 name="segway_accessories" xmlns:xacro="http://ros.org/wiki/xacro">
+    <xacro:include filename="$(find poli_description)/urdf/sensors/hokuyo_ust20lx.urdf.xacro"/>
+
+    <property name="M_PI" value="3.14159"/>
+
+    <xacro:macro name="segway_accessories">
+	<xacro:hokuyo_ust20lx laser_prefix="$(optenv LASER1_PREFIX base)" parent="$(optenv VECTOR_PARENT_LINK base_chassis_link)">
+	 <origin xyz="$(optenv LASER1_XYZ 0.35 0 0.2)" rpy="$(optenv LASER1_RPY 3.14159 0 0)" />
+	</xacro:hokuyo_ust20lx>
+
+        <!-- Second laser Sensor -->
+        <xacro:if value="$(optenv SEGWAY_HAS_SECOND_2D_LASER false)">
+            <xacro:if value="$(optenv SEGWAY_LASER2_IS_HOKUYO false)">
+                <xacro:hokuyo_ust20lx laser_prefix="$(optenv LASER2_PREFIX base)" parent="$(optenv VECTOR_PARENT_LINK base_chassis_link)">
+                    <origin xyz="$(optenv LASER2_XYZ 0.35 0 0.2)" rpy="$(optenv LASER2_RPY 3.14159 0 0)" />
+                </xacro:hokuyo_ust20lx>
+            </xacro:if>
+        </xacro:if>
+
+    </xacro:macro>
+</robot>

poli_description/urdf/segway_components/battery.urdf.xacro

@@ -0,0 +1,44 @@
+<?xml version="1.0"?>
+<!--
+Software License Agreement (BSD)
+
+\file      battery.urdf.xacro
+\authors   Patrick Hussey
+\copyright Copyright (c) 2015, Stanley Innovation, Inc., 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 Stanley Innovation, Inc. 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="battery">
+
+	<xacro:macro name="battery" params="battery_prefix parent *origin">
+        <joint name="${battery_prefix}_battery_joint" type="fixed">
+            <xacro:insert_block name="origin" />
+            <parent link="${parent}" />
+            <child link="${battery_prefix}_battery_link" />
+        </joint>
+
+	    <link name="${battery_prefix}_battery_link">
+			<visual>
+				<geometry>
+					<mesh filename="package://poli_description/meshes/visual/battery.dae" />
+				</geometry>
+			</visual>
+		</link>
+    </xacro:macro>
+
+</robot>