def RealShoot(self, power, pos) :
msg = Twist()
msg.linear.x = 0
msg.linear.y = 0
msg.angular.z = 0
self.cmdvel_pub.publish(msg)
self.hold_pub.publish(False)
time.sleep(0.5)
msg = Int32()
msg.data = power
print("Real shooting: ", power)
self.shoot_pub.publish(msg)
time.sleep(0.5)
self.hold_pub.publish(True)
def drive_forward(self):
"""
Drive the robot forward a small amount
:return: void
"""
print(self.area)
if 0 < self.area < 30000:
print("Driving Forward +++++++++++++++++++++")
msg = Twist()
msg.linear.x = .6
msg.linear.y = 0
msg.linear.z = 0
msg.angular.x = 0
msg.angular.y = 0
msg.angular.z = 0
self.yaw_pub.publish(msg)
rospy.sleep(.2)
self.stopped_flag = False
else:
print("Stopped -----------------------------")
msg = Twist()
msg.linear.x = 0
msg.linear.y = 0
msg.linear.z = 0
msg.angular.x = 0
msg.angular.y = 0
msg.angular.z = 0
self.yaw_pub.publish(msg)
self.stopped_flag = True
if self.area > 30000 and self.stopped_flag and self.pickup_done:
# Drive Forward
print("Found Can, Driving forward")
rospy.sleep(3)
for i in range(100):
msg = Twist()
msg.linear.x = .6
msg.linear.y = 0
msg.linear.z = 0
msg.angular.x = 0
msg.angular.y = 0
msg.angular.z = 0
self.yaw_pub.publish(msg)
rospy.sleep(5)
msg = Twist()
msg.linear.x = 0
msg.linear.y = 0
msg.linear.z = 0
msg.angular.x = 0
msg.angular.y = 0
msg.angular.z = 0
self.yaw_pub.publish(msg)
print("Done driving forward, picking up can")
rospy.sleep(3)
msg = Bool()
msg.data = True
self.pickup_ready.publish(msg)
self.pickup_done = False
while not self.pickup_done:
rospy.sleep(1)
