def execute(self, userdata): if self.preempt_requested(): rospy.logwarn("preemption requested!!!") self.recall_preempt() # reset preemption flag for next request return "preempted" base_goal = Utils.get_value_of(userdata.goal.parameters, "destination_location") base_orientation = Utils.get_value_of(userdata.goal.parameters, "destination_orientation") rospy.loginfo("Destination: " + str(base_goal)) pose = None pose = param_server_utils.get_pose_from_param_server(base_goal) if base_orientation is not None: pose.pose.orientation = param_server_utils.get_orientation_from_param_server( base_orientation) # Add empty result msg (because if none of the state do it, action server gives error) userdata.result = GenericExecuteResult() # giving feedback to the user feedback = GenericExecuteFeedback() feedback.current_state = "MOVE_BASE" feedback.text = "[move_base_safe] moving the base to " + base_goal userdata.feedback = feedback if pose: self.pub.publish(pose) self.dbc_pose_pub.publish(pose) return "succeeded" else: return "failed"
def start_cb(*args, **kwargs): userdata = args[0] sm_state = args[1][0] feedback = GenericExecuteFeedback() feedback.current_state = sm_state userdata.feedback = feedback
def transition_cb(*args, **kwargs): userdata = args[0] sm_state = args[1][0] publisher = args[2] publisher.publish(sm_state) feedback = GenericExecuteFeedback() feedback.current_state = sm_state userdata.feedback = feedback
def execute(self, userdata): if Utils.get_value_of(userdata.goal.parameters, "next_action") == "PERCEIVE": arm_goal = "look_at_workspace_from_far" elif Utils.get_value_of(userdata.goal.parameters, "next_action") == "UNSTAGE": arm_goal = "platform_middle_pre" else: return "skipped" # giving feedback to the user feedback = GenericExecuteFeedback() feedback.current_state = "MOVE_ARM" feedback.text = "[move_base_safe] Moving the arm to " + arm_goal userdata.feedback = feedback userdata.move_arm_to = arm_goal return "succeeded"
def execute(self, userdata): # Add empty result msg (because if none of the state do it, action server gives error) userdata.result = GenericExecuteResult() userdata.feedback = GenericExecuteFeedback( current_state="GetPoseToPlaceOject", text="Getting pose to place obj", ) location = Utils.get_value_of(userdata.goal.parameters, "location") if location is None: rospy.logwarn('"location" not provided. Using default.') return "failed" self.place_pose = None self.status = None self.platform_name_pub.publish(String(data=location)) # wait for messages to arrive start_time = rospy.Time.now() rate = rospy.Rate(10) # 10hz while not (rospy.is_shutdown()): if rospy.Time.now() - start_time > self.timeout: break if self.place_pose is not None and self.status is not None: break rate.sleep() if (self.place_pose is not None and self.status is not None and self.status == "e_success"): userdata.move_arm_to = self.place_pose return "succeeded" else: return "failed"
def execute(self, userdata): if Utils.get_value_of(userdata.goal.parameters, 'next_action') == 'PERCEIVE': arm_goal = 'look_at_workspace_from_far' elif Utils.get_value_of(userdata.goal.parameters, 'next_action') == 'UNSTAGE': arm_goal = "platform_middle_pre" else: return 'skipped' # giving feedback to the user feedback = GenericExecuteFeedback() feedback.current_state = 'MOVE_ARM' feedback.text = '[move_base_safe] Moving the arm to ' + arm_goal userdata.feedback = feedback userdata.move_arm_to = arm_goal return 'succeeded'
def execute(self, userdata): userdata.arm_pose_index = 0 # reset arm position for new request # Add empty result msg (because if none of the state do it, action server gives error) userdata.result = GenericExecuteResult() userdata.feedback = GenericExecuteFeedback(current_state='Setup', text='Setting up') return 'succeeded'
def execute(self, userdata): userdata.handle_pose = None # reset handle pose # Add empty result msg (because if none of the state do it, action server gives error) userdata.result = GenericExecuteResult() userdata.feedback = GenericExecuteFeedback(current_state="Setup", text="Setting up") return "succeeded"
def execute(self, userdata): # Add empty result msg (because if none of the state do it, action server gives error) userdata.result = GenericExecuteResult() userdata.feedback = GenericExecuteFeedback(current_state='SelectObject', text='selecting object') obj = Utils.get_value_of(userdata.goal.parameters, 'peg') self.publisher.publish(String(data=obj)) rospy.sleep(0.2) # let the topic survive for some time return 'succeeded'
def execute(self, userdata): result = GenericExecuteResult() for i, obj in enumerate(self.perceived_obj_names): result.results.append(KeyValue(key='obj_' + str(i + 1), value=obj)) userdata.result = result userdata.feedback = GenericExecuteFeedback() #place holder self.perceived_obj_names = [] # clear perceived objects for next call return 'succeeded'
def execute(self, userdata): platform = Utils.get_value_of(userdata.goal.parameters, "platform") if platform is None: rospy.logwarn('Missing parameter "platform". Using default.') platform = "PLATFORM_MIDDLE" platform = platform.lower() if self.arm_target == "pre": platform = platform + "_pre" userdata.move_arm_to = platform # Add empty result msg (because if none of the state do it, action server gives error) userdata.result = GenericExecuteResult() userdata.feedback = GenericExecuteFeedback( current_state="SetupMoveArm", text="Moving arm to " + platform) return "succeeded"
def execute(self, userdata): userdata.arm_pose_index = 0 # reset arm position for new request # Add empty result msg (because if none of the state do it, action server gives error) userdata.result = GenericExecuteResult() userdata.feedback = GenericExecuteFeedback(current_state="Setup", text="Setting up") # This server runs in two modes: # 1. three_view mode - perceive using motion of arm in three directions <straight, left, right> # 2. single_view mode - only perceiving in one direction <straight> perception_mode = Utils.get_value_of(userdata.goal.parameters, "perception_mode") if perception_mode is not None and perception_mode == "single_view": userdata.arm_pose_list = ["look_at_workspace_from_near"] else: userdata.arm_pose_list = [ "look_at_workspace_from_near", "look_at_workspace_from_near_left", "look_at_workspace_from_near_right", ] return "succeeded"
def execute(self, userdata): # Add empty result msg (because if none of the state do it, action server gives error) userdata.result = GenericExecuteResult() userdata.feedback = GenericExecuteFeedback(current_state='Setup', text='Setting up') return 'succeeded'