1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33 import roslib.message
34 import struct
35 import logging
36 import time
37 import rospy
38
39 from math import radians
40 from create_driver import SENSOR_GROUP_PACKET_LENGTHS
41
42
43 _struct_BI = struct.Struct(">BI")
44 _struct_12B2hBHhb7HBH5B4h = struct.Struct(">12B2hBHhb7HBH5B4h")
45
47 """
48 unpack serialized message in str into this message instance
49 @param buff: byte array of serialized message
50 @type buff: str
51 """
52 try:
53 _x = msg
54 (_x.bumps_wheeldrops, _x.wall, _x.cliff_left, _x.cliff_front_left, _x.cliff_front_right, _x.cliff_right, _x.virtual_wall, _x.motor_overcurrents, _x.dirt_detector_left, _x.dirt_detector_right, _x.remote_opcode, _x.buttons, _x.distance, _x.angle, _x.charging_state, _x.voltage, _x.current, _x.temperature, _x.charge, _x.capacity, _x.wall_signal, _x.cliff_left_signal, _x.cliff_front_left_signal, _x.cliff_front_right_signal, _x.cliff_right_signal, _x.user_digital_inputs, _x.user_analog_input, _x.charging_sources_available, _x.oi_mode, _x.song_number, _x.song_playing, _x.number_of_stream_packets, _x.requested_velocity, _x.requested_radius, _x.requested_right_velocity, _x.requested_left_velocity,) = _struct_12B2hBHhb7HBH5B4h.unpack(buff[0:52])
55
56 msg.wall = bool(msg.wall)
57 msg.cliff_left = bool(msg.cliff_left)
58 msg.cliff_front_left = bool(msg.cliff_front_left)
59 msg.cliff_front_right = bool(msg.cliff_front_right)
60 msg.cliff_right = bool(msg.cliff_right)
61 msg.virtual_wall = bool(msg.virtual_wall)
62 msg.song_playing = bool(msg.song_playing)
63
64
65 msg.angle = radians(msg.angle)
66 msg.header.stamp = rospy.Time.from_seconds(timestamp)
67 msg.distance = float(msg.distance) / 1000.
68
69 msg.requested_velocity = float(msg.requested_velocity) / 1000.
70 msg.requested_radius = float(msg.requested_radius) / 1000.
71 msg.requested_right_velocity = float(msg.requested_right_velocity) / 1000.
72 msg.requested_left_velocity = float(msg.requested_left_velocity) / 1000.
73
74 return msg
75 except struct.error, e:
76 raise roslib.message.DeserializationError(e)
77
79
83 """Reqeust a sensor packet."""
84 with self.robot.sci.lock:
85 self.robot.sci.flush_input()
86 self.robot.sci.sensors(packet_id)
87
88
89
90
91 stamp = time.time()
92 length = SENSOR_GROUP_PACKET_LENGTHS[packet_id]
93 return self.robot.sci.read(length), stamp
94
99