From 68b751cdcc66449df61ff1afb3cf11481d45eb95 Mon Sep 17 00:00:00 2001
From: Thomas Timm Andersen <thomas.timm.dk@gmail.com>
Date: Fri, 25 Sep 2015 10:19:58 +0200
Subject: [PATCH] Fixed order of starting and stopping controllers

---
 launch/ur5_ros_control.launch |  6 +++---
 src/do_output.cpp             | 12 ++++++------
 src/ur_driver.cpp             |  1 -
 src/ur_hardware_interface.cpp | 31 ++++++++++++++++---------------
 4 files changed, 25 insertions(+), 25 deletions(-)

diff --git a/launch/ur5_ros_control.launch b/launch/ur5_ros_control.launch
index 8064177..28e31ca 100644
--- a/launch/ur5_ros_control.launch
+++ b/launch/ur5_ros_control.launch
@@ -30,11 +30,11 @@
     <rosparam file="$(find ur_modern_driver)/config/ur5_controllers.yaml" command="load"/>
 
     <!-- spawn controller manager -->
-    <node name="ros_control_controller_manager" pkg="controller_manager" type="spawner" respawn="false"
-	  output="screen" args="joint_state_controller force_torque_sensor_controller position_based_position_trajectory_controller" />
+    <node name="ros_control_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
+	  output="screen" args="joint_state_controller force_torque_sensor_controller velocity_based_position_trajectory_controller" />
 	<!-- load other controller --> 
 	<node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager" respawn="false"
-	  output="screen" args="load velocity_based_position_trajectory_controller" /> 
+	  output="screen" args="load position_based_position_trajectory_controller" /> 
 
     <!-- Convert joint states to /tf tranforms -->
     <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
diff --git a/src/do_output.cpp b/src/do_output.cpp
index 2327231..a14b174 100644
--- a/src/do_output.cpp
+++ b/src/do_output.cpp
@@ -9,39 +9,39 @@
  * ----------------------------------------------------------------------------
  */
 
-#include "ur_modern_driver/du_output.h"
+#include "ur_modern_driver/do_output.h"
 
 void print_debug(std::string inp) {
 #ifdef ROS_BUILD
-	ROS_DEBUG(inp);
+	ROS_DEBUG(inp.c_str());
 #else
 	printf("DEBUG: %s\n", inp.c_str());
 #endif
 }
 void print_info(std::string inp) {
 #ifdef ROS_BUILD
-	ROS_INFO(inp);
+	ROS_INFO(inp.c_str());
 #else
 	printf("INFO: %s\n", inp.c_str());
 #endif
 }
 void print_warning(std::string inp) {
 #ifdef ROS_BUILD
-	ROS_WARN(inp);
+	ROS_WARN(inp.c_str());
 #else
 	printf("WARNING: %s\n", inp.c_str());
 #endif
 }
 void print_error(std::string inp) {
 #ifdef ROS_BUILD
-	ROS_ERROR(inp);
+	ROS_ERROR(inp.c_str());
 #else
 	printf("ERROR: %s\n", inp.c_str());
 #endif
 }
 void print_fatal(std::string inp) {
 #ifdef ROS_BUILD
-	ROS_FATAL(inp);
+	ROS_FATAL(inp.c_str());
 	ros::shutdown();
 #else
 	printf("FATAL: %s\n", inp.c_str());
diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp
index ee144a1..9a60858 100644
--- a/src/ur_driver.cpp
+++ b/src/ur_driver.cpp
@@ -257,7 +257,6 @@ void UrDriver::uploadProg() {
 	cmd_str += "end\n";
 
 	rt_interface_->addCommandToQueue(cmd_str);
-
 	UrDriver::openServo();
 }
 
diff --git a/src/ur_hardware_interface.cpp b/src/ur_hardware_interface.cpp
index 20860db..a96e87d 100644
--- a/src/ur_hardware_interface.cpp
+++ b/src/ur_hardware_interface.cpp
@@ -211,6 +211,7 @@ bool UrHardwareInterface::canSwitch(
 			}
 		}
 	}
+
 // we can always stop a controller
 	return true;
 }
@@ -218,21 +219,6 @@ bool UrHardwareInterface::canSwitch(
 void UrHardwareInterface::doSwitch(
 		const std::list<hardware_interface::ControllerInfo>& start_list,
 		const std::list<hardware_interface::ControllerInfo>& stop_list) {
-	for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it =
-			start_list.begin(); controller_it != start_list.end();
-			++controller_it) {
-		if (controller_it->hardware_interface
-				== "hardware_interface::VelocityJointInterface") {
-			velocity_interface_running_ = true;
-			ROS_DEBUG("Starting velocity interface");
-		}
-		if (controller_it->hardware_interface
-				== "hardware_interface::PositionJointInterface") {
-			position_interface_running_ = true;
-			robot_->uploadProg();
-			ROS_DEBUG("Starting position interface");
-		}
-	}
 	for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it =
 			stop_list.begin(); controller_it != stop_list.end();
 			++controller_it) {
@@ -249,6 +235,21 @@ void UrHardwareInterface::doSwitch(
 			ROS_DEBUG("Stopping position interface");
 		}
 	}
+	for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it =
+			start_list.begin(); controller_it != start_list.end();
+			++controller_it) {
+		if (controller_it->hardware_interface
+				== "hardware_interface::VelocityJointInterface") {
+			velocity_interface_running_ = true;
+			ROS_DEBUG("Starting velocity interface");
+		}
+		if (controller_it->hardware_interface
+				== "hardware_interface::PositionJointInterface") {
+			position_interface_running_ = true;
+			robot_->uploadProg();
+			ROS_DEBUG("Starting position interface");
+		}
+	}
 
 }