def execute(self, userdata):
'''
Sends a dock goal to the specified sub area
'''
self.area_list = []
self.current_area_idx = 0
feedback_msg = ExecuteExperimentFeedback()
feedback_msg.command_name = self.name
feedback_msg.state = ExecuteExperimentFeedback.ONGOING
action_msg = Action()
action_msg.type = 'DOCK'
action_msg.action_id = str(uuid.uuid4())
area_msg = Area()
area_msg.id = self.area_id
area_msg.name = self.area_name
sub_area = SubArea()
sub_area.id = self.sub_area_id
sub_area.name = self.sub_area_name
area_msg.sub_areas.append(sub_area)
action_msg.areas.append(area_msg)
action_msg.sub_areas.append(SubArea())
action_goal = DockGoal()
action_goal.action = action_msg
action_goal.load_type = self.load_type
action_goal.load_id = self.load_id
print('[{0}] Docking in area {1}, sub_area {2} to load {3}'.format(
self.name, self.area_id, self.sub_area_id, self.load_id))
self.action_server.send_goal(action_goal)
self.wait_for_action_result(feedback_msg)
# if the GOTO action could not be completed within the alloted
# time, we send a failure feedback message and stop the experiment
if self.experiment_server.is_preempt_requested():
self.action_server.cancel_all_goals()
self.__report_failure(feedback_msg,
'[{0}] dock preempted'.format(self.name))
elif not self.action_completed or self.action_failed:
self.__report_failure(
feedback_msg,
'[{0}] Docking did not finish in time; giving up'.format(
self.name))
self.cleanup(ExecuteExperimentFeedback.FAILED)
return 'failed'
self.cleanup(ExecuteExperimentFeedback.FINISHED)
return 'done'
def cleanup(self, last_feedback):
'''Does the following cleanup
- send the last feedback message
- send the state status
- unregister sub
:last_feedback: int
:state: int
'''
feedback_msg = ExecuteExperimentFeedback()
feedback_msg.command_name = self.name
feedback_msg.stamp = rospy.Time.now()
feedback_msg.state = last_feedback
self.send_feedback(feedback_msg)
self.undock_progress_sub.unregister()
def execute(self, userdata):
'''Sends a base velocity message to "self.vel_topic".
'''
twist_msg = Twist()
feedback_msg = ExecuteExperimentFeedback()
feedback_msg.command_name = self.name
feedback_msg.state = ExecuteExperimentFeedback.ONGOING
elapsed = 0.
print('[{0}] Moving with vel: x: {1} m/s, y: {2} m/s, theta: {3} rad/s'.format(self.name, self.vel_x,
self.vel_y, self.vel_theta))
start_time = time.time()
while elapsed < self.duration:
twist_msg.linear.x = self.vel_x
twist_msg.linear.y = self.vel_y
twist_msg.angular.z = self.vel_theta
self.vel_pub.publish(twist_msg)
feedback_msg.stamp = rospy.Time.now()
self.send_feedback(feedback_msg)
elapsed = time.time() - start_time
rospy.sleep(0.05)
print('[{0}] Stopping motion'.format(self.name))
twist_msg.linear.x = 0.
twist_msg.linear.y = 0.
twist_msg.angular.z = 0.
self.vel_pub.publish(twist_msg)
feedback_msg.stamp = rospy.Time.now()
feedback_msg.state = ExecuteExperimentFeedback.FINISHED
self.send_feedback(feedback_msg)
return 'done'
def execute(self, userdata):
'''Sends an EXIT_ELEVATOR action and waits for it to finish. Returns "done" if
the action finishes within "self.timeout_s" seconds; returns "failed" otherwise.
'''
feedback_msg = ExecuteExperimentFeedback()
feedback_msg.command_name = self.name
feedback_msg.state = ExecuteExperimentFeedback.ONGOING
action_goal = NavElevatorGoal()
action_goal.action.type = 'EXIT_ELEVATOR'
outside_area_msg = Area()
outside_area_msg.id = str(self.outside_area_id)
outside_area_msg.name = self.outside_area_name
outside_area_msg.type = 'local_area'
action_goal.action.areas.append(outside_area_msg)
print('[{0}] Exiting elevator; outside area ID {1}'.format(
self.name, self.outside_area_id))
self.action_server.send_goal(action_goal)
self.wait_for_action_result(feedback_msg)
# if the EXIT_ELEVATOR action could not be completed within the alloted
# time, we send a failure feedback message and stop the experiment
if self.experiment_server.is_preempt_requested():
self.action_server.cancel_all_goals()
self.__report_failure(
feedback_msg,
'[{0}] Exiting elevator preempted'.format(self.name))
elif not self.action_completed or self.action_failed:
self.__report_failure(
feedback_msg,
'[{0}] Could not exit elevator within alloted time; giving up'.
format(self.name))
self.elevator_progress_sub.unregister()
return 'failed'
feedback_msg.stamp = rospy.Time.now()
feedback_msg.state = ExecuteExperimentFeedback.FINISHED
self.send_feedback(feedback_msg)
self.elevator_progress_sub.unregister()
return 'done'
def execute(self, userdata):
'''Sends a RIDE_ELEVATOR action and waits for it to finish. Returns "done" if
the action finishes within "self.timeout_s" seconds; returns "failed" otherwise.
'''
feedback_msg = ExecuteExperimentFeedback()
feedback_msg.command_name = self.name
feedback_msg.state = ExecuteExperimentFeedback.ONGOING
action_goal = NavElevatorGoal()
action_goal.action.type = 'RIDE_ELEVATOR'
action_goal.action.goal_floor = self.destination_floor
print('[{0}] Riding elevator to floor {1}'.format(
self.name, self.destination_floor))
self.action_server.send_goal(action_goal)
self.wait_for_action_result(feedback_msg)
# if the RIDE_ELEVATOR action could not be completed within the alloted
# time, we send a failure feedback message and stop the experiment
if self.experiment_server.is_preempt_requested():
self.action_server.cancel_all_goals()
self.__report_failure(
feedback_msg,
'[{0}] Elevator ride preempted'.format(self.name))
elif not self.action_completed or self.action_failed:
self.__report_failure(
feedback_msg,
'[{0}] Elevator ride did not finish within the alloted time; giving up'
.format(self.name))
self.elevator_progress_sub.unregister()
return 'failed'
feedback_msg.stamp = rospy.Time.now()
feedback_msg.state = ExecuteExperimentFeedback.FINISHED
self.send_feedback(feedback_msg)
self.elevator_progress_sub.unregister()
return 'done'
def execute(self, userdata):
'''Sends a navigation goal for each area in self.areas.
'''
if self.use_planner:
if not self.source or not self.destination:
return 'failed'
feedback_msg = ExecuteExperimentFeedback()
feedback_msg.command_name = self.name
feedback_msg.state = ExecuteExperimentFeedback.ONGOING
userdata.areas = []
req = GetPathPlanGoal(
start_floor=self.source['floor'],
start_area=self.source['area_name'],
start_sub_area=self.source['subarea_name'],
destination_floor=self.destination['floor'],
destination_area=self.destination['area_name'],
destination_sub_area=self.destination['subarea_name'])
self.path_planner_client.send_goal(req,
done_cb=self.path_planner_cb)
elapsed = 0.
start_time = time.time()
while elapsed < self.timeout_s and not self.action_completed:
feedback_msg.stamp = rospy.Time.now()
self.send_feedback(feedback_msg)
elapsed = time.time() - start_time
rospy.sleep(0.05)
if not self.action_completed: # timeout occurred
feedback_msg.stamp = rospy.Time.now()
feedback_msg.state = ExecuteExperimentFeedback.FINISHED
self.send_feedback(feedback_msg)
return 'failed'
areas = []
for area in self.result.path_plan.areas:
area_dict = {}
area_dict['area_id'] = area.id
area_dict['area_name'] = area.name
area_dict['area_type'] = area.type
area_dict['subarea_id'] = area.sub_areas[
0].id if area.sub_areas else ""
area_dict['subarea_name'] = area.sub_areas[
0].name if area.sub_areas else ""
areas.append(area_dict)
userdata.areas = areas
userdata.area_floor = self.source['floor']
else:
userdata.areas = self.areas
userdata.area_floor = self.area_floor
feedback_msg = ExecuteExperimentFeedback()
feedback_msg.command_name = self.name
feedback_msg.stamp = rospy.Time.now()
feedback_msg.state = ExecuteExperimentFeedback.FINISHED
self.send_feedback(feedback_msg)
return 'done'
def execute(self, userdata):
'''Sends a navigation goal for each area in userdata.areas.
'''
self.area_list = []
feedback_msg = ExecuteExperimentFeedback()
feedback_msg.command_name = self.name
feedback_msg.state = ExecuteExperimentFeedback.ONGOING
if ('areas' not in userdata.keys()) or (len(userdata.areas) == 0) or \
('area_floor' not in userdata.keys()):
self.cleanup(ExecuteExperimentFeedback.FINISHED)
return 'done'
action_goal = GoToGoal()
action_goal.action.type = 'GOTO'
action_goal.action.start_floor = userdata.area_floor
action_goal.action.goal_floor = userdata.area_floor
for area_data in userdata.areas:
area = area_data['area_name'].encode('ascii')
self.area_list.append(area)
area_msg = Area()
area_msg.id = area_data['area_id'].encode('ascii')
area_msg.name = area_data['area_name'].encode('ascii')
area_msg.type = area_data['area_type'].encode('ascii')
area_msg.floor_number = userdata.area_floor
subarea_msg = SubArea()
subarea_msg.id = area_data['subarea_id'].encode('ascii')
subarea_msg.name = area_data['subarea_name'].encode('ascii')
subarea_msg.floor_number = userdata.area_floor
area_msg.sub_areas.append(subarea_msg)
action_goal.action.areas.append(area_msg)
print('[{0}] Going to first area {1}'.format(self.name, self.area_list[0]))
self.action_server.send_goal(action_goal)
self.wait_for_action_result(feedback_msg)
result = self.action_server.get_result()
if result:
if result.success:
self.action_completed = True
else:
self.action_failed = True
else:
self.action_failed = True
# if the GOTO action could not be completed within the alloted
# time, we send a failure feedback message and stop the experiment
if self.experiment_server.is_preempt_requested():
self.action_server.cancel_all_goals()
self.__report_failure(feedback_msg,
'[{0}] GOTO action preempted'.format(self.name))
elif not self.action_completed or self.action_failed:
self.__report_failure(feedback_msg,
'[{0}] Destination could not be reached within the alloted time of {1}s; giving up'.format(self.name, self.timeout_s))
self.cleanup(ExecuteExperimentFeedback.FAILED)
return 'failed'
self.cleanup(ExecuteExperimentFeedback.FINISHED)
return 'done'
def execute(self, userdata):
'''Sends a WAIT_FOR_ELEVATOR action and waits for it to finish;
if the elevator arrives within "self.timeout_s" seconds,
sends an ENTER_ELEVATOR action and waits for it to finish.
Stops the experiment if either of the actions fails.
'''
feedback_msg = ExecuteExperimentFeedback()
feedback_msg.command_name = self.name
feedback_msg.state = ExecuteExperimentFeedback.ONGOING
# we first send a WAIT_FOR_ELEVATOR action to the robot
# so that it can wait for the elevator to arrive
action_goal = NavElevatorGoal()
action_goal.action.type = 'WAIT_FOR_ELEVATOR'
action_goal.action.elevator.elevator_id = self.elevator_id
action_goal.action.elevator.door_id = self.elevator_door_id
action_goal.action.start_floor = self.area_floor
action_goal.action.goal_floor = self.area_floor
print('[{0}] Waiting for elevator {1} at door {2}'.format(self.name,
self.elevator_id,
self.elevator_door_id))
self.action_server.send_goal(action_goal)
self.wait_for_action_result(feedback_msg)
# if the WAIT_FOR_ELEVATOR action could not be completed within the alloted
# time, we send a failure feedback message and stop the experiment
if self.experiment_server.is_preempt_requested():
self.action_server.cancel_all_goals()
self.__report_failure(feedback_msg,
'[{0}] Waiting for elevator preempted'.format(self.name))
elif not self.action_completed or self.action_failed:
self.__report_failure(feedback_msg,
'[{0}] Elevator did not arrive within the alloted time; giving up'.format(self.name))
self.elevator_progress_sub.unregister()
return 'failed'
# if the WAIT_FOR_ELEVATOR action ends in success, we proceed
# by sending an ENTER_ELEVATOR action to the robot
action_goal = NavElevatorGoal()
action_goal.action.type = 'ENTER_ELEVATOR'
action_goal.action.start_floor = self.area_floor
action_goal.action.goal_floor = self.area_floor
print('[{0}] Entering elevator {1} at door {2}'.format(self.name,
self.elevator_id,
self.elevator_door_id))
self.action_server.send_goal(action_goal)
self.wait_for_action_result(feedback_msg)
# if the ENTER_ELEVATOR action could not be completed successfully,
# we send a failure feedback message and stop the experiment
if self.experiment_server.is_preempt_requested():
self.action_server.cancel_all_goals()
self.__report_failure(feedback_msg,
'[{0}] Entering elevator preempted'.format(self.name))
elif not self.action_completed or self.action_failed:
self.__report_failure(feedback_msg,
'[{0}] Could not enter elevator; giving up'.format(self.name))
self.elevator_progress_sub.unregister()
return 'failed'
feedback_msg.stamp = rospy.Time.now()
feedback_msg.state = ExecuteExperimentFeedback.FINISHED
self.send_feedback(feedback_msg)
self.elevator_progress_sub.unregister()
return 'done'