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