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 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 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 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 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'