From 628f65da72302bae717466a88cfe47c7571598e0 Mon Sep 17 00:00:00 2001
From: rguzman <rguzman@robotnik.es>
Date: Mon, 2 Feb 2015 18:12:48 +0100
Subject: [PATCH] rb1_torso - changes to simulate as part of rb1 robot

---
 .../config/rb1_torso_control.yaml             |  2 +-
 .../launch/rb1_torso_control.launch           |  4 +--
 rb1_torso_control/package.xml                 |  8 ++---
 .../launch/rb1_torso_rviz.launch              |  2 +-
 .../robots/rb1_torso.urdf.xacro               |  2 +-
 .../robots/rb1_torso_standalone.urdf.xacro    | 36 +++++++++++++++++++
 .../urdf/rb1_torso_3dof.gazebo.xacro          |  3 +-
 .../urdf/rb1_torso_3dof.urdf.xacro            |  2 +-
 8 files changed, 48 insertions(+), 11 deletions(-)
 create mode 100644 rb1_torso_description/robots/rb1_torso_standalone.urdf.xacro

diff --git a/rb1_torso_control/config/rb1_torso_control.yaml b/rb1_torso_control/config/rb1_torso_control.yaml
index 69ff8be..9777d7f 100644
--- a/rb1_torso_control/config/rb1_torso_control.yaml
+++ b/rb1_torso_control/config/rb1_torso_control.yaml
@@ -1,4 +1,4 @@
-rb1_torso:
+rb1:
   j1_torso_controller:
     type: effort_controllers/JointPositionController    
     joint: j1_torso
diff --git a/rb1_torso_control/launch/rb1_torso_control.launch b/rb1_torso_control/launch/rb1_torso_control.launch
index f391d89..ffee2f7 100644
--- a/rb1_torso_control/launch/rb1_torso_control.launch
+++ b/rb1_torso_control/launch/rb1_torso_control.launch
@@ -3,7 +3,7 @@
 
   <!-- load the controllers -->
   <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
-    output="screen" ns="/rb1_torso" args="--namespace=/rb1_torso
+    output="screen" ns="/rb1" args="--namespace=/rb1
                       j1_torso_controller
                       j1_head_controller
                       j2_head_controller
@@ -16,7 +16,7 @@
   <!-- convert joint states to TF transforms for rviz, etc -->
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
     respawn="false" output="screen">
-    <remap from="/joint_states" to="/rb1_torso/joint_states" />
+    <remap from="/joint_states" to="/rb1/joint_states" />
   </node>
 
 </launch>
diff --git a/rb1_torso_control/package.xml b/rb1_torso_control/package.xml
index 2342689..250d9f2 100644
--- a/rb1_torso_control/package.xml
+++ b/rb1_torso_control/package.xml
@@ -41,11 +41,11 @@
   <!--   <test_depend>gtest</test_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>robot_state_publisher</build_depend>
-  <build_depend>ros_control</build_depend>
-  <build_depend>ros_controllers</build_depend>
+  <build_depend>controller_manager</build_depend>
+  <build_depend>controller_manager_msgs</build_depend>
   <run_depend>robot_state_publisher</run_depend>
-  <run_depend>ros_control</run_depend>
-  <run_depend>ros_controllers</run_depend>
+  <run_depend>controller_manager</run_depend>
+  <run_depend>controller_manager_msgs</run_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->
diff --git a/rb1_torso_description/launch/rb1_torso_rviz.launch b/rb1_torso_description/launch/rb1_torso_rviz.launch
index d830d98..bffb095 100644
--- a/rb1_torso_description/launch/rb1_torso_rviz.launch
+++ b/rb1_torso_description/launch/rb1_torso_rviz.launch
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <launch>
 
-  <arg name="urdf_file" default="$(find xacro)/xacro.py '$(find rb1_torso_description)/robots/rb1_torso.urdf.xacro'" />
+  <arg name="urdf_file" default="$(find xacro)/xacro.py '$(find rb1_torso_description)/robots/rb1_torso_standalone.urdf.xacro'" />
   <param name="robot_description" command="$(arg urdf_file)" />
 
   <!-- send fake joint values -->
diff --git a/rb1_torso_description/robots/rb1_torso.urdf.xacro b/rb1_torso_description/robots/rb1_torso.urdf.xacro
index 0841804..2889796 100644
--- a/rb1_torso_description/robots/rb1_torso.urdf.xacro
+++ b/rb1_torso_description/robots/rb1_torso.urdf.xacro
@@ -29,6 +29,6 @@
    </xacro:sensor_asus_xtion_pro> 
 
 	<!-- Gazebo ros control plugins -->
-	<xacro:ros_control/>
+	<!-- xacro:ros_control/ -->
 
 </robot>
diff --git a/rb1_torso_description/robots/rb1_torso_standalone.urdf.xacro b/rb1_torso_description/robots/rb1_torso_standalone.urdf.xacro
new file mode 100644
index 0000000..5dbf1b7
--- /dev/null
+++ b/rb1_torso_description/robots/rb1_torso_standalone.urdf.xacro
@@ -0,0 +1,36 @@
+<?xml version="1.0"?>
+<robot name="rb1_torso" xmlns:xacro="http://www.ros.org/wiki/xacro"> 
+
+	<!-- ***************** -->
+	<!-- Imported elements -->
+	<!-- ***************** -->	
+	<!-- First we import all posible elements defined in the urdf.xacro files. All these elements are defined as macro:xacros -->
+
+	<!-- Import RB1 arm elements -->
+	<xacro:include filename="$(find rb1_torso_description)/urdf/rb1_torso_3dof.urdf.xacro" />
+	
+	<xacro:include filename="$(find rb1_torso_description)/urdf/rb1_torso_3dof.gazebo.xacro" />
+
+	<!-- Import all available sensors -->
+	<xacro:include filename="$(find rb1_torso_description)/urdf/sensors/all_sensors.urdf.xacro" />
+	
+	<!-- ***************** -->
+	<!-- Global parameters -->
+	<!-- ***************** -->  
+	<xacro:property name="PI" value="3.1415926535897931"/>
+  	
+	<!-- Flag to select the high or low quality model -->
+	<xacro:property name="hq" value="true" />
+
+	<!-- RB-1 torso -->
+	<xacro:rb1_torso_3dof name="rb1_torso_3dof" hq="${hq}" />
+
+	<!-- Ixtion sensor in head -->
+	<xacro:sensor_asus_xtion_pro name="xtion_head" parent="link2_head">		
+        <origin xyz="0.055 0.000 0.028" rpy="0 0 0"/> 
+   </xacro:sensor_asus_xtion_pro> 
+
+	<!-- Gazebo ros control plugins -->
+	<!-- xacro:ros_control/ -->
+
+</robot>
diff --git a/rb1_torso_description/urdf/rb1_torso_3dof.gazebo.xacro b/rb1_torso_description/urdf/rb1_torso_3dof.gazebo.xacro
index 2ab65b8..6046bf0 100644
--- a/rb1_torso_description/urdf/rb1_torso_3dof.gazebo.xacro
+++ b/rb1_torso_description/urdf/rb1_torso_3dof.gazebo.xacro
@@ -5,7 +5,8 @@
   <xacro:macro name="ros_control">
     <gazebo>
       <plugin name="ros_control" filename="libgazebo_ros_control.so">
-        <robotNamespace>/rb1_torso</robotNamespace>
+		<robotNamespace>/rb1</robotNamespace>
+		<!-- robotNamespace>/rb1_torso</robotNamespace -->        
         <robotParam>robot_description</robotParam>
         <controlPeriod>0.001</controlPeriod> <!-- controlPeriod>0.003</controlPeriod --> 
 	    <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
diff --git a/rb1_torso_description/urdf/rb1_torso_3dof.urdf.xacro b/rb1_torso_description/urdf/rb1_torso_3dof.urdf.xacro
index 8bffe5a..6f5f6bc 100644
--- a/rb1_torso_description/urdf/rb1_torso_3dof.urdf.xacro
+++ b/rb1_torso_description/urdf/rb1_torso_3dof.urdf.xacro
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <robot name="rb1_torso_3dof" xmlns:xacro="http://www.ros.org/wiki/xacro"> 
 
-  <xacro:include filename="$(find rb1_torso_description)/urdf/rb1_torso_3dof.gazebo.xacro" />
+  <!-- xacro:include filename="$(find rb1_torso_description)/urdf/rb1_torso_3dof.gazebo.xacro" / -->
 
   <xacro:property name="joint_effort_limit" value="100.0"/>
   <xacro:property name="joint_velocity_limit" value="3.0"/>