From adc223cbd837d9331db774031ef42f0e7f73a4a6 Mon Sep 17 00:00:00 2001
From: Thomas Timm Andersen <thomas.timm.dk@gmail.com>
Date: Fri, 19 Feb 2016 14:58:28 +0100
Subject: [PATCH] Added prefix args to ros_control launch files and to the
 tool0_controller transform frame

---
 launch/ur10_ros_control.launch |  9 +++++++--
 launch/ur3_bringup.launch      |  1 +
 launch/ur3_ros_control.launch  |  4 ++++
 launch/ur5_ros_control.launch  | 10 +++++++---
 launch/ur_common.launch        |  4 ++--
 src/ur_ros_wrapper.cpp         |  5 +++--
 6 files changed, 24 insertions(+), 9 deletions(-)

diff --git a/launch/ur10_ros_control.launch b/launch/ur10_ros_control.launch
index ce6c1e1..bbf576a 100644
--- a/launch/ur10_ros_control.launch
+++ b/launch/ur10_ros_control.launch
@@ -1,20 +1,24 @@
+<?xml version="1.0"?>
 <launch>
 
   <!-- GDB functionality -->
   <arg name="debug" default="false" />
   <arg unless="$(arg debug)" name="launch_prefix" value="" />
   <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
-  
+
   <arg name="robot_ip"/>
   <arg name="limited" default="false"/>
   <arg name="min_payload"  default="0.0"/>
-  <arg name="max_payload"  default="10.0"/>
+  <arg name="max_payload"  default="3.0"/>
+  <arg name="prefix" default="" />  
   <arg name="max_velocity" default="10.0"/> <!-- [rad/s] -->
   <!-- robot model -->
   <include file="$(find ur_description)/launch/ur10_upload.launch">
     <arg name="limited" value="$(arg limited)"/>
+    <arg name="prefix" value="$(arg prefix)" />
   </include>
 
+
   <!-- Load hardware interface -->
   <node name="ur_hardware_interface" pkg="ur_modern_driver" type="ur_driver" output="screen" launch-prefix="$(arg launch_prefix)">
     <param name="robot_ip_address" type="str" value="$(arg robot_ip)"/>
@@ -22,6 +26,7 @@
     <param name="max_payload" type="double" value="$(arg max_payload)"/>
     <param name="max_velocity" type="double" value="$(arg max_velocity)"/>
     <param name="use_ros_control" type="bool" value="True"/>
+    <arg name="prefix" value="$(arg prefix)" />
   </node>
 
   <!-- Load controller settings -->
diff --git a/launch/ur3_bringup.launch b/launch/ur3_bringup.launch
index c5d4a8f..9d267e1 100644
--- a/launch/ur3_bringup.launch
+++ b/launch/ur3_bringup.launch
@@ -25,6 +25,7 @@
     <arg name="robot_ip" value="$(arg robot_ip)"/>
     <arg name="min_payload"  value="$(arg min_payload)"/>
     <arg name="max_payload"  value="$(arg max_payload)"/>
+    <arg name="prefix" value="$(arg prefix)" />
   </include>
 
 </launch>
diff --git a/launch/ur3_ros_control.launch b/launch/ur3_ros_control.launch
index 1c1cd89..c9a7fd2 100644
--- a/launch/ur3_ros_control.launch
+++ b/launch/ur3_ros_control.launch
@@ -1,3 +1,4 @@
+<?xml version="1.0"?>
 <launch>
 
   <!-- GDB functionality -->
@@ -9,10 +10,12 @@
   <arg name="limited" default="false"/>
   <arg name="min_payload"  default="0.0"/>
   <arg name="max_payload"  default="3.0"/>
+  <arg name="prefix" default="" />  
   <arg name="max_velocity" default="10.0"/> <!-- [rad/s] -->
   <!-- robot model -->
   <include file="$(find ur_description)/launch/ur3_upload.launch">
     <arg name="limited" value="$(arg limited)"/>
+    <arg name="prefix" value="$(arg prefix)" />
   </include>
 
 
@@ -23,6 +26,7 @@
     <param name="max_payload" type="double" value="$(arg max_payload)"/>
     <param name="max_velocity" type="double" value="$(arg max_velocity)"/>
     <param name="use_ros_control" type="bool" value="True"/>
+    <arg name="prefix" value="$(arg prefix)" />
   </node>
 
   <!-- Load controller settings -->
diff --git a/launch/ur5_ros_control.launch b/launch/ur5_ros_control.launch
index b81b017..301ffea 100644
--- a/launch/ur5_ros_control.launch
+++ b/launch/ur5_ros_control.launch
@@ -1,21 +1,24 @@
+<?xml version="1.0"?>
 <launch>
 
   <!-- GDB functionality -->
   <arg name="debug" default="false" />
   <arg unless="$(arg debug)" name="launch_prefix" value="" />
   <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
-  
+
   <arg name="robot_ip"/>
   <arg name="limited" default="false"/>
   <arg name="min_payload"  default="0.0"/>
-  <arg name="max_payload"  default="5.0"/>
+  <arg name="max_payload"  default="3.0"/>
+  <arg name="prefix" default="" />  
   <arg name="max_velocity" default="10.0"/> <!-- [rad/s] -->
   <!-- robot model -->
   <include file="$(find ur_description)/launch/ur5_upload.launch">
     <arg name="limited" value="$(arg limited)"/>
+    <arg name="prefix" value="$(arg prefix)" />
   </include>
 
-  
+
   <!-- Load hardware interface -->
   <node name="ur_hardware_interface" pkg="ur_modern_driver" type="ur_driver" output="screen" launch-prefix="$(arg launch_prefix)">
     <param name="robot_ip_address" type="str" value="$(arg robot_ip)"/>
@@ -23,6 +26,7 @@
     <param name="max_payload" type="double" value="$(arg max_payload)"/>
     <param name="max_velocity" type="double" value="$(arg max_velocity)"/>
     <param name="use_ros_control" type="bool" value="True"/>
+    <arg name="prefix" value="$(arg prefix)" />
   </node>
 
   <!-- Load controller settings -->
diff --git a/launch/ur_common.launch b/launch/ur_common.launch
index 8b7836c..cc48d98 100644
--- a/launch/ur_common.launch
+++ b/launch/ur_common.launch
@@ -13,8 +13,8 @@
   <arg name="max_payload" />
   <arg name="prefix" default="" />
   <arg name="servoj_time" default="0.008" />
-  <arg name="base_frame"  default="base"/>
-  <arg name="tool_frame"  default="tool0_controller"/>
+  <arg name="base_frame" default="$(arg prefix)base_link" />
+  <arg name="tool_frame" default="$(arg prefix)tool0_controller" />
     
   <!-- The max_velocity parameter is only used for debugging in the ur_driver. It's not related to actual velocity limits -->
   <arg name="max_velocity" default="10.0"/> <!-- [rad/s] -->
diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp
index abfbda1..6280868 100644
--- a/src/ur_ros_wrapper.cpp
+++ b/src/ur_ros_wrapper.cpp
@@ -157,9 +157,10 @@ public:
 		robot_.setServojTime(servoj_time);
 
         //Base and tool frames
-        base_frame_ = "base";
-        tool_frame_ = "tool0_controller";
+        base_frame_ = joint_prefix + "base_link";
+        tool_frame_ =  joint_prefix + "tool0_controller";
         if (ros::param::get("~base_frame", base_frame_)) {
+            base_frame_ = base_frame_;
             sprintf(buf, "Base frame set to: %s", base_frame_.c_str());
             print_debug(buf);
         }