+
+ 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")