+ self.send_range(self.pub_range_bwd, "sonar_backward", Range.ULTRASOUND, dist, 0.04, self.range_sensor_max, self.range_sensor_fov)
+
+ def update_dist_backward(self):
+ if self.pub_range_bwd.get_num_connections() > 0:
+ self.start_dist_srf(0x7)
+
+ def get_dist_forward_right(self):
+ if self.pub_range_fwd_right.get_num_connections() > 0:
+ dist = self.read_dist_srf(0x19)
+ self.send_range(self.pub_range_fwd_right, "sonar_forward_right", Range.ULTRASOUND, dist, 0.04, self.range_sensor_max, self.range_sensor_fov)
+
+ def update_dist_forward_right(self):
+ if self.pub_range_fwd_right.get_num_connections() > 0:
+ self.start_dist_srf(0xb)
+
+ def led_stripe_received(self, msg):
+ for led in msg.leds:
+ self.pStripe.set(led.num, red=led.red, green=led.green, blue=led.blue)
+ self.pStripe.update()
+
+ def check_docked(self):
+ if self.bDocked and not self.bDocked_last:
+ rospy.loginfo("Docking event")
+ elif not self.bDocked and self.bDocked_last:
+ if not self.bStayDocked or self.cur_vel != (0, 0):
+ rospy.loginfo("Undocking event")
+ else:
+ rospy.loginfo("Undocking event..redocking")
+ thread.start_new_thread(self.redock, ())
+
+ self.bDocked_last = self.bDocked
+
+ def redock(self):
+ self.cmd_vel = (-0.1, 0)
+ for i in range(100):
+ if self.bDocked:
+ break
+ sleep(0.01)
+ self.cmd_vel = (0, 0)
+ if self.bDocked:
+ rospy.loginfo("Redocking done")
+ else:
+ rospy.logerr("Redocking failed")