Example #1
0
    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)
Example #2
0
    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!')
Example #3
0
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'
Example #4
0
    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)
Example #5
0
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
Example #6
0
    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!')