Package robot_script :: Package event_simulators :: Module fakeHuman
[hide private]
[frames] | no frames]

Source Code for Module robot_script.event_simulators.fakeHuman

  1  #!/usr/bin/env python 
  2  import roslib 
  3  roslib.load_manifest('geometry_msgs') 
  4  roslib.load_manifest('tf') 
  5  roslib.load_manifest('rospy') 
  6  roslib.load_manifest('visualization_msgs') 
  7   
  8  # Generic libraries 
  9  import sys,os,time 
 10  from math import * 
 11  from random import randint 
 12  from collections import OrderedDict; 
 13   
 14  # ROS libraries 
 15  import rospy 
 16  from tf import TransformListener, TransformBroadcaster, transformations 
 17  from geometry_msgs.msg import * 
 18  from std_msgs.msg import Header,ColorRGBA 
 19  from visualization_msgs.msg import * 
 20   
 21  # Superclass: 
 22  from event_simulator import EventSimulator; 
 23   
24 -class FakeHuman(EventSimulator):
25 "Simulating the perceptual data we would get from a human" 26
27 - def __init__(self):
28 self.tfBroadcaster = TransformBroadcaster() 29 q = transformations.quaternion_from_euler(0,0,pi) 30 self.humanPose = Pose(Point(5, 0, 1.65), Quaternion(*q)) 31 self.markerPublisher = rospy.Publisher('visualization_marker', Marker) 32 rospy.loginfo('Initialized.') 33 self.startTime = time.time() 34 rospy.init_node('fake_human_node', anonymous=True)
35
36 - def start(self, motionSchedule):
37 38 self.motionSchedule = motionSchedule; 39 self.motionScheduleKeys = motionSchedule.keys(); 40 # Current index into the motion schedule keys: 41 self.motionScheduleIndex = 0; 42 43 threadSchedule = OrderedDict(); 44 # Create a schedule that calls the callback function (i.e. self.update()) 45 # at 0.2 seconds, and then repeats. The None is the argument passed 46 # to self.update(). 47 threadSchedule[0.2] = None; 48 self.startTime = time.time(); 49 super(FakeHuman, self).start(threadSchedule, self.update, repeat=True);
50
51 - def publishTFPose(self, pose, name, parent):
52 if (pose != None): 53 self.tfBroadcaster.sendTransform((pose.position.x, pose.position.y, pose.position.z), 54 (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w), 55 rospy.Time.now(), name, parent)
56
57 - def getDiff(self, target, current, speed):
58 diff = target - current 59 if diff > speed: 60 return speed, False 61 elif diff < -speed: 62 return -speed, False 63 return diff, True
64
65 - def moveTowardsTarget(self, speed):
66 xDiff, xReached = self.getDiff(self.target[0], self.humanPose.position.x, speed[0]) 67 yDiff, yReached = self.getDiff(self.target[1], self.humanPose.position.y, speed[1]) 68 self.humanPose.position.x += xDiff 69 self.humanPose.position.y += yDiff 70 return (xReached and yReached)
71
72 - def update(self, dummy):
73 ''' 74 Called repeatedly from the EventSimulator thread. Moves the Rviz human towards 75 various locations in small increments. Locations, speed, and move times are 76 taken from self.motionSchedule, which was passed into the start() method. 77 Example schedule is the following dict (keys are fractional seconds):: 78 79 motionSchedule[2] = None; 80 motionSchedule[5] = {target: [2,-1], speed: [0.1,0.03]}; 81 motionSchedule[10] = None; 82 motionSchedule[12] = {target: [4,-2], speed: [.1,.055]}; 83 motionSchedule[16] = None; 84 motionSchedule[20] = {target: [0.6,.5], speed: [.08,.1]}; 85 motionSchedule[27] = None; 86 87 @param dummy: EventSimulator calls with an argument, which is None in our case, and unused. 88 @type dummy: None. 89 ''' 90 91 timePassed = time.time() - self.startTime; 92 93 # Get current motion's upper time bound: 94 keyframeTime = self.motionScheduleKeys[self.motionScheduleIndex]; 95 # Is time later than that end time of current motion? 96 while timePassed >= keyframeTime: 97 # Grab the next schedule entry: 98 self.motionScheduleIndex += 1; 99 if self.motionScheduleIndex >= len(self.motionSchedule): 100 # Ran out of schedule entries. Start over: 101 self.motionScheduleIndex = 0; 102 self.startTime = time.time(); 103 timePassed = time.time() - self.startTime; 104 break; 105 else: 106 # Grab the new schedule entry's upper time bound (the dict key): 107 keyframeTime = self.motionScheduleKeys[self.motionScheduleIndex]; 108 109 try: 110 # Get motion x/y and x/y speeds: 111 keyframeTarget = self.motionSchedule[keyframeTime]['target']; 112 keyframeSpeed = self.motionSchedule[keyframeTime]['speed']; 113 except TypeError: 114 # Motion schedule value for this entry is None; do nothing: 115 self.visualizeFakeHuman(); 116 return; 117 118 self.target = keyframeTarget; 119 # Move a little bit: 120 self.moveTowardsTarget(keyframeSpeed); 121 self.visualizeFakeHuman();
122 123
124 - def visualizeFakeHuman(self):
125 # Visualize the fake human: 126 if (self.humanPose != None): 127 self.publishTFPose(self.humanPose, 'fake_human', 'odom_combined') 128 129 m = Marker(type=Marker.SPHERE, id=0, lifetime=rospy.Duration(2), pose=self.humanPose, 130 scale=Vector3(0.3,0.3,0.3), header=Header(frame_id='odom_combined'), 131 color=ColorRGBA(1.0, 0.0, 0.0, 0.8)) 132 self.markerPublisher.publish(m)
133 134 135 if __name__ == "__main__": 136 137 human = FakeHuman() 138 motionSchedule = OrderedDict(); 139 motionSchedule[2] = None; 140 motionSchedule[5] = {'target': [2,-1], 'speed': [0.1,0.03]}; 141 motionSchedule[10] = None; 142 motionSchedule[12] = {'target': [4,-2], 'speed': [.1,.055]}; 143 motionSchedule[16] = None; 144 motionSchedule[20] = {'target': [0.6,.5], 'speed': [.08,.1]}; 145 motionSchedule[27] = None; 146 147 #rospy.spin() 148 # while(not rospy.is_shutdown()): 149 # human.update() 150 human.start(motionSchedule); 151 time.sleep(0.1) 152