def forward(self, dist):
'''sends a forward command to the node that moves the bot'''
cmd2 = MoveCommand()
cmd2.direction = 'forward'
cmd2.distance = dist
cmd2.speed = 0.4
self.pub_bot_commands.publish(cmd2)
def rotate(self, deg):
rospy.loginfo('sending rotate command for %f deg',
self.anglesFromObject)
cmd = MoveCommand()
cmd.direction = 'counter-clock-wise-rotate'
cmd.distance = abs(deg)
if deg > 0:
cmd.speed = -0.3
else:
cmd.speed = 0.3
rospy.loginfo('published ged:%f , speed: %f', cmd.distance, cmd.speed)
self.pub_bot_commands.publish(cmd)
def pushObj(self):
if not self.findObjectPutInCenter():
rospy.loginfo('find obj is False')
return False
rospy.loginfo('find obj is True, moving to push it')
result = False
cmdForward = MoveCommand()
cmdForward.direction = 'forward'
cmdRotate = MoveCommand()
cmdRotate.direction = 'counter-clock-wise-rotate'
self.rotate(-15)
rospy.sleep(2)
rospy.loginfo('self.anglesFromObject:%f', self.anglesFromObject)
while (abs(self.anglesFromObject) > 10 and
abs(self.anglesFromObject) < 500) or self.forward_dist > 0.5:
rospy.loginfo(
'rotate clockWise so object will bein the center, shift:%f',
self.anglesFromObject)
if abs(self.anglesFromObject) > 10:
cmdRotate.distance = 8
if self.anglesFromObject > 0:
rospy.loginfo(
'rotate clockWise so object will bein the center, shift:%f',
self.anglesFromObject)
cmdRotate.speed = -0.2
else:
cmdRotate.speed = 0.2
rospy.loginfo(
'rotate counterClockWise so object will bein the center, shift:%f',
self.anglesFromObject)
else:
cmdRotate.distance = 0
cmdRotate.speed = 0
self.pub_bot_commands.publish(cmdRotate)
rospy.sleep(2)
if self.anglesFromObject < 100000 and self.anglesFromObject < 50 and self.forward_dist > 0.4:
rospy.loginfo('moving to object, distance: %f',
self.forward_dist)
cmdForward.distance = 0.57
cmdForward.speed = 0.03
result = True
#else:
# rospy.loginfo('self.anglesFromObject: %f, self.anglesFromObject:f, self.forward_dist:%f', self.anglesFromObject, self.forward_dist)
# cmdForward.distance = 0
# cmdForward.speed = 0
self.pub_bot_commands.publish(cmdForward)
rospy.sleep(0.5)
if result:
rospy.sleep(6)
cmdForward.distance = -0.45
cmdForward.speed = -0.2
self.pub_bot_commands.publish(cmdForward)
rospy.sleep(4)
return result
def forward_50cm(self):
cmd = MoveCommand()
cmd.direction = 'forward'
cmd.distance = 0.5
cmd.speed = 0.4
self.pub_bot_commands.publish(cmd)
def forward(self, dist):
cmd2 = MoveCommand()
cmd2.direction = 'forward'
cmd2.distance = dist
cmd2.speed = 0.4
self.pub_bot_commands.publish(cmd2)
def hitObject(self):
# put object in frame
while True:
interval = 0
direction = 0
rospy.loginfo('starting auto move: trying to put object in frame')
while self.anglesFromObject > 100000: # when object not in frame
interval = interval + 1
if not interval % 2 == 0:
fullRound = 0
max = 0
while self.anglesFromObject > 100000 and (
fullRound < 350 or self.forward_dist < 1.5):
rospy.loginfo(
'trying to put object in frame, self.anglesFromObject:%f, fullRound:%f, self.forward_dist:%f',
self.anglesFromObject, fullRound,
self.forward_dist)
fullRound = fullRound + 30
if direction % 3 == 0:
self.rotate(30)
else:
self.rotate(-30)
rospy.sleep(2.5)
if self.anglesFromObject > max:
max = self.anglesFromObject
if fullRound > 359 and (max -
0.1) < self.anglesFromObject:
break
else:
rospy.loginfo(
'trying to put object in frame, moving straight, self.forward_dist:%f',
self.forward_dist)
direction = direction + 1
self.forward(8)
rospy.sleep(3)
rospy.loginfo('auto move: object in frame, trying to go to it')
cmdRotate = MoveCommand()
cmdRotate.direction = 'counter-clock-wise-rotate'
cmdForward = MoveCommand()
cmdForward.direction = 'forward'
self.pub_bot_commands.publish(cmdRotate)
self.pub_bot_commands.publish(cmdForward)
# go to object
maxTimeToGoToObject = 20
startGoingToObject = rospy.Time.now().to_sec()
while (abs(self.anglesFromObject) > 10 and abs(
self.anglesFromObject) < 500) or self.forward_dist > 0.5:
if abs(self.anglesFromObject) > 10:
cmdRotate.distance = 5
if self.anglesFromObject > 0:
rospy.loginfo(
'rotate clockWise so object will bein the center, shift:%f',
self.anglesFromObject)
cmdRotate.speed = -0.2
else:
cmdRotate.speed = 0.2
rospy.loginfo(
'rotate counterClockWise so object will bein the center, shift:%f',
self.anglesFromObject)
else:
cmdRotate.distance = 0
cmdRotate.speed = 0
if self.forward_dist > 0.5:
rospy.loginfo('moving to object, distance: %f',
self.forward_dist)
cmdForward.distance = 0.2
cmdForward.speed = 0.2
else:
cmdForward.distance = 0
cmdForward.speed = 0
# if bot is stuck (laser problems etc..)
if (rospy.Time.now().to_sec() -
startGoingToObject) > maxTimeToGoToObject:
cmdForward.distance = 5
cmdForward.speed = 1
cmdRotate.distance = 180
cmdRotate.speed = 1
self.pub_bot_commands.publish(cmdForward)
self.pub_bot_commands.publish(cmdRotate)
rospy.sleep(5)
cmdForward.distance = 0
cmdForward.speed = 0
cmdRotate.distance = 0
cmdRotate.speed = 0
break
self.pub_bot_commands.publish(cmdForward)
self.pub_bot_commands.publish(cmdRotate)
rospy.sleep(0.2)
# stop bot
cmdRotate.distance = 0
cmdRotate.speed = 0
cmdForward.distance = 0
cmdForward.speed = 0
self.pub_bot_commands.publish(cmdForward)
self.pub_bot_commands.publish(cmdRotate)
if self.forward_dist < 0.6 and abs(self.anglesFromObject) < 400:
break