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 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):
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):
result = GenericExecuteResult()
for i, obj in enumerate(self.perceived_cavity_names):
result.results.append(KeyValue(key="cavity_" + str(i + 1), value=obj))
rospy.loginfo(result)
userdata.result = result
self.perceived_cavity_names = [] # clear perceived objects for next call
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'
