class MockExplorer():
def __init__(self, exploration_topic):
self.robot_pose_ = PoseStamped()
self.listener = tf.TransformListener()
self.navigation_succedes = True
self.reply = False
self.preempted = 0
self.entered_exploration = False
self.do_exploration_as_ = SimpleActionServer(
exploration_topic,
DoExplorationAction,
execute_cb=self.do_exploration_cb,
auto_start=False)
self.do_exploration_as_.start()
def __del__(self):
self.do_exploration_as_.__del__()
def do_exploration_cb(self, goal):
rospy.loginfo('do_exploration_cb')
self.entered_exploration = 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 = DoExplorationFeedback()
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.do_exploration_as_.publish_feedback(feedback)
if self.do_exploration_as_.is_preempt_requested():
self.preempted += 1
rospy.loginfo("Preempted!")
self.entered_exploration = False
self.do_exploration_as_.set_preempted(DoExplorationResult())
return None
else:
result = DoExplorationResult()
self.reply = False
self.preempted = 0
self.entered_exploration = False
if self.navigation_succedes:
self.do_exploration_as_.set_succeded(result)
else:
self.do_exploration_as_.set_aborted(result)
class MockGui():
def __init__(self, gui_validation_topic):
self.reply = False
self.preempted = 0
self.victimValid = True
self.victimFoundx = 0
self.victimFoundy = 0
self.probability = 0
self.sensorIDsFound = []
self.gui_validate_victim_as_ = SimpleActionServer(
gui_validation_topic,
ValidateVictimGUIAction,
execute_cb=self.gui_validate_victim_cb,
auto_start=False)
self.gui_validate_victim_as_.start()
def __del__(self):
self.gui_validate_victim_as_.__del__()
def gui_validate_victim_cb(self, goal):
rospy.loginfo('gui_validate_victim_cb')
self.reply = False
self.victimFoundx = goal.victimFoundx
self.victimFoundy = goal.victimFoundy
self.probability = goal.probability
self.sensorIDsFound = goal.sensorIDsFound
print goal
while not self.reply:
rospy.sleep(0.5)
if self.gui_validate_victim_as_.is_preempt_requested():
preempted += 1
self.gui_validate_victim_as_.set_preempted()
break
else:
self.preempted = 0
result = ValidateVictimGUIResult(victimValid=self.victimValid)
self.gui_validate_victim_as_.set_succeeded(result)
class MockEndEffectorPlanner():
def __init__(self, end_effector_planner_topic):
self.moves_end_effector = False
self.move_end_effector_succeeded = False
self.reply = False
self.command = 0
self.point_of_interest = ""
self.center_point = ""
self.end_effector_planner_as_ = SimpleActionServer(
end_effector_planner_topic,
MoveEndEffectorAction,
execute_cb=self.end_effector_planner_cb,
auto_start=False)
self.end_effector_planner_as_.start()
def __del__(self):
self.end_effector_planner_as_.__del__()
def end_effector_planner_cb(self, goal):
rospy.loginfo('end_effector_planner_cb')
self.moves_end_effector = True
self.command = goal.command
self.point_of_interest = goal.point_of_interest
self.center_point = goal.center_point
while not self.reply:
rospy.sleep(1.)
if self.end_effector_planner_as_.is_preempt_requested():
self.end_effector_planner_as_.set_preempted(MoveEndEffectorResult())
self.moves_end_effector = False
return None
else:
self.reply = False
result = MoveEndEffectorResult()
if self.move_end_effector_succeeded:
self.moves_end_effector = False
self.end_effector_planner_as_.set_succeeded(result)
else:
self.moves_end_effector = False
self.end_effector_planner_as_.set_aborted(result)
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)
