class BrickGraspingActionState(EventState):
""" brick grasping results """
RESULT_SUCCESS = 0
RESULT_STOPPED_BY_COMMAND = 1
RESULT_OBJECT_NOT_RELIABLE = 2
RESULT_OBJECT_NOT_VISIBLE = 3
RESULT_GRASPING_TIMOUT = 4
RESULT_GRASPING_FAILED = 5
""" brick grasping object types """
OBJECT_RED = 1
OBJECT_GREEN = 2
OBJECT_BLUE = 3
OBJECT_ORANGE = 4
WALL = 5
""" result ids """
RESULT_SUCCESS = 0
RESULT_STOPPED_BY_COMMAND = 1
RESULT_OBJECT_NOT_RELIABLE = 2
RESULT_OBJECT_NOT_VISIBLE = 3
RESULT_GRASPING_TIMOUT = 4
RESULT_GRASPING_FAILED = 5
def __init__(self, return_altitude):
# See example_state.py for basic explanations.
super(BrickGraspingActionState,
self).__init__(outcomes=['grasped', 'grasping_error'],
input_keys=['brick_type'])
self._topic = 'brick_grasping'
self._client = ProxyActionClient(
{self._topic:
GraspingAction}) # pass required clients as dict (topic: type)
self._return_altitude = return_altitude
Logger.loginfo('[GraspingAction]: Client initialized')
# It may happen that the action client fails to send the action goal.
self._error = False
def execute(self, userdata):
# While this state is active, check if the action has been finished and evaluate the result.
# Check if the client failed to send the goal.
if self._error:
return 'grasping_error'
# Check if the action has been finished
if self._client.has_result(self._topic):
result = self._client.get_result(self._topic)
Logger.loginfo("[GraspingAction]: ended %s" % str(result.message))
# In this example, we also provide the amount of cleaned dishes as output key.
if result.success:
return 'grasped'
else:
return 'grasping_error'
"""feedback params are"""
feedback = self._client.get_feedback(self._topic)
if feedback is not None:
rospy.loginfo_throttle(
5, "[GraspingAction]: Current feedback msg: %s" %
str(feedback.message))
def on_enter(self, userdata):
# When entering this state, we send the action goal once to let the robot start its work.
brick_type = userdata.brick_type
# Create the goal.
goal = GraspingGoal()
#fill the goal
Logger.loginfo("[GraspingAction]: want to grasp brick type %s" %
(str(brick_type)))
goal.goal = brick_type
goal.return_altitude = self._return_altitude #where to return after grasping
# Send the goal.
self._error = False # make sure to reset the error state since a previous state execution might have failed
try:
Logger.loginfo("[GraspingAction]: sending goal %s to topic %s" %
(str(goal), str(self._topic)))
self._client.send_goal(self._topic, goal)
except Exception as e:
# Since a state failure not necessarily causes a behavior failure, it is recommended to only print warnings, not errors.
# Using a linebreak before appending the error log enables the operator to collapse details in the GUI.
Logger.logwarn(
'[GraspingAction]: Failed to send the GraspingAction command:\n%s'
% str(e))
self._error = True
def on_exit(self, userdata):
# Make sure that the action is not running when leaving this state.
# A situation where the action would still be active is for example when the operator manually triggers an outcome.
if not self._client.has_result(self._topic):
self._client.cancel(self._topic)
Logger.loginfo('[GraspingAction]: Cancelled active action goal.')
class WallFollowingActionState(EventState):
'''
State for calling wall_following action server.
'''
def __init__(self, action_server_name, desired_yaw, desired_speed, desired_wall_distance):
# See example_state.py for basic explanations.
super(WallFollowingActionState, self).__init__( outcomes = ['successed', 'aborted', 'preempted'])
self._topic = action_server_name
self._desired_yaw = desired_yaw
self._desired_speed = desired_speed
self._desired_wall_distance = desired_wall_distance
self._client = ProxyActionClient({self._topic: FollowingAction}) # pass required clients as dict (topic: type)
Logger.loginfo('[WallFollowing]: Action client initialized')
# It may happen that the action client fails to send the action goal.
self._error = False
def execute(self, userdata):
# While this state is active, check if the action has been finished and evaluate the result.
# Check if the client failed to send the goal.
if self._error:
return 'aborted'
# Check if the action has been finished
if self._client.has_result(self._topic):
status = self._client.get_state(self._topic)
result = self._client.get_result(self._topic)
if status == GoalStatus.SUCCEEDED:
Logger.loginfo("[WallFollowing]: %s" % str(result.message))
return 'successed'
elif status == GoalStatus.PREEMPTED:
Logger.logwarn('[WallFollowing]: Action preempted')
return 'preempted'
elif status in [GoalStatus.REJECTED, GoalStatus.RECALLED, GoalStatus.ABORTED]:
Logger.logerr('[WallFollowing]: Action failed: %s' % str(result.message))
return 'aborted'
"""feedback params are"""
feedback = self._client.get_feedback(self._topic)
if feedback is not None:
Logger.loginfo("[WallFollowing]: Current feedback msg: %s" % feedback.message)
def on_enter(self, userdata):
# When entering this state, we send the action goal once to let the robot start its work.
# Create the goal.
goal = FollowingGoal()
#fill the goal
goal.yaw = self._desired_yaw
goal.wall_distance = self._desired_wall_distance
goal.speed = self._desired_speed
# Send the goal.
self._error = False # make sure to reset the error state since a previous state execution might have failed
try:
self._client.send_goal(self._topic, goal)
except Exception as e:
# Since a state failure not necessarily causes a behavior failure, it is recommended to only print warnings, not errors.
# Using a linebreak before appending the error log enables the operator to collapse details in the GUI.
Logger.logerr('[WallFollowing]: Failed to send the goal:\n%s' % str(e))
self._error = True
def on_exit(self, userdata):
# Make sure that the action is not running when leaving this state.
# A situation where the action would still be active is for example when the operator manually triggers an outcome.
if not self._client.has_result(self._topic):
self._client.cancel(self._topic)
Logger.loginfo('[WallFollowing]: Cancelled active action goal.')
class SaraFollow(EventState):
"""
Make Sara follow an entity using move_base.
-- Distance float distance to keep between sara and the entity
-- ReplanPeriod float Time in seconds between each replanning for movebase
># ID int entity ID.
"""
def __init__(self, distance=1, ReplanPeriod=0.5):
"""Constructor"""
super(SaraFollow, self).__init__(outcomes=['failed'],
input_keys=['ID'])
# Prepare the action client for move_base
self._action_topic = "/move_base"
self._client = ProxyActionClient({self._action_topic: MoveBaseAction})
# Initialise the subscribers
self.entities_topic = "/entities"
self._pose_topic = "/robot_pose"
self._sub = ProxySubscriberCached({
self._pose_topic: Pose,
self.entities_topic: Entities
})
# Get the parameters
self.targetDistance = distance
self.ReplanPeriod = ReplanPeriod
# Initialise the status variables
self.MyPose = None
self.lastPlanTime = 0
def execute(self, userdata):
"""Wait for action result and return outcome accordingly"""
# Initialise the Entity
Entity = None
# Get my last position
if self._sub.has_msg(self._pose_topic):
self.MyPose = self._sub.get_last_msg(self._pose_topic)
# Get the entity's position
if self._sub.has_msg(self.entities_topic):
message = self._sub.get_last_msg(self.entities_topic)
for entity in message.entities:
if entity.ID == userdata.ID:
Entity = entity
# If both my pose and the entyties are known, we start following.
if self.MyPose and Entity:
# Calculate a new goal if the desired period has passed.
if self.lastPlanTime + self.ReplanPeriod < rospy.get_time():
self.lastPlanTime = rospy.get_time()
self.setGoal(
self.generateGoal(self.MyPose.position, Entity.position))
# Check if movebase can reach the goal
if self._client.has_result(self._action_topic):
status = self._client.get_state(self._action_topic)
if status in [
GoalStatus.PREEMPTED, GoalStatus.REJECTED,
GoalStatus.RECALLED, GoalStatus.ABORTED
]:
Logger.logwarn('Navigation failed: %s' % str(status))
return 'failed'
def on_enter(self, userdata):
"""Clean the costmap"""
rospy.wait_for_service('/move_base/clear_costmaps')
serv = rospy.ServiceProxy('/move_base/clear_costmaps', Empty)
serv()
self.MyPose = None
self.Entity = None
self.Counter = 0
def on_exit(self, userdata):
if not self._client.has_result(self._action_topic):
self._client.cancel(self._action_topic)
Logger.loginfo('Cancelled active action goal.')
def on_pause(self):
pose = self._client.get_feedback(self._action_topic)
self.setGoal(pose)
def generateGoal(self, MyPosition, EntityPosition):
# Calculate the safe distance to reach
GoalPose = Pose()
# Calculate the diference in position
dx = EntityPosition.x - MyPosition.x
dy = EntityPosition.y - MyPosition.y
# Calculate the desired distance to the person.
# If the person is too close, the robot must not go backward.
distanceToPerson = max(self.targetDistance, (dx**2 + dy**2)**0.5)
# Calculate the desired position to reach.
GoalPose.position.x = EntityPosition.x - dx / distanceToPerson * self.targetDistance
GoalPose.position.y = EntityPosition.y - dy / distanceToPerson * self.targetDistance
# Calculate the desired direction.
angle = math.atan2(dy, dx)
qt = quaternion_from_euler(0, 0, angle)
GoalPose.orientation.w = qt[3]
GoalPose.orientation.x = qt[0]
GoalPose.orientation.y = qt[1]
GoalPose.orientation.z = qt[2]
return GoalPose
def setGoal(self, pose):
# Generate the goal
goal = MoveBaseGoal()
goal.target_pose.pose = pose
goal.target_pose.header.frame_id = "map"
# Send the action goal for execution
try:
Logger.loginfo("sending goal" + str(goal))
self._client.send_goal(self._action_topic, goal)
except Exception as e:
Logger.logwarn("Unable to send navigation action goal:\n%s" %
str(e))
self._failed = True
class FireExtinguishActionState(EventState):
'''
State for calling fire_detect action server.
'''
def result_to_outcome(self, argument):
switcher = {
0: "successed",
1: "lost_target",
}
return switcher.get(argument, "error")
def __init__(self, action_server_name):
# See example_state.py for basic explanations.
super(FireExtinguishActionState, self).__init__(
outcomes=['successed', 'lost_target', 'error', 'preempted'],
input_keys=['goal'],
output_keys=['is_tank_depleted'])
self._topic = action_server_name
self._client = ProxyActionClient(
{self._topic:
firemanAction}) # pass required clients as dict (topic: type)
Logger.loginfo('[FireExtinguish]: Action client initialized')
# It may happen that the action client fails to send the action goal.
self._error = False
def execute(self, userdata):
# While this state is active, check if the action has been finished and evaluate the result.
# Check if the client failed to send the goal.
if self._error:
userdata.is_tank_depleted = False
return 'error'
# Check if the action has been finished
if self._client.has_result(self._topic):
status = self._client.get_state(self._topic)
result = self._client.get_result(self._topic)
Logger.loginfo("[FireExtinguish]: message %s" %
str(result.message))
if status == GoalStatus.SUCCEEDED:
Logger.loginfo("[FireExtinguish]: %s" % str(result.message))
userdata.is_tank_depleted = True
return self.result_to_outcome(result.result_id)
elif status == GoalStatus.PREEMPTED:
userdata.is_tank_depleted = False
Logger.logwarn('[FireExtinguish]: Action preempted')
return 'preempted'
elif status in [
GoalStatus.REJECTED, GoalStatus.RECALLED,
GoalStatus.ABORTED
]:
Logger.logerr('[FireExtinguish]: Action failed: %s' %
str(result.message))
userdata.is_tank_depleted = False
return self.result_to_outcome(result.result_id)
"""feedback params are"""
feedback = self._client.get_feedback(self._topic)
if feedback is not None:
Logger.loginfo(
"[FireExtinguish]: Feedback - Water reservoir %3d %%" %
feedback.water_percent)
def on_enter(self, userdata):
# When entering this state, we send the action goal once to let the robot start its work.
# Create the goal.
goal = firemanGoal()
goal.where_x = userdata.goal.x
goal.where_y = userdata.goal.y
goal.where_z = userdata.goal.z
goal.extinguish = True
Logger.loginfo('[FireExtinguish]: Action client received new goal:')
Logger.loginfo(goal)
# Send the goal.
self._error = False # make sure to reset the error state since a previous state execution might have failed
try:
self._client.send_goal(self._topic, goal)
except Exception as e:
# Since a state failure not necessarily causes a behavior failure, it is recommended to only print warnings, not errors.
# Using a linebreak before appending the error log enables the operator to collapse details in the GUI.
Logger.logerr('[FireExtinguish]: Failed to send the goal:\n%s' %
str(e))
self._error = True
def on_exit(self, userdata):
# Make sure that the action is not running when leaving this state.
# A situation where the action would still be active is for example when the operator manually triggers an outcome.
if not self._client.has_result(self._topic):
self._client.cancel(self._topic)
Logger.loginfo('[FireExtinguish]: Cancelled active action goal.')
class TurnInPlaceUsinActonServerState(EventState):
'''
Turns the robot in place to a set degree. Uses vision_follower action server.
Start the server with rosrun vision_follower spin_server.py before use.
-- turn_angle float The angle that the robot should make
-- t_speed float Speed at which to turn the robot
<= done If turn completed successfully
<= failed If in some reason the turn fails.
'''
def __init__(self, turn_angle, t_speed):
super(TurnInPlaceUsinActonServerState, self).__init__(outcomes=['done', 'failed'])
self._turn_angle = turn_angle
self._t_speed = t_speed
self._topic = '/spin_server_X'
#create the action client passing it the spin_topic_name and msg_type
self._client = ProxyActionClient({self._topic: SpinAction}) # pass required clients as dict (topic: type)
# It may happen that the action client fails to send the action goal.
self._error = False
def execute(self, userdata):
# While this state is active, check if the action has been finished and evaluate the result.
# Check if the client failed to send the goal.
if self._error:
return 'failed'
# Check if the action has been finished
if self._client.has_result(self._topic):
result = self._client.get_result(self._topic)
result_val = result.done
# Based on the result, decide which outcome to trigger.
if result_val == "done":
#set the output value to the input value
# userdata.data_OUT = userdata.data_IN
return 'done'
else:
return 'failed'
# Check if there is any feedback
if self._client.has_feedback(self._topic):
feedback = self._client.get_feedback(self._topic)
Logger.loginfo("Current heading is: %s" % feedback.heading)
# If the action has not yet finished, no outcome will be returned and the state stays active.
def on_enter(self, userdata):
# When entering this state, we send the action goal once to let the robot start its work.
# Create the goal.
goal = SpinGoal()
goal.angle = self._turn_angle
goal.turn_speed = self._t_speed
# Send the goal.
self._error = False # make sure to reset the error state since a previous state execution might have failed
try:
self._client.send_goal(self._topic, goal)
except Exception as e:
# Since a state failure not necessarily causes a behavior failure, it is recommended to only print warnings, not errors.
# Using a linebreak before appending the error log enables the operator to collapse details in the GUI.
Logger.logwarn('Failed to send the Spin command:\n%s' % str(e))
self._error = True
def on_exit(self, userdata):
# Make sure that the action is not running when leaving this state.
# A situation where the action would still be active is for example when the operator manually triggers an outcome.
if not self._client.has_result(self._topic):
self._client.cancel(self._topic)
Logger.loginfo('Cancelled active spin action goal.')
def on_start(self):
Logger.loginfo("Robot Turn state: READY!")
def on_stop(self):
Logger.loginfo("Robot Turn: Disengaged!")
#same implementation as on exit
if not self._client.has_result(self._topic):
self._client.cancel(self._topic)
Logger.loginfo('Cancelled active spin action goal.')
class SaraMoveBase(EventState):
"""
Navigates Sara to a global position and orientation using move_base.
># pose Pose2D Target waypoint for navigation.
<= arrived Navigation to target pose succeeded.
<= failed Navigation to target pose failed.
"""
def __init__(self, reference="map"):
"""Constructor"""
super(SaraMoveBase, self).__init__(outcomes=['arrived', 'failed'],
input_keys=['pose'])
self._action_topic = "/move_base"
self._client = ProxyActionClient({self._action_topic: MoveBaseAction})
if self._client.is_available(self._action_topic):
self._arrived = False
self._failed = False
self._pose = None
self.reference = reference
self.simulation = False
else:
self.simulation = True
def execute(self, userdata):
"""Wait for action result and return outcome accordingly"""
if not self.simulation:
if self._arrived:
return 'arrived'
if self._failed:
return 'failed'
if self._client.has_result(self._action_topic):
status = self._client.get_state(self._action_topic)
if status == GoalStatus.SUCCEEDED:
self._arrived = True
return 'arrived'
elif status in [
GoalStatus.PREEMPTED, GoalStatus.REJECTED,
GoalStatus.RECALLED, GoalStatus.ABORTED
]:
Logger.logwarn('Navigation failed: %s' % str(status))
self._failed = True
return 'failed'
else:
Logger.logwarn("Movebase is running in simulation mode!")
Logger.logwarn("destination pose:\n" + str(userdata.pose))
return "arrived"
def on_enter(self, userdata):
"""Create and send action goal"""
if not self.simulation:
self._arrived = False
self._failed = False
# Create and populate action goal
if type(userdata.pose) == Pose:
self._pose = userdata.pose
elif type(userdata.pose) == Pose2D:
pt = Point(x=userdata.pose.x, y=userdata.pose.y)
qt = transformations.quaternion_from_euler(
0, 0, userdata.pose.theta)
self._pose = Pose(position=pt, orientation=Quaternion(*qt))
self.setGoal(self._pose)
else:
Logger.logwarn(
"Movebase not found! Will run in simulation mode...")
def on_exit(self, userdata):
if not self.simulation:
if not self._client.has_result(self._action_topic):
self._client.cancel(self._action_topic)
Logger.loginfo('Cancelled active action goal.')
def on_pause(self):
if not self.simulation:
pose = self._client.get_feedback(self._action_topic)
self.setGoal(pose)
def on_resume(self, userdata):
if not self.simulation:
self.setGoal(self._pose)
def setGoal(self, pose):
if not self.simulation:
rospy.wait_for_service('/move_base/clear_costmaps')
serv = rospy.ServiceProxy('/move_base/clear_costmaps', Empty)
serv()
goal = MoveBaseGoal()
goal.target_pose.pose = pose
goal.target_pose.header.frame_id = self.reference
# goal.target_pose.header.stamp.secs = 5.0
# Send the action goal for execution
try:
self._client.send_goal(self._action_topic, goal)
except Exception as e:
Logger.logwarn("Unable to send navigation action goal:\n%s" %
str(e))
self._failed = True
class WindowFlyerActionState(EventState):
'''
State for calling window_flyer action server.
'''
def result_to_outcome(self, argument):
switcher = {
0: "successed",
1: "server_not_initialized",
2: "window_not_found",
3: "already_flythrough",
4: "timeout",
}
return switcher.get(argument, "error")
def __init__(self, action_server_name, just_approach, yaw_offset=0):
# See example_state.py for basic explanations.
super(WindowFlyerActionState,
self).__init__(outcomes=[
'successed', 'window_not_found', 'already_flythrough',
'server_not_initialized', 'error', 'timeout', 'preempted'
],
input_keys=['window_id'],
output_keys=['window_position'])
self._topic = action_server_name
self._yaw_offset = yaw_offset
self._just_approach = just_approach
self._client = ProxyActionClient({
self._topic: FlythroughWindowAction
}) # pass required clients as dict (topic: type)
Logger.loginfo('[WindowFlyerAction]: Action client initialized')
# It may happen that the action client fails to send the action goal.
self._error = False
def execute(self, userdata):
# While this state is active, check if the action has been finished and evaluate the result.
# Check if the client failed to send the goal.
if self._error:
return 'error'
# Check if the action has been finished
if self._client.has_result(self._topic):
status = self._client.get_state(self._topic)
result = self._client.get_result(self._topic)
if status == GoalStatus.SUCCEEDED:
Logger.loginfo("[WindowFlyerAction]: %s" % str(result.message))
Logger.loginfo(
"[WindowFlyerAction]: Finished on position: %s" %
str(result.final_position))
reference_srv = mrs_msgs.srv.ReferenceStampedSrvRequest()
reference_srv.header = result.final_position.header
reference_srv.reference = result.final_position.reference
reference_srv.reference.yaw = reference_srv.reference.yaw + self._yaw_offset
userdata.window_position = reference_srv
return 'successed'
elif status == GoalStatus.PREEMPTED:
Logger.logwarn('[WindowFlyerAction]: Action preempted')
return 'preempted'
elif status in [
GoalStatus.REJECTED, GoalStatus.RECALLED,
GoalStatus.ABORTED
]:
Logger.logerr('[WindowFlyerAction]: Action failed: %s' %
str(result.message))
return self.result_to_outcome(result.result_id)
"""feedback params are"""
feedback = self._client.get_feedback(self._topic)
if feedback is not None:
Logger.loginfo("[WindowFlyerAction]: Feedback msg: %s" %
feedback.message)
def on_enter(self, userdata):
# When entering this state, we send the action goal once to let the robot start its work.
# Create the goal.
goal = FlythroughWindowGoal()
#fill the goal
goal.window_id = userdata.window_id
goal.fly_inside_confirmation = not self._just_approach
# Send the goal.
self._error = False # make sure to reset the error state since a previous state execution might have failed
try:
self._client.send_goal(self._topic, goal)
except Exception as e:
# Since a state failure not necessarily causes a behavior failure, it is recommended to only print warnings, not errors.
# Using a linebreak before appending the error log enables the operator to collapse details in the GUI.
Logger.logerr('[WindowFlyerAction]: Failed to send the goal:\n%s' %
str(e))
self._error = True
def on_exit(self, userdata):
# Make sure that the action is not running when leaving this state.
# A situation where the action would still be active is for example when the operator manually triggers an outcome.
if not self._client.has_result(self._topic):
self._client.cancel(self._topic)
Logger.loginfo(
'[WindowFlyerAction]: Cancelled active action goal.')
class FlyToActionState(EventState):
'''
State for calling mbzirc_flyto_planner action server.
'''
def __init__(self, action_server_name):
# See example_state.py for basic explanations.
super(FlyToActionState,
self).__init__(outcomes=['reached', 'aborted', 'preempted'],
input_keys=['goal', 'flying_velocity'])
self._topic = action_server_name
self._client = ProxyActionClient(
{self._topic:
FlyToAction}) # pass required clients as dict (topic: type)
Logger.loginfo('[FlyToAction]: Action client initialized')
# It may happen that the action client fails to send the action goal.
self._error = False
def execute(self, userdata):
# While this state is active, check if the action has been finished and evaluate the result.
# Check if the client failed to send the goal.
if self._error:
return 'aborted'
# Check if the action has been finished
if self._client.has_result(self._topic):
status = self._client.get_state(self._topic)
result = self._client.get_result(self._topic)
#print("result type",type(result))
#print("status type",type(status))
if status == GoalStatus.SUCCEEDED:
Logger.loginfo("[FlyToAction]: %s" % str(result.message))
Logger.loginfo(
"[FlyToAction]: on exit goal reached x,y,z,yaw is %s,%s,%s,%s"
% (str(userdata.goal.x), str(userdata.goal.y),
str(userdata.goal.z), str(userdata.goal.yaw)))
return 'reached'
elif status == GoalStatus.PREEMPTED:
Logger.logwarn('[FlyToAction]: Action preempted')
return 'preempted'
elif status in [
GoalStatus.REJECTED, GoalStatus.RECALLED,
GoalStatus.ABORTED
]:
Logger.logerr('[FlyToAction]: Action failed: %s' %
str(result.message))
return 'aborted'
"""feedback params are"""
feedback = self._client.get_feedback(self._topic)
if feedback is not None:
Logger.loginfo(
"[FlyToAction]: Current feedback msg: distance_to_goal: %f" %
feedback.distance_to_goal)
def on_enter(self, userdata):
# When entering this state, we send the action goal once to let the robot start its work.
# Create the goal.
goal = FlyToGoal()
Logger.loginfo(
"[FlyToAction]: on enter goal x,y,z,yaw is %s,%s,%s,%s" %
(str(userdata.goal.x), str(userdata.goal.y), str(
userdata.goal.z), str(userdata.goal.yaw)))
#fill the goal
goal.points.append(userdata.goal)
goal.velocity = userdata.flying_velocity
# Send the goal.
self._error = False # make sure to reset the error state since a previous state execution might have failed
try:
self._client.send_goal(self._topic, goal)
except Exception as e:
# Since a state failure not necessarily causes a behavior failure, it is recommended to only print warnings, not errors.
# Using a linebreak before appending the error log enables the operator to collapse details in the GUI.
Logger.logwarn(
'[FlyToAction]: Failed to send the FlyToAction goal:\n%s' %
str(e))
self._error = True
def on_exit(self, userdata):
# Make sure that the action is not running when leaving this state.
# A situation where the action would still be active is for example when the operator manually triggers an outcome.
if not self._client.has_result(self._topic):
self._client.cancel(self._topic)
Logger.loginfo('[FlyToAction]: Cancelled active action goal.')
class LidarFlierActionState(EventState):
'''
State for calling lidar_flier action server.
'''
def result_to_outcome(self, argument):
switcher = {
0: "successed", # 0 - orbit completed
1: "successed", # 1 - goto completed
2: "server_not_initialized", # 2 - not initialized
3:
"no_valid_points_in_scan", # 3 - no valid points in laser scan (too far from building)
4: "stop_requested", # 4 - stop requested
}
return switcher.get(argument, "error")
def __init__(self, action_server_name):
# See example_state.py for basic explanations.
super(LidarFlierActionState, self).__init__(outcomes=[
'successed', 'no_valid_points_in_scan', 'stop_requested',
'server_not_initialized', 'error', 'preempted'
],
input_keys=['goal'])
self._topic = action_server_name
self._client = ProxyActionClient(
{self._topic:
lfAction}) # pass required clients as dict (topic: type)
Logger.loginfo('[LidarFlierAction]: Action client initialized')
# It may happen that the action client fails to send the action goal.
self._error = False
def execute(self, userdata):
# While this state is active, check if the action has been finished and evaluate the result.
# Check if the client failed to send the goal.
if self._error:
return 'error'
# Check if the action has been finished
if self._client.has_result(self._topic):
status = self._client.get_state(self._topic)
result = self._client.get_result(self._topic)
Logger.loginfo("[LidarFlierAction]: message %s" %
str(result.message))
if status == GoalStatus.SUCCEEDED:
Logger.loginfo("[LidarFlierAction]: %s" % str(result.message))
return self.result_to_outcome(result.result_id)
elif status == GoalStatus.PREEMPTED:
Logger.logwarn('[LidarFlierAction]: Action preempted')
return 'preempted'
elif status in [
GoalStatus.REJECTED, GoalStatus.RECALLED,
GoalStatus.ABORTED
]:
Logger.logerr('[LidarFlierAction]: Action failed: %s' %
str(result.message))
return self.result_to_outcome(result.result_id)
"""feedback params are"""
feedback = self._client.get_feedback(self._topic)
if feedback is not None:
Logger.loginfo("[LidarFlierAction]: Feedback msg: %3d" %
feedback.progress)
def on_enter(self, userdata):
# When entering this state, we send the action goal once to let the robot start its work.
# Create the goal.
goal = copy.deepcopy(userdata.goal)
Logger.loginfo('[LidarFlierAction]: Action client received new goal:')
Logger.loginfo(goal)
# Send the goal.
self._error = False # make sure to reset the error state since a previous state execution might have failed
try:
self._client.send_goal(self._topic, goal)
except Exception as e:
# Since a state failure not necessarily causes a behavior failure, it is recommended to only print warnings, not errors.
# Using a linebreak before appending the error log enables the operator to collapse details in the GUI.
Logger.logerr('[LidarFlierAction]: Failed to send the goal:\n%s' %
str(e))
self._error = True
def on_exit(self, userdata):
# Make sure that the action is not running when leaving this state.
# A situation where the action would still be active is for example when the operator manually triggers an outcome.
if not self._client.has_result(self._topic):
self._client.cancel(self._topic)
Logger.loginfo('[LidarFlierAction]: Cancelled active action goal.')
class BrickPlacingActionState(EventState):
""" brick grasping results """
RESULT_SUCCESS = 0
RESULT_STOPPED_BY_COMMAND = 1
RESULT_OBJECT_NOT_RELIABLE = 2
RESULT_OBJECT_NOT_VISIBLE = 3
RESULT_GRASPING_TIMOUT = 4
RESULT_GRASPING_FAILED = 5
""" brick grasping object types """
OBJECT_RED = 1
OBJECT_GREEN = 2
OBJECT_BLUE = 3
OBJECT_ORANGE = 4
WALL = 5
WALL_RED = 6
WALL_GREEN = 7
WALL_BLUE = 8
BRCIK_TYPE_TO_WALL_PLACING_TYPE = {
OBJECT_RED: WALL_RED,
OBJECT_GREEN: WALL_GREEN,
OBJECT_BLUE: WALL_BLUE
}
""" result ids """
RESULT_SUCCESS = 0
RESULT_STOPPED_BY_COMMAND = 1
RESULT_OBJECT_NOT_RELIABLE = 2
RESULT_OBJECT_NOT_VISIBLE = 3
RESULT_GRASPING_TIMOUT = 4
RESULT_GRASPING_FAILED = 5
def __init__(self):
# See example_state.py for basic explanations.
super(BrickPlacingActionState, self).__init__(
outcomes=['placed', 'placing_error'],
input_keys=['placing_position', 'brick_type', 'return_altitude'],
output_keys=['placing_result'])
self._topic = 'brick_grasping'
self._client = ProxyActionClient(
{self._topic:
GraspingAction}) # pass required clients as dict (topic: type)
Logger.loginfo('[PlacingAction]: Client initialized')
# It may happen that the action client fails to send the action goal.
self._error = False
def execute(self, userdata):
# While this state is active, check if the action has been finished and evaluate the result.
# Check if the client failed to send the goal.
if self._error:
Logger.logerr('[PlacingAction]: ending with "placed"')
return 'placing_error'
# Check if the action has been finished
if self._client.has_result(self._topic):
result = self._client.get_result(self._topic)
Logger.loginfo("[PlacingAction]: ended with result %s" %
str(result.message))
if result.success:
Logger.loginfo('[PlacingAction]: ending with "placed"')
userdata.placing_result = True
return 'placed'
else:
Logger.logwarn('[PlacingAction]: ending with "placing_error"')
userdata.placing_result = False
return 'placing_error'
"""feedback params are"""
feedback = self._client.get_feedback(self._topic)
if feedback is not None:
rospy.loginfo_throttle(
5, "[PlacingAction]: Current feedback msg: %s" %
(str(feedback.message)))
def on_enter(self, userdata):
# When entering this state, we send the action goal once to let the robot start its work.
placing_position = userdata.placing_position
brick_type = userdata.brick_type
return_altitude = userdata.return_altitude
# Create the goal.
goal = GraspingGoal()
if BrickPlacingActionState.BRCIK_TYPE_TO_WALL_PLACING_TYPE.get(
brick_type) is None:
Logger.logerr(
'[PlacingAction]: No placing action for brick type %s' %
str(brick_type))
goal.goal = self.WALL
else:
goal.goal = BrickPlacingActionState.BRCIK_TYPE_TO_WALL_PLACING_TYPE[
brick_type]
goal.placing_position.position.x = placing_position.position.x # used only for dropping
goal.placing_position.position.y = placing_position.position.y # used only for dropping
goal.placing_position.position.z = placing_position.position.z # used only for dropping
goal.placing_position.heading = placing_position.heading # used only for dropping
goal.return_altitude = return_altitude
# Send the goal.
self._error = False # make sure to reset the error state since a previous state execution might have failed
try:
self._client.send_goal(self._topic, goal)
except Exception as e:
# Since a state failure not necessarily causes a behavior failure, it is recommended to only print warnings, not errors.
# Using a linebreak before appending the error log enables the operator to collapse details in the GUI.
Logger.logwarn(
'[PlacingAction]: Failed to send the GraspingAction command:\n%s'
% str(e))
self._error = True
def on_exit(self, userdata):
# Make sure that the action is not running when leaving this state.
# A situation where the action would still be active is for example when the operator manually triggers an outcome.
if not self._client.has_result(self._topic):
self._client.cancel(self._topic)
Logger.loginfo('[PlacingAction]: Cancelled active action goal.')
