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