cob_frame_tracker: interactive_frame

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 # Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
00004 #
00005 # Licensed under the Apache License, Version 2.0 (the "License");
00006 # you may not use this file except in compliance with the License.
00007 # You may obtain a copy of the License at
00008 #
00009 #   http://www.apache.org/licenses/LICENSE-2.0
00010 #
00011 # Unless required by applicable law or agreed to in writing, software
00012 # distributed under the License is distributed on an "AS IS" BASIS,
00013 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00014 # See the License for the specific language governing permissions and
00015 # limitations under the License.
00016 
00017 
00018 import sys
00019 import math
00020 import copy
00021 from copy import deepcopy
00022 
00023 import rospy
00024 import tf
00025 from std_srvs.srv import Empty
00026 from cob_srvs.srv import SetString
00027 from geometry_msgs.msg import Point, PoseStamped
00028 from visualization_msgs.msg import Marker, InteractiveMarker, InteractiveMarkerControl
00029 from interactive_markers.interactive_marker_server import *
00030 from interactive_markers.menu_handler import *
00031 
00032 
00033 class InteractiveFrameTarget:
00034     def __init__(self):
00035         self.br = tf.TransformBroadcaster()
00036         self.listener = tf.TransformListener()
00037 
00038         #get this from the frame_tracker parameters
00039         if rospy.has_param('cartesian_controller/chain_tip_link'):
00040             self.active_frame = rospy.get_param("cartesian_controller/chain_tip_link")
00041         else:
00042             rospy.logerr("No chain_tip_link specified. Aborting!")
00043             sys.exit()
00044         if rospy.has_param('cartesian_controller/tracking_frame'):
00045             self.tracking_frame = rospy.get_param("cartesian_controller/tracking_frame")
00046         else:
00047             rospy.logerr("No tracking_frame specified. Aborting!")
00048             sys.exit()
00049         if rospy.has_param('cartesian_controller/root_frame'):
00050             self.root_frame = rospy.get_param("cartesian_controller/root_frame")
00051         else:
00052             rospy.logerr("No root_frame specified. Setting to 'base_link'!")
00053             self.root_frame = "base_link"
00054 
00055         if rospy.has_param('cartesian_controller/movable_trans'):
00056             self.movable_trans = rospy.get_param("cartesian_controller/movable_trans")
00057         else:
00058             rospy.logerr("No movable_trans specified. Setting True!")
00059             self.movable_trans = True
00060         if rospy.has_param('cartesian_controller/movable_rot'):
00061             self.movable_rot = rospy.get_param("cartesian_controller/movable_rot")
00062         else:
00063             rospy.logerr("No movable_rot specified. Setting True!")
00064             self.movable_rot = True
00065 
00066         self.tracking = False
00067         print "Waiting for StartTrackingServer..."
00068         rospy.wait_for_service('frame_tracker/start_tracking')
00069         print "...done!"
00070         self.start_tracking_client = rospy.ServiceProxy('frame_tracker/start_tracking', SetString)
00071 
00072         print "Waiting for StopTrackingServer..."
00073         rospy.wait_for_service('frame_tracker/stop_tracking')
00074         print "...done!"
00075         self.stop_tracking_client = rospy.ServiceProxy('frame_tracker/stop_tracking', Empty)
00076 
00077         self.target_pose = PoseStamped()
00078         self.target_pose.header.stamp = rospy.Time.now()
00079         self.target_pose.header.frame_id = self.root_frame
00080         self.target_pose.pose.orientation.w = 1.0
00081 
00082         ##give tf_listener some time to fill cache
00083         #try:
00084             #rospy.sleep(1.0)
00085         #except rospy.ROSInterruptException as e:
00086             ##print "ROSInterruptException"
00087             #pass
00088 
00089         transform_available = False
00090         while not transform_available:
00091             try:
00092                 (trans,rot) = self.listener.lookupTransform(self.root_frame, self.active_frame, rospy.Time(0))
00093             except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
00094                 #rospy.logwarn("Waiting for transform...")
00095                 try:
00096                     rospy.sleep(0.1)
00097                 except rospy.ROSInterruptException as e:
00098                     #print "ROSInterruptException"
00099                     pass
00100                 continue
00101             transform_available = True
00102 
00103         self.target_pose.pose.position.x = trans[0]
00104         self.target_pose.pose.position.y = trans[1]
00105         self.target_pose.pose.position.z = trans[2]
00106         self.target_pose.pose.orientation.x = rot[0]
00107         self.target_pose.pose.orientation.y = rot[1]
00108         self.target_pose.pose.orientation.z = rot[2]
00109         self.target_pose.pose.orientation.w = rot[3]
00110 
00111         self.ia_server = InteractiveMarkerServer("marker_server")
00112         self.int_marker = InteractiveMarker()
00113         self.int_marker.header.frame_id = self.root_frame
00114         self.int_marker.pose = self.target_pose.pose
00115         self.int_marker.name = "interactive_target"
00116         self.int_marker.description = self.tracking_frame
00117         self.int_marker.scale = 0.8
00118 
00119         # create a grey box marker
00120         box_marker = Marker()
00121         box_marker.type = Marker.CUBE
00122         box_marker.scale.x = 0.1
00123         box_marker.scale.y = 0.1
00124         box_marker.scale.z = 0.1
00125         box_marker.color.r = 0.0
00126         box_marker.color.g = 0.5
00127         box_marker.color.b = 0.5
00128         box_marker.color.a = 1.0
00129         control_3d = InteractiveMarkerControl()
00130         control_3d.always_visible = True
00131         control_3d.name = "move_rotate_3D"
00132         control_3d.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D
00133         control_3d.markers.append( box_marker )
00134         self.int_marker.controls.append(control_3d)
00135 
00136         control = InteractiveMarkerControl()
00137         control.always_visible = True
00138         control.orientation.w = 1
00139         control.orientation.x = 1
00140         control.orientation.y = 0
00141         control.orientation.z = 0
00142         if(self.movable_trans):
00143             control.name = "move_x"
00144             control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
00145             self.int_marker.controls.append(deepcopy(control))
00146             control.name = "move_y"
00147             control.orientation.x = 0
00148             control.orientation.y = 1
00149             control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
00150             self.int_marker.controls.append(deepcopy(control))
00151             control.name = "move_z"
00152             control.orientation.y = 0
00153             control.orientation.z = 1
00154             control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
00155             self.int_marker.controls.append(deepcopy(control))
00156         if(self.movable_rot):
00157             control.orientation.w = 1
00158             control.orientation.x = 1
00159             control.orientation.y = 0
00160             control.orientation.z = 0
00161             control.name = "rotate_x"
00162             control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
00163             self.int_marker.controls.append(deepcopy(control))
00164             control.name = "rotate_y"
00165             control.orientation.x = 0
00166             control.orientation.y = 1
00167             control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
00168             self.int_marker.controls.append(deepcopy(control))
00169             control.name = "rotate_z"
00170             control.orientation.y = 0
00171             control.orientation.z = 1
00172             control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
00173             self.int_marker.controls.append(deepcopy(control))
00174 
00175         self.ia_server.insert(self.int_marker, self.marker_fb)
00176 
00177         #create menu
00178         self.menu_handler = MenuHandler()
00179         self.menu_handler.insert( "StartTracking", callback=self.start_tracking )
00180         self.menu_handler.insert( "StopTracking", callback=self.stop_tracking )
00181         self.menu_handler.insert( "ResetTracking", callback=self.reset_tracking )
00182         self.int_marker_menu = InteractiveMarker()
00183         self.int_marker_menu.header.frame_id = self.root_frame
00184         self.int_marker_menu.name = "marker_menu"
00185         self.int_marker_menu.description = rospy.get_namespace()
00186         self.int_marker_menu.scale = 1.0
00187         self.int_marker_menu.pose.position.z = 1.2
00188         control = InteractiveMarkerControl()
00189         control.interaction_mode = InteractiveMarkerControl.MENU
00190         control.name = "menu_control"
00191         control.description= "InteractiveTargetMenu"
00192         self.int_marker_menu.controls.append(copy.deepcopy(control))
00193         self.ia_server.insert(self.int_marker_menu, self.menu_fb)
00194         self.menu_handler.apply( self.ia_server, self.int_marker_menu.name )
00195         self.ia_server.applyChanges()
00196 
00197     def start_tracking(self, fb):
00198         #print "start_tracking pressed"
00199         try:
00200             res = self.start_tracking_client(data=self.tracking_frame)
00201             print res
00202             self.tracking = True
00203         except rospy.ServiceException, e:
00204             print "Service call failed: %s"%e
00205             self.tracking = False
00206 
00207     def stop_tracking(self, fb):
00208         #print "stop_tracking pressed"
00209         try:
00210             res = self.stop_tracking_client()
00211             print res
00212             self.tracking = False
00213         except rospy.ServiceException, e:
00214             print "Service call failed: %s"%e
00215             self.tracking = False
00216 
00217     def reset_tracking(self, fb):
00218         #print "reset_tracking pressed"
00219         self.stop_tracking(fb)
00220         self.update_marker()
00221 
00222     def update_marker(self):
00223         transform_available = False
00224         while not transform_available:
00225             try:
00226                 (trans,rot) = self.listener.lookupTransform(self.root_frame, self.active_frame, rospy.Time(0))
00227             except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
00228                 rospy.logwarn("Waiting for transform...")
00229                 try:
00230                     rospy.sleep(0.1)
00231                 except rospy.ROSInterruptException as e:
00232                     #print "ROSInterruptException"
00233                     pass
00234                 continue
00235             transform_available = True
00236 
00237         reset_pose = PoseStamped()
00238         reset_pose.header.frame_id = self.root_frame
00239         reset_pose.header.stamp = rospy.Time.now()
00240         reset_pose.pose.position.x = trans[0]
00241         reset_pose.pose.position.y = trans[1]
00242         reset_pose.pose.position.z = trans[2]
00243         reset_pose.pose.orientation.x = rot[0]
00244         reset_pose.pose.orientation.y = rot[1]
00245         reset_pose.pose.orientation.z = rot[2]
00246         reset_pose.pose.orientation.w = rot[3]
00247 
00248         self.target_pose = reset_pose
00249         self.ia_server.setPose(self.int_marker.name, reset_pose.pose);
00250         self.ia_server.applyChanges()
00251 
00252     def menu_fb(self, fb):
00253         pass
00254 
00255     def marker_fb(self, fb):
00256         #p = feedback.pose.position
00257         #print feedback.marker_name + " is now at " + str(p.x) + ", " + str(p.y) + ", " + str(p.z)
00258         self.target_pose.header = fb.header
00259         self.target_pose.pose = fb.pose
00260         self.ia_server.applyChanges()
00261 
00262      def run(self):
00263         if(not self.tracking):
00264             self.update_marker()
00265 
00266         self.br.sendTransform(
00267             (self.target_pose.pose.position.x, self.target_pose.pose.position.y, self.target_pose.pose.position.z),
00268             (self.target_pose.pose.orientation.x, self.target_pose.pose.orientation.y, self.target_pose.pose.orientation.z, self.target_pose.pose.orientation.w),
00269             rospy.Time.now(), self.tracking_frame, self.target_pose.header.frame_id)
00270 
00271 
00272 if __name__ == "__main__":
00273     rospy.init_node("interactive_frame_target")
00274 
00275     ilt = InteractiveFrameTarget()
00276 
00277     r = rospy.Rate(50)
00278     while not rospy.is_shutdown():
00279         ilt.run()
00280         try:
00281             r.sleep()
00282         except rospy.ROSInterruptException as e:
00283             #print "ROSInterruptException"
00284             pass