Ejemplo n.º 1
0
def main(args=None):
    rclpy.init(args=args)

    node = rclpy.create_node('minimal_client_async')

    cli = node.create_client(AddTwoInts, 'add_two_ints')

    req = AddTwoInts.Request()
    req.a = 41
    req.b = 1
    while not cli.wait_for_service(timeout_sec=1.0):
        node.get_logger().info('service not available, waiting again...')

    future = cli.call_async(req)
    while rclpy.ok():
        rclpy.spin_once(node)
        if future.done():
            if future.result() is not None:
                node.get_logger().info(
                    'Result of add_two_ints: for %d + %d = %d' %
                    (req.a, req.b, future.result().sum))
            else:
                node.get_logger().info('Service call failed %r' % (future.exception(),))
            break

    node.destroy_node()
    rclpy.shutdown()
Ejemplo n.º 2
0
def main(args=None):
    rclpy.init(args=args)

    node = rclpy.create_node('minimal_publisher')
    publisher = node.create_publisher(String, 'topic', 10)

    msg = String()
    i = 0

    def timer_callback():
        nonlocal i
        msg.data = 'Hello World: %d' % i
        i += 1
        node.get_logger().info('Publishing: "%s"' % msg.data)
        publisher.publish(msg)

    timer_period = 0.5  # seconds
    timer = node.create_timer(timer_period, timer_callback)

    rclpy.spin(node)

    # Destroy the timer attached to the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    node.destroy_timer(timer)
    node.destroy_node()
    rclpy.shutdown()
Ejemplo n.º 3
0
def talker(message_name, rmw_implementation, number_of_cycles):
    import rclpy
    from rclpy.qos import qos_profile_default
    from rclpy.impl.rmw_implementation_tools import select_rmw_implementation
    from message_fixtures import get_test_msg

    message_pkg = 'test_communication'
    module = importlib.import_module(message_pkg + '.msg')
    msg_mod = getattr(module, message_name)

    select_rmw_implementation(rmw_implementation)

    rclpy.init([])

    node = rclpy.create_node('talker')

    chatter_pub = node.create_publisher(
        msg_mod, 'test_message_' + message_name, qos_profile_default)

    cycle_count = 0
    print('talker: beginning loop')
    msgs = get_test_msg(message_name)
    while rclpy.ok() and cycle_count < number_of_cycles:
        cycle_count += 1
        msg_count = 0
        for msg in msgs:
            chatter_pub.publish(msg)
            msg_count += 1
            print('publishing message #%d' % msg_count)
            time.sleep(0.1)
        time.sleep(1)
    rclpy.shutdown()
Ejemplo n.º 4
0
def main(args=None):
    global logger
    rclpy.init(args=args)

    node = rclpy.create_node('minimal_action_server')
    logger = node.get_logger()

    # Use a ReentrantCallbackGroup to enable processing multiple goals concurrently
    # Default goal callback accepts all goals
    # Default cancel callback rejects cancel requests
    action_server = ActionServer(
        node,
        Fibonacci,
        'fibonacci',
        execute_callback=execute_callback,
        cancel_callback=cancel_callback,
        callback_group=ReentrantCallbackGroup())

    # Use a MultiThreadedExecutor to enable processing goals concurrently
    executor = MultiThreadedExecutor()

    rclpy.spin(node, executor=executor)

    action_server.destroy()
    node.destroy_node()
    rclpy.shutdown()
Ejemplo n.º 5
0
def replier(service_name, number_of_cycles, namespace):
    from test_msgs.service_fixtures import get_test_srv
    import rclpy

    service_pkg = 'test_msgs'
    module = importlib.import_module(service_pkg + '.srv')
    srv_mod = getattr(module, service_name)

    rclpy.init(args=[])

    node = rclpy.create_node('replier', namespace=namespace)

    srv_fixtures = get_test_srv(service_name)

    chatter_callback = functools.partial(
        replier_callback, srv_fixtures=srv_fixtures)

    node.create_service(
        srv_mod, 'test/service/' + service_name, chatter_callback)

    spin_count = 1
    print('replier: beginning loop')
    while rclpy.ok() and spin_count < number_of_cycles:
        rclpy.spin_once(node, timeout_sec=2)
        spin_count += 1
        print('spin_count: ' + str(spin_count))
    node.destroy_node()
    rclpy.shutdown()
Ejemplo n.º 6
0
def talker(message_name, number_of_cycles, namespace):
    from test_msgs.message_fixtures import get_test_msg
    import rclpy

    message_pkg = 'test_msgs'
    module = importlib.import_module(message_pkg + '.msg')
    msg_mod = getattr(module, message_name)

    rclpy.init(args=[])

    node = rclpy.create_node('talker', namespace=namespace)

    chatter_pub = node.create_publisher(
        msg_mod, 'test/message/' + message_name)

    cycle_count = 0
    print('talker: beginning loop')
    msgs = get_test_msg(message_name)
    while rclpy.ok() and cycle_count < number_of_cycles:
        msg_count = 0
        for msg in msgs:
            chatter_pub.publish(msg)
            msg_count += 1
            print('publishing message #%d' % msg_count)
            time.sleep(0.01)
        cycle_count += 1
        time.sleep(0.1)
    node.destroy_node()
    rclpy.shutdown()
Ejemplo n.º 7
0
def main(args=None):
    rclpy.init(args)

    node = rclpy.create_node("minimal_client_async")

    cli = node.create_client(AddTwoInts, "add_two_ints")

    req = AddTwoInts.Request()
    req.a = 41
    req.b = 1
    # TODO(mikaelarguedas) remove this once wait for service implemented
    # wait for connection to be established
    # (no wait for service in Python yet)
    time.sleep(1)
    cli.call(req)
    while rclpy.ok():
        # TODO(mikaelarguedas) This is not the final API, and this does not scale
        # for multiple pending requests. This will change once an executor model is implemented
        # In the future the response will not be stored in cli.response
        if cli.response is not None:
            print("Result of add_two_ints: for %d + %d = %d" % (req.a, req.b, cli.response.sum))
            break
        rclpy.spin_once(node)

    rclpy.shutdown()
Ejemplo n.º 8
0
 def run(self):
     self.node = rclpy.create_node('topic_monitor')
     try:
         run_topic_listening(self.node, self.topic_monitor, self.options)
     except KeyboardInterrupt:
         self.stop()
         raise
Ejemplo n.º 9
0
def main(argv=sys.argv[1:]):
    parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
    parser.add_argument(
        '--reliable', dest='reliable', action='store_true',
        help='set qos profile to reliable')
    parser.set_defaults(reliable=False)
    parser.add_argument(
        '-n', '--number_of_cycles', type=int, default=20,
        help='number of sending attempts')
    args = parser.parse_args(argv)
    rclpy.init()

    if args.reliable:
        custom_qos_profile = qos_profile_default
        print('Reliable publisher')
    else:
        custom_qos_profile = qos_profile_sensor_data
        print('Best effort publisher')

    node = rclpy.create_node('talker_qos')

    chatter_pub = node.create_publisher(String, 'chatter_qos', custom_qos_profile)

    msg = String()

    cycle_count = 0
    while rclpy.ok() and cycle_count < args.number_of_cycles:
        msg.data = 'Hello World: {0}'.format(cycle_count)
        print('Publishing: "{0}"'.format(msg.data))
        chatter_pub.publish(msg)
        cycle_count += 1
        sleep(1)
Ejemplo n.º 10
0
def main(args=None):
    rclpy.init(args=args)
    node = rclpy.create_node('minimal_client')
    cli = node.create_client(AddTwoInts, 'add_two_ints')

    req = AddTwoInts.Request()
    req.a = 41
    req.b = 1
    # TODO(mikaelarguedas) remove this once wait for service implemented
    # wait for connection to be established
    # (no wait for service in Python yet)
    time.sleep(1)

    cli.call(req)
    # when calling wait for future
    # spin should not be called in the main loop
    cli.wait_for_future()
    # TODO(mikaelarguedas) This is not the final API, and this does not scale
    # for multiple pending requests. This will change once an executor model is implemented
    # In the future the response will not be stored in cli.response
    print(
        'Result of add_two_ints: for %d + %d = %d' %
        (req.a, req.b, cli.response.sum))

    node.destroy_node()
    rclpy.shutdown()
Ejemplo n.º 11
0
def func_destroy_timers():
    import rclpy
    rclpy.init()

    node = rclpy.create_node('test_node3')

    timer1 = node.create_timer(0.1, None)
    timer2 = node.create_timer(1, None)
    timer2  # noqa

    assert 2 == len(node.timers)

    assert node.destroy_timer(timer1) is True

    assert 1 == len(node.timers)
    try:
        assert node.destroy_node() is True
    except SystemError:
        ret = False
    else:
        assert 0 == len(node.timers)
        assert node.handle is None
        ret = True
    finally:
        rclpy.shutdown()
        return ret
Ejemplo n.º 12
0
def func_destroy_node_twice():
    import rclpy
    rclpy.init()
    node = rclpy.create_node('test_node2')
    assert node.destroy_node() is True
    assert node.destroy_node() is True
    return True
Ejemplo n.º 13
0
def main(argv=sys.argv[1:]):
    parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
    parser.add_argument(
        '--reliable', dest='reliable', action='store_true',
        help='set qos profile to reliable')
    parser.set_defaults(reliable=False)
    parser.add_argument(
        '-n', '--number_of_cycles', type=int, default=20,
        help='max number of spin_once iterations')
    args = parser.parse_args(argv)
    rclpy.init()

    if args.reliable:
        custom_qos_profile = qos_profile_default
        print('Reliable listener')
    else:
        custom_qos_profile = qos_profile_sensor_data
        print('Best effort listener')

    node = rclpy.create_node('listener_qos')

    sub = node.create_subscription(String, 'chatter_qos', chatter_callback, custom_qos_profile)

    assert sub  # prevent unused warning

    cycle_count = 0
    while rclpy.ok() and cycle_count < args.number_of_cycles:
        rclpy.spin_once(node)
        cycle_count += 1
Ejemplo n.º 14
0
def _rostopic_echo_test(topic):
    rclpy.init(None)
    node = rclpy.create_node('rostopic')
    # TODO FIXME no path resolving for topics yet implemented thereby the initial "/" is
    # omited.
    sub = node.create_subscription(String, topic[1:], chatter_callback, qos_profile_default)
    while rclpy.ok():
       rclpy.spin_once(node)
Ejemplo n.º 15
0
def get_state(lifecycle_node):
    node = rclpy.create_node('lc_client_py')

    cli = node.create_client(GetState, lifecycle_node + '__get_state')
    req = GetState.Request()
    cli.call(req)
    cli.wait_for_future()
    print('%s is in state %s(%u)'
          % (lifecycle_node, cli.response.current_state.label, cli.response.current_state.id))
Ejemplo n.º 16
0
def main(argv=sys.argv):
    rclpy.init(args=argv)

    node = rclpy.create_node(
        'initial_params_node', namespace='/', allow_undeclared_parameters=True)
    rclpy.spin(node)

    rclpy.shutdown()
    return 0
Ejemplo n.º 17
0
def get_available_states(lifecycle_node):
    node = rclpy.create_node('lc_client_py')

    cli = node.create_client(GetAvailableStates, lifecycle_node + '__get_available_states')
    req = GetAvailableStates.Request()
    cli.call(req)
    cli.wait_for_future()
    print('%s has %u available states' % (lifecycle_node, len(cli.response.available_states)))
    for state in cli.response.available_states:
        print('id: %u\tlabel: %s' % (state.id, state.label))
Ejemplo n.º 18
0
def main(args=None):
    rclpy.init(args)

    node = rclpy.create_node('add_two_ints_server')
    minimal_service = MinimalService(node)
    minimal_service  # prevent unused variable warning

    while rclpy.ok():
        rclpy.spin_once(node)

    rclpy.shutdown()
Ejemplo n.º 19
0
def node_fixture(request):
    """Create a fixture with a node and helper executable."""
    rclpy.init()
    node = rclpy.create_node('tests_yaml', allow_undeclared_parameters=True)
    try:
        yield {
            'node': node,
            'executable': request.param,
        }
    finally:
        node.destroy_node()
        rclpy.shutdown()
Ejemplo n.º 20
0
def func_destroy_node():
    import rclpy
    rclpy.init()
    node = rclpy.create_node('test_node1')
    ret = True
    try:
        node.destroy_node()
    except RuntimeError:
        ret = False
    finally:
        rclpy.shutdown()
        return ret
Ejemplo n.º 21
0
def main(args=None):
    if args is None:
        args = sys.argv

    rclpy.init(args)

    node = rclpy.create_node('listener')

    sub = node.create_subscription(String, 'chatter', chatter_callback, qos_profile_default)
    assert sub  # prevent unused warning

    rclpy.spin(node)
Ejemplo n.º 22
0
def func_corrupt_node_handle():
    import rclpy
    rclpy.init()
    node = rclpy.create_node('test_node5')
    try:
        node.handle = 'garbage'
        ret = False
    except AttributeError:
        node.destroy_node()
        ret = True
    finally:
        rclpy.shutdown()
        return ret
Ejemplo n.º 23
0
def func_destroy_corrupted_node():
    import rclpy
    rclpy.init()
    node = rclpy.create_node('test_node2')
    assert node.destroy_node() is True
    node._handle = 'garbage'
    ret = False
    try:
        node.destroy_node()
    except SystemError:
        ret = True
    finally:
        rclpy.shutdown()
        return ret
Ejemplo n.º 24
0
def main(args=None):
    rclpy.init(args)

    node = rclpy.create_node('add_two_ints_server')

    srv = node.create_service(AddTwoInts, 'add_two_ints', add_two_ints_callback)
    while rclpy.ok():
        rclpy.spin_once(node)

    # Destroy the service attached to the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    node.destroy_service(srv)
    rclpy.shutdown()
Ejemplo n.º 25
0
def main(args=None):
    rclpy.init(args)

    node = rclpy.create_node('minimal_publisher')

    minimal_subscriber = MinimalSubscriber(node)
    minimal_subscriber  # prevent unused variable warning
    while rclpy.ok():
        rclpy.spin_once(node)

    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    node.destroy_node()
    rclpy.shutdown()
Ejemplo n.º 26
0
def main(args=None):
    rclpy.init(args=args)

    node = rclpy.create_node('minimal_subscriber')

    subscription = node.create_subscription(
        String, 'topic', lambda msg: print('I heard: [%s]' % msg.data))
    subscription  # prevent unused variable warning

    rclpy.spin(node)

    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    node.destroy_node()
    rclpy.shutdown()
Ejemplo n.º 27
0
def main(args=None):
    rclpy.init(args=args)

    node = rclpy.create_node('minimal_subscriber')

    subscription = node.create_subscription(String, 'topic', chatter_callback)
    subscription  # prevent unused variable warning

    while rclpy.ok():
        rclpy.spin_once(node)

    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    node.destroy_node()
    rclpy.shutdown()
Ejemplo n.º 28
0
def main(args=None):
    if args is None:
        args = sys.argv

    rclpy.init(args)

    # TODO(wjwwood): move this import back to the top of the file when
    # it is possible to import the messages before rclpy.init().
    from std_msgs.msg import String

    node = rclpy.create_node('listener')

    sub = node.create_subscription(String, 'chatter', chatter_callback, qos_profile_default)
    assert sub  # prevent unused warning

    while rclpy.ok():
        rclpy.spin_once(node)
Ejemplo n.º 29
0
def get_available_transitions(lifecycle_node):
    node = rclpy.create_node('lc_client_py')

    cli = node.create_client(
        GetAvailableTransitions, lifecycle_node + '__get_available_transitions')
    req = GetAvailableTransitions.Request()
    cli.call(req)
    cli.wait_for_future()
    print('%s has %u available transitions'
          % (lifecycle_node, len(cli.response.available_transitions)))
    for transition in cli.response.available_transitions:
        print('Transition id: %u\tlabel: %s'
              % (transition.transition.id, transition.transition.label))
        print('\tStart id: %u\tlabel: %s'
              % (transition.start_state.id, transition.start_state.label))
        print('\tGoal  id: %u\tlabel: %s'
              % (transition.goal_state.id, transition.goal_state.label))
Ejemplo n.º 30
0
def main(args=None):
    rclpy.init(args=args)
    node = rclpy.create_node('minimal_client')
    # Node's default callback group is mutually exclusive. This would prevent the client response
    # from being processed until the timer callback finished, but the timer callback in this
    # example is waiting for the client response
    cb_group = ReentrantCallbackGroup()
    cli = node.create_client(AddTwoInts, 'add_two_ints', callback_group=cb_group)
    did_run = False
    did_get_result = False

    async def call_service():
        nonlocal cli, node, did_run, did_get_result
        did_run = True
        try:
            req = AddTwoInts.Request()
            req.a = 41
            req.b = 1
            future = cli.call_async(req)
            result = await future
            if result is not None:
                node.get_logger().info(
                    'Result of add_two_ints: for %d + %d = %d' %
                    (req.a, req.b, result.sum))
            else:
                node.get_logger().info('Service call failed %r' % (future.exception(),))
        finally:
            did_get_result = True

    while not cli.wait_for_service(timeout_sec=1.0):
        node.get_logger().info('service not available, waiting again...')

    timer = node.create_timer(0.5, call_service, callback_group=cb_group)

    while rclpy.ok() and not did_run:
        rclpy.spin_once(node)

    if did_run:
        # call timer callback only once
        timer.cancel()

    while rclpy.ok() and not did_get_result:
        rclpy.spin_once(node)

    node.destroy_node()
    rclpy.shutdown()
Ejemplo n.º 31
0
 def test_create_node_with_relative_namespace(self):
     node_name = 'create_node_test'
     namespace = 'ns'
     node = rclpy.create_node(node_name, namespace=namespace)
     self.assertEqual('/ns', node.get_namespace())
Ejemplo n.º 32
0
def main(args=None):
    rclpy.init(args=args)

    node = rclpy.create_node('keyboard_input3')
    publisher = node.create_publisher(Twist, 'turtle1/cmd_vel', 10)

    def get_keys():  #gets keyboard input
        key = ''
        k = ord(getch.getch())  #converts keypress to ord value
        if (k == 119):
            key = 'up'  #up
        elif (k == 115):
            key = 'down'  #down
        elif (k == 97):
            key = 'left'  #left
        elif (k == 100):
            key = 'right'  #right
        elif (k == 113):
            key = 'left_fwd'  #turn left and forward
        elif (k == 101):
            key = 'right_fwd'  #turn right and forward
        elif (k == 114):
            key = 'accel'  #accelerate
        elif (k == 116):
            key = 'deccel'  #deccelerate
        elif (k == 27):
            sys.exit("Exited Progam")
        else:
            key = ''
        #rospy.loginfo(str(key)) #write val to terminal
        return key

    def velocityOut(speed, turn):  #for printing/getting speed & turn angle
        return "speed & turn angle: \tspeed %s\tturn %s " % (speed, turn)

    def keyboard_input():
        speed = 0.5  #default speed val
        turn = 1.0  #default turn val
        x = 0
        y = 0
        z = 0
        th = 0
        status = 0

        while (1):
            input = get_keys()
            if (input == 'up'):  #up
                x = 1
                y = 0
                z = 0
                th = 0
            elif (input == 'down'):  #down
                x = -1
                y = 0
                z = 0
                th = 0
            elif (input == 'left'):  #left
                x = 0
                y = 0
                z = 0
                th = 1
            elif (input == 'right'):  #right
                x = 0
                y = 0
                z = 0
                th = -1
            elif (input == 'left_fwd'):  #left + forward
                x = 1
                y = 0
                z = 0
                th = 1
            elif (input == 'right_fwd'):  #right + forward
                x = 1
                y = 0
                z = 0
                th = -1
            elif (input == 'accel'):  #accelerate by 10%
                x = 0
                y = 0
                z = 0
                th = 0
                speed += 0.1
            elif (input == 'deccel'):  #deccelerate by 10%
                x = 0
                y = 0
                z = 0
                th = 0
                speed -= 0.1
            else:
                x = 0
                y = 0
                z = 0
                th = 0

            twist = Twist()
            twist.linear.x = x * speed
            twist.linear.y = y * speed
            twist.linear.z = z * speed
            twist.angular.x = 0.0
            twist.angular.y = 0.0
            twist.angular.z = th * turn
            publisher.publish(twist)
            print(velocityOut(speed,
                              twist.angular.z))  #write speed/angle to terminal

    print(directionInfo)
    keyboard_input()

    rclpy.spin(node)

    node.destroy_node()
    rclpy.shutdown()
Ejemplo n.º 33
0
from time import sleep
import rclpy
from std_msgs.msg import String
from CopyNet import predict_continuous
from DataPreProcessing import frame_parse, json_lint, assemble_graph, FIFOBuffer

# Press the green button in the gutter to run the script.
if __name__ == '__main__':
    nl_input = "go to the door and turn left"
    parsed = frame_parse(nl_input)

    rclpy.init()
    node = rclpy.create_node('semantic_parser')
    pub = node.create_publisher(String, 'talker', 10)
    msg = String()

    while rclpy.ok():

        decoded_output = []
        for x in parsed:
            output = predict_continuous(x)
            linted_output = json_lint(output)
            decoded_output.append(linted_output)
        graph = assemble_graph(decoded_output)
        msg.data = str(graph)
        pub.publish(msg)
        sleep(0.5)

Ejemplo n.º 34
0
    def __init__(self):
        """
        Initialize the MARA environemnt
        """
        # Manage command line args
        args = ut_generic.getParserArgsRobot().parse_args()
        self.gzclient = args.gzclient
        self.realSpeed = args.realSpeed
        # self.realSpeed = True
        self.debug = args.debug
        self.multiInstance = args.multiInstance
        self.port = args.port
        # Set the path of the corresponding URDF file
        if self.realSpeed:
            urdf = "biped.urdf"
            self.urdfPath = get_prefix_path(
                "lobot_description"
            ) + "/share/lobot_description/robots/" + urdf
        else:
            print("Non real speed not yet supported. Use real speed instead. ")

        # TODO: Include launch logic here, refer to code from the .launch.py files
        # Note that after including the launch logic the code will no longer be debuggable due to multi process stuff

        # Create the node after the new ROS_DOMAIN_ID is set in generate_launch_description()
        rclpy.init()
        self.node = rclpy.create_node(self.__class__.__name__)

        # class variables
        self._observation_msg = None
        self.max_episode_steps = 1024  # default value, can be updated from baselines
        self.iterator = 0
        self.reset_jnts = True
        self._collision_msg = None

        #############################
        #   Environment hyperparams
        #############################
        EE_POINTS = np.asmatrix([[0, 0, 0]])
        EE_VELOCITIES = np.asmatrix([[0, 0, 0]])

        # # Topics for the robot publisher and subscriber.
        JOINT_PUBLISHER = '/lobot_arm/control'
        # Get Joint names from the parameter server
        get_joints_client = self.node.create_client(
            GetAllJoints,
            "/GetAllControlJoints",
            qos_profile=qos_profile_services_default)
        req = GetAllJoints.Request()
        req.robot = "lobot_arm"
        while not get_joints_client.wait_for_service(timeout_sec=3.0):
            self.node.get_logger().info(
                'service not available, waiting again...')

        future = get_joints_client.call_async(req)
        rclpy.spin_until_future_complete(self.node, future)
        if future.result() is not None:
            joint_names = future.result().joints
            self.node.get_logger().info('Number of joints: %d' %
                                        (len(joint_names)))
        else:
            self.node.get_logger().info('Service call failed %r' %
                                        (future.exception(), ))
        JOINT_ORDER = joint_names
        INITIAL_JOINTS = np.full((len(joint_names)), 0.0).tolist()
        reset_condition = {
            'initial_positions': INITIAL_JOINTS,
            'initial_velocities': []
        }
        #############################

        m_jointOrder = copy.deepcopy(JOINT_ORDER)

        # Initialize target end effector position
        self.environment = {
            'jointOrder': m_jointOrder,
            'reset_conditions': reset_condition,
            'tree_path': self.urdfPath,
            'end_effector_points': EE_POINTS,
        }

        # Subscribe to the appropriate topics, taking into account the particular robot
        self._pub = self.node.create_publisher(
            JointControl, JOINT_PUBLISHER, qos_profile=qos_profile_sensor_data)
        self._sub = self.node.create_subscription(JointState, "/joint_states",
                                                  self.observation_callback,
                                                  qos_profile_sensor_data)

        # TODO: Make the clock node run on a separate thread so weird issues like outdated clock can stop happening
        self.lock = threading.Lock()
        self.clock_node = rclpy.create_node(self.__class__.__name__ + "_clock")
        self._sub_clock = self.clock_node.create_subscription(
            RosClock,
            '/clock',
            self.clock_callback,
            qos_profile=qos_profile_sensor_data)
        self.exec = rclpy.executors.MultiThreadedExecutor()
        self.exec.add_node(self.clock_node)
        t1 = threading.Thread(target=self.spinClockNode, daemon=True)
        t1.start()
        # self._imu_sub = self.node.create_subscription(JointState, "/lobot_IMU_controller/out", self.imu_callback, qos_profile_sensor_data)
        # self._sub = self.node.create_subscription(JointTrajectoryControllerState, JOINT_SUBSCRIBER, self.observation_callback, qos_profile=qos_profile_sensor_data)
        self._reset_sim = self.node.create_client(Empty, '/reset_simulation')
        self._physics_pauser = self.node.create_client(Empty, '/pause_physics')
        self._robot_resetter = self.node.create_client(Empty,
                                                       '/lobot_arm/reset')
        self._physics_unpauser = self.node.create_client(
            Empty, '/unpause_physics')
        self.delete_entity = self.node.create_client(DeleteEntity,
                                                     '/delete_entity')
        self.numJoints = len(JOINT_ORDER)
        # Initialize a KDL Jacobian solver from the chain.
        # self.jacSolver = ChainJntToJacSolver(self.mara_chain)

        # Observable dimensions, each joint has 2 (joint position + joint velocity), the IMU gives 6
        self.obs_dim = self.numJoints * 2 + 6

        # # Here idially we should find the control range of the robot. Unfortunatelly in ROS/KDL there is nothing like this.
        # # I have tested this with the mujoco enviroment and the output is always same low[-1.,-1.], high[1.,1.]

        low = -np.pi * np.ones(self.numJoints) * 0.4
        high = np.pi * np.ones(self.numJoints) * 0.4

        self.action_space = spaces.Box(low, high)

        high = np.inf * np.ones(self.obs_dim)
        low = -high
        self.observation_space = spaces.Box(low, high)

        self.seed()
        self.buffer_dist_rewards = []
        self.buffer_tot_rewards = []
        self.collided = 0

        # Set the time source
        self._sim_time = 0
        self._sim_time_msg = builtin_interfaces.msg.Time()
def robot_description_client(init_node):
    return RobotDescriptionClient(
        rclpy.create_node('robot_description_client'))
Ejemplo n.º 36
0
 def test_create_node_with_namespace(self):
     node_name = 'create_node_test'
     namespace = '/ns'
     rclpy.create_node(node_name, namespace=namespace)
Ejemplo n.º 37
0
 def test_initially_no_executor(self):
     node = rclpy.create_node('my_node', context=self.context)
     try:
         assert node.executor is None
     finally:
         node.destroy_node()
Ejemplo n.º 38
0
def main():
    # A flow of typical ROS program
    # 1. Initialization
    rclpy.init()
    # 2. Create one or more nodes
    node = rclpy.create_node('autopilot')
    cb_group = rclpy.callback_groups.ReentrantCallbackGroup()
    client = node.create_client(GetPosition,
                                'get_position',
                                callback_group=cb_group)
    ready = False
    while not ready:
        node.get_logger().info('waiting for the service availablity.')
        ready = client.wait_for_service(timeout_sec=1.0)

    current_position = GetPosition.Response()

    async def execute_callback(goal_handle):
        """Generate autopilot sequence"""
        nonlocal current_position
        # align x-axis position
        goal_head = 'east' if current_position.x < goal_handle.request.goal_x else 'west'

        feedback = AutoPilot.Feedback()
        feedback.partial_cmds = []
        while current_position.head != goal_head:
            feedback.partial_cmds.append(cmd['turn_left'])
            node.get_logger().info('auto steering: turn left')
            goal_handle.publish_feedback(feedback)
            if goal_handle.is_cancel_requested:
                node.get_logger().info('Goal canceled')
                goal_handle.canceled()
                return AutoPilot.Result()
            time.sleep(1)
            req = GetPosition.Request()
            current_position = await client.call_async(req)

        while current_position.x != goal_handle.request.goal_x:
            feedback.partial_cmds.append(cmd['move_forward'])
            node.get_logger().info('auto steering: move forward')
            goal_handle.publish_feedback(feedback)
            if goal_handle.is_cancel_requested:
                node.get_logger().info('Goal canceled')
                goal_handle.canceled()
                return AutoPilot.Result()
            time.sleep(1)
            req = GetPosition.Request()
            current_position = await client.call_async(req)

        goal_head = 'north' if current_position.y < goal_handle.request.goal_y else 'south'
        while current_position.head != goal_head:
            feedback.partial_cmds.append(cmd['turn_left'])
            node.get_logger().info('auto steering: turn left')
            goal_handle.publish_feedback(feedback)
            if goal_handle.is_cancel_requested:
                node.get_logger().info('Goal canceled')
                goal_handle.canceled()
                return AutoPilot.Result()
            time.sleep(1)
            req = GetPosition.Request()
            current_position = await client.call_async(req)

        while current_position.y != goal_handle.request.goal_y:
            feedback.partial_cmds.append(cmd['move_forward'])
            node.get_logger().info('auto steering: move forward')
            goal_handle.publish_feedback(feedback)
            if goal_handle.is_cancel_requested:
                node.get_logger().info('Goal canceled')
                goal_handle.canceled()
                return AutoPilot.Result()

            time.sleep(1)
            req = GetPosition.Request()
            current_position = await client.call_async(req)

        result = AutoPilot.Result()
        result.cmds = feedback.partial_cmds
        goal_handle.succeed()
        return result

    async def goal_callback(goal_request):
        """Accepts or rejects a client request to begin an action."""
        nonlocal current_position
        # request GetPosition service
        req = GetPosition.Request()
        future = client.call_async(req)
        try:
            current_position = await future
        except Exception as e:
            node.get_logger().info('Destination goal failed: {}'.format(e))
            return rclpy.action.GoalResponse.REJECT
        node.get_logger().info('Destination goal accepted!')
        return rclpy.action.GoalResponse.ACCEPT

    def cancel_callback(goal_handle):
        """Accepts or rejects a client request to cancel an action."""
        node.get_logger().info('Destination goal canceled!')
        return rclpy.action.CancelResponse.ACCEPT

    # 3. Process node callbacks
    action_server = rclpy.action.ActionServer(
        node,
        AutoPilot,
        'autopilot',
        execute_callback=execute_callback,
        callback_group=cb_group,
        goal_callback=goal_callback,
        cancel_callback=cancel_callback)
    node.get_logger().info('Action server in action!')

    rclpy.spin(node, executor=rclpy.executors.MultiThreadedExecutor())
    # 4. Shutdown
    action_server.destroy()
    node.destroy_node()
    rclpy.shutdown()
Ejemplo n.º 39
0
 def setUpClass(cls):
     rclpy.init()
     cls.node = rclpy.create_node('TestClient')
Ejemplo n.º 40
0
import rclpy
from rclpy.qos import qos_profile_sensor_data
from hrim_actuator_rotaryservo_msgs.msg import StateRotaryServo


# Function that will be called once a message is published to the topic we are subscribed
def minimal_callback(msg):
    print('Position:', str(msg.position))


# -------- #

rclpy.init(args=None)

# Create Node with name "mara_minimal_subscriber"
node = rclpy.create_node('mara_minimal_subscriber')

# Subscribe to topic "/hrim_actuation_servomotor_000000000001/state_axis1" and link it to "minimal_callback" function
node.create_subscription(StateRotaryServo,
                         '/hrim_actuator_rotaryservo_000000000001/state_axis1',
                         minimal_callback,
                         qos_profile=qos_profile_sensor_data
                         )  # QoS profile for reading (joint) sensors

# Spin listening to all subscribed topics
rclpy.spin(node)

node.destroy_node()
rclpy.shutdown()
Ejemplo n.º 41
0
def main():
    rclpy.init()

    sim_time_param = is_sim_time()

    node = rclpy.create_node(
        'send_waypoint_file',
        allow_undeclared_parameters=True,
        automatically_declare_parameters_from_overrides=True,
        parameter_overrides=[sim_time_param])

    node.get_logger().info('Send a waypoint file, namespace=%s' %
                           node.get_namespace())

    if node.has_parameter('filename'):
        filename = node.get_parameter(
            'filename').get_parameter_value().string_value
    else:
        raise RuntimeError('No filename found')

    # Important...ensure the clock has been updated when using sim time
    while node.get_clock().now() == rclpy.time.Time(
            clock_type=node.get_clock().clock_type):
        rclpy.spin_once(node)

    # If no start time is provided: start *now*.
    start_time = time_in_float_sec(node.get_clock().now())
    start_now = True
    if node.has_parameter('start_time'):
        start_time = node.get_parameter('start_time').value
        if start_time < 0.0:
            node.get_logger().warn('Negative start time, setting it to 0.0')
            start_time = 0.0
            start_now = True
        else:
            start_now = False

    node.get_logger().info('Start time=%.2f s' % start_time)

    interpolator = node.get_parameter_or(
        'interpolator', 'lipb').get_parameter_value().string_value

    srv_name = 'init_waypoints_from_file'

    init_wp = node.create_client(InitWaypointsFromFile, srv_name)

    ready = init_wp.wait_for_service(timeout_sec=30)
    if not ready:
        raise RuntimeError('Service not available! Closing node...')

    (sec, nsec) = float_sec_to_int_sec_nano(start_time)

    req = InitWaypointsFromFile.Request()
    req.start_time = rclpy.time.Time(seconds=sec, nanoseconds=nsec).to_msg()
    req.start_now = start_now
    req.filename = String(data=filename)
    req.interpolator = String(data=interpolator)

    future = init_wp.call_async(req)

    rclpy.spin_until_future_complete(node, future)
    try:
        response = future.result()
    except Exception as e:
        node.get_logger().error('Service call ' + srv_name +
                                ' failed, error=' + repr(e))
    else:
        node.get_logger().info('Waypoints file successfully received, '
                               'filename=%s' % filename)

    node.destroy_node()
Ejemplo n.º 42
0
 def setUp(self):
     # Create a ROS node for tests
     self.node = rclpy.create_node(
         'test_thrusters_allocator',
         allow_undeclared_parameters=True,
         automatically_declare_parameters_from_overrides=True)
Ejemplo n.º 43
0
def main():
    rclpy.init()

    sim_time_param = is_sim_time()

    node = rclpy.create_node(
        'set_thrusters_states',
        allow_undeclared_parameters=True,
        automatically_declare_parameters_from_overrides=True,
        parameter_overrides=[sim_time_param])

    node.get_logger().info(
        'Set the state of thrusters for vehicle, namespace=' +
        node.get_namespace())

    thread = threading.Thread(target=rclpy.spin, args=(node, ), daemon=True)
    thread.start()

    # Make sure the clock has been updated when using sim time
    while node.get_clock().now() == rclpy.time.Time(
            clock_type=node.get_clock().clock_type):
        pass

    starting_time = 0.0
    if node.has_parameter('starting_time'):
        starting_time = node.get_parameter('starting_time').value
    if starting_time < 0:
        starting_time = time_in_float_sec(node.get_clock().now())

    node.get_logger().info('Starting time={} s'.format(starting_time))

    duration = 0.0
    if node.has_parameter('duration'):
        duration = node.get_parameter('duration').value

    if duration == 0.0:
        raise RuntimeError('Duration not set, leaving node...')

    node.get_logger().info('Duration [s]=%s' %
                           ('Inf.' if duration < 0 else str(duration)))

    if node.has_parameter('is_on'):
        is_on = bool(
            node.get_parameter('is_on').get_parameter_value().bool_value)
    else:
        raise RuntimeError('is_on state flag not provided')

    thruster_id = -1
    if node.has_parameter('thruster_id'):
        if node.get_parameter(
                'thruster_id').type_ == rclpy.Parameter.Type.INTEGER:
            thruster_id = node.get_parameter(
                'thruster_id').get_parameter_value().integer_value
    else:
        raise RuntimeError('Thruster ID not given')

    if thruster_id < 0:
        raise RuntimeError('Invalid thruster ID')

    node.get_logger().info('Setting state of thruster #{} as {}'.format(
        thruster_id, 'ON' if is_on else 'OFF'))

    vehicle_name = node.get_namespace().replace('/', '')

    srv_name = build_service_name(vehicle_name, thruster_id,
                                  'set_thruster_state')

    try:
        set_state = node.create_client(SetThrusterState, srv_name)
    except Exception as e:
        raise RuntimeError('Service call failed, error=' + str(e))

    if not set_state.wait_for_service(timeout_sec=2):
        raise RuntimeError('Service %s not available! Closing node...' %
                           (srv_name))

    FREQ = 100
    rate = node.create_rate(FREQ)

    while time_in_float_sec(node.get_clock().now()) < starting_time:
        # Just a guard for really short timeouts
        if 1.0 / FREQ < starting_time:
            rate.sleep()

    # First request
    req = SetThrusterState.Request()
    req.on = is_on

    future = set_state.call_async(req)

    # NB : spining is done from another thread
    while not future.done():
        pass

    try:
        response = future.result()
    except Exception as e:
        node.get_logger().error('Service call ' + srv_name +
                                ' failed, error=' + repr(e))
    else:
        if response.success:
            node.get_logger().info('Time={} s'.format(
                time_in_float_sec(node.get_clock().now())))
            node.get_logger().info('Current state of thruster #{}={}'.format(
                thruster_id, 'ON' if is_on else 'OFF'))
        else:
            node.get_logger().error('Service call ' + srv_name +
                                    ' returned a "failed" value')

    # Returning to previous state
    if duration > 0:
        while time_in_float_sec(
                node.get_clock().now()) < starting_time + duration:
            if 1.0 / FREQ < starting_time + duration:
                rate.sleep()

        req.on = not is_on
        success = set_state.call(req)

        while not future.done():
            pass

        try:
            response = future.result()
        except Exception as e:
            node.get_logger().error('Service call ' + srv_name +
                                    ' failed, error=' + str(e))
        else:
            if response.success:
                node.get_logger().info('Time={} s'.format(
                    time_in_float_sec(node.get_clock().now())))
                node.get_logger().info(
                    'Returning to previous state of thruster #{}={}'.format(
                        thruster_id, 'ON' if not is_on else 'OFF'))
            else:
                node.get_logger().error('Service call ' + srv_name +
                                        ' returned a "failed" value')

    node.get_logger().info('Leaving node...')

    node.destroy_node()
    rclpy.shutdown()
    thread.join()
def main():

    settings = termios.tcgetattr(sys.stdin)
    rclpy.init()

    qos = QoSProfile(depth=10)
    node = rclpy.create_node('teleop_keyboard')
    pub = node.create_publisher(Twist, 'cmd_vel', qos)

    idx = 0
    status = 0
    target_linear_velocity = 0.0
    target_angular_velocity = 0.0
    control_linear_velocity = 0.0
    control_angular_velocity = 0.0

    try:
        print(msg)
        while (1):

            key = get_key(settings, idx)
            print(key)
            idx = idx + 1
            if key == 'w':
                #target_linear_velocity =\
                #    check_linear_limit_velocity(target_linear_velocity + LIN_VEL_STEP_SIZE)
                target_linear_velocity = target_linear_velocity + 0.02
                status = status + 1
                print_vels(target_linear_velocity, target_angular_velocity)
            elif key == 'x':
                #target_linear_velocity =\
                #    check_linear_limit_velocity(target_linear_velocity - LIN_VEL_STEP_SIZE)
                target_linear_velocity = target_linear_velocity - 0.02
                status = status + 1
                print_vels(target_linear_velocity, target_angular_velocity)
            elif key == 'a':
                target_angular_velocity =\
                    check_angular_limit_velocity(target_angular_velocity + ANG_VEL_STEP_SIZE)
                status = status + 1
                print_vels(target_linear_velocity, target_angular_velocity)
            elif key == 'd':
                target_angular_velocity =\
                    check_angular_limit_velocity(target_angular_velocity - ANG_VEL_STEP_SIZE)
                status = status + 1
                print_vels(target_linear_velocity, target_angular_velocity)
            elif key == ' ' or key == 's':
                target_linear_velocity = 0.0
                control_linear_velocity = 0.0
                target_angular_velocity = 0.0
                control_angular_velocity = 0.0
                print_vels(target_linear_velocity, target_angular_velocity)
            else:
                if (key == '\x03'):
                    break

            if idx == 14:
                idx = 0

            if status == 20:
                print(msg)
                status = 0

            twist = Twist()

            control_linear_velocity = make_simple_profile(
                control_linear_velocity, target_linear_velocity,
                (LIN_VEL_STEP_SIZE / 2.0))

            twist.linear.x = control_linear_velocity
            twist.linear.y = 0.0
            twist.linear.z = 0.0

            control_angular_velocity = make_simple_profile(
                control_angular_velocity, target_angular_velocity,
                (ANG_VEL_STEP_SIZE / 2.0))

            twist.angular.x = 0.0
            twist.angular.y = 0.0
            twist.angular.z = control_angular_velocity

            pub.publish(twist)

    except Exception as e:
        print(e)

    finally:
        twist = Twist()
        twist.linear.x = 0.0
        twist.linear.y = 0.0
        twist.linear.z = 0.0

        twist.angular.x = 0.0
        twist.angular.y = 0.0
        twist.angular.z = 0.0

        pub.publish(twist)

        termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
Ejemplo n.º 45
0
def main():
    rclpy.init()

    sim_time_param = is_sim_time()

    node = rclpy.create_node(
        'set_body_wrench',
        allow_undeclared_parameters=True,
        automatically_declare_parameters_from_overrides=True,
        parameter_overrides=[sim_time_param])

    node.get_logger().info('Apply programmed perturbation to vehicle ' +
                           node.get_namespace())

    # sim_time = rclpy.parameter.Parameter('use_sim_time', rclpy.Parameter.Type.BOOL, True)
    # node.set_parameters([sim_time])

    starting_time = 0.0
    if node.has_parameter('starting_time'):
        starting_time = float(node.get_parameter('starting_time').value)

    #starting time_clock = node.get_clock().now() + rclpy.duration.Duration(nanoseconds = starting_time * 1e9)
    node.get_logger().info('Starting time in = {} s'.format(starting_time))

    duration = 0.0
    if node.has_parameter('duration'):
        duration = float(node.get_parameter('duration').value)

    #compare to eps ?
    if duration <= 0.0:
        node.get_logger().info('Duration not set, leaving node...')
        sys.exit(-1)

    node.get_logger().info('Duration [s]=' +
                           ('Inf.' if duration < 0 else str(duration)))

    force = [0.0, 0.0, 0.0]
    if node.has_parameter('force'):
        force = node.get_parameter('force').value
        #print(force)
        if len(force) != 3:
            raise RuntimeError('Invalid force vector')
        #Ensure type is float
        force = [float(elem) for elem in force]

    node.get_logger().info('Force [N]=' + str(force))

    torque = [0.0, 0.0, 0.0]
    if node.has_parameter('torque'):
        torque = node.get_parameter('torque').value
        if len(torque) != 3:
            raise RuntimeError('Invalid torque vector')
        #Ensure type is float
        torque = [float(elem) for elem in torque]

    node.get_logger().info('Torque [N]=' + str(torque))

    service_name = '/gazebo/apply_link_wrench'
    try:
        apply_wrench = node.create_client(ApplyLinkWrench, service_name)
    except Exception as e:
        node.get_logger().error('Creation of service ' + service_name +
                                ' failed, error=' + str(e))
        sys.exit(-1)

    try:
        ready = apply_wrench.wait_for_service(timeout_sec=10)
        if not ready:
            raise RuntimeError('')
    except:
        node.get_logger().error('Service ' + service_name +
                                ' not available! Closing node...')
        sys.exit(-1)

    ns = node.get_namespace().replace('/', '')

    body_name = '%s/base_link' % ns

    FREQ = 100
    rate = node.create_rate(
        FREQ
    )  #, rclpy.clock.Clock(clock_type=rclpy.clock.ClockType.STEADY_TIME))
    thread = threading.Thread(target=rclpy.spin, args=(node, ), daemon=True)
    thread.start()
    while time_in_float_sec(node.get_clock().now()) < starting_time:
        #rclpy.spin_once(node)
        #Just a guard for really short timeouts
        if 1.0 / FREQ < starting_time:
            rate.sleep()

    # time.sleep(starting_time)
    # if starting_time >= 0:
    #     rate = node.create_rate(100)
    #     while node.get_clock().now() < starting_time_clock:
    #         rate.sleep()

    apw = ApplyLinkWrench.Request()
    apw.link_name = body_name
    apw.reference_frame = 'world'
    apw.reference_point = Point(x=0.0, y=0.0, z=0.0)
    apw.wrench = Wrench()
    apw.wrench.force = Vector3(x=force[0], y=force[1], z=force[2])
    apw.wrench.torque = Vector3(x=torque[0], y=torque[1], z=torque[2])
    apw.start_time = node.get_clock().now().to_msg()
    apw.duration = rclpy.time.Duration(seconds=duration).to_msg()

    future = apply_wrench.call_async(apw)

    #NB : spining is done from another thread
    while rclpy.ok():
        if future.done():
            try:
                response = future.result()
            except Exception as e:
                node.get_logger().info('Could not apply link wrench %r' %
                                       (e, ))
            else:
                node.get_logger().info('Link wrench perturbation applied!')
                node.get_logger().info('\tFrame: ' + body_name)
                node.get_logger().info('\tDuration [s]: ' + str(duration))
                node.get_logger().info('\tForce [N]: ' + str(force))
                node.get_logger().info('\tTorque [Nm]: ' + str(torque))
            break
Ejemplo n.º 46
0
  res.response = '\n'.join(results)

  return res


# global
clients = []
cfg = ros2_config.get()

nodename = cfg[0]['name']

for i in cfg:
  clients.append(i['name'])

rclpy.init()
node = rclpy.create_node(nodename+"_master")

event_loop = asyncio.get_event_loop()

print("Starting master services:")
#print("  master/signup")
#s1 = node.create_service(StrReqStrRes, 'master/signup',  signup_callback)
print("  master/cmd_all")
s2 = node.create_service(StrReqStrRes, 'master/cmd_all', cmdall_callback)

rclpy.spin(node)

# Destroy the node explicitly
# (optional - Done automatically when node is garbage collected)
node.destroy_node()
rclpy.shutdown()
Ejemplo n.º 47
0
 def setUpClass(cls):
     cls.context = rclpy.context.Context()
     rclpy.init(context=cls.context)
     cls.node = rclpy.create_node(TEST_NODE,
                                  namespace=TEST_NAMESPACE,
                                  context=cls.context)
    node.destroy_node()


# ==========================================
#    返回页面
# ==========================================
@app.route('/')
def index():
    return render_template('index.html', async_mode=socketio.async_mode)


# ============================================================================================================================
# =====================================================================  others   ============================================

# ==========================================
#    init node
# ==========================================
rclpy.init()
node = rclpy.create_node('referee_web')

# ==========================================
#    init namespace class
# ==========================================
socketio.on_namespace(RefereeSocketHandler('/'))

if __name__ == '__main__':
    if len(sys.argv) == 3:
        robot_name = str(sys.argv[1])
        port = int(sys.argv[2])
    socketio.run(app, host='0.0.0.0', port=2350)
def main():
    rclpy.init()

    sim_time_param = is_sim_time()

    # Compared to ROS 1 version, the node name was changed
    node = rclpy.create_node(
        'start_helical_trajectory',
        allow_undeclared_parameters=True,
        automatically_declare_parameters_from_overrides=True,
        parameter_overrides=[sim_time_param])

    node.get_logger().info('Starting the helical trajectory creator')

    # Ensure the clock has been updated when using sim time
    while node.get_clock().now() == rclpy.time.Time(
            clock_type=node.get_clock().clock_type):
        rclpy.spin_once(node)

    # If no start time is provided: start *now*.
    start_time = time_in_float_sec(node.get_clock().now())
    start_now = False
    if node.has_parameter('start_time'):
        start_time = node.get_parameter('start_time').value
        if start_time < 0.0:
            node.get_logger().warn('Negative start time, setting it to 0.0')
            start_time = 0.0
            start_now = True
    else:
        start_now = True

    param_labels = [
        'radius', 'center', 'n_points', 'heading_offset', 'duration',
        'n_turns', 'delta_z', 'max_forward_speed'
    ]
    params = dict()

    for label in param_labels:
        if not node.has_parameter(label):
            node.get_logger().error(
                '{} must be provided for the trajectory generation!'.format(
                    label))
            sys.exit(-1)

        params[label] = node.get_parameter(label).value

    if len(params['center']) != 3:
        raise RuntimeError(
            'Center of circle must have 3 components (x, y, z):' +
            str(params['center']))

    if params['n_points'] <= 2:
        raise RuntimeError('Number of points must be at least 2' +
                           str(params['n_points']))

    if params['max_forward_speed'] <= 0:
        raise RuntimeError('Velocity limit must be positive' +
                           str(params['max_forward_speed']))

    srv_name = 'start_helical_trajectory'
    traj_gen = node.create_client(InitHelicalTrajectory,
                                  'start_helical_trajectory')

    if not traj_gen.wait_for_service(timeout_sec=20):
        raise RuntimeError('Service %s not available! Closing node...' %
                           (traj_gen.srv_name))

    node.get_logger().info(
        'Generating trajectory that starts at t={} s'.format(start_time))

    # Convert the time value
    (sec, nsec) = float_sec_to_int_sec_nano(start_time)

    req = InitHelicalTrajectory.Request()
    req.start_time = rclpy.time.Time(seconds=sec, nanoseconds=nsec).to_msg()
    req.start_now = start_now
    req.radius = params['radius']
    req.center = Point(params['center'][0], params['center'][1],
                       params['center'][2])
    req.is_clockwise = False
    req.angle_offset = 0.0
    req.n_points = params['n_points']
    req.heading_offset = params['heading_offset'] * pi / 180
    req.max_forward_speed = params['max_forward_speed']
    req.duration = params['duration']
    req.n_turns = params['n_turns']
    req.delta_z = params['delta_z']

    future = traj_gen.call_async(req)
    rclpy.spin_until_future_complete(node, future)
    try:
        response = future.result()
    except Exception as e:
        node.get_logger().error('Service call ' + srv_name +
                                ' failed, error=' + str(e))
    else:
        node.get_logger().info('Trajectory successfully generated!')

    node.destroy_node()
Ejemplo n.º 50
0
 def test_create_node(self):
     node_name = 'create_node_test'
     rclpy.create_node(node_name, context=self.context).destroy_node()
Ejemplo n.º 51
0
    def __init__(self, rsw_id, params):
        #(__init__における handler の組み込み場所)
        self.node = rclpy.create_node(node_name)

        self.node.declare_parameter('~node_name')
        self.node.declare_parameter('~rsw_id')
        self.node.declare_parameter('~use_axis')

        self.name = self.node.get_parameter('~node_name').get_parameter_value().string_value
        self.rsw_id = self.node.get_parameter('~rsw_id').get_parameter_value().string_value
        self._use_axis = self.node.get_parameter('~use_axis').get_parameter_value().string_value

        self.params = {}
        for ax in self._use_axis:
            params[ax] = {}
            #params[ax]['mode'] = rospy.get_param('~{ax}_mode'.format(**locals()), default_mode)
            #params[ax]['pulse_conf'] = [eval(rospy.get_param('~{ax}_pulse_conf'.format(**locals()), default_pulse_conf))] #evalとは???
            self.node.declare_parameter('~{ax}_mode')
            self.node.declare_parameter('~{ax}_pulse_conf')

            self.params[ax]['mode'] = self.node.get_parameter('~{ax}_mode').get_parameter_value().string_value
            self.params[ax]['pulse_conf'] = self.node.get_parameter('~{ax}_pulse_conf').get_parameter_value().string_value


            self.mp = {}
                # mp['clock'] = rospy.get_param('~{ax}_clock'.format(**locals()), default_clock)
                # mp['acc_mode'] = rospy.get_param('~{ax}_acc_mode'.format(**locals()), default_acc_mode)
                # mp['low_speed'] = rospy.get_param('~{ax}_low_speed'.format(**locals()), default_low_speed)
                # mp['speed'] = rospy.get_param('~{ax}_speed'.format(**locals()), default_speed)
                # mp['acc'] = rospy.get_param('~{ax}_acc'.format(**locals()), default_acc)
                # mp['dec'] = rospy.get_param('~{ax}_dec'.format(**locals()), default_dec)
                # mp['step'] = rospy.get_param('~{ax}_step'.format(**locals()), default_step)

            self.node.declare_parameter('~{ax}_clock')
            self.node.declare_parameter('~{ax}_acc_mode')
            self.node.declare_parameter('~{ax}_low_speed')
            self.node.declare_parameter('~{ax}_speed')
            self.node.declare_parameter('~{ax}_acc')
            self.node.declare_parameter('~{ax}_dec')
            self.node.declare_parameter('~{ax}_step')


            self.mp['clock'] = self.node.get_parameter('~{ax}_clock').get_parameter_value().string_value #mpにはselfはいらない?? paramsに格納するから
            self.mp['acc_mode'] = self.node.get_parameter('~{ax}_acc_mode').get_parameter_value().string_value
            self.mp['low_speed'] = self.node.get_parameter('~{ax}_low_speed').get_parameter_value().string_value
            self.mp['speed'] = self.node.get_parameter('~{ax}_speed').get_parameter_value().string_value
            self.mp['acc'] = self.node.get_parameter('~{ax}_acc').get_parameter_value().string_value
            self.mp['dec'] = self.node.get_parameter('~{ax}_dec').get_parameter_value().string_value
            self.mp['step'] = self.node.get_parameter('~{ax}_step').get_parameter_value().string_value
            self.params[ax]['motion'] = mp
            continue

    #元driver
        self.func_queue = queue.Queue()
        self.pub = {}
        self.use_axis = ''.join([ax for ax in self.params])

        self.mode = [self.params[ax]['mode'] for ax in self.use_axis]
        self.motion = {ax: self.params[ax]['motion'] for ax in self.use_axis}


    #元handler
        #use axis
        self.current_speed = {ax: 0 for ax in self.use_axis}
        self.current_step = {ax: 0 for ax in self.use_axis}
        self.current_moving = {ax: 0 for ax in self.use_axis}
        self.last_direction = {ax: 0 for ax in self.use_axis}
        self.do_status = [0, 0, 0, 0]

        self.move_mode = {ax: p['mode'] for ax, p in self.params.items()}
        self.default_speed = {ax: p['motion']['speed'] for ax, p in self.params.items()}
        self.low_speed = {ax: p['motion']['low_speed'] for ax, p in self.params.items()}

        self.mot = pyinterface.open(7415, self.rsw_id)
        [self.mot.set_pulse_out(ax, 'method', self.params[ax]['pulse_conf']) for ax in self.use_axis]
        self.mot.set_motion(self.use_axis, self.mode, self.motion)


    #元handler 命令値を受け取る


        base = '/pyinterface/pci7415/rsw{self.rsw_id}'.format(**locals())

        for ax in self.use_axis:
            b = '{base}/{ax}/'.format(**locals())
            # rospy.Subscriber(b+'step_cmd', std_msgs.msg.Int64, self.set_step, callback_args=ax)
            self.node.create_subscription(std_msgs.msg.Int64, b+'step_cmd', partial(self.set_step, ax))

            # rospy.Subscriber(b+'speed_cmd', std_msgs.msg.Float64, self.set_speed, callback_args=ax)
            self.node.create_subscription(std_msgs.msg.Int64, b+'speed_cmd', partial(self.set_speed, ax))

        for do_num in range(1,5):
            self.node.create_subscription(std_msgs.msg.Int64, '{}/output_do{}_cmd'.format(base, do_num), partial(self.set_do, do_num))
            #rospy.Subscriber('{}/output_do{}_cmd'.format(base, do_num), std_msgs.msg.Int64, self.set_do, callback_args=do_num)


        # loop start 元driver
        # self.th = threading.Thread(target=self.loop)
        # self.th.setDaemon(True)
        # self.th.start()
        self.node.create_timer(1e-5,self.loop)

        return
Ejemplo n.º 52
0
 def test_create_node_with_empty_namespace(self):
     node_name = 'create_node_test'
     namespace = ''
     node = rclpy.create_node(node_name, namespace=namespace)
     self.assertEqual('/', node.get_namespace())
Ejemplo n.º 53
0
def main():
    parser = argparse.ArgumentParser(
        formatter_class=argparse.ArgumentDefaultsHelpFormatter)
    parser.add_argument(
        'data_name',
        nargs='?',
        default='topic1',
        help='Name of the data (must comply with ROS topic rules)')

    parser.add_argument('--best-effort',
                        action='store_true',
                        default=False,
                        help="Set QoS reliability option to 'best effort'")

    parser.add_argument('--transient-local',
                        action='store_true',
                        default=False,
                        help="Set QoS durability option to 'transient local'")

    parser.add_argument('--depth',
                        type=int,
                        default=default_depth,
                        help='Size of the QoS history depth')

    parser.add_argument(
        '--keep-all',
        action='store_true',
        default=False,
        help=
        "Set QoS history option to 'keep all' (unlimited depth, subject to resource limits)"
    )

    parser.add_argument('--payload-size',
                        type=int,
                        default=0,
                        help='Size of data payload to send')

    parser.add_argument('--period',
                        type=float,
                        default=0.5,
                        help='Time in seconds between messages')

    parser.add_argument(
        '--end-after',
        type=int,
        help='Script will exit after publishing this many messages')

    args = parser.parse_args()

    reliability_suffix = '_best_effort' if args.best_effort else ''
    topic_name = '{0}_data{1}'.format(args.data_name, reliability_suffix)

    rclpy.init()
    node = rclpy.create_node('%s_pub' % topic_name)
    node_logger = node.get_logger()

    qos_profile = QoSProfile(depth=args.depth)

    if args.best_effort:
        node_logger.info('Reliability: best effort')
        qos_profile.reliability = QoSReliabilityPolicy.BEST_EFFORT
    else:
        node_logger.info('Reliability: reliable')
        qos_profile.reliability = QoSReliabilityPolicy.RELIABLE

    if args.keep_all:
        node_logger.info('History: keep all')
        qos_profile.history = QoSHistoryPolicy.KEEP_ALL
    else:
        node_logger.info('History: keep last')
        qos_profile.history = QoSHistoryPolicy.KEEP_LAST

    node_logger.info('Depth: {0}'.format(args.depth))

    if args.transient_local:
        node_logger.info('Durability: transient local')
        qos_profile.durability = QoSDurabilityPolicy.TRANSIENT_LOCAL
    else:
        node_logger.info('Durability: volatile')
        qos_profile.durability = QoSDurabilityPolicy.VOLATILE

    data_pub = node.create_publisher(Header, topic_name, qos_profile)
    node_logger.info('Publishing on topic: {0}'.format(topic_name))

    node_logger.info('Payload size: {0}'.format(args.payload_size))
    data = 'a' * args.payload_size

    msg = Header()
    cycle_count = 0

    def publish_msg(val):
        msg.frame_id = '{0}_{1}'.format(val, data)
        data_pub.publish(msg)
        node_logger.info('Publishing: "{0}"'.format(val))

    while rclpy.ok():
        if args.end_after is not None and cycle_count >= args.end_after:
            publish_msg(-1)
            sleep(0.1)  # allow reliable messages to get re-sent if needed
            return

        publish_msg(cycle_count)
        cycle_count += 1

        try:
            sleep(args.period)
        except KeyboardInterrupt:
            publish_msg(-1)
            sleep(0.1)
            raise
Ejemplo n.º 54
0
 def test_create_node(self):
     node_name = 'create_node_test'
     rclpy.create_node(node_name)
Ejemplo n.º 55
0
    def __init__(self):
        """
        Initialize the MARA environemnt
        """
        # Manage command line args
        args = ut_generic.getArgsParserMARA().parse_args()
        self.gzclient = args.gzclient
        self.realSpeed = args.realSpeed
        self.velocity = args.velocity
        self.multiInstance = args.multiInstance
        self.port = args.port

        # Set the path of the corresponding URDF file
        if self.realSpeed:
            urdf = "reinforcement_learning/mara_robot_run.urdf"
            urdfPath = get_prefix_path(
                "mara_description") + "/share/mara_description/urdf/" + urdf
        else:
            urdf = "reinforcement_learning/mara_robot_train.urdf"
            urdfPath = get_prefix_path(
                "mara_description") + "/share/mara_description/urdf/" + urdf

        # Launch mara in a new Process
        self.launch_subp = ut_launch.startLaunchServiceProcess(
            ut_launch.generateLaunchDescriptionMara(self.gzclient,
                                                    self.realSpeed,
                                                    self.multiInstance,
                                                    self.port, urdfPath))

        # Create the node after the new ROS_DOMAIN_ID is set in generate_launch_description()
        rclpy.init(args=None)
        self.node = rclpy.create_node(self.__class__.__name__)

        # class variables
        self._observation_msg = None
        self.max_episode_steps = 1024  #default value, can be updated from baselines
        self.iterator = 0
        self.reset_jnts = True
        self._collision_msg = None

        #############################
        #   Environment hyperparams
        #############################
        # Target, where should the agent reach
        self.targetPosition = np.asarray([-0.40028, 0.095615,
                                          0.72466])  # close to the table
        self.target_orientation = np.asarray(
            [0., 0.7071068, 0.7071068, 0.])  # arrow looking down [w, x, y, z]
        # self.targetPosition = np.asarray([-0.386752, -0.000756, 1.40557]) # easy point
        # self.target_orientation = np.asarray([-0.4958324, 0.5041332, 0.5041331, -0.4958324]) # arrow looking opposite to MARA [w, x, y, z]

        EE_POINTS = np.asmatrix([[0, 0, 0]])  # offset
        EE_VELOCITIES = np.asmatrix([[0, 0, 0]])

        # Initial joint position
        INITIAL_JOINTS = np.array([0., 0., 0., 0., 0., 0.])

        # # Topics for the robot publisher and subscriber.
        JOINT_PUBLISHER = '/mara_controller/command'
        JOINT_SUBSCRIBER = '/mara_controller/state'

        # joint names:
        MOTOR1_JOINT = 'motor1'
        MOTOR2_JOINT = 'motor2'
        MOTOR3_JOINT = 'motor3'
        MOTOR4_JOINT = 'motor4'
        MOTOR5_JOINT = 'motor5'
        MOTOR6_JOINT = 'motor6'
        EE_LINK = 'ee_link'

        # Set constants for links
        WORLD = 'world'
        BASE = 'base_robot'
        MARA_MOTOR1_LINK = 'motor1_link'
        MARA_MOTOR2_LINK = 'motor2_link'
        MARA_MOTOR3_LINK = 'motor3_link'
        MARA_MOTOR4_LINK = 'motor4_link'
        MARA_MOTOR5_LINK = 'motor5_link'
        MARA_MOTOR6_LINK = 'motor6_link'
        EE_LINK = 'ee_link'

        JOINT_ORDER = [
            MOTOR1_JOINT, MOTOR2_JOINT, MOTOR3_JOINT, MOTOR4_JOINT,
            MOTOR5_JOINT, MOTOR6_JOINT
        ]
        LINK_NAMES = [
            WORLD, BASE, MARA_MOTOR1_LINK, MARA_MOTOR2_LINK, MARA_MOTOR3_LINK,
            MARA_MOTOR4_LINK, MARA_MOTOR5_LINK, MARA_MOTOR6_LINK, EE_LINK
        ]

        reset_condition = {
            'initial_positions': INITIAL_JOINTS,
            'initial_velocities': []
        }
        #############################

        m_jointOrder = copy.deepcopy(JOINT_ORDER)
        m_linkNames = copy.deepcopy(LINK_NAMES)

        # Initialize target end effector position
        self.environment = {
            'jointOrder': m_jointOrder,
            'linkNames': m_linkNames,
            'reset_conditions': reset_condition,
            'tree_path': urdfPath,
            'end_effector_points': EE_POINTS,
        }

        # Subscribe to the appropriate topics, taking into account the particular robot
        self._pub = self.node.create_publisher(
            JointTrajectory,
            JOINT_PUBLISHER,
            qos_profile=qos_profile_sensor_data)
        self._sub = self.node.create_subscription(
            JointTrajectoryControllerState,
            JOINT_SUBSCRIBER,
            self.observation_callback,
            qos_profile=qos_profile_sensor_data)
        self._sub_coll = self.node.create_subscription(
            ContactState,
            '/gazebo_contacts',
            self.collision_callback,
            qos_profile=qos_profile_sensor_data)
        self.reset_sim = self.node.create_client(Empty, '/reset_simulation')

        # Initialize a tree structure from the robot urdf.
        # Note that the xacro of the urdf is updated by hand.
        # The urdf must be compiled.
        _, self.ur_tree = tree_urdf.treeFromFile(self.environment['tree_path'])
        # Retrieve a chain structure between the base and the start of the end effector.
        self.mara_chain = self.ur_tree.getChain(
            self.environment['linkNames'][0],
            self.environment['linkNames'][-1])
        self.numJoints = self.mara_chain.getNrOfJoints()
        # Initialize a KDL Jacobian solver from the chain.
        self.jacSolver = ChainJntToJacSolver(self.mara_chain)

        self.obs_dim = self.numJoints + 10

        # Here idially we should find the control range of the robot. Unfortunatelly in ROS/KDL there is nothing like this.
        # I have tested this with the mujoco enviroment and the output is always same low[-1.,-1.], high[1.,1.]
        # Does not change anything
        low = -np.pi * np.ones(self.numJoints)
        high = np.pi * np.ones(self.numJoints)

        self.action_space = spaces.Box(low, high)

        high = np.inf * np.ones(self.obs_dim)
        low = -high
        self.observation_space = spaces.Box(low, high)

        # Spawn Target element in gazebo.
        # node & spawn_cli initialized in super class
        spawn_cli = self.node.create_client(SpawnEntity, '/spawn_entity')

        while not spawn_cli.wait_for_service(timeout_sec=1.0):
            self.node.get_logger().info(
                '/spawn_entity service not available, waiting again...')

        modelXml = ut_gazebo.getTargetSdf()

        pose = Pose()
        pose.position.x = self.targetPosition[0]
        pose.position.y = self.targetPosition[1]
        pose.position.z = self.targetPosition[2]
        pose.orientation.x = self.target_orientation[1]
        pose.orientation.y = self.target_orientation[2]
        pose.orientation.z = self.target_orientation[3]
        pose.orientation.w = self.target_orientation[0]

        #override previous spawn_request element.
        self.spawn_request = SpawnEntity.Request()
        self.spawn_request.name = "target"
        self.spawn_request.xml = modelXml
        self.spawn_request.robot_namespace = ""
        self.spawn_request.initial_pose = pose
        self.spawn_request.reference_frame = "world"

        #ROS2 Spawn Entity
        target_future = spawn_cli.call_async(self.spawn_request)
        rclpy.spin_until_future_complete(self.node, target_future)

        # Seed the environment
        self.seed()
        self.buffer_dist_rewards = []
        self.buffer_tot_rewards = []
        self.collided = 0
Ejemplo n.º 56
0
 def setUpClass(cls):
     rclpy.init()
     cls.node = rclpy.create_node('my_node', namespace='/my_ns')
Ejemplo n.º 57
0
 def test_create_node_with_namespace(self):
     node_name = 'create_node_test'
     namespace = '/ns'
     rclpy.create_node(node_name, namespace=namespace,
                       context=self.context).destroy_node()
Ejemplo n.º 58
0
 def test_create_node_invalid_namespace(self):
     node_name = 'create_node_test_invalid_namespace'
     namespace = '/invalid_namespace?'
     with self.assertRaisesRegex(InvalidNamespaceException,
                                 'must not contain characters'):
         rclpy.create_node(node_name, namespace=namespace)
def main(args=None):
    if args is None:
        args = sys.argv

    rclpy.init(args)
    node = rclpy.create_node('teleop_twist_keyboard')

    pub = node.create_publisher(Twist, 'cmd_vel', qos_profile_default)

    speed = 0.5
    turn = 1.0
    x = 0
    y = 0
    z = 0
    th = 0
    status = 0

    try:
        print(msg)
        print(vels(speed, turn))
        while (1):
            key = getKey()
            if key in moveBindings.keys():
                x = moveBindings[key][0]
                y = moveBindings[key][1]
                z = moveBindings[key][2]
                th = moveBindings[key][3]
            elif key in speedBindings.keys():
                speed = speed * speedBindings[key][0]
                turn = turn * speedBindings[key][1]

                print(vels(speed, turn))
                if (status == 14):
                    print(msg)
                status = (status + 1) % 15
            else:
                x = 0
                y = 0
                z = 0
                th = 0
                if (key == '\x03'):
                    break

            twist = Twist()
            twist.linear.x = x * speed
            twist.linear.y = y * speed
            twist.linear.z = z * speed
            twist.angular.x = 0.0
            twist.angular.y = 0.0
            twist.angular.z = th * turn
            pub.publish(twist)

    except:
        print(e)

    finally:
        twist = Twist()
        twist.linear.x = 0.0
        twist.linear.y = 0.0
        twist.linear.z = 0.0
        twist.angular.x = 0.0
        twist.angular.y = 0.0
        twist.angular.z = 0.0
        pub.publish(twist)

        termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
Ejemplo n.º 60
0
 def test_create_node_invalid_name(self):
     node_name = 'create_node_test_invalid_name?'
     with self.assertRaisesRegex(InvalidNodeNameException,
                                 'must not contain characters'):
         rclpy.create_node(node_name, context=self.context)