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!')