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 struct
34 import logging
35 import time
36 import math
37
38 import rospy
39 import create_driver
40
41 import std_msgs.msg as std_msgs
42
43 import robot_types
44
46
47 ROOMBA_PULSES_TO_M = 0.000445558279992234
48
50 self._robot = robot
51 self._sensor_state_struct = struct.Struct(">12B2hBHhb7HBH5B4h2HB6H2B4hb")
52 self._last_encoder_counts = None
53
55 """Reqeust a sensor packet."""
56 with self._robot.sci.lock:
57 self._robot.sci.flush_input()
58 self._robot.sci.sensors(packet_id)
59
60
61
62
63 stamp = time.time()
64 length = create_driver.SENSOR_GROUP_PACKET_LENGTHS[packet_id]
65 return self._robot.sci.read(length), stamp
66
68 try:
69 (self.bumps_wheeldrops,
70 self.wall,
71 self.cliff_left, self.cliff_front_left, self.cliff_front_right, self.cliff_right,
72 self.virtual_wall,
73 self.motor_overcurrents,
74 self.dirt_detector_left, self.dirt_detector_right,
75 self.remote_opcode, self.buttons,
76 self.distance, self.angle,
77 self.charging_state,
78 self.voltage, self.current, self.temperature, self.charge, self.capacity,
79 self.wall_signal, self.cliff_left_signal, self.cliff_front_left_signal,
80 self.cliff_front_right_signal, self.cliff_right_signal,
81 self.user_digital_inputs, self.user_analog_input,
82 self.charging_sources_available,
83 self.oi_mode,
84 self.song_number, self.song_playing,
85 self.number_of_stream_packets,
86 self.requested_velocity, self.requested_radius,
87 self.requested_right_velocity, self.requested_left_velocity,
88 self.encoder_counts_left, self.encoder_counts_right,
89 self.light_bumper,
90 self.light_bump_left, self.light_bump_front_left, self.light_bump_center_left,
91 self.light_bump_center_right, self.light_bump_front_right, self.light_bump_right,
92 self.ir_opcode_left, self.ir_opcode_right,
93 self.left_motor_current, self.right_motor_current,
94 self.main_brish_current, self.side_brush_current,
95 self.statis, ) = self._sensor_state_struct.unpack(buffer[0:80])
96 except struct.error, e:
97 raise roslib.message.DeserializationError(e)
98
99 self.wall = bool(self.wall)
100 self.cliff_left = bool(self.cliff_left)
101 self.cliff_front_left = bool(self.cliff_front_left)
102 self.cliff_front_right = bool(self.cliff_front_right)
103 self.cliff_right = bool(self.cliff_right)
104 self.virtual_wall = bool(self.virtual_wall)
105 self.song_playing = bool(self.song_playing)
106
107
108 self.header = std_msgs.Header(stamp=rospy.Time.from_seconds(timestamp))
109 self.requested_velocity = float(self.requested_velocity) / 1000.
110 self.requested_radius = float(self.requested_radius) / 1000.
111 self.requested_right_velocity = float(self.requested_right_velocity) / 1000.
112 self.requested_left_velocity = float(self.requested_left_velocity) / 1000.
113
114
115
116
117 if self._last_encoder_counts:
118 count_delta_left = self._normalize_encoder_count(
119 self.encoder_counts_left - self._last_encoder_counts[0], 0xffff)
120 count_delta_right = self._normalize_encoder_count(
121 self.encoder_counts_right - self._last_encoder_counts[1], 0xffff)
122 distance_left = count_delta_left * self.ROOMBA_PULSES_TO_M
123 distance_right = count_delta_right * self.ROOMBA_PULSES_TO_M
124 self.distance = (distance_left + distance_right) / 2.0
125 self.angle = (distance_right - distance_left) / robot_types.ROBOT_TYPES['roomba'].wheel_separation
126 else:
127 self.disance = 0
128 self.angle = 0
129 self._last_encoder_counts = (self.encoder_counts_left, self.encoder_counts_right)
130
132 if count_delta >= maximal_count / 2:
133 return count_delta - maximal_count + 1
134 elif count_delta <= -maximal_count / 2:
135 return count_delta + maximal_count + 1
136 return count_delta
137
177
179 buff, timestamp = self._request_packet(100)
180 if buff is not None:
181 self._deserialize(buff, timestamp)
182 self._encode_message(sensor_state)
183