def __init__(self):
self._pub = rospy.Publisher(
"node_online",
mayfield_msgs.msg.NodeStatus,
latch=True,
queue_size=1,
)
# Controllers need to be loaded before we can run
mayfield_utils.wait_for_nodes(node_names=['controllers'], )
# Joint states keeps track of the latest position of all the robot's
# joints
self._joint_states = mobile_base.JointStates()
# Head client allows us to send trajectories down to the hardware
self._head_client = mobile_base.HeadClient(self._joint_states)
# Make sure the head client is ready to be used
assert self._head_client.wait_for_server(timeout=rospy.Duration(30.0))
# Vision client activates and deactivates face detection
self._vision_client = VisionClient()
# Activate vision detections. This eats up a lot of CPU and it may
# not be possible to run this at all times, especially when mapping
self._vision_client.activate_module(
module_name=VisionClient.FACE_DETECTOR)
self._vision_sub = rospy.Subscriber("vision/results",
vision_msgs.msg.FrameResults,
self._face_cb)
def __init__(self):
self._pub = rospy.Publisher(
"node_online",
mayfield_msgs.msg.NodeStatus,
latch=True,
queue_size=1,
)
# mobile base driver needs to be running before we can run:
mayfield_utils.wait_for_nodes(
node_names=['controllers'],
)
self._light_client = mobile_base.ChestLightClient()
self._anim = PulseAnimation(
framerate=self.CHEST_LIGHT_FRAMERATE
)
# By default, we want the color of the chest light to be controlled
# by the battery level of the robot. Subscribe to mobile_base/power
# to get this information
self._power_sub = rospy.Subscriber(
"mobile_base/power",
mobile_base_driver.msg.Power,
self._power_cb
)
def __init__(self):
self._pub = rospy.Publisher(
"node_online",
mayfield_msgs.msg.NodeStatus,
latch=True,
queue_size=1,
)
# Controllers need to be loaded before we can run
mayfield_utils.wait_for_nodes(node_names=['controllers'], )
self._map_manager = MapManager()
self._power_monitor = None # Created when we start to run
self._nav_client = may_nav_py.nav.Nav(on_nav_cb=lambda: None,
on_nav_end_cb=lambda: None,
on_move_cb=lambda fb, tilt: None,
robot_pose_se2=lambda: None)
# If you're using RVIZ, you can send a nav goal to the robot on the
# /move_base_simple/goal topic. We'll listen on that topic and
# forward the goal over to may_nav
self._nav_goal_sub = rospy.Subscriber("move_base_simple/goal",
geometry_msgs.msg.PoseStamped,
self._goal_received_cb)
def __init__(self):
self._pub = rospy.Publisher(
"node_online",
mayfield_msgs.msg.NodeStatus,
latch=True,
queue_size=1,
)
# Controllers need to be loaded before we can run
mayfield_utils.wait_for_nodes(node_names=['controllers'], )
self._safety_client = mobile_base.SafetyClient()
# Override the safety controller for all events that we don't
# handle so they'll be ignored by the hardware
self.UNHANDLED_EVENTS = (
self._safety_client.safety_statuses() # All statuses
- self.HANDLED_EVENTS # Minus the ones we handle
)
self._safety_client.safety_override(self.UNHANDLED_EVENTS)
self._sync_lock = threading.Lock()
self._block_twists = False
# Set up publishers before subscribers because once subscribers are
# set up, we can start getting callbacks
self._cmd_vel_pub = rospy.Publisher("mobile_base/commands/velocity",
geometry_msgs.msg.Twist,
queue_size=1)
self._cmd_vel_sub = rospy.Subscriber("cmd_vel",
geometry_msgs.msg.Twist,
self._forward_twists)
def __init__(self, map_name):
self._pub = rospy.Publisher(
"node_online",
mayfield_msgs.msg.NodeStatus,
latch=True,
queue_size=1,
)
# Controllers need to be loaded before we can run
mayfield_utils.wait_for_nodes(
node_names=['controllers'],
)
# Joint states keeps track of the latest position of all the robot's
# joints
self._joint_states = mobile_base.JointStates()
self._power_monitor = None # Created when we start to run
self._map_manager = OortMapManager()
# catch ctrl-c and signal the main thread that we should save the map
# and quit
def signal_handler(sig, frame):
self._mapping_complete = True
signal.signal(signal.SIGINT, signal_handler)
self._mapping_complete = False
self.map_name = map_name
self._start_mapping()
def __init__(self):
self._pub = rospy.Publisher(
"node_online",
mayfield_msgs.msg.NodeStatus,
latch=True,
queue_size=1,
)
# Controllers need to be loaded before we can run
mayfield_utils.wait_for_nodes(node_names=['controllers'], )
# Joint states keeps track of the latest position of all the robot's
# joints
self._joint_states = mobile_base.JointStates()
# Head client allows us to send trajectories down to the hardware
self._head_client = mobile_base.HeadClient(self._joint_states)
self._power_monitor = None # Created when we start to run
self._map_manager = MapManager()
self._map_manager.onMapGrew.connect(self._map_grew)
self._mapping_complete = False
# Make sure the head client is ready to be used
assert self._head_client.wait_for_server(timeout=rospy.Duration(30.0))
def setUpClass(cls):
cls.dock_srv = rospy.ServiceProxy(
"/sim_interface/dock",
gizmo_hw_sim.srv.Dock
)
# Wait for the DUT to be online before starting
mayfield_utils.wait_for_nodes(['nav_controller'])
def setUpClass(cls):
cls.power_srv = rospy.ServiceProxy(
"/sim_interface/power",
gizmo_hw_sim.srv.Power
)
# Wait for the DUT to be online before starting
mayfield_utils.wait_for_nodes(['chest_light_controller'])
def setUpClass(cls):
cls.dock_srv = rospy.ServiceProxy("/sim_interface/dock",
gizmo_hw_sim.srv.Dock)
cls.goal_pub = rospy.Publisher("move_base_simple/goal",
geometry_msgs.msg.PoseStamped)
# Wait for the DUT to be online before starting
mayfield_utils.wait_for_nodes(['nav_controller'])
def setUpClass(cls):
# Fake touch-messages - alas not simulated :-(
cls.touch_pub = rospy.Publisher("mobile_base/touch",
mobile_base_driver.msg.Touch,
latch=True)
# Get one light message latched on the output of the DUT so
# the LightMonitor can work correctly
cls.touch_pub.publish([False] * 8)
# Wait for the DUT to be online before starting
mayfield_utils.wait_for_nodes(['cap_touch_chest_light'])
def __init__(self):
self._pub = rospy.Publisher(
"node_online",
mayfield_msgs.msg.NodeStatus,
latch=True,
queue_size=1,
)
# Controllers need to be loaded before we can run
mayfield_utils.wait_for_nodes(node_names=['controllers'], )
self._map_manager = OortMapManager()
self._power_monitor = None # Created when we start to run
def setUpClass(cls):
# For sending drive commands to the safety controller:
cls.vel_pub = rospy.Publisher(
"/cmd_vel",
geometry_msgs.msg.Twist,
latch=True,
queue_size=2,
)
# For triggering the bump shell:
rospy.wait_for_service("/sim_interface/kick")
cls.kick_srv = rospy.ServiceProxy(
"/sim_interface/kick",
gizmo_hw_sim.srv.Kick
)
# Wait for the DUT to be online before starting
mayfield_utils.wait_for_nodes(['safety_controller'])
def __init__(self):
self._pub = rospy.Publisher(
"node_online",
mayfield_msgs.msg.NodeStatus,
latch=True,
queue_size=1,
)
# Controllers need to be loaded before we can run
mayfield_utils.wait_for_nodes(node_names=['controllers'], )
# Joint states keeps track of the latest position of all the robot's
# joints
self._joint_states = mobile_base.JointStates()
self._power_monitor = None # Created when we start to run
self._map_manager = OortMapManager()
self._mapping_complete = False
def setUpClass(cls):
# To monitor joints:
cls.joints = mobile_base.JointStates()
# For sending drive commands to the safety controller:
cls.vel_pub = rospy.Publisher(
"/cmd_vel",
geometry_msgs.msg.Twist,
latch=True,
queue_size=1,
)
cls.map_state_srv = rospy.ServiceProxy("/oort_ros_mapping/map/state",
oort_msgs.srv.GetString)
cls.dock_srv = rospy.ServiceProxy("/sim_interface/dock",
gizmo_hw_sim.srv.Dock)
# Wait for the DUT to be online before starting
mayfield_utils.wait_for_nodes(['mapping_controller'])
def __init__(self):
self._pub = rospy.Publisher(
"node_online",
mayfield_msgs.msg.NodeStatus,
latch=True,
queue_size=1,
)
# mobile base driver needs to be running before we can run:
mayfield_utils.wait_for_nodes(
node_names=['controllers'],
)
self._light_client = mobile_base.ChestLightClient()
self._touch_sub = rospy.Subscriber(
"mobile_base/touch",
mobile_base_driver.msg.Touch,
self._touch_cb
)
def setUpClass(cls):
cls.joints = mobile_base.JointStates()
cls.vision_client = vision_bridge.VisionClient()
# Wait for the DUT to be online before starting
mayfield_utils.wait_for_nodes(['head_controller'])
