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

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

    msg = String()
    i = 0

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

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

    while rclpy.ok():
        rclpy.spin_once(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()
Esempio n. 2
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
Esempio n. 3
0
 def test_node_name_replacement(self, node_name_replacement):
     for attempt in range(self.ATTEMPTS):
         if node_name_replacement in self.node.get_node_names():
             break
         time.sleep(self.TIME_BETWEEN_ATTEMPTS)
         rclpy.spin_once(self.node, timeout_sec=0)
     self.assertIn(node_name_replacement, self.node.get_node_names())
Esempio n. 4
0
def run_topic_listening(node, topic_monitor, options):
    """Subscribe to relevant topics and manage the data received from susbcriptions."""
    while rclpy.ok():
        # Check if there is a new topic online
        # TODO(dhood): use graph events rather than polling
        topic_names_and_types = node.get_topic_names_and_types()

        it = zip(topic_names_and_types.topic_names, topic_names_and_types.type_names)
        for topic_name, type_name in it:
            if not topic_monitor.is_supported_type(type_name):
                continue

            # Infer the appropriate QoS profile from the topic name
            topic_info = topic_monitor.get_topic_info(topic_name)
            if topic_info is None:
                # The topic is not for being monitored
                continue

            is_new_topic = topic_name and topic_name not in topic_monitor.monitored_topics
            if is_new_topic:
                # Register new topic with the monitor
                qos_profile = qos_profile_default
                qos_profile.depth = QOS_DEPTH
                if topic_info['reliability'] == 'best_effort':
                    qos_profile.reliability = \
                        QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE
                topic_monitor.add_monitored_topic(
                    Header, topic_name, node, qos_profile,
                    options.expected_period, options.allowed_latency, options.stale_time)

        if topic_monitor.monitored_topics:
            # Wait for messages with a timeout, otherwise this thread will block any other threads
            # until a message is received
            rclpy.spin_once(node, timeout_sec=0.05)
Esempio n. 5
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()
Esempio n. 6
0
def service_loop():
  global ros_node
  global ros_thread_loop
  if __ros_version__ == 2:
    while ros_thread_loop :
      rclpy.spin_once(ros_node, timeout_sec=1.0)
    print("..Terminate RosThread")
Esempio n. 7
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()
Esempio n. 8
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()
Esempio n. 9
0
 def test_service_replacement(self, service_replacement):
     for attempt in range(self.ATTEMPTS):
         if service_replacement in self.get_services():
             break
         time.sleep(self.TIME_BETWEEN_ATTEMPTS)
         rclpy.spin_once(self.node, timeout_sec=0)
     self.assertNotIn(service_replacement, self.get_topics())
     self.assertIn(service_replacement, self.get_services())
Esempio n. 10
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)
Esempio n. 11
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()
Esempio n. 12
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

    while rclpy.ok():
        rclpy.spin_once(node)
Esempio n. 13
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()
Esempio n. 14
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()
Esempio n. 15
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()
Esempio n. 16
0
def main(args=None):
    rclpy.init(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

    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()
Esempio n. 17
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()
Esempio n. 18
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)
def main(args=None):
    rclpy.init(args=args)

    minimal_client = MinimalClientAsync()
    minimal_client.send_request()

    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 minimal_client.cli.response is not None:
            print(
                'Result of add_two_ints: for %d + %d = %d' %
                (minimal_client.req.a, minimal_client.req.b, minimal_client.cli.response.sum))
            break
        rclpy.spin_once(minimal_client)

    minimal_client.destroy_node()
    rclpy.shutdown()
Esempio n. 20
0
def func_zero_callback(args):
    import rclpy

    period = float(args[0])

    rclpy.init()
    node = rclpy.create_node('test_timer_no_callback')

    callbacks = []
    timer = node.create_timer(period, lambda: callbacks.append(len(callbacks)))

    rclpy.spin_once(node, period / 2.)

    node.destroy_timer(timer)
    rclpy.shutdown()
    assert len(callbacks) == 0, \
        "shouldn't have received any callback, received %d" % len(callbacks)

    return True
Esempio n. 21
0
def main(args=None):
    rclpy.init(args=args)

    minimal_client = MinimalClientAsync()
    minimal_client.send_open_request()

    while rclpy.ok():
        rclpy.spin_once(minimal_client)
        if minimal_client.future.done():
            try:
                response = minimal_client.future.result()
            except Exception as e:
                minimal_client.get_logger().info('Service call failed %r' %
                                                 (e, ))
            else:
                if response.success == False:
                    minimal_client.send_close_request()
            break

    minimal_client.destroy_node()
    rclpy.shutdown()
Esempio n. 22
0
def main(args=None):
    rclpy.init(args=args)

    node = rclpy.create_node('minimal_client_async')

    minimal_client = MinimalClientAsync(node)
    minimal_client.send_request()

    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 minimal_client.cli.response is not None:
            print(
                'Result of add_two_ints: for %d + %d = %d' %
                (minimal_client.req.a, minimal_client.req.b, minimal_client.cli.response.sum))
            break
        rclpy.spin_once(node)

    node.destroy_node()
    rclpy.shutdown()
Esempio n. 23
0
    def test_run_no_configure(self, pipeline_manager, proc_info, proc_output):
        goal_msg = RunPipeline.Goal()
        goal_msg.input = 0
        future = self.run_client.send_goal_async(goal_msg)

        future_count = 0
        result = None
        while rclpy.ok():
            rclpy.spin_once(self.node)
            if future.done():
                future_count += 1
                if future_count == 1:
                    future = future.result().get_result_async()
                if future_count == 2:
                    result = future.result().result
                    break
        if result.output == 1:
            proc_output.assertWaitFor(
                expected_output='Pipeline is not configured')
        else:
            self.fail('Should abort due to unconfigured pipeline')
Esempio n. 24
0
def main(args=None):
    rclpy.init(args=args)
    client_vel = service_node_vel()
    client_vel.client_request()

    while rclpy.ok():
        rclpy.spin_once(client_vel)
        if client_vel.response.done():
            try:
                response = client_vel.response.result()
            except Exception as e:
                client_vel.get_logger().info('Service call failed %r' % (e, ))
            else:
                client_vel.get_logger().info(
                    ' Two wheel speeds set to %d , %d ' %
                    (client_vel.value.left_speed,
                     client_vel.value.right_speed))
            break

    client_vel.destroy_node()
    rclpy.shutdown()
    def test_node_init_state(self):
        """
        Test init state.

        Expect fault_injection_node does not publish /diagnostics.status
        while the talker does not to publish
        """
        msg_buffer = []

        self.test_node.create_subscription(DiagnosticArray, "/diagnostics",
                                           lambda msg: msg_buffer.append(msg),
                                           10)

        # Wait until the talker transmits two messages over the ROS topic
        end_time = time.time() + self.evaluation_time
        while time.time() < end_time:
            rclpy.spin_once(self.test_node, timeout_sec=0.1)

        # Return False if no valid data is received
        self.assertEqual(
            self.get_num_valid_data(msg_buffer, DiagnosticStatus.ERROR), 0)
Esempio n. 26
0
    def call_service(self, srv_type, srv_name, model_name, model_xml=None, robot_namespace=None, initial_pose=None, reference_frame=None):

        service_node = ServiceNode(srv_type,srv_name)
        if srv_type == SpawnEntity:
            service_node.send_spawn(model_name, model_xml,robot_namespace,initial_pose,reference_frame)
        else:
            service_node.send_delete(model_name)

        rclpy.spin_once(service_node)
        if service_node.future.done():
            try:
                response = service_node.future.result()
            except Exception as e:
                service_node.get_logger().info(
                    'Service call failed %r' % (e,))
            else:
                service_node.get_logger().info("Successfully executed service")
                service_node.get_logger().info(
                    'Result of service: '+response.status_message)

        service_node.destroy_node()
Esempio n. 27
0
    def take_observation(self):
        """
        Take observation from the environment and return it.
        :return: state.
        """
        # # Take an observation
        rclpy.spin_once(self.node)
        obs_img = self._observation_img

        # Check that the observation is not prior to the action
        while obs_img is None:
            rclpy.spin_once(self.node)
            obs_img = self._observation_img

        img = self.bridge.imgmsg_to_cv2(obs_img, "bgr8")
        img = PIL_Image.fromarray(img)
        obs_img = np.asarray(img, dtype="uint8")

        self._observation_img = None

        return obs_img
Esempio n. 28
0
def random_positions(node: rclpy.node.Node) -> Optional[numpy.ndarray]:
    if "client" not in random_positions.__dict__:
        random_positions.client = node.create_client(RandomPositions,
                                                     '/random_positions')
    if random_positions.client.wait_for_service(timeout_sec=5.0):
        # node.get_logger().debug('Calling service /create_marker')
        srv_call = random_positions.client.call_async(
            RandomPositions.Request())
        while rclpy.ok():
            if srv_call.done():
                break
            rclpy.spin_once(node)
        # Sort the joint and position pairs such that arm_1_joint is first, and arm_3_joint is last
        joint_val_pairs = list(
            zip(srv_call.result().joints,
                srv_call.result().positions))
        joint_val_pairs.sort()
        positions = [v for _, v in joint_val_pairs]
        return numpy.array(positions)
    node.get_logger().error('Service /random_positions unavailable')
    return None
Esempio n. 29
0
def main(args=None):
    rclpy.init(args=args)

    cmd_vel_publisher = CmdVelPublisher()
    start_time = cmd_vel_publisher.get_clock().now().to_msg().sec
    clock_now = start_time
    time_delta = 0

    while (clock_now - start_time) < 5:
        rclpy.spin_once(cmd_vel_publisher)
        clock_now = cmd_vel_publisher.get_clock().now().to_msg().sec

        time_delta = clock_now - start_time
        cmd_vel_publisher.get_logger().info(f'{time_delta} seconds passed')

    cmd_vel_publisher.stop_robot()

    cmd_vel_publisher.get_logger().info('\n==== Stop Publishing ====')
    cmd_vel_publisher.destroy_node()

    rclpy.shutdown()
def main(args=None):
    rclpy.init(args=args)

    minimal_client = MinimalClientAsync()
    minimal_client.send_request()

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

    minimal_client.destroy_node()
    rclpy.shutdown()
Esempio n. 31
0
 def _delete_entity(self):
     # Delete entity from gazebo on shutdown if bond flag enabled
     self.get_logger().info('Deleting entity [{}]'.format(self.args.entity))
     client = self.create_client(
         DeleteEntity, '%s/delete_entity' % self.args.gazebo_namespace)
     if client.wait_for_service(timeout_sec=5.0):
         req = DeleteEntity.Request()
         req.name = self.args.entity
         self.get_logger().info('Calling service %s/delete_entity' %
                                self.args.gazebo_namespace)
         srv_call = client.call_async(req)
         while rclpy.ok():
             if srv_call.done():
                 self.get_logger().info('Deleting status: %s' %
                                        srv_call.result().status_message)
                 break
             rclpy.spin_once(self)
     else:
         self.get_logger().error(
             'Service %s/delete_entity unavailable. ' +
             'Was Gazebo started with GazeboRosFactory?')
Esempio n. 32
0
    def test_run_comp_not_valid(self, pipeline_manager, proc_info, proc_output):
        req = ConfigurePipeline.Request()
        pipeline_type = PipelineType()
        test_type = 'example'
        pipeline_type.type = test_type
        req.pipeline_type = pipeline_type
        test_file_name = 'test_run_comp_not_valid'
        req.config_file_name = test_file_name
        future = self.configure_client.call_async(req)

        while rclpy.ok():
            rclpy.spin_once(self.node)
            if future.done():
                break

        goal_msg = RunPipeline.Goal()
        goal_msg.input = 0
        future = self.run_client.send_goal_async(goal_msg)

        future_count = 0
        result = None
        while rclpy.ok():
            rclpy.spin_once(self.node)
            if future.done():
                future_count += 1
                if future_count == 1:
                    future = future.result().get_result_async()
                if future_count == 2:
                    result = future.result().result
                    break
        if result.output == 1:
            component = 'example::DoesNotExist'
            proc_output.assertWaitFor(
                expected_output='Failed to find class with the requested plugin name \'{}\' in the loaded library'.format(component))
            proc_output.assertWaitFor(
                expected_output='The component {} was not loaded successfully'.format(component))
            proc_output.assertWaitFor(
                expected_output='Aborting the pipeline')
        else:
            self.fail('Should abort from component not able to load')
    def run(self):
        """
        Runs the pipeline sequence

        For each pipeline type in the sequence, configures the pipeline with the type,
        then runs the pipeline and gets the result.
        """
        for i in range(len(self.pipelines)):
            pipeline = self.pipelines[i]
            future = self._send_configure_request(pipeline)
            success = False
            # Configure pipeline
            while rclpy.ok():
                rclpy.spin_once(self)
                if future.done():
                    success = future.result().success
                    break
            if not success:
                self.get_logger().error("Error while configuring pipeline")
                break
            # Run pipeline
            future = self._run_current_pipeline()
            while rclpy.ok():
                rclpy.spin_once(self)
                if future.done():
                    future = future.result().get_result_async()
                    break
            # Get pipeline result
            while rclpy.ok():
                rclpy.spin_once(self)
                if future.done():
                    result = future.result().result
                    break
Esempio n. 34
0
    def take_observation(self):
        """
        Take observation from the environment and return it.
        :return: state.
        """
        # Take an observation
        rclpy.spin_once(self.node)
        obs_message = self._observation_msg
        if obs_message is None:
            print("Last observation is empty")
            return None
        # Collect the end effector points and velocities in cartesian coordinates for the process_observations state.
        # Collect the present joint angles and velocities from ROS for the state.
        last_observations = ut_mara.process_observations(obs_message, self.environment)
        # Get Jacobians from present joint angles and KDL trees
        # The Jacobians consist of a 6x6 matrix getting its from from
        # (joint angles) x (len[x, y, z] + len[roll, pitch, yaw])
        ee_link_jacobians = ut_mara.get_jacobians(last_observations, self.num_joints, self.jac_solver)
        if self.environment['link_names'][-1] is None:
            print("End link is empty!!")
            return None
        else:
            translation, rot = general_utils.forward_kinematics(self.mara_chain,
                                                self.environment['link_names'],
                                                last_observations[:self.num_joints],
                                                base_link=self.environment['link_names'][0], # make the table as the base to get the world coordinate system
                                                end_link=self.environment['link_names'][-1])

            current_ee_pos_tgt = np.ndarray.flatten(general_utils.get_ee_points(self.environment['end_effector_points'], translation, rot).T)
            ee_pos_points = current_ee_pos_tgt - self.target_position

            ee_velocities = ut_mara.get_ee_points_velocities(ee_link_jacobians, self.environment['end_effector_points'], rot, last_observations)

            # Concatenate the information that defines the robot state
            # vector, typically denoted asrobot_id 'x'.
            state = np.r_[np.reshape(last_observations, -1),
                          np.reshape(ee_pos_points, -1),
                          np.reshape(ee_velocities, -1),]

            return state
Esempio n. 35
0
def _call_get_transitions(node, states, service_name):
    clients = {}
    futures = {}
    # create clients
    for node_name in states.keys():
        clients[node_name] = node.create_client(GetAvailableTransitions,
                                                f'{node_name}/{service_name}')

    # wait until all clients have been called
    while True:
        for node_name in [n for n in states.keys() if n not in futures]:
            # call as soon as ready
            client = clients[node_name]
            if client.service_is_ready():
                request = GetAvailableTransitions.Request()
                future = client.call_async(request)
                futures[node_name] = future

        if len(futures) == len(clients):
            break
        rclpy.spin_once(node, timeout_sec=1.0)

    # wait for all responses
    for future in futures.values():
        rclpy.spin_until_future_complete(node, future)

    # return transitions from current state or exception for each node
    transitions = {}
    for node_name, future in futures.items():
        if future.result() is not None:
            response = future.result()
            transitions[node_name] = []
            for transition_description in response.available_transitions:
                if (states[node_name] is None
                        or transition_description.start_state
                        == states[node_name]):
                    transitions[node_name].append(transition_description)
        else:
            transitions[node_name] = future.exception()
    return transitions
Esempio n. 36
0
def main(args=None):
    rclpy.init(args=args)
    client_node = rclpy.create_node('minimal_action_client')
    ac_list = []

    action_client_1 = MinimalActionClient(node=client_node,
                                          server_name='fibonacci_1')
    action_client_2 = MinimalActionClient(node=client_node,
                                          server_name='fibonacci_2')

    ac_list.append(action_client_1)
    ac_list.append(action_client_2)
    # action_client_1.send_goal()
    # action_client_2.send_goal()

    while True:
        for i in range(len(ac_list)):

            if ac_list[i].goal_done():
                print("I'm done")
                print("result", ac_list[i].get_result())
                ac_list[i].set_done(False)
                ac_list[i].set_result(None)
                ac_list[i].set_free(True)
                print("apply policy")

            else:
                if ac_list[i].client_free():
                    ac_list[i].set_free(False)
                    if i == 0:
                        ac_list[i].send_goal(5)
                    else:
                        ac_list[i].send_goal(6)
                    print("sent goal")
                else:
                    rclpy.spin_once(client_node)
                    print("spinning")

    # rclpy.spin(client_node)
    print("print executed")
Esempio n. 37
0
def main(args=None):
   rclpy.init(args=args)
 
   minimal_client = MinimalClientAsync()
   minimal_client.send_request()
 
   while rclpy.ok():
       rclpy.spin_once(minimal_client)
       if minimal_client.future.done():
           try:
               response = minimal_client.future.result()
           except Exception as e:
               minimal_client.get_logger().info(
                   'Service call failed %r' % (e,))
           else:
               minimal_client.get_logger().info(
                   'Setting SPIKE motor to %d' %
                   (minimal_client.req.speed))
           break
 
   minimal_client.destroy_node()
   rclpy.shutdown()
Esempio n. 38
0
def main(args=None):

    rclpy.init(args=args)
    que_cli = QueueClient()

    try:
        while rclpy.ok():
            rclpy.spin_once(que_cli, timeout_sec=1.0)
            if que_cli.future is None:
                que_cli.send_request()
            else:
                if que_cli.future.done():
                    rsp = que_cli.future.result()
                    que_cli.get_logger().info("Get Last Data: [{}]".format(
                        rsp.values[-1].string_value))
                    que_cli.future = None

    except KeyboardInterrupt:
        pass

    que_cli.destroy_node()
    rclpy.shutdown()
Esempio n. 39
0
def main():
    rclpy.init(args=sys.argv)

    node = rclpy.create_node(NODE_NAME)
    node.create_subscription(
        AppCommand,  # msg_type
        Topics.SEND_APP_COMMAND.value,  # topic
        send_app_command_callback,  # callback
        qos_profile_sensor_data  # QoS Profile
    )
    node.create_service(
        GetAppData,  # srv_type
        Services.GET_APP_DATA.value,  # srv_name
        get_app_data_callback  # callback
    )

    print(f"{NODE_NAME} is UP.")

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

    print(f"{NODE_NAME} is DOWN.")
def main(args=None):
    env: LobotArmEnv = gym.make('LobotArmDiscrete-v1')  # Discrete with noise
    action_space: Type[MultiDiscrete] = env.action_space
    rclpy.spin_once(env.node)
    while True:
        print("-------------Starting----------------")
        for x in range(500):
            # action = numpy.array([1, 0, 4])
            action = action_space.sample()
            action_do_nothing = numpy.array([2, 2, 2])
            observation, reward, done, info = env.step(action_do_nothing)
            # Type hints
            observation: numpy.ndarray
            reward: float
            done: bool
            info: dict
            if done:
                break
        time.sleep(0.8)
        print("-------------Resetting environment---------------")
        env.reset()
        print("-------------Reset finished----------------")
Esempio n. 41
0
def subscriber(node, topic_name, message_type, callback):
    if message_type is None:
        topic_names_and_types = get_topic_names_and_types(node=node)
        try:
            expanded_name = expand_topic_name(topic_name, node.get_name(), node.get_namespace())
        except ValueError as e:
            raise RuntimeError(e)
        try:
            validate_full_topic_name(expanded_name)
        except rclpy.exceptions.InvalidTopicNameException as e:
            raise RuntimeError(e)
        for n, t in topic_names_and_types:
            if n == expanded_name:
                if len(t) > 1:
                    raise RuntimeError(
                        "Cannot echo topic '%s', as it contains more than one type: [%s]" %
                        (topic_name, ', '.join(t))
                    )
                message_type = t[0]
                break
        else:
            raise RuntimeError(
                'Could not determine the type for the passed topic')

    # TODO(dirk-thomas) this logic should come from a rosidl related package
    try:
        package_name, message_name = message_type.split('/', 2)
        if not package_name or not message_name:
            raise ValueError()
    except ValueError:
        raise RuntimeError('The passed message type is invalid')
    module = importlib.import_module(package_name + '.msg')
    msg_module = getattr(module, message_name)

    node.create_subscription(
        msg_module, topic_name, callback, qos_profile=qos_profile_sensor_data)

    while rclpy.ok():
        rclpy.spin_once(node)
    def test_set_io(self):
        """Test to set an IO and check whether it has been set."""
        self.io_msg = None
        io_states_sub = self.node.create_subscription(
            IOStates,
            "/io_and_status_controller/io_states",
            self.io_msg_cb,
            rclpy.qos.qos_profile_system_default,
        )

        pin = 0
        set_io_req = SetIO.Request()
        set_io_req.fun = 1
        set_io_req.pin = pin
        set_io_req.state = 1.0

        self.call_service(self.set_io_client, set_io_req)
        pin_state = False

        end_time = time.time() + 5
        while not pin_state and time.time() < end_time:
            rclpy.spin_once(self.node, timeout_sec=0.1)
            if self.io_msg is not None:
                pin_state = self.io_msg.digital_out_states[pin].state

        self.assertEqual(pin_state, 1)

        set_io_req.state = 0.0
        self.call_service(self.set_io_client, set_io_req)

        end_time = time.time() + 5
        while pin_state and time.time() < end_time:
            rclpy.spin_once(self.node, timeout_sec=0.1)
            if self.io_msg is not None:
                pin_state = self.io_msg.digital_out_states[pin].state

        self.assertEqual(pin_state, 0)

        self.node.destroy_subscription(io_states_sub)
Esempio n. 43
0
    def run(self):
        # Start main thread loop
        while rclpy.ok() and not self.exitFlag:
            rclpy.spin_once(self.node)

            (setResponse, getResponse) = self.node.getRequestResponses()
            if setResponse is not None:
                self.regulatorSettingsSetDone.emit(setResponse.success,
                                                   setResponse.error_text)

            if getResponse is not None:
                self.regulatorSettingsGetDone.emit(getResponse)

            sleep(0.01)

        # Cleanup: disable motors
        self.setEnabled(False)
        # Cleanup: destroy timers (threads!)
        self.node.destroyTimers()

        self.node.destroy_node()
        rclpy.shutdown()
Esempio n. 44
0
def main():
    init()
    rclpy.init()
    real = yf_node.YF_RealSpeed(nodeName["RealSpeed"], "RealSpeed", "pub")
    soll = yf_node.YF_SollSpeed(nodeName["SollSpeed"], "SollSpeed", "sub")
    rclpy.spin_once(soll.node, timeout_sec=0.1)

    q_subNode = Queue(1)
    q_pubNode = Queue(1)
    q_subNode.put_nowait(soll)
    q_pubNode.put_nowait(real)

    t_sub = threading.Thread(target=subSpin, args=(q_subNode, ))
    t_pub = threading.Thread(target=pubSpin, args=(q_pubNode, ))
    t_sub.start()
    t_pub.start()
    Motor_serial = SerialThread("/dev/ttyUSB0")

    t_read = threading.Thread(target=Motor_serial.read, daemon=False)
    t_write = threading.Thread(target=Motor_serial.write, daemon=False)

    Motor_serial.alive = True
    t_read.start()
    t_write.start()

    try:
        while True:  #:
            Motor_serial.control_data = soll.subMsg
            real.publishMsg(Motor_serial.read_data)
            time.sleep(0.25)

    finally:
        Motor_serial.control_data = [0.0, 0.0, 0.0, 0.0]
        time.sleep(.7)
        Motor_serial.stop()
        time.sleep(.5)
        t_read.join()
        t_write.join()
        time.sleep(.1)
Esempio n. 45
0
        def retrieve_urdf(self, timeout_sec: float = 5) -> None:
            """Retrieve the URDF file from the /robot_description topic.

            Will raise an EnvironmentError if the topic is unavailable.
            """
            qos_profile = QoSProfile(depth=1)
            qos_profile.durability = QoSDurabilityPolicy.TRANSIENT_LOCAL

            self.urdf = None

            def urdf_received(msg: String):
                self.urdf = msg.data

            self.create_subscription(
                msg_type=String,
                topic='/robot_description',
                qos_profile=qos_profile,
                callback=urdf_received,
            )
            rclpy.spin_once(self, timeout_sec=timeout_sec)
            if self.urdf is None:
                raise EnvironmentError('Could not retrieve the URDF!')
Esempio n. 46
0
    def spin(self):
        """ Start event loop"""
        if self.transport == 'ZMQ':
            # Event Loop calback option
            if self.sub_callback != None:
                stream_sub = zmqstream.ZMQStream(self.sub_socket)
                stream_sub.on_recv(self.__process_message)
                ioloop.IOLoop.instance().start()
        elif self.transport == 'ROS':
            import rospy
            """Function used to spin ROS subscriber"""
            rospy.spin()

        elif self.transport == 'ROS2':
            import rclpy
            while rclpy.ok():
                rclpy.spin_once(self.node_ros2)
            # Destroy the node explicitly
            # (optional - otherwise it will be done automatically
            # when the garbage collector destroys the node object)
            self.node_ros2.destroy_node()
            rclpy.shutdown()
    def run(self):
        jointSub = self.node_.create_subscription(JointState, "/Joint_state",
                                                  self.jointCallback)

        intSub = self.node_.create_subscription(Int16MultiArray, "/Int_Info",
                                                self.intCallback)

        floatSub = self.node_.create_subscription(Float32MultiArray,
                                                  "/Ioniq_info",
                                                  self.floatCallback)

        self.accelPub = self.node_.create_publisher(Int16, '/dbw_cmd/Accel',
                                                    qos_profile_default)
        self.brakePub = self.node_.create_publisher(Int16, '/dbw_cmd/Brake',
                                                    qos_profile_default)
        self.steerPub = self.node_.create_publisher(Int16, '/dbw_cmd/Steer',
                                                    qos_profile_default)
        self.cruise_speed_Pub = self.node_.create_publisher(
            Int16, '/Cruise_speed', qos_profile_default)
        while rclpy.ok():
            rclpy.spin_once(self.node_)
            time.sleep(0.002)
def main(args=None):
    rclpy.init(args=args)

    minimal_client = MinimalClientAsync()
    minimal_client.send_request()

    while rclpy.ok():
        rclpy.spin_once(minimal_client)
        if minimal_client.future.done():
            try:
                response = minimal_client.future.result()
            except Exception as e:
                minimal_client.get_logger().info('Service call failed %r' %
                                                 (e, ))
            else:
                minimal_client.get_logger().info(
                    'Result of add_two_ints: for %d + %d = %d' %
                    (minimal_client.req.a, minimal_client.req.b, response.sum))
            break

    minimal_client.destroy_node()
    rclpy.shutdown()
Esempio n. 49
0
 def test_configure_valid_type_no_yaml(self, pipeline_manager, proc_info,
                                       proc_output):
     req = ConfigurePipeline.Request()
     pipeline_type = PipelineType()
     test_type = 'example'
     pipeline_type.type = test_type
     req.pipeline_type = pipeline_type
     test_file_name = 'test_no_yaml'
     req.config_file_name = test_file_name
     future = self.configure_client.call_async(req)
     while rclpy.ok():
         rclpy.spin_once(self.node)
         if future.done():
             res = future.result()
             if res.success is False:
                 proc_output.assertWaitFor(
                     expected_output='Could not find {}.yaml'.format(
                         test_file_name))
             else:
                 self.fail('Expected configure pipeline service to fail'
                           ' due to no yaml associated to pipeline type')
             break
Esempio n. 50
0
def main(args=None):

    rclpy.init(args=args)
    srv_cli = AsyncServiceClient()

    try:
        while rclpy.ok():
            rclpy.spin_once(srv_cli, timeout_sec=0.1)
            if srv_cli.future is None:
                srv_cli.send_request()
            else:
                if srv_cli.future.done():
                    rsp = srv_cli.future.result()
                    srv_cli.get_logger().info(
                        'Result of add_two_ints: for %d + %d = %d' %
                        (srv_cli.req.a, srv_cli.req.b, rsp.sum))
                    srv_cli.future = None
    except KeyboardInterrupt:
        pass

    srv_cli.destroy_node()
    rclpy.shutdown()
Esempio n. 51
0
def func_number_callbacks(args):
    import time

    import rclpy

    period = float(args[0])

    rclpy.init()
    node = rclpy.create_node('test_timer_no_callback')

    callbacks = []
    timer = node.create_timer(period, lambda: callbacks.append(len(callbacks)))
    begin_time = time.time()

    while rclpy.ok() and time.time() - begin_time < 4.5 * period:
        rclpy.spin_once(node, period / 10)

    node.destroy_timer(timer)
    rclpy.shutdown()
    assert len(callbacks) == 4, 'should have received 4 callbacks, received %s' % str(callbacks)

    return True
Esempio n. 52
0
def main(args=None):
    rclpy.init(args=args)

    # node = rclpy.create_node('PubAndSub.py')
    # sub_key = node.create_subscription(String4, 'key', key_sub_callback, 10)
    # pub_control = node.create_publisher(Carcontrol1, 'control', 10)
    Control_Subscriber = ControlSubscriber()
    Control_Publisher = ControlPublisher()

    i = 0
    while True:
        rclpy.spin_once(Control_Subscriber)

        i += 1
        Control_Publisher.key_pub(Control_Subscriber.con_msg)
        Control_Publisher.get_logger().info(
            '%d Publishing: emergency:%d  speed:%d steer:%d' %
            (i, Control_Subscriber.con_msg.emergency,
             Control_Subscriber.con_msg.speed,
             Control_Subscriber.con_msg.steer))

    rclpy.shutdown()
Esempio n. 53
0
def test_talker_listener():
    launch_desc = LaunchDescriptor()
    launch_desc.add_process(
        cmd=[sys.executable, os.path.join(this_dir, 'talker.py')],
        name='test_talker_listener__talker'
    )
    launcher = DefaultLauncher()
    launcher.add_launch_descriptor(launch_desc)
    async_launcher = AsynchronousLauncher(launcher)
    async_launcher.start()

    import rclpy

    rclpy.init([])

    from rclpy.qos import qos_profile_default

    from std_msgs.msg import String
    assert String.__class__._TYPE_SUPPORT is not None

    node = rclpy.create_node('listener')

    received_messages = []

    chatter_callback = functools.partial(
        listener_callback, received_messages=received_messages)
    # TODO(wjwwood): should the subscription object hang around?
    node.create_subscription(String, 'chatter', chatter_callback, qos_profile_default)

    spin_count = 0
    while len(received_messages) == 0 and spin_count < 10 and launcher.is_launch_running():
        rclpy.spin_once(node)  # This will wait 1 second or until something has been done.
        spin_count += 1

    async_launcher.terminate()
    async_launcher.join()

    assert len(received_messages) > 0, "Should have received a message from talker"
Esempio n. 54
0
def listener(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('listener')

    received_messages = []
    expected_msgs = get_test_msg(message_name)

    chatter_callback = functools.partial(
        listener_cb, received_messages=received_messages, expected_msgs=expected_msgs)

    node.create_subscription(
        msg_mod, 'test_message_' + message_name, chatter_callback,
        qos_profile_default)

    spin_count = 1
    print('subscriber: beginning loop')
    while (rclpy.ok() and spin_count < number_of_cycles and
           len(received_messages) != len(expected_msgs)):
        rclpy.spin_once(node)
        spin_count += 1
    rclpy.shutdown()

    assert len(received_messages) == len(expected_msgs),\
        'Should have received {} {} messages from talker'.format(len(expected_msgs), message_name)
Esempio n. 55
0
def func_cancel_reset_timer(args):
    import time

    import rclpy

    period = float(args[0])

    rclpy.init()
    node = rclpy.create_node('test_timer_no_callback')

    callbacks = []
    timer = node.create_timer(period, lambda: callbacks.append(len(callbacks)))
    begin_time = time.time()

    assert not timer.is_canceled()

    while rclpy.ok() and time.time() - begin_time < 2.5 * period:
        rclpy.spin_once(node, period / 10)

    assert len(callbacks) == 2, 'should have received 2 callbacks, received %d' % len(callbacks)
    assert not timer.is_canceled()

    timer.cancel()
    assert timer.is_canceled()
    callbacks = []
    begin_time = time.time()
    while rclpy.ok() and time.time() - begin_time < 2.5 * period:
        rclpy.spin_once(node, period / 10)

    assert timer.is_canceled()
    assert [] == callbacks, \
        "shouldn't have received any callback, received %d" % len(callbacks)

    timer.reset()
    assert not timer.is_canceled()
    begin_time = time.time()
    while rclpy.ok() and time.time() - begin_time < 2.5 * period:
        rclpy.spin_once(node, period / 10)

    assert not timer.is_canceled()
    assert len(callbacks) == 2, 'should have received 2 callbacks, received %d' % len(callbacks)
    node.destroy_timer(timer)
    rclpy.shutdown()

    return True
Esempio n. 56
0
 def _run(self):
     while not self._stop.is_set():
         rclpy.spin_once(self._node, timeout_sec=self._time_between_spins)