Merge branch 'indigo_dev' of https://github.com/ipa320/cob_control in…

…to indigo_dev

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
 )
 

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()
 

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)

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)

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;
 }
 
@@ -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())

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();
 

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")
@@ -26,15 +27,16 @@ def move_lin(pose_goal, frame_id, profile, rotate_only=False):
     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()
     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")
@@ -47,12 +49,12 @@ def move_circ(pose_center, frame_id, start_angle, end_angle, radius, profile, ro
     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()