eddiebot_node.eddie_sensor

47 48 EDDIE_PULSES_TO_M = 0.000445558279992234 49

50 - def __init__(self, robot):

51 self._robot = robot 52 self._sensor_state_struct = struct.Struct(">12B2hBHhb7HBH5B4h2HB6H2B4hb") 53 self._last_encoder_counts = None

54

55 - def _request_packet(self, packet_id):

56 """Reqeust a sensor packet.""" 57 with self._robot.sci.lock: 58 self._robot.sci.flush_input() 59 # self._robot.sci.sensors(packet_id) Remarked because Eddiebot can not retrieve multiple sensor information 60 output = self._robot.sensors() 61 #kwc: there appears to be a 10-20ms latency between sending the 62 #sensor request and fully reading the packet. Based on 63 #observation, we are preferring the 'before' stamp rather than 64 #after. 65 stamp = time.time() 66 # length = eddiebot_driver.SENSOR_GROUP_PACKET_LENGTHS[packet_id] 67 # return self._robot.sci.read(length), stamp 68 return output, stamp

69

70 - def _deserialize(self, buffer, timestamp):

71 try: 72 (bumps_wheeldrops,#1 73 wall,#2 74 cliff_left, #3 75 cliff_front_left, #4 76 cliff_front_right,#5 77 cliff_right,#6 78 virtual_wall,#7 79 motor_overcurrents,#8 80 dirt_detector_left, #9 81 dirt_detector_right,#10 82 remote_opcode, #11 83 buttons,#12 84 distance, #13 85 angle,#14 86 charging_state, #15 87 voltage, #16 88 current, #17 89 temperature,#18 90 charge, #19 91 capacity,#20 92 wall_signal,#21 93 cliff_left_signal,#22 94 cliff_front_left_signal,#23 95 cliff_front_right_signal,#24 96 cliff_right_signal,#25 97 user_digital_inputs, #26 98 user_analog_input, #27 99 charging_sources_available, #28 100 oi_mode, #29 101 song_number, #30 102 song_playing, #31 103 number_of_stream_packets, #32 104 requested_velocity, #33 105 requested_radius, #34 106 requested_right_velocity, #35 107 requested_left_velocity, #36 108 encoder_counts_left, #37 109 encoder_counts_right, #38 110 light_bumper, #39 111 light_bump_left, #40 112 light_bump_front_left, #41 113 light_bump_center_left, #42 114 light_bump_center_right, #43 115 light_bump_front_right, #44 116 light_bump_right, #45 117 ir_opcode_left, #46 118 ir_opcode_right, #47 119 left_motor_current, #48 120 right_motor_current, #49 121 main_brish_current, #50 122 side_brush_current, #51 123 statis ) = (buffer[0], #52 124 buffer[1], 125 buffer[2], 126 buffer[3], 127 buffer[4], 128 buffer[5], 129 buffer[6], 130 buffer[7], 131 buffer[8], 132 buffer[9], 133 buffer[10], 134 buffer[11], 135 buffer[12], 136 buffer[13], 137 buffer[14], 138 buffer[15], 139 buffer[16], 140 buffer[17], 141 buffer[18], 142 buffer[19], 143 buffer[20], 144 buffer[21], 145 buffer[22], 146 buffer[23], 147 buffer[24], 148 buffer[25], 149 buffer[26], 150 buffer[27], 151 buffer[28], 152 buffer[29], 153 buffer[30], 154 buffer[31], 155 buffer[32], 156 buffer[33], 157 buffer[34], 158 buffer[35], 159 buffer[36], 160 buffer[37], 161 buffer[38], 162 buffer[39], 163 buffer[40], 164 buffer[41], 165 buffer[42], 166 buffer[43], 167 buffer[44], 168 buffer[45], 169 buffer[46], 170 buffer[47], 171 buffer[48], 172 buffer[49], 173 buffer[50], 174 buffer[51]) 175 except struct.error, e: 176 raise roslib.message.DeserializationError(e) 177 178 # self.wall = bool(self.wall) 179 # self.cliff_left = bool(self.cliff_left) 180 # self.cliff_front_left = bool(self.cliff_front_left) 181 # self.cliff_front_right = bool(self.cliff_front_right) 182 # self.cliff_right = bool(self.cliff_right) 183 # self.virtual_wall = bool(self.virtual_wall) 184 # self.song_playing = bool(self.song_playing) 185 # 186 # # do unit conversions 187 self.header = std_msgs.Header(stamp=rospy.Time.from_seconds(timestamp)) 188 # self.requested_velocity = float(self.requested_velocity) / 1000. 189 # self.requested_radius = float(self.requested_radius) / 1000. 190 # self.requested_right_velocity = float(self.requested_right_velocity) / 1000. 191 # self.requested_left_velocity = float(self.requested_left_velocity) / 1000. 192 193 self.requested_velocity = 0. 194 self.requested_radius = 0. 195 self.requested_right_velocity = 0. 196 self.requested_left_velocity = 0. 197 198 199 200 # The distance and angle calculation sent by the robot seems to 201 # be really bad. Re-calculate the values using the raw enconder 202 # counts. 203 # if self._last_encoder_counts: 204 # count_delta_left = self._normalize_encoder_count( 205 # self.encoder_counts_left - self._last_encoder_counts[0], 0xffff) 206 # count_delta_right = self._normalize_encoder_count( 207 # self.encoder_counts_right - self._last_encoder_counts[1], 0xffff) 208 # distance_left = count_delta_left * self.EDDIE_PULSES_TO_M 209 # distance_right = count_delta_right * self.EDDIE_PULSES_TO_M 210 # self.distance = (distance_left + distance_right) / 2.0 211 # self.angle = (distance_right - distance_left) / robot_types.ROBOT_TYPES['eddie'].wheel_separation 212 # else: 213 # self.disance = 0 214 # self.angle = 0 215 # self._last_encoder_counts = (self.encoder_counts_left, self.encoder_counts_right) 216 self.encoder_counts_left = 0. 217 self.encoder_counts_right = 0. 218 self.angle = 0. 219 self._last_encoder_counts = 0. 220 221 # ================== Added =========================== 222 self.bumps_wheeldrops = bumps_wheeldrops 223 self.wall = wall 224 self.cliff_left = cliff_left 225 self.cliff_front_left = cliff_front_left 226 self.cliff_front_right = cliff_front_right 227 self.cliff_right = cliff_right 228 self.virtual_wall = virtual_wall 229 self.motor_overcurrents = motor_overcurrents 230 self.dirt_detector_left = dirt_detector_left 231 self.dirt_detector_right = dirt_detector_right 232 self.remote_opcode = remote_opcode 233 self.buttons = buttons 234 self.distance = distance 235 self.angle = angle 236 self.charging_state = charging_state 237 self.voltage = voltage 238 self.current = current 239 self.temperature = temperature 240 self.charge = charge 241 self.capacity = capacity 242 self.wall_signal = wall_signal 243 self.cliff_left_signal = cliff_left_signal 244 self.cliff_front_left_signal = cliff_front_left_signal 245 self.cliff_front_right_signal = cliff_front_right_signal 246 self.cliff_right_signal = cliff_right_signal 247 self.user_digital_inputs = user_digital_inputs 248 self.user_analog_input = user_analog_input 249 self.charging_sources_available = charging_sources_available 250 self.oi_mode = oi_mode 251 self.song_number = song_number 252 self.song_playing = song_playing 253 self.number_of_stream_packets = number_of_stream_packets 254 self.requested_velocity = requested_velocity 255 self.requested_radius = requested_radius 256 self.requested_right_velocity = requested_right_velocity 257 self.requested_left_velocity = requested_left_velocity 258 self.encoder_counts_left = encoder_counts_left 259 self.encoder_counts_right = encoder_counts_right 260 self.light_bumper = light_bumper 261 self.light_bump_left = light_bump_left 262 self.light_bump_front_left = light_bump_front_left 263 self.light_bump_center_left = light_bump_center_left 264 self.light_bump_center_right = light_bump_center_right 265 self.light_bump_front_right = light_bump_front_right 266 self.light_bump_right = light_bump_right 267 self.ir_opcode_left = ir_opcode_left 268 self.ir_opcode_right = ir_opcode_right 269 self.left_motor_current = left_motor_current 270 self.right_motor_current = right_motor_current 271 self.main_brish_current = main_brish_current 272 self.side_brush_current = side_brush_current 273 self.statis = statis

274 275

276 - def _normalize_encoder_count(self, count_delta, maximal_count):

277 if count_delta >= maximal_count / 2: 278 return count_delta - maximal_count + 1 279 elif count_delta <= -maximal_count / 2: 280 return count_delta + maximal_count + 1 281 return count_delta

282

283 - def _encode_message(self, message):

284 message.header = self.header 285 message.bumps_wheeldrops = self.bumps_wheeldrops 286 message.wall = self.wall 287 message.cliff_left = self.cliff_left 288 message.cliff_front_left = self.cliff_front_left 289 message.cliff_front_right = self.cliff_front_right 290 message.cliff_right = self.cliff_right 291 message.virtual_wall = self.virtual_wall 292 message.motor_overcurrents = self.motor_overcurrents 293 message.dirt_detector_left = self.dirt_detector_left 294 message.dirt_detector_right = self.dirt_detector_right 295 message.remote_opcode = self.remote_opcode 296 message.buttons = self.buttons 297 message.distance = self.distance 298 message.angle = self.angle 299 message.charging_state = self.charging_state 300 message.voltage = self.voltage 301 message.current = self.current 302 message.temperature = self.temperature 303 message.charge = self.charge 304 message.capacity = self.capacity 305 message.wall_signal = self.wall_signal 306 message.cliff_left_signal = self.cliff_left_signal 307 message.cliff_front_left_signal = self.cliff_front_left_signal 308 message.cliff_front_right_signal = self.cliff_front_right_signal 309 message.cliff_right_signal, = self.cliff_right_signal, 310 message.user_digital_inputs = self.user_digital_inputs 311 message.user_analog_input, = self.user_analog_input, 312 message.charging_sources_available = self.charging_sources_available 313 message.oi_mode = self.oi_mode 314 message.song_number = self.song_number 315 message.song_playing = self.song_playing 316 message.number_of_stream_packets, = self.number_of_stream_packets, 317 message.requested_velocity = self.requested_velocity 318 message.requested_radius, = self.requested_radius, 319 message.requested_right_velocity = self.requested_right_velocity 320 message.requested_left_velocity = self.requested_left_velocity 321 return message

322

323 - def get_all(self, sensor_state):

324 buff, timestamp = self._request_packet(100) 325 if buff is not None: 326 self._deserialize(buff, timestamp) 327 self._encode_message(sensor_state)

Source Code for Module eddiebot_node.eddie_sensor_handler