- 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")
+
+ def enableManipulator(self, msg):
+ rospy.loginfo("Set Manipulator: %d", msg.data)
+ i2c_write_reg(0x52, 0x0f, chr(msg.data))
+ return SetBoolResponse(True, "")