class MockNavigation():
def __init__(self, move_base_topic):
self.robot_pose_ = PoseStamped()
self.listener = tf.TransformListener()
self.navigation_succedes = True
self.reply = False
self.preempted = 0
self.moves_base = False
self.target_position = Point()
self.target_orientation = Quaternion()
self.move_base_as_ = SimpleActionServer(
move_base_topic,
MoveBaseAction,
execute_cb = self.move_base_cb,
auto_start = False)
self.move_base_as_.start()
def __del__(self):
self.move_base_as_.__del__()
def move_base_cb(self, goal):
rospy.loginfo('move_base_cb')
self.target_position = goal.target_pose.pose.position
self.target_orientation = goal.target_pose.pose.orientation
self.moves_base = True
while not self.reply:
rospy.sleep(0.2)
(trans, rot) = self.listener.lookupTransform('/map', '/base_footprint', rospy.Time(0))
self.robot_pose_.pose.position.x = trans[0]
self.robot_pose_.pose.position.y = trans[1]
feedback = MoveBaseFeedback()
feedback.base_position.pose.position.x = \
self.robot_pose_.pose.position.x
feedback.base_position.pose.position.y = \
self.robot_pose_.pose.position.y
self.move_base_as_.publish_feedback(feedback)
if self.move_base_as_.is_preempt_requested():
self.preempted += 1
rospy.loginfo("Preempted!")
self.moves_base = False
self.move_base_as_.set_preempted()
return None
if self.move_base_as_.is_new_goal_available():
self.preempted += 1
self.move_base_cb(self.move_base_as_.accept_new_goal())
return None
else:
result = MoveBaseResult()
self.reply = False
self.preempted = 0
self.moves_base = False
self.target_position = Point()
self.target_orientation = Quaternion()
if self.navigation_succedes:
self.move_base_as_.set_succeeded(result)
else:
self.move_base_as_.set_aborted(result)
class ActionServerHandler(object):
"""
Interface to action server which is more useful for behaviors.
"""
def __init__(self, action_name, action_type):
self.goal_queue = Queue(1)
self.result_queue = Queue(1)
self.lock = Blackboard().lock
self.action_name = action_name
self.cancel_next_goal = False
self._as = SimpleActionServer(action_name, action_type, execute_cb=self.execute_cb, auto_start=False)
self._as.register_preempt_callback(self.cancel_cb)
self._as.start()
def cancel_cb(self):
self.cancel_next_goal = self._as.is_new_goal_available()
def execute_cb(self, goal):
"""
:type goal: MoveGoal
"""
with self.lock.acquire_timeout(0) as got_lock:
if got_lock and not self.cancel_next_goal:
self.my_state = Status.RUNNING
self.goal_queue.put(goal)
self.result_queue.get()()
else:
self.my_state = Status.FAILURE
r = self._as.action_server.ActionResultType()
r.error = DetectShelfLayersResult.SERVER_BUSY
print_with_prefix('rejected goal because server busy server busy', self.action_name)
self._as.set_aborted(r)
self.cancel_next_goal = False
def get_goal(self):
try:
goal = self.goal_queue.get_nowait()
return goal
except Empty:
return None
def has_goal(self):
return not self.goal_queue.empty()
def send_preempted(self, result=None):
def call_me_now():
self._as.set_preempted(result)
self.result_queue.put(call_me_now)
def send_aborted(self, result=None):
def call_me_now():
self._as.set_aborted(result)
self.result_queue.put(call_me_now)
def send_result(self, result=None):
"""
:type result: MoveResult
"""
def call_me_now():
self._as.set_succeeded(result)
self.result_queue.put(call_me_now)
def is_preempt_requested(self):
return self._as.is_preempt_requested()
class move_base_fake_node:
def __init__(self):
self.action_server = SimpleActionServer(
'move_base',
MoveBaseAction,
execute_cb=self.execute_callback,
auto_start=False
) # Simple action server will pretend to be move_base
# Movement goal state
self.goal = None # Is a goal set
self.valid_goal = False # Is this a valid goal
self.current_goal_started = False # Has the goal been started (i.e. have we told our Bug algorithm to use this point and start)
self.current_goal_complete = False # Has the Bug algorithm told us it completed
self.position = None # move_base feedback reports the current direction
## TO DO!!
# Need a service provided by this node or something for the Bug algorithm to tell us it is done
# Bug service to start and stop Bug algorithm
# Bug service to set a new goal in Bug algorithm
# rospy.wait_for_service()
# rospy.wait_for_service()
self.subscriber_odometry = rospy.Subscriber(
'odom/', Odometry, self.callback_odometry
) # We need to read the robots current point for the feedback
self.subscriber_simple_goal = rospy.Subscriber(
'/move_base_simple/goal/', PoseStamped, self.callback_simple_goal
) # Our return goal is done with /move_base_simple/goal/
self.goal_pub = rospy.Publisher(
'/move_base/goal/', MoveBaseActionGoal,
queue_size=10) # /move_base_simple/goal/ gets published here
self.action_server.start()
def execute_callback(self, move_base_goal):
self.goal = move_base_goal.target_pose.pose # Set the provided goal as the current goal
rospy.logdebug('[Move Base Fake] Execute Callback: {}'.format(
str(self.goal.position)))
self.valid_goal = True # Assume it is valid
self.current_goal_started = False # It hasnt started yet
self.current_goal_complete = False # It hasnt been completed
r = rospy.Rate(1)
while not rospy.is_shutdown():
# Always start by checking if there is a new goal that preempts the current one
if self.action_server.is_preempt_requested():
## TO DO!!
# Tell Bug algorithm to stop before changing or stopping goal
if self.action_server.is_new_goal_available():
new_goal = self.action_server.accept_new_goal(
) # There is a new goal
rospy.logdebug('[Move Base Fake] New Goal: {}'.format(
str(self.goal.position)))
self.goal = new_goal.target_pose.pose # Set the provided goal as the current goal
self.valid_goal = True # Assume it is valid
self.current_goal_started = False # It hasnt started yet
self.current_goal_complete = False # It hasnt been completed
else:
self.action_server.set_preempted(
) # No new goal, we've just been told to stop
self.goal = None # Stop everything
self.valid_goal = False
self.current_goal_started = False
self.current_goal_complete = False
return
# Start goal
if self.valid_goal and not self.current_goal_started:
rospy.logdebug('[Move Base Fake] Starting Goal')
## TO DO !!
# Call the Bug services/topics etc to tell Bug algorithm new target point and then to start
self.current_goal_started = True # Only start once
# Feedback ever loop just reports current location
feedback = MoveBaseFeedback()
feedback.base_position.pose.position = self.position
self.action_server.publish_feedback(feedback)
# Completed is set in a callback that you need to link to a service or subscriber
if self.current_goal_complete:
rospy.logdebug('[Move Base Fake] Finishing Goal')
## TO DO!!
# Tell Bug algorithm to stop before changing or stopping goal
self.goal = None # Stop everything
self.valid_goal = False
self.current_goal_started = False
self.current_goal_complete = False
self.action_server.set_succeeded(
MoveBaseResult(), 'Goal reached') # Send success message
return
r.sleep()
# Shutdown
rospy.logdebug('[Move Base Fake] Shutting Down')
## TO DO!!
# Tell Bug algorithm to stop before changing or stopping goal
self.goal = None # Stop everything
self.valid_goal = False
self.current_goal_started = False
self.current_goal_complete = False
# you need to connect this to something being called/published from the Bug algorithm
def callback_complete(self, success):
# TO DO!!
# Implement some kind of service or subscriber so the Bug algorithm can tell this node it is complete
self.current_goal_complete = success.data
def callback_odometry(self, odom):
self.position = odom.pose.pose.position
# Simple goals get republished to the correct topic
def callback_simple_goal(self, goal):
rospy.logdebug('[Move Base Fake] Simple Goal: {}'.format(
str(goal.pose.position)))
action_goal = MoveBaseActionGoal()
action_goal.header.stamp = rospy.Time.now()
action_goal.goal.target_pose = goal
self.goal_pub.publish(action_goal)
class ActionPerformer(Node, ABC):
def __init__(self, name: str):
super().__init__(name, "action")
self.name = name
# Service for action point computation
self._action_point_server = Service(get_action_point_service(name),
ComputeActionPoint,
self._compute_action_point)
# Action server for this action
self._action_server = SimpleActionServer(get_action_server(name),
PerformAction,
auto_start=False)
self._action_server.register_goal_callback(self._on_goal)
self._action_server.register_preempt_callback(self._on_preempt)
self._action_server.start()
# Function managing the action
def returns(self, state=ActionStatus.DONE):
'''
Function to call when the action is interrupted
with a given state
'''
# Create result message
result = PerformResult()
result.state = state
# Send back to client
self._action_server.set_succeeded(result)
# Hidden handlers
def _compute_action_point(
self,
request: ComputeActionPointRequest) -> ComputeActionPointResponse:
# Extract data from request and call performer's function
result: ActionPoint = self.compute_action_point(
Argumentable().from_list(request.args), request.robot_pos)
response = ComputeActionPointResponse()
response.action_point = result
return response
def _on_goal(self):
if self._action_server.is_new_goal_available():
goal: PerformGoal = self._action_server.accept_new_goal()
# run action with given args
self.start(Argumentable().from_list(goal.arguments))
def _on_preempt(self):
self.cancel()
self._action_server.set_preempted()
# Function to be defined by inherited actions
@abstractmethod
def compute_action_point(self, args: Argumentable,
robot_pos: Pose2D) -> ActionPoint:
pass
@abstractmethod
def start(self, args: Argumentable):
pass
def cancel(self):
pass