impact_watcher.py
Go to the documentation of this file.
00001 import roslib; roslib.load_manifest('pr2_precise_trajectory')
00002 import rospy
00003 from pr2_controllers_msgs.msg import *
00004 from pr2_gripper_sensor_msgs.msg import *
00005 from actionlib import SimpleActionClient, SimpleGoalState
00006 from sensor_msgs.msg import JointState
00007 import trajectory_msgs.msg
00008 
00009 class ImpactWatcher:
00010     def __init__(self, names=['r_gripper_sensor_controller'], rate=200):
00011         self.accel = rospy.get_param('acceleration_trigger', 70.0)
00012         self.slip = rospy.get_param('slip_trigger', 0.008)
00013         self.trigger = PR2GripperEventDetectorCommand.ACC  # use just acceleration as our contact signal
00014         self.clients = {}
00015         for name in names:
00016             self.clients[name] = SimpleActionClient("%s/event_detector"%name, PR2GripperEventDetectorAction)
00017             #wait for the action servers to come up 
00018             rospy.loginfo("[IMPACT] Waiting for %s controllers"%name)
00019             self.clients[name].wait_for_server()
00020             rospy.loginfo("[IMPACT] Got %s controllers"%name)
00021 
00022         self.rate = rospy.Rate(rate)
00023 
00024     def wait_for_impact(self):
00025         place_goal = PR2GripperEventDetectorGoal()
00026         place_goal.command.trigger_conditions                  = self.trigger
00027         place_goal.command.acceleration_trigger_magnitude = self.accel  # m/^2
00028         place_goal.command.slip_trigger_magnitude            = self.slip     # slip gain
00029 
00030         for name, client in self.clients.iteritems():
00031             client.send_goal(place_goal)
00032 
00033         #wait for a slap
00034         while(not self.is_slap_done() and not rospy.is_shutdown()):
00035             self.rate.sleep()
00036 
00037     def is_slap_done(self):
00038         done = True
00039         for name, client in self.clients.iteritems():
00040             done = done and client.get_state() > SimpleGoalState.ACTIVE
00041         return done
00042 
00043 


pr2_precise_trajectory
Author(s): David V. Lu!!
autogenerated on Fri Aug 28 2015 12:12:32