1
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
9 import sys,os,time
10 from math import *
11 from random import randint
12 from collections import OrderedDict;
13
14
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
22 from event_simulator import EventSimulator;
23
25 "Simulating the perceptual data we would get from a human"
26
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
41 self.motionScheduleIndex = 0;
42
43 threadSchedule = OrderedDict();
44
45
46
47 threadSchedule[0.2] = None;
48 self.startTime = time.time();
49 super(FakeHuman, self).start(threadSchedule, self.update, repeat=True);
50
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
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
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
94 keyframeTime = self.motionScheduleKeys[self.motionScheduleIndex];
95
96 while timePassed >= keyframeTime:
97
98 self.motionScheduleIndex += 1;
99 if self.motionScheduleIndex >= len(self.motionSchedule):
100
101 self.motionScheduleIndex = 0;
102 self.startTime = time.time();
103 timePassed = time.time() - self.startTime;
104 break;
105 else:
106
107 keyframeTime = self.motionScheduleKeys[self.motionScheduleIndex];
108
109 try:
110
111 keyframeTarget = self.motionSchedule[keyframeTime]['target'];
112 keyframeSpeed = self.motionSchedule[keyframeTime]['speed'];
113 except TypeError:
114
115 self.visualizeFakeHuman();
116 return;
117
118 self.target = keyframeTarget;
119
120 self.moveTowardsTarget(keyframeSpeed);
121 self.visualizeFakeHuman();
122
123
125
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
148
149
150 human.start(motionSchedule);
151 time.sleep(0.1)
152