def __new__(cls, ns, name, addr, master_uri, remappings):
# constraints: anything blocking here should print something if it's
# taking a long time in order to avoid confusion
self = object.__new__(cls)
if ns:
assert ns[0] == '/'
assert not ns.endswith('/')
self._ns = ns # valid values: '', '/a', '/a/b'
assert '/' not in name
self._name = self._ns + '/' + name
self._shutdown_callbacks = set()
reactor.addSystemEventTrigger('before', 'shutdown', self.shutdown)
self._addr = addr
self._master_uri = master_uri
self._remappings = remappings
self._master_proxy = rosxmlrpc.Proxy(xmlrpc.Proxy(self._master_uri), self._name)
self._is_running = True
self._xmlrpc_handlers = {}
self._xmlrpc_server = reactor.listenTCP(0, server.Site(_XMLRPCSlave(self)))
self._shutdown_callbacks.add(self._xmlrpc_server.loseConnection)
self._xmlrpc_server_uri = 'http://%s:%i/' % (self._addr, self._xmlrpc_server.getHost().port)
self._tcpros_handlers = {}
@util.cancellableInlineCallbacks
def _handle_tcpros_conn(conn):
try:
header = tcpros.deserialize_dict((yield conn.receiveString()))
def default(header, conn):
conn.sendString(tcpros.serialize_dict(dict(error='unhandled connection')))
conn.transport.loseConnection()
if 'service' in header:
self._tcpros_handlers.get(('service', header['service']), default)(header, conn)
elif 'topic' in header:
self._tcpros_handlers.get(('topic', header['topic']), default)(header, conn)
else:
conn.sendString(tcpros.serialize_dict(dict(error='no topic or service name detected')))
conn.transport.loseConnection()
except:
conn.transport.loseConnection()
raise
def _make_tcpros_protocol(addr):
conn = tcpros.Protocol()
_handle_tcpros_conn(conn)
return conn
self._tcpros_server = reactor.listenTCP(0, util.AutoServerFactory(_make_tcpros_protocol))
self._shutdown_callbacks.add(self._tcpros_server.loseConnection)
self._tcpros_server_uri = 'rosrpc://%s:%i' % (self._addr, self._tcpros_server.getHost().port)
self._tcpros_server_addr = self._addr, self._tcpros_server.getHost().port
while True:
try:
other_node_uri = yield self._master_proxy.lookupNode(self._name)
except rosxmlrpc.Error:
break # assume that error means unknown node
except Exception:
traceback.print_exc()
yield util.wall_sleep(1) # pause so we don't retry immediately
else:
other_node_proxy = rosxmlrpc.Proxy(xmlrpc.Proxy(other_node_uri), self._name)
try:
yield util.wrap_timeout(other_node_proxy.shutdown('new node registered with same name'), 3)
except error.ConnectionRefusedError:
pass
except Exception:
traceback.print_exc()
break
try:
self._use_sim_time = yield self.get_param('/use_sim_time')
except rosxmlrpc.Error: # assume that error means not found
self._use_sim_time = False
if self._use_sim_time:
def got_clock(msg):
self._sim_time = msg.clock
self._clock_sub = self.subscribe('/clock', Clock, got_clock)
# make sure self._sim_time gets set before we continue
yield util.wrap_time_notice(self._clock_sub.get_next_message(), 1,
'getting simulated time from /clock')
for k, v in self._remappings.iteritems():
if k.startswith('_') and not k.startswith('__'):
yield self.set_param(self.resolve_name('~' + k[1:]), yaml.load(v))
self.advertise_service('~get_loggers', GetLoggers, lambda req: GetLoggersResponse())
self.advertise_service('~set_logger_level', SetLoggerLevel, lambda req: SetLoggerLevelResponse())
defer.returnValue(self)
def run(self, args):
if not self.pose:
raise Exception('Cant move: No odom')
commands = args.commands
arguments = commands[1::2]
commands = commands[0::2]
self.send_feedback('Switching trajectory to lqrrt')
self.change_trajectory('lqrrt')
self.send_feedback('Switching wrench to autonomous')
yield self.change_wrench('autonomous')
for i in xrange(len(commands)):
command = commands[i]
argument = arguments[i]
action_kwargs = {
'move_type': args.movetype,
'speed_factor': args.speedfactor
}
action_kwargs['blind'] = args.blind
if args.speedfactor is not None:
if ',' in args.speedfactor:
sf = np.array(map(float,
args.speedfactor[1:-1].split(',')))
else:
sf = [float(args.speedfactor)] * 3
action_kwargs['speed_factor'] = sf
if args.plantime is not None:
action_kwargs['initial_plan_time'] = float(args.plantime)
if args.focus is not None:
focus = np.array(map(float, args.focus[1:-1].split(',')))
focus[2] = 1 # Tell lqrrt we want to look at the point
point = numpy_to_point(focus)
action_kwargs['focus'] = point
if command == 'custom':
# Let the user input custom commands, the eval may be dangerous so do away with that at some point.
self.send_feedback(
"Moving with the command: {}".format(argument))
res = yield eval(
"self.move.{}.go(move_type='{move_type}')".format(
argument, **action_kwargs))
elif command == 'rviz':
self.send_feedback('Select a 2D Nav Goal in RVIZ')
target_pose = yield util.wrap_time_notice(
self.rviz_goal.get_next_message(), 2, "Rviz goal")
self.send_feedback('RVIZ pose recieved!')
res = yield self.move.to_pose(target_pose).go(**action_kwargs)
elif command == 'circle':
self.send_feedback('Select a Publish Point in RVIZ')
target_point = yield util.wrap_time_notice(
self.rviz_point.get_next_message(), 2, "Rviz point")
self.send_feedback('RVIZ point recieved!')
target_point = rosmsg_to_numpy(target_point.point)
direction = 'cw' if argument == '-1' else 'ccw'
res = yield self.move.circle_point(
target_point, direction=direction).go(**action_kwargs)
else:
shorthand = {
"f": "forward",
"b": "backward",
"l": "left",
"r": "right",
"yl": "yaw_left",
"yr": "yaw_right"
}
command = command if command not in shorthand.keys(
) else shorthand[command]
movement = getattr(self.move, command)
trans_move = command[:3] != "yaw"
unit = "m" if trans_move else "rad"
amount = argument
# See if there's a non standard unit at the end of the argument
if not argument[-1].isdigit():
last_digit_index = [
i for i, c in enumerate(argument) if c.isdigit()
][-1] + 1
amount = float(argument[:last_digit_index])
unit = argument[last_digit_index:]
# Get the kwargs to pass to the action server
station_hold = amount == '0'
if station_hold:
action_kwargs['move_type'] = MoveGoal.HOLD
msg = "Moving {} ".format(
command) if trans_move else "Yawing {} ".format(
command[4:])
self.send_feedback(msg + "{}{}".format(amount, unit))
res = yield movement(float(amount), unit).go(**action_kwargs)
if res.failure_reason is not '':
raise Exception('Move failed. Reason: {}'.format(
res.failure_reason))
defer.returnValue('Move completed successfully!')
class Navigator(BaseMission):
circle = "CIRCLE"
cross = "CROSS"
triangle = "TRIANGLE"
red = "RED"
green = "GREEN"
blue = "BLUE"
net_stc_results = None
net_entrance_results = None
max_grinch_effort = 500
def __init__(self, **kwargs):
super(Navigator, self).__init__(**kwargs)
@classmethod
@util.cancellableInlineCallbacks
def _init(cls, mission_runner):
super(Navigator, cls)._init(mission_runner)
cls.is_vrx = yield cls.nh.get_param("/is_vrx")
cls._moveto_client = action.ActionClient(cls.nh, 'move_to', MoveAction)
# For missions to access clicked points / poses
cls.rviz_goal = cls.nh.subscribe("/move_base_simple/goal", PoseStamped)
cls.rviz_point = cls.nh.subscribe("/clicked_point", PointStamped)
cls._change_wrench = cls.nh.get_service_client('/wrench/select',
MuxSelect)
cls._change_trajectory = cls.nh.get_service_client(
'/trajectory/select', MuxSelect)
cls._database_query = cls.nh.get_service_client(
'/database/requests', ObjectDBQuery)
cls._reset_pcodar = cls.nh.get_service_client('/pcodar/reset', Trigger)
cls._pcodar_set_params = cls.nh.get_service_client(
'/pcodar/set_parameters', Reconfigure)
cls.pose = None
def odom_set(odom):
return setattr(cls, 'pose', mil_tools.odometry_to_numpy(odom)[0])
cls._odom_sub = cls.nh.subscribe('odom', Odometry, odom_set)
if cls.is_vrx:
yield cls._init_vrx()
else:
yield cls._init_not_vrx()
@classmethod
def _init_vrx(cls):
cls.killed = False
cls.odom_loss = False
cls.tf_listener = tf.TransformListener(cls.nh)
cls.set_vrx_classifier_enabled = cls.nh.get_service_client(
'/vrx_classifier/set_enabled', SetBool)
@classmethod
@util.cancellableInlineCallbacks
def _init_not_vrx(cls):
cls.vision_proxies = {}
cls._load_vision_services()
cls.launcher_state = "inactive"
cls._actuator_timing = yield cls.nh.get_param("~actuator_timing")
cls.mission_params = {}
cls._load_mission_params()
# If you don't want to use txros
cls.ecef_pose = None
cls.killed = '?'
cls.odom_loss = '?'
if (yield cls.nh.has_param('/is_simulation')):
cls.sim = yield cls.nh.get_param('/is_simulation')
else:
cls.sim = False
def enu_odom_set(odom):
return setattr(cls, 'ecef_pose',
mil_tools.odometry_to_numpy(odom)[0])
cls._ecef_odom_sub = cls.nh.subscribe('absodom', Odometry,
enu_odom_set)
cls.hydrophones = TxHydrophonesClient(cls.nh)
cls.poi = TxPOIClient(cls.nh)
cls._grinch_lower_time = yield cls.nh.get_param("~grinch_lower_time")
cls._grinch_raise_time = yield cls.nh.get_param("~grinch_raise_time")
cls.grinch_limit_switch_pressed = False
cls._grinch_limit_switch_sub = yield cls.nh.subscribe(
'/limit_switch', Bool, cls._grinch_limit_switch_cb)
cls._winch_motor_pub = cls.nh.advertise("/grinch_winch/cmd", Command)
cls._grind_motor_pub = cls.nh.advertise("/grinch_spin/cmd", Command)
try:
cls._actuator_client = cls.nh.get_service_client(
'/actuator_driver/actuate', SetValve)
cls._camera_database_query = cls.nh.get_service_client(
'/camera_database/requests', navigator_srvs.CameraDBQuery)
cls._change_wrench = cls.nh.get_service_client(
'/wrench/select', MuxSelect)
cls._change_trajectory = cls.nh.get_service_client(
'/trajectory/select', MuxSelect)
except AttributeError, err:
fprint("Error getting service clients in nav singleton init: {}".
format(err),
title="NAVIGATOR",
msg_color='red')
cls.tf_listener = tf.TransformListener(cls.nh)
# Vision
cls.obstacle_course_vision_enable = cls.nh.get_service_client(
'/vision/obsc/enable', SetBool)
cls.docks_vision_enable = cls.nh.get_service_client(
'/vision/docks/enable', SetBool)
yield cls._make_alarms()
if cls.sim:
fprint("Sim mode active!", title="NAVIGATOR")
yield cls.nh.sleep(.5)
else:
# We want to make sure odom is working before we continue
fprint("Action client do you yield?", title="NAVIGATOR")
yield util.wrap_time_notice(cls._moveto_client.wait_for_server(),
2, "Lqrrt action server")
fprint("Yes he yields!", title="NAVIGATOR")
fprint("Waiting for odom...", title="NAVIGATOR")
odom = util.wrap_time_notice(cls._odom_sub.get_next_message(), 2,
"Odom listener")
enu_odom = util.wrap_time_notice(
cls._ecef_odom_sub.get_next_message(), 2, "ENU Odom listener")
yield defer.gatherResults([odom, enu_odom
]) # Wait for all those to finish
cls.docking_scan = 'NA'
def __new__(cls, ns, name, addr, master_uri, remappings):
# constraints: anything blocking here should print something if it's
# taking a long time in order to avoid confusion
self = object.__new__(cls)
if ns:
assert ns[0] == '/'
assert not ns.endswith('/')
self._ns = ns # valid values: '', '/a', '/a/b'
assert '/' not in name
self._name = self._ns + '/' + name
self._shutdown_callbacks = set()
reactor.addSystemEventTrigger('before', 'shutdown', self.shutdown)
self._addr = addr
self._master_uri = master_uri
self._remappings = remappings
self._master_proxy = rosxmlrpc.Proxy(xmlrpc.Proxy(self._master_uri),
self._name)
self._is_running = True
self._xmlrpc_handlers = {}
self._xmlrpc_server = reactor.listenTCP(
0, server.Site(_XMLRPCSlave(self)))
self._shutdown_callbacks.add(self._xmlrpc_server.loseConnection)
self._xmlrpc_server_uri = 'http://%s:%i/' % (
self._addr, self._xmlrpc_server.getHost().port)
self._tcpros_handlers = {}
@util.cancellableInlineCallbacks
def _handle_tcpros_conn(conn):
try:
header = tcpros.deserialize_dict((yield conn.receiveString()))
def default(header, conn):
conn.sendString(
tcpros.serialize_dict(
dict(error='unhandled connection')))
conn.transport.loseConnection()
if 'service' in header:
self._tcpros_handlers.get(('service', header['service']),
default)(header, conn)
elif 'topic' in header:
self._tcpros_handlers.get(('topic', header['topic']),
default)(header, conn)
else:
conn.sendString(
tcpros.serialize_dict(
dict(error='no topic or service name detected')))
conn.transport.loseConnection()
except:
conn.transport.loseConnection()
raise
def _make_tcpros_protocol(addr):
conn = tcpros.Protocol()
_handle_tcpros_conn(conn)
return conn
self._tcpros_server = reactor.listenTCP(
0, util.AutoServerFactory(_make_tcpros_protocol))
self._shutdown_callbacks.add(self._tcpros_server.loseConnection)
self._tcpros_server_uri = 'rosrpc://%s:%i' % (
self._addr, self._tcpros_server.getHost().port)
self._tcpros_server_addr = self._addr, self._tcpros_server.getHost(
).port
while True:
try:
other_node_uri = yield self._master_proxy.lookupNode(
self._name)
except rosxmlrpc.Error:
break # assume that error means unknown node
except Exception:
traceback.print_exc()
yield util.wall_sleep(1) # pause so we don't retry immediately
else:
other_node_proxy = rosxmlrpc.Proxy(
xmlrpc.Proxy(other_node_uri), self._name)
try:
yield util.wrap_timeout(
other_node_proxy.shutdown(
'new node registered with same name'), 3)
except error.ConnectionRefusedError:
pass
except Exception:
traceback.print_exc()
break
try:
self._use_sim_time = yield self.get_param('/use_sim_time')
except rosxmlrpc.Error: # assume that error means not found
self._use_sim_time = False
if self._use_sim_time:
def got_clock(msg):
self._sim_time = msg.clock
self._clock_sub = self.subscribe('/clock', Clock, got_clock)
# make sure self._sim_time gets set before we continue
yield util.wrap_time_notice(self._clock_sub.get_next_message(), 1,
'getting simulated time from /clock')
for k, v in self._remappings.iteritems():
if k.startswith('_') and not k.startswith('__'):
yield self.set_param(self.resolve_name('~' + k[1:]),
yaml.load(v))
self.advertise_service('~get_loggers', GetLoggers,
lambda req: GetLoggersResponse())
self.advertise_service('~set_logger_level', SetLoggerLevel,
lambda req: SetLoggerLevelResponse())
defer.returnValue(self)
class Navigator(BaseTask):
circle = "CIRCLE"
cross = "CROSS"
triangle = "TRIANGLE"
red = "RED"
green = "GREEN"
blue = "BLUE"
def __init__(self, **kwargs):
super(Navigator, self).__init__(**kwargs)
@classmethod
@util.cancellableInlineCallbacks
def _init(cls, task_runner):
super(Navigator, cls)._init(task_runner)
cls.vision_proxies = {}
cls._load_vision_services()
cls._actuator_timing = yield cls.nh.get_param("~actuator_timing")
cls.mission_params = {}
cls._load_mission_params()
# If you don't want to use txros
cls.pose = None
cls.ecef_pose = None
cls.killed = '?'
cls.odom_loss = '?'
if (yield cls.nh.has_param('/is_simulation')):
cls.sim = yield cls.nh.get_param('/is_simulation')
else:
cls.sim = False
# For missions to access clicked points / poses
cls.rviz_goal = cls.nh.subscribe("/move_base_simple/goal", PoseStamped)
cls.rviz_point = cls.nh.subscribe("/clicked_point", PointStamped)
cls._moveto_client = action.ActionClient(cls.nh, 'move_to', MoveAction)
def odom_set(odom):
return setattr(cls, 'pose', mil_tools.odometry_to_numpy(odom)[0])
cls._odom_sub = cls.nh.subscribe('odom', Odometry, odom_set)
def enu_odom_set(odom):
return setattr(cls, 'ecef_pose', mil_tools.odometry_to_numpy(odom)[0])
cls._ecef_odom_sub = cls.nh.subscribe('absodom', Odometry, enu_odom_set)
cls.hydrophones = TxHydrophonesClient(cls.nh)
try:
cls._actuator_client = cls.nh.get_service_client('/actuator_driver/actuate', SetValve)
cls._database_query = cls.nh.get_service_client('/database/requests', ObjectDBQuery)
cls._camera_database_query = cls.nh.get_service_client(
'/camera_database/requests', navigator_srvs.CameraDBQuery)
cls._change_wrench = cls.nh.get_service_client('/wrench/select', MuxSelect)
cls._change_trajectory = cls.nh.get_service_client('/trajectory/select', MuxSelect)
except AttributeError, err:
fprint("Error getting service clients in nav singleton init: {}".format(
err), title="NAVIGATOR", msg_color='red')
cls.tf_listener = tf.TransformListener(cls.nh)
yield cls._make_alarms()
if cls.sim:
fprint("Sim mode active!", title="NAVIGATOR")
yield cls.nh.sleep(.5)
else:
# We want to make sure odom is working before we continue
fprint("Action client do you yield?", title="NAVIGATOR")
yield util.wrap_time_notice(cls._moveto_client.wait_for_server(), 2, "Lqrrt action server")
fprint("Yes he yields!", title="NAVIGATOR")
fprint("Waiting for odom...", title="NAVIGATOR")
odom = util.wrap_time_notice(cls._odom_sub.get_next_message(), 2, "Odom listener")
enu_odom = util.wrap_time_notice(cls._ecef_odom_sub.get_next_message(), 2, "ENU Odom listener")
yield defer.gatherResults([odom, enu_odom]) # Wait for all those to finish
def run(self, args):
if not self.pose:
raise Exception('Cant move: No odom')
commands = args.commands
arguments = commands[1::2]
commands = commands[0::2]
self.send_feedback('Switching wrench to autonomous')
yield self.change_wrench('autonomous')
for i in xrange(len(commands)):
command = commands[i]
argument = arguments[i]
action_kwargs = {'move_type': args.movetype, 'speed_factor': args.speedfactor}
action_kwargs['blind'] = args.blind
if args.speedfactor is not None:
if ',' in args.speedfactor:
sf = np.array(map(float, args.speedfactor[1:-1].split(',')))
else:
sf = [float(args.speedfactor)] * 3
action_kwargs['speed_factor'] = sf
if args.plantime is not None:
action_kwargs['initial_plan_time'] = float(args.plantime)
if args.focus is not None:
focus = np.array(map(float, args.focus[1:-1].split(',')))
focus[2] = 1 # Tell lqrrt we want to look at the point
point = numpy_to_point(focus)
action_kwargs['focus'] = point
if command == 'custom':
# Let the user input custom commands, the eval may be dangerous so do away with that at some point.
self.send_feedback("Moving with the command: {}".format(argument))
res = yield eval("self.move.{}.go(move_type='{move_type}')".format(argument, **action_kwargs))
elif command == 'rviz':
self.send_feedback('Select a 2D Nav Goal in RVIZ')
target_pose = yield util.wrap_time_notice(self.rviz_goal.get_next_message(), 2, "Rviz goal")
self.send_feedback('RVIZ pose recieved!')
res = yield self.move.to_pose(target_pose).go(**action_kwargs)
elif command == 'circle':
self.send_feedback('Select a Publish Point in RVIZ')
target_point = yield util.wrap_time_notice(self.rviz_point.get_next_message(), 2, "Rviz point")
self.send_feedback('RVIZ point recieved!')
target_point = rosmsg_to_numpy(target_point.point)
distance = np.linalg.norm(target_point - self.pose[0])
target_point = rosmsg_to_numpy(target_point.point)
direction = 'cw' if argument == '-1' else 'ccw'
res = yield self.move.circle_point(target_point, direction=direction).go(radius=distance)
else:
shorthand = {"f": "forward", "b": "backward", "l": "left",
"r": "right", "yl": "yaw_left", "yr": "yaw_right"}
command = command if command not in shorthand.keys() else shorthand[command]
movement = getattr(self.move, command)
trans_move = command[:3] != "yaw"
unit = "m" if trans_move else "rad"
amount = argument
# See if there's a non standard unit at the end of the argument
if not argument[-1].isdigit():
last_digit_index = [i for i, c in enumerate(argument) if c.isdigit()][-1] + 1
amount = float(argument[:last_digit_index])
unit = argument[last_digit_index:]
# Get the kwargs to pass to the action server
station_hold = amount == '0'
if station_hold:
action_kwargs['move_type'] = 'hold'
msg = "Moving {} ".format(command) if trans_move else "Yawing {} ".format(command[4:])
self.send_feedback(msg + "{}{}".format(amount, unit))
res = yield movement(float(amount), unit).go(**action_kwargs)
if res.failure_reason is not '':
raise Exception('Move failed. Reason: {}'.format(res.failure_reason))
defer.returnValue('Move completed successfully!')
