From 8019425a80ac1abdb080b2cf2cd4dcdeaa62c21b Mon Sep 17 00:00:00 2001 From: ipa-fxm Date: Wed, 14 Dec 2016 15:15:15 +0100 Subject: [PATCH 1/6] broadcast target_frame before tracking --- cob_cartesian_controller/src/cartesian_controller.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/cob_cartesian_controller/src/cartesian_controller.cpp b/cob_cartesian_controller/src/cartesian_controller.cpp index e67f060e..f419c81a 100644 --- a/cob_cartesian_controller/src/cartesian_controller.cpp +++ b/cob_cartesian_controller/src/cartesian_controller.cpp @@ -153,8 +153,6 @@ bool CartesianController::stopTracking() bool CartesianController::posePathBroadcaster(const geometry_msgs::PoseArray& cartesian_path) { bool success = true; - double epsilon = 0.1; - int failure_counter = 0; ros::Rate rate(update_rate_); tf::Transform transform; @@ -203,6 +201,11 @@ void CartesianController::goalCallback() // Publish Preview utils_.previewPath(cartesian_path); + + // initially broadcast target_frame + tf::Transform identity = tf::Transform(); + identity.setIdentity(); + tf_broadcaster_.sendTransform(tf::StampedTransform(identity, ros::Time::now(), chain_tip_link_, target_frame_)); // Activate Tracking if (!startTracking()) From 07069da70584ee689b4391f090f0e8ba182c5d37 Mon Sep 17 00:00:00 2001 From: ipa-fxm Date: Wed, 14 Dec 2016 15:15:46 +0100 Subject: [PATCH 2/6] fix and harmonize scripts --- .../scripts/test_move_around_torus.py | 7 ++++--- cob_cartesian_controller/scripts/test_move_circ.py | 14 +++++++------- cob_cartesian_controller/scripts/test_move_lin.py | 14 +++++++------- .../simple_cartesian_interface.py | 14 ++++++++------ 4 files changed, 26 insertions(+), 23 deletions(-) diff --git a/cob_cartesian_controller/scripts/test_move_around_torus.py b/cob_cartesian_controller/scripts/test_move_around_torus.py index 6379e572..1a61df4d 100755 --- a/cob_cartesian_controller/scripts/test_move_around_torus.py +++ b/cob_cartesian_controller/scripts/test_move_around_torus.py @@ -114,10 +114,11 @@ def init_dyn_recfg(): if __name__ == '__main__': rospy.init_node('test_move_around_torus') - client = actionlib.SimpleActionClient('cartesian_trajectory_action', CartesianControllerAction) - rospy.logwarn("Waiting for ActionServer...") + action_name = rospy.get_namespace()+'cartesian_trajectory_action' + client = actionlib.SimpleActionClient(action_name, CartesianControllerAction) + rospy.logwarn("Waiting for ActionServer: %s", action_name) client.wait_for_server() - rospy.logwarn("...done") + rospy.logwarn("...done") init_dyn_recfg() diff --git a/cob_cartesian_controller/scripts/test_move_circ.py b/cob_cartesian_controller/scripts/test_move_circ.py index b6fcca32..de017d0b 100755 --- a/cob_cartesian_controller/scripts/test_move_circ.py +++ b/cob_cartesian_controller/scripts/test_move_circ.py @@ -8,8 +8,9 @@ if __name__ == '__main__': rospy.init_node('test_move_circ') - client = actionlib.SimpleActionClient('cartesian_trajectory_action', CartesianControllerAction) - rospy.logwarn("Waiting for ActionServer...") + action_name = rospy.get_namespace()+'cartesian_trajectory_action' + client = actionlib.SimpleActionClient(action_name, CartesianControllerAction) + rospy.logwarn("Waiting for ActionServer: %s", action_name) client.wait_for_server() rospy.logwarn("...done") @@ -25,17 +26,16 @@ goal.move_circ.pose_center.orientation.z = 0.0 goal.move_circ.pose_center.orientation.w = 1.0 goal.move_circ.frame_id = 'world' - goal.move_circ.rotate_only = False goal.move_circ.start_angle = 0 * math.pi / 180.0 goal.move_circ.end_angle = 90 * math.pi / 180.0 goal.move_circ.radius = 0.3 - goal.move_circ.profile.vel = 0.1 - goal.move_circ.profile.accl = 0.2 - goal.move_circ.profile.profile_type = Profile.SINOID + goal.profile.vel = 0.1 + goal.profile.accl = 0.2 + goal.profile.profile_type = Profile.SINOID - print goal + #print goal # Send the goal client.send_goal(goal) diff --git a/cob_cartesian_controller/scripts/test_move_lin.py b/cob_cartesian_controller/scripts/test_move_lin.py index 32268936..13c325f7 100755 --- a/cob_cartesian_controller/scripts/test_move_lin.py +++ b/cob_cartesian_controller/scripts/test_move_lin.py @@ -7,8 +7,9 @@ if __name__ == '__main__': rospy.init_node('test_move_lin') - client = actionlib.SimpleActionClient('cartesian_trajectory_action', CartesianControllerAction) - rospy.logwarn("Waiting for ActionServer...") + action_name = rospy.get_namespace()+'cartesian_trajectory_action' + client = actionlib.SimpleActionClient(action_name, CartesianControllerAction) + rospy.logwarn("Waiting for ActionServer: %s", action_name) client.wait_for_server() rospy.logwarn("...done") @@ -24,13 +25,12 @@ goal.move_lin.pose_goal.orientation.z = 0.0 goal.move_lin.pose_goal.orientation.w = 1.0 goal.move_lin.frame_id = 'world' - goal.move_lin.rotate_only = False - goal.move_lin.profile.vel = 0.2 - goal.move_lin.profile.accl = 0.1 - goal.move_lin.profile.profile_type = Profile.SINOID + goal.profile.vel = 0.2 + goal.profile.accl = 0.1 + goal.profile.profile_type = Profile.SINOID - print goal + #print goal # Send the goal client.send_goal(goal) diff --git a/cob_cartesian_controller/src/simple_cartesian_interface/simple_cartesian_interface.py b/cob_cartesian_controller/src/simple_cartesian_interface/simple_cartesian_interface.py index 548c01e8..e9f2dcae 100644 --- a/cob_cartesian_controller/src/simple_cartesian_interface/simple_cartesian_interface.py +++ b/cob_cartesian_controller/src/simple_cartesian_interface/simple_cartesian_interface.py @@ -9,9 +9,10 @@ from cob_cartesian_controller.msg import Profile -def move_lin(pose_goal, frame_id, profile, rotate_only=False): - client = actionlib.SimpleActionClient('cartesian_trajectory_action', CartesianControllerAction) - rospy.logwarn("Waiting for ActionServer...") +def move_lin(pose_goal, frame_id, profile): + action_name = rospy.get_namespace()+'cartesian_trajectory_action' + client = actionlib.SimpleActionClient(action_name, CartesianControllerAction) + rospy.logwarn("Waiting for ActionServer: %s", action_name) success = client.wait_for_server(rospy.Duration(2.0)) if(not success): return (success, "ActionServer not available within timeout") @@ -32,9 +33,10 @@ def move_lin(pose_goal, frame_id, profile, rotate_only=False): result = client.get_result() return (result.success, result.message) -def move_circ(pose_center, frame_id, start_angle, end_angle, radius, profile, rotate_only=False): - client = actionlib.SimpleActionClient('cartesian_trajectory_action', CartesianControllerAction) - rospy.logwarn("Waiting for ActionServer...") +def move_circ(pose_center, frame_id, start_angle, end_angle, radius, profile): + action_name = rospy.get_namespace()+'cartesian_trajectory_action' + client = actionlib.SimpleActionClient(action_name, CartesianControllerAction) + rospy.logwarn("Waiting for ActionServer: %s", action_name) success = client.wait_for_server(rospy.Duration(2.0)) if(not success): return (success, "ActionServer not available within timeout") From c2f651f42cc09f05b3cc255a37c2b6dfd484afdb Mon Sep 17 00:00:00 2001 From: ipa-fxm Date: Wed, 14 Dec 2016 15:15:59 +0100 Subject: [PATCH 3/6] add install tags --- cob_cartesian_controller/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/cob_cartesian_controller/CMakeLists.txt b/cob_cartesian_controller/CMakeLists.txt index d13dbbff..74e6647b 100644 --- a/cob_cartesian_controller/CMakeLists.txt +++ b/cob_cartesian_controller/CMakeLists.txt @@ -67,6 +67,7 @@ install(DIRECTORY include/${PROJECT_NAME}/ ) install(PROGRAMS scripts/test_move_lin.py scripts/test_move_circ.py scripts/test_move_santa.py + scripts/test_move_lin_interface.py scripts/test_move_circ_interface.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}/scripts ) From 06bce7d79d5d5fdc5332f6dbb5caddbcfaa9e621 Mon Sep 17 00:00:00 2001 From: ipa-fxm Date: Wed, 14 Dec 2016 15:16:06 +0100 Subject: [PATCH 4/6] remove obsolete script --- .../scripts/example_move_lin.py | 37 ------------------- 1 file changed, 37 deletions(-) delete mode 100755 cob_cartesian_controller/scripts/example_move_lin.py diff --git a/cob_cartesian_controller/scripts/example_move_lin.py b/cob_cartesian_controller/scripts/example_move_lin.py deleted file mode 100755 index 63f88716..00000000 --- a/cob_cartesian_controller/scripts/example_move_lin.py +++ /dev/null @@ -1,37 +0,0 @@ -#! /usr/bin/env python -import rospy -import actionlib - -from cob_cartesian_controller.msg import CartesianControllerAction, CartesianControllerGoal -from cob_cartesian_controller.msg import Profile - -if __name__ == '__main__': - rospy.init_node('test_move_lin') - client = actionlib.SimpleActionClient('arm/cartesian_trajectory_action', CartesianControllerAction) - rospy.logwarn("Waiting for ActionServer...") - client.wait_for_server() - rospy.logwarn("...done") - - # Fill in the goal here - goal = CartesianControllerGoal() - - goal.move_type = CartesianControllerGoal.LIN - goal.move_lin.pose_goal.position.x = 0.3 - goal.move_lin.pose_goal.position.y = 0.3 - goal.move_lin.pose_goal.position.z = 0.5 - goal.move_lin.pose_goal.orientation.x = 0.0 - goal.move_lin.pose_goal.orientation.y = 0.0 - goal.move_lin.pose_goal.orientation.z = 0.0 - goal.move_lin.pose_goal.orientation.w = 1.0 - goal.move_lin.frame_id = 'world' - goal.move_lin.rotate_only = False - - goal.profile.vel = 0.05 - goal.profile.accl = 0.1 - goal.profile.profile_type = Profile.SINOID - - print goal - - # Send the goal - client.send_goal(goal) - client.wait_for_result() From 8d9baf7641b96da31db5bc5a18fff8c7308b56f4 Mon Sep 17 00:00:00 2001 From: ipa-fxm Date: Wed, 14 Dec 2016 15:33:25 +0100 Subject: [PATCH 5/6] clear preview --- cob_cartesian_controller/src/cartesian_controller_utils.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/cob_cartesian_controller/src/cartesian_controller_utils.cpp b/cob_cartesian_controller/src/cartesian_controller_utils.cpp index 9f47e8f1..4ad34de3 100644 --- a/cob_cartesian_controller/src/cartesian_controller_utils.cpp +++ b/cob_cartesian_controller/src/cartesian_controller_utils.cpp @@ -145,6 +145,9 @@ void CartesianControllerUtils::previewPath(const geometry_msgs::PoseArray pose_a marker.color.g = 0.0; marker.color.b = 1.0; marker.color.a = 1.0; + + // clear marker_array_ to only show preview of current path + marker_array_.markers.clear(); double id = marker_array_.markers.size(); From d7c5d089a287fd14e60e6a7f61fe506c3dad4643 Mon Sep 17 00:00:00 2001 From: ipa-fxm Date: Wed, 14 Dec 2016 15:33:33 +0100 Subject: [PATCH 6/6] optimize output --- cob_cartesian_controller/src/cartesian_controller.cpp | 2 +- .../simple_cartesian_interface.py | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/cob_cartesian_controller/src/cartesian_controller.cpp b/cob_cartesian_controller/src/cartesian_controller.cpp index f419c81a..49fc43c7 100644 --- a/cob_cartesian_controller/src/cartesian_controller.cpp +++ b/cob_cartesian_controller/src/cartesian_controller.cpp @@ -82,7 +82,7 @@ bool CartesianController::initialize() as_->registerPreemptCallback(boost::bind(&CartesianController::preemptCallback, this)); as_->start(); - ROS_INFO("...done!"); + ROS_INFO("Cartesian Controller running"); return true; } diff --git a/cob_cartesian_controller/src/simple_cartesian_interface/simple_cartesian_interface.py b/cob_cartesian_controller/src/simple_cartesian_interface/simple_cartesian_interface.py index e9f2dcae..cb7b54b0 100644 --- a/cob_cartesian_controller/src/simple_cartesian_interface/simple_cartesian_interface.py +++ b/cob_cartesian_controller/src/simple_cartesian_interface/simple_cartesian_interface.py @@ -27,7 +27,7 @@ def move_lin(pose_goal, frame_id, profile): client.send_goal(goal) print "goal sent" state = client.get_state() - print state + # print state client.wait_for_result() print "result received" result = client.get_result() @@ -49,12 +49,12 @@ def move_circ(pose_center, frame_id, start_angle, end_angle, radius, profile): goal.move_circ.end_angle = end_angle goal.move_circ.radius = radius goal.profile = profile - print goal + # print goal client.send_goal(goal) print "goal sent" state = client.get_state() - print state + # print state client.wait_for_result() print "result received" result = client.get_result()