def __move_base_along_x(self, distance_to_move):
movement_speed = np.sign(distance_to_move) * 0.1 # m/s
movement_duration = distance_to_move / movement_speed
move_forward_goal = MoveForwardGoal()
move_forward_goal.movement_duration = movement_duration
move_forward_goal.speed = movement_speed
self.move_forward_client.send_goal(move_forward_goal)
self.move_forward_client.wait_for_result()
self.move_forward_client.get_result()
def __move_base_along_x(self, distance_to_move):
'''Sends a MoveForwardGoal to move the base for the given distance.
The call is non-blocking, i.e. the function returns immediately after
sending the goal without waiting for the action to complete.
'''
movement_speed = np.sign(distance_to_move) * 0.1 # m/s
movement_duration = distance_to_move / movement_speed
move_forward_goal = MoveForwardGoal()
move_forward_goal.movement_duration = movement_duration
move_forward_goal.speed = movement_speed
self.move_forward_client.send_goal(move_forward_goal)
def execute(self, userdata):
goal = MoveForwardGoal()
goal.movement_duration = self.movement_duration
goal.speed = self.speed
self.move_forward_client.send_goal(goal)
timeout_duration = rospy.Duration.from_sec(self.timeout)
self.move_forward_client.wait_for_result(timeout_duration)
result = self.move_forward_client.get_result()
if result and result.success:
return 'succeeded'
else:
return 'failed'
def running(self):
rospy.loginfo('[enter_door] Waiting for door to open')
while not self.door_open:
rospy.sleep(self.waiting_sleep_duration)
rospy.loginfo('[enter_door] Door open; entering')
goal = MoveForwardGoal()
goal.movement_duration = self.movement_duration
goal.speed = self.speed
self.move_forward_client.send_goal(goal)
timeout_duration = rospy.Duration.from_sec(self.timeout)
self.move_forward_client.wait_for_result(timeout_duration)
result = self.move_forward_client.get_result()
if result and result.success:
self.result = self.set_result(True)
self.result = self.set_result(False)
return FTSMTransitions.DONE