From ecc13f8f1d682e520c0bd55275a29ae876ef1d7f Mon Sep 17 00:00:00 2001
From: Florian Walch <florian.walch@franka.de>
Date: Thu, 22 Feb 2018 17:16:39 +0100
Subject: [PATCH] Move move_to_start.py to franka_example_controllers

---
 CHANGELOG.md                                                 | 1 +
 franka_example_controllers/CMakeLists.txt                    | 2 +-
 .../launch/move_to_start.launch                              | 2 +-
 franka_example_controllers/package.xml                       | 1 +
 .../scripts/move_to_start.py                                 | 0
 .../src/cartesian_pose_example_controller.cpp                | 2 +-
 .../src/cartesian_velocity_example_controller.cpp            | 5 +++--
 .../src/joint_position_example_controller.cpp                | 2 +-
 .../src/joint_velocity_example_controller.cpp                | 2 +-
 9 files changed, 10 insertions(+), 7 deletions(-)
 rename {panda_moveit_config => franka_example_controllers}/launch/move_to_start.launch (82%)
 rename {panda_moveit_config => franka_example_controllers}/scripts/move_to_start.py (100%)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index ebfa573..b52aa50 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -11,6 +11,7 @@ Requires `libfranka` >= 0.3.0
   * Fixed linker errors when building with `-DFranka_DIR` while an older version of
     `ros-kinetic-libfranka` is installed
   * Added gripper joint state publisher to `franka_visualization`
+  * Moved `move_to_start.py` example script to `franka_example_controllers`
 
 ## 0.2.2 - 2018-01-31
 
diff --git a/franka_example_controllers/CMakeLists.txt b/franka_example_controllers/CMakeLists.txt
index 235c5cb..bce35bb 100644
--- a/franka_example_controllers/CMakeLists.txt
+++ b/franka_example_controllers/CMakeLists.txt
@@ -97,7 +97,7 @@ install(FILES franka_example_controllers_plugin.xml
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
 catkin_install_python(
-  PROGRAMS scripts/interactive_marker.py
+  PROGRAMS scripts/interactive_marker.py scripts/move_to_start.py
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 
diff --git a/panda_moveit_config/launch/move_to_start.launch b/franka_example_controllers/launch/move_to_start.launch
similarity index 82%
rename from panda_moveit_config/launch/move_to_start.launch
rename to franka_example_controllers/launch/move_to_start.launch
index 1082569..ed8accb 100644
--- a/panda_moveit_config/launch/move_to_start.launch
+++ b/franka_example_controllers/launch/move_to_start.launch
@@ -13,6 +13,6 @@
     <arg name="arm_id" value="$(arg arm_id)" />
   </include>
   <group ns="$(arg arm_id)">
-    <node name="move_to_start" pkg="panda_moveit_config" type="move_to_start.py" output="screen" required="true" />
+    <node name="move_to_start" pkg="franka_example_controllers" type="move_to_start.py" output="screen" required="true" />
   </group>
 </launch>
diff --git a/franka_example_controllers/package.xml b/franka_example_controllers/package.xml
index 22b271e..ea2fb5a 100644
--- a/franka_example_controllers/package.xml
+++ b/franka_example_controllers/package.xml
@@ -31,6 +31,7 @@
   <exec_depend>franka_control</exec_depend>
   <exec_depend>franka_description</exec_depend>
   <exec_depend>message_runtime</exec_depend>
+  <exec_depend>panda_moveit_config</exec_depend>
   <exec_depend>rospy</exec_depend>
 
   <export>
diff --git a/panda_moveit_config/scripts/move_to_start.py b/franka_example_controllers/scripts/move_to_start.py
similarity index 100%
rename from panda_moveit_config/scripts/move_to_start.py
rename to franka_example_controllers/scripts/move_to_start.py
diff --git a/franka_example_controllers/src/cartesian_pose_example_controller.cpp b/franka_example_controllers/src/cartesian_pose_example_controller.cpp
index 223de37..4389fa0 100644
--- a/franka_example_controllers/src/cartesian_pose_example_controller.cpp
+++ b/franka_example_controllers/src/cartesian_pose_example_controller.cpp
@@ -54,7 +54,7 @@ bool CartesianPoseExampleController::init(hardware_interface::RobotHW* robot_har
       if (std::abs(state_handle.getRobotState().q_d[i] - q_start[i]) > 0.1) {
         ROS_ERROR_STREAM(
             "CartesianPoseExampleController: Robot is not in the expected starting position for "
-            "running this example. Run `roslaunch panda_moveit_config move_to_start.launch "
+            "running this example. Run `roslaunch franka_example_controllers move_to_start.launch "
             "robot_ip:=<robot-ip> load_gripper:=<has-attached-gripper>` first.");
         return false;
       }
diff --git a/franka_example_controllers/src/cartesian_velocity_example_controller.cpp b/franka_example_controllers/src/cartesian_velocity_example_controller.cpp
index 7ce5b69..ad7b64b 100644
--- a/franka_example_controllers/src/cartesian_velocity_example_controller.cpp
+++ b/franka_example_controllers/src/cartesian_velocity_example_controller.cpp
@@ -54,8 +54,9 @@ bool CartesianVelocityExampleController::init(hardware_interface::RobotHW* robot
       if (std::abs(state_handle.getRobotState().q_d[i] - q_start[i]) > 0.1) {
         ROS_ERROR_STREAM(
             "CartesianVelocityExampleController: Robot is not in the expected starting position "
-            "for running this example. Run `roslaunch panda_moveit_config move_to_start.launch "
-            "robot_ip:=<robot-ip> load_gripper:=<has-attached-gripper>` first.");
+            "for running this example. Run `roslaunch franka_example_controllers "
+            "move_to_start.launch robot_ip:=<robot-ip> load_gripper:=<has-attached-gripper>` "
+            "first.");
         return false;
       }
     }
diff --git a/franka_example_controllers/src/joint_position_example_controller.cpp b/franka_example_controllers/src/joint_position_example_controller.cpp
index 847da23..b630014 100644
--- a/franka_example_controllers/src/joint_position_example_controller.cpp
+++ b/franka_example_controllers/src/joint_position_example_controller.cpp
@@ -46,7 +46,7 @@ bool JointPositionExampleController::init(hardware_interface::RobotHW* robot_har
     if (std::abs(position_joint_handles_[i].getPosition() - q_start[i]) > 0.1) {
       ROS_ERROR_STREAM(
           "JointPositionExampleController: Robot is not in the expected starting position for "
-          "running this example. Run `roslaunch panda_moveit_config move_to_start.launch "
+          "running this example. Run `roslaunch franka_example_controllers move_to_start.launch "
           "robot_ip:=<robot-ip> load_gripper:=<has-attached-gripper>` first.");
       return false;
     }
diff --git a/franka_example_controllers/src/joint_velocity_example_controller.cpp b/franka_example_controllers/src/joint_velocity_example_controller.cpp
index bcc3ca2..dbe4c23 100644
--- a/franka_example_controllers/src/joint_velocity_example_controller.cpp
+++ b/franka_example_controllers/src/joint_velocity_example_controller.cpp
@@ -61,7 +61,7 @@ bool JointVelocityExampleController::init(hardware_interface::RobotHW* robot_har
       if (std::abs(state_handle.getRobotState().q_d[i] - q_start[i]) > 0.1) {
         ROS_ERROR_STREAM(
             "JointVelocityExampleController: Robot is not in the expected starting position for "
-            "running this example. Run `roslaunch panda_moveit_config move_to_start.launch "
+            "running this example. Run `roslaunch franka_example_controllers move_to_start.launch "
             "robot_ip:=<robot-ip> load_gripper:=<has-attached-gripper>` first.");
         return false;
       }
-- 
GitLab