$search

vaccuum_follower.py

Go to the documentation of this file.

#! /usr/bin/env python

import roslib; roslib.load_manifest('pr2_dremel_server')
import rospy
import actionlib
import math
import copy
import numpy
import tf.listener

import simple_robot_control

import geometry_msgs.msg



pos_def = (-0.176, 0.274, 0.174)
orient_def = (0.356, 0.586, -0.365, 0.630)

class VacFollower:
    
    def __init__(self):
        self.arml = simple_robot_control.Arm('l')
        self.listener = tf.TransformListener()
        
    
    
    def follow(self,pos, orient):
        self.listener.waitForTransform('r_gripper_tool_frame', 'l_wrist_roll_link', rospy.Time(), rospy.Duration(4.0))
        while rospy.is_shutdown() == False:
            now = rospy.Time.now()
#            print "wait for transform"
            self.listener.waitForTransform('r_gripper_tool_frame', 'l_wrist_roll_link', now, rospy.Duration(4.0))
            (trans_l,rot_l) = self.listener.lookupTransform( 'r_gripper_tool_frame', 'l_wrist_roll_link', rospy.Time(0))
            if numpy.linalg.norm(numpy.array(pos) - trans_l) > 0.005:
                self.arml.goToPose(pos, orient, 'r_gripper_tool_frame' , .555550, seed_angles = (0.884, -0.146, 1.920, -1.675, -11.036, -1.158, -2.970))
            else:
                rospy.sleep(.30)
        
    
        
    
if __name__ == '__main__':
    rospy.init_node('vacuum_follower')
    vafo = VacFollower()
    vafo.follow(pos_def, orient_def)
        

---

pr2_dremel_server
Author(s): Christian Bersch, Benjamin Pitzer (maintained by Benjamin Pitzer)
autogenerated on Sun Mar 3 12:07:41 2013