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):
self.goal.parameters = []
if self.platform is not None:
self.goal.parameters.append(
KeyValue(key="platform", value=str(self.platform).upper()))
obj = Utils.get_value_of(userdata.goal.parameters, "peg")
if obj is not None:
self.goal.parameters.append(
KeyValue(key="object", value=str(obj).upper()))
# initialise platform in goal if not already initialised
current_platform = Utils.get_value_of(self.goal.parameters, "platform")
if current_platform is None:
platform = Utils.get_value_of(userdata.goal.parameters, "platform")
if platform is None:
rospy.logwarn("Platform not provided. Using default")
platform = "platform_middle"
self.goal.parameters.append(
KeyValue(key="platform", value=str(platform).upper()))
self.client.send_goal(self.goal)
self.client.wait_for_result(rospy.Duration.from_sec(30.0))
state = self.client.get_state()
if state == GoalStatus.SUCCEEDED:
return "success"
else:
return "failed"
def execute(self, userdata):
self.goal.parameters = []
self.goal.parameters.append(
KeyValue(key="arm_safe_position", value="barrier_tape"))
# if location is sent as an argument to this state, set it here
if self.destination_location is not None:
self.goal.parameters.append(
KeyValue(key="destination_location",
value=str(self.destination_location).upper()))
current_destination_location = Utils.get_value_of(
self.goal.parameters, "destination_location")
# if location has not been set read it from userdata (either
# destination_location or location)
if current_destination_location is None:
location = Utils.get_value_of(userdata.goal.parameters,
"destination_location")
if location is None:
location = Utils.get_value_of(userdata.goal.parameters,
"location")
if location is None:
rospy.logerr(
"Location not specified. Not calling move_base")
return 'failed'
self.goal.parameters.append(
KeyValue(key="destination_location",
value=str(location).upper()))
self.client.send_goal(self.goal)
self.client.wait_for_result(rospy.Duration.from_sec(int(15.0)))
state = self.client.get_state()
if state == GoalStatus.SUCCEEDED:
return "success"
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):
target_location = Utils.get_value_of(userdata.goal.parameters,
'location')
if target_location is not None:
target_pose = Utils.get_pose_from_param_server(target_location)
robot_pose = self.get_robot_pose()
xdiff = target_pose.pose.position.x - robot_pose[0]
ydiff = target_pose.pose.position.y - robot_pose[1]
if (xdiff > self.distance_threshold
or ydiff > self.distance_threshold):
return 'no'
else:
return 'yes'
else:
return 'unavailable'
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):
location = Utils.get_value_of(userdata.goal.parameters, "location")
print("[Place Object Server] Location received : ", location)
if (location == "SH01") or (location == "SH02"):
return "shelf"
else:
return "not_shelf"
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):
dont_be_safe_flag = Utils.get_value_of(userdata.goal.parameters,
"dont_be_safe")
if dont_be_safe_flag is not None and dont_be_safe_flag.upper(
) == "TRUE":
return "unsafe"
else:
return "safe"
def execute(self, userdata):
dont_be_safe_flag = Utils.get_value_of(userdata.goal.parameters,
'dont_be_safe')
if dont_be_safe_flag is not None and dont_be_safe_flag.upper(
) == 'TRUE':
return 'unsafe'
else:
return 'safe'
def execute(self, userdata):
# initialise platform in goal if not already initialised
current_platform = Utils.get_value_of(self.goal.parameters, 'platform')
if current_platform is None:
platform = Utils.get_value_of(userdata.goal.parameters, 'platform')
if platform is None:
rospy.logwarn('Platform not provided. Using default')
platform = 'platform_middle'
self.goal.parameters.append(
KeyValue(key='platform', value=str(platform).upper()))
self.client.send_goal(self.goal)
self.client.wait_for_result(rospy.Duration.from_sec(30.0))
state = self.client.get_state()
if state == GoalStatus.SUCCEEDED:
return 'success'
else:
return 'failed'
def execute(self, userdata):
obj = Utils.get_value_of(userdata.goal.parameters, "object")
if obj is None:
rospy.logwarn('Missing parameter "object". Using default.')
return "light"
for heavy_object in userdata.heavy_objects:
if heavy_object.upper() in obj.upper():
return "heavy"
return "light"
def execute(self, userdata):
obj = Utils.get_value_of(userdata.goal.parameters, "object")
if obj is None:
rospy.logwarn('Missing parameter "object". Using default.')
return "large"
for large_object in userdata.large_objects:
if large_object.upper() in obj.upper():
return "large"
return "small"
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):
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"
rospy.loginfo("Sending following goal to perceive location server")
rospy.loginfo(goal)
client.send_goal(goal)
timeout = 25.0
finished_within_time = client.wait_for_result(rospy.Duration.from_sec(int(timeout)))
if not finished_within_time:
client.cancel_goal()
state = client.get_state()
result = client.get_result()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Action SUCCESS")
print("Perceived objects:")
for i in range(20):
obj_key = "obj_" + str(i+1)
obj_key_id = "obj_" + str(i+1) + "_id"
obj_name = Utils.get_value_of(result.results, obj_key)
if obj_name == None:
break
obj_id = Utils.get_value_of(result.results, obj_key_id)
print(obj_name.ljust(15), str(obj_id).ljust(3))
elif state == GoalStatus.ABORTED:
rospy.logerr("Action FAILED")
else:
rospy.logwarn("State: " + str(state))
rospy.loginfo(result)
