├── .gitignore ├── CONTRIBUTING.md ├── LICENSE ├── README.md ├── action_tutorials ├── README.md ├── action_tutorials_cpp │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── README.md │ ├── include │ │ └── action_tutorials_cpp │ │ │ └── visibility_control.h │ ├── package.xml │ └── src │ │ ├── fibonacci_action_client.cpp │ │ └── fibonacci_action_server.cpp └── action_tutorials_py │ ├── CHANGELOG.rst │ ├── README.md │ ├── action_tutorials_py │ ├── __init__.py │ ├── fibonacci_action_client.py │ └── fibonacci_action_server.py │ ├── package.xml │ ├── resource │ └── action_tutorials_py │ ├── setup.cfg │ ├── setup.py │ └── test │ ├── test_copyright.py │ ├── test_flake8.py │ ├── test_pep257.py │ └── test_xmllint.py ├── composition ├── CHANGELOG.rst ├── CMakeLists.txt ├── Doxyfile ├── README.md ├── include │ └── composition │ │ ├── client_component.hpp │ │ ├── listener_component.hpp │ │ ├── node_like_listener_component.hpp │ │ ├── server_component.hpp │ │ ├── talker_component.hpp │ │ └── visibility_control.h ├── launch │ └── composition_demo_launch.py ├── package.xml ├── src │ ├── client_component.cpp │ ├── dlopen_composition.cpp │ ├── linktime_composition.cpp │ ├── listener_component.cpp │ ├── manual_composition.cpp │ ├── node_like_listener_component.cpp │ ├── server_component.cpp │ └── talker_component.cpp └── test │ ├── composition_all.regex │ ├── composition_pubsub.regex │ ├── composition_srv.regex │ ├── test_api_pubsub_composition.py.in │ ├── test_api_srv_composition.py.in │ ├── test_api_srv_composition_client_first.py.in │ ├── test_dlopen_composition.py.in │ ├── test_linktime_composition.py.in │ └── test_manual_composition.py.in ├── demo_nodes_cpp ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── img │ ├── allocator_tutorial.png │ ├── content_filtering_messaging.png │ ├── even_parameters_node.png │ ├── one_off_timer.png │ ├── parameter_blackboard.png │ ├── serialized_messaging.png │ ├── server_client.png │ └── talker_listener.png ├── include │ └── demo_nodes_cpp │ │ └── visibility_control.h ├── launch │ ├── services │ │ ├── add_two_ints_async_launch.py │ │ ├── add_two_ints_async_launch.xml │ │ ├── add_two_ints_launch.py │ │ ├── add_two_ints_launch.xml │ │ └── introspect_services_launch.py │ └── topics │ │ ├── talker_listener_best_effort_launch.py │ │ ├── talker_listener_best_effort_launch.xml │ │ ├── talker_listener_best_effort_launch.yaml │ │ ├── talker_listener_launch.py │ │ ├── talker_listener_launch.xml │ │ └── talker_listener_launch.yaml ├── package.xml ├── src │ ├── events │ │ └── matched_event_detect.cpp │ ├── logging │ │ └── use_logger_service.cpp │ ├── parameters │ │ ├── even_parameters_node.cpp │ │ ├── list_parameters.cpp │ │ ├── list_parameters_async.cpp │ │ ├── parameter_blackboard.cpp │ │ ├── parameter_event_handler.cpp │ │ ├── parameter_events.cpp │ │ ├── parameter_events_async.cpp │ │ ├── set_and_get_parameters.cpp │ │ ├── set_and_get_parameters_async.cpp │ │ └── set_parameters_callback.cpp │ ├── services │ │ ├── add_two_ints_client.cpp │ │ ├── add_two_ints_client_async.cpp │ │ ├── add_two_ints_server.cpp │ │ ├── introspection_client.cpp │ │ └── introspection_service.cpp │ ├── timers │ │ ├── one_off_timer.cpp │ │ └── reuse_timer.cpp │ └── topics │ │ ├── allocator_tutorial_custom.cpp │ │ ├── allocator_tutorial_pmr.cpp │ │ ├── content_filtering_publisher.cpp │ │ ├── content_filtering_subscriber.cpp │ │ ├── listener.cpp │ │ ├── listener_best_effort.cpp │ │ ├── listener_serialized_message.cpp │ │ ├── talker.cpp │ │ ├── talker_loaned_message.cpp │ │ └── talker_serialized_message.cpp └── test │ ├── add_two_ints_client.txt │ ├── add_two_ints_client_async.txt │ ├── add_two_ints_server.txt │ ├── content_filtering_publisher.txt │ ├── content_filtering_subscriber-rmw_connextdds.txt │ ├── content_filtering_subscriber-rmw_fastrtps_cpp.txt │ ├── content_filtering_subscriber.txt │ ├── list_parameters.txt │ ├── list_parameters_async.txt │ ├── listener.regex │ ├── matched_event_detect.txt │ ├── parameter_events.txt │ ├── parameter_events_async.txt │ ├── set_and_get_parameters.txt │ ├── set_and_get_parameters_async.txt │ ├── talker.txt │ ├── test_executables_tutorial.py.in │ └── use_logger_service.txt ├── demo_nodes_cpp_native ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── include │ └── demo_nodes_cpp_native │ │ └── visibility_control.h ├── package.xml ├── src │ └── talker.cpp └── test │ ├── talker.regex │ └── test_executables_tutorial.py.in ├── demo_nodes_py ├── CHANGELOG.rst ├── README.md ├── demo_nodes_py │ ├── __init__.py │ ├── events │ │ ├── __init__.py │ │ └── matched_event_detect.py │ ├── logging │ │ ├── __init__.py │ │ └── use_logger_service.py │ ├── parameters │ │ ├── __init__.py │ │ ├── async_param_client.py │ │ ├── params.yaml │ │ └── set_parameters_callback.py │ ├── services │ │ ├── __init__.py │ │ ├── add_two_ints_client.py │ │ ├── add_two_ints_client_async.py │ │ ├── add_two_ints_server.py │ │ └── introspection.py │ └── topics │ │ ├── __init__.py │ │ ├── listener.py │ │ ├── listener_qos.py │ │ ├── listener_serialized.py │ │ ├── talker.py │ │ └── talker_qos.py ├── img │ ├── qos_listener_talker.png │ ├── serialized_subscriber.png │ ├── server_client.png │ ├── set_parameters_callback.png │ └── talker_listener.png ├── package.xml ├── resource │ └── demo_nodes_py ├── setup.cfg ├── setup.py └── test │ ├── test_copyright.py │ ├── test_flake8.py │ ├── test_pep257.py │ └── test_xmllint.py ├── dummy_robot ├── dummy_map_server │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── README.md │ ├── img │ │ └── occupancy_grid.png │ ├── package.xml │ └── src │ │ └── dummy_map_server.cpp ├── dummy_robot_bringup │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── config │ │ └── dummy_robot.rviz │ ├── launch │ │ ├── dummy_robot_bringup_launch.py │ │ ├── dummy_robot_bringup_launch.xml │ │ ├── dummy_robot_bringup_launch.yaml │ │ └── single_rrbot.urdf │ └── package.xml └── dummy_sensors │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── README.md │ ├── package.xml │ └── src │ ├── dummy_joint_states.cpp │ └── dummy_laser.cpp ├── image_tools ├── CHANGELOG.rst ├── CMakeLists.txt ├── Doxyfile ├── README.md ├── doc │ └── qos-best-effort.png ├── img │ └── result.png ├── include │ └── image_tools │ │ ├── cv_mat_sensor_msgs_image_type_adapter.hpp │ │ └── visibility_control.h ├── package.xml ├── src │ ├── burger.cpp │ ├── burger.hpp │ ├── cam2image.cpp │ ├── cv_mat_sensor_msgs_image_type_adapter.cpp │ ├── policy_maps.hpp │ └── showimage.cpp └── test │ ├── cam2image.txt │ ├── showimage.regex │ └── test_executables_demo.py.in ├── intra_process_demo ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── img │ ├── image_pipeline_all_in_one.png │ ├── image_pipeline_all_in_one_rqtgraph.png │ ├── image_pipeline_all_separately.png │ ├── image_pipeline_with_two_image_views.png │ ├── image_pipeline_with_two_image_views_rqtgraph.png │ └── two_node_pipeline.png ├── include │ └── image_pipeline │ │ ├── camera_node.hpp │ │ ├── common.hpp │ │ ├── image_view_node.hpp │ │ └── watermark_node.hpp ├── package.xml ├── src │ ├── cyclic_pipeline │ │ └── cyclic_pipeline.cpp │ ├── image_pipeline │ │ ├── camera_node.cpp │ │ ├── image_pipeline_all_in_one.cpp │ │ ├── image_pipeline_with_two_image_view.cpp │ │ ├── image_view_node.cpp │ │ └── watermark_node.cpp │ └── two_node_pipeline │ │ └── two_node_pipeline.cpp └── test │ ├── cyclic_pipeline.regex │ ├── test_executables_demo.py.in │ └── two_node_pipeline.regex ├── lifecycle ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.rst ├── launch │ └── lifecycle_demo_launch.py ├── package.xml ├── src │ ├── lifecycle_listener.cpp │ ├── lifecycle_service_client.cpp │ └── lifecycle_talker.cpp └── test │ └── test_lifecycle.py ├── lifecycle_py ├── CHANGELOG.rst ├── README.rst ├── launch │ └── lifecycle_demo_launch.py ├── lifecycle_py │ ├── __init__.py │ └── talker.py ├── package.xml ├── resource │ └── lifecycle_py ├── setup.cfg ├── setup.py └── test │ ├── test_copyright.py │ ├── test_flake8.py │ ├── test_lifecycle.py │ ├── test_pep257.py │ └── test_xmllint.py ├── logging_demo ├── CHANGELOG.rst ├── CMakeLists.txt ├── Doxyfile ├── include │ └── logging_demo │ │ ├── logger_config_component.hpp │ │ ├── logger_usage_component.hpp │ │ └── visibility_control.h ├── package.xml ├── src │ ├── logger_config_component.cpp │ ├── logger_usage_component.cpp │ └── logging_demo_main.cpp ├── srv │ └── ConfigLogger.srv └── test │ ├── logging_demo_main_debug_severity.txt │ ├── logging_demo_main_default_severity.txt │ └── test_logging_demo.py.in ├── pendulum_control ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── include │ └── pendulum_control │ │ ├── pendulum_controller.hpp │ │ ├── pendulum_motor.hpp │ │ └── rtt_executor.hpp ├── package.xml ├── src │ ├── pendulum_demo.cpp │ ├── pendulum_logger.cpp │ └── pendulum_teleop.cpp └── test │ ├── execute_with_delay.py │ ├── pendulum_demo.regex │ ├── pendulum_demo_teleop.regex │ ├── pendulum_logger.regex │ ├── pendulum_teleop.txt │ ├── test_pendulum_demo.py.in │ └── test_pendulum_teleop.py.in ├── pendulum_msgs ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── msg │ ├── JointCommand.msg │ ├── JointState.msg │ └── RttestResults.msg └── package.xml ├── pytest.ini ├── quality_of_service_demo ├── README.md ├── rclcpp │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── include │ │ └── quality_of_service_demo │ │ │ ├── common_nodes.hpp │ │ │ └── visibility_control.h │ ├── package.xml │ ├── params_file │ │ ├── example_qos_overrides.yaml │ │ └── example_qos_overrides_with_wildcard.yaml │ └── src │ │ ├── common_nodes.cpp │ │ ├── deadline.cpp │ │ ├── incompatible_qos.cpp │ │ ├── interactive_publisher.cpp │ │ ├── interactive_subscriber.cpp │ │ ├── lifespan.cpp │ │ ├── liveliness.cpp │ │ ├── message_lost_listener.cpp │ │ ├── message_lost_talker.cpp │ │ ├── qos_overrides_listener.cpp │ │ ├── qos_overrides_talker.cpp │ │ ├── utils.cpp │ │ └── utils.hpp └── rclpy │ ├── .gitignore │ ├── CHANGELOG.rst │ ├── package.xml │ ├── quality_of_service_demo_py │ ├── __init__.py │ ├── common_nodes.py │ ├── deadline.py │ ├── incompatible_qos.py │ ├── lifespan.py │ ├── liveliness.py │ ├── message_lost_listener.py │ ├── qos_overrides_listener.py │ └── qos_overrides_talker.py │ ├── resource │ └── quality_of_service_demo_py │ ├── setup.cfg │ ├── setup.py │ └── test │ ├── test_linters.py │ └── test_xmllint.py ├── topic_monitor ├── CHANGELOG.rst ├── README.md ├── doc │ ├── message_size_comparison.png │ └── reliability_comparison.png ├── launch │ ├── depth_demo_launch.py │ ├── fragmentation_demo_launch.py │ └── reliability_demo_launch.py ├── package.xml ├── resource │ └── topic_monitor ├── setup.cfg ├── setup.py ├── test │ ├── test_flake8.py │ ├── test_pep257.py │ └── test_xmllint.py └── topic_monitor │ ├── README.md │ ├── __init__.py │ └── scripts │ ├── __init__.py │ ├── data_publisher.py │ └── topic_monitor.py └── topic_statistics_demo ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── include └── topic_statistics_demo │ ├── imu_talker_listener_nodes.hpp │ ├── string_talker_listener_nodes.hpp │ └── topic_statistics_listener.hpp ├── package.xml └── src ├── display_topic_statistics.cpp ├── imu_talker_listener_nodes.cpp ├── string_talker_listener_nodes.cpp └── topic_statistics_listener.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte compiled Python 2 | __pycache__/ 3 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | Any contribution that you make to this repository will 2 | be under the Apache 2 License, as dictated by that 3 | [license](http://www.apache.org/licenses/LICENSE-2.0.html): 4 | 5 | ~~~ 6 | 5. Submission of Contributions. Unless You explicitly state otherwise, 7 | any Contribution intentionally submitted for inclusion in the Work 8 | by You to the Licensor shall be under the terms and conditions of 9 | this License, without any additional terms or conditions. 10 | Notwithstanding the above, nothing herein shall supersede or modify 11 | the terms of any separate license agreement you may have executed 12 | with Licensor regarding such Contributions. 13 | ~~~ 14 | 15 | Contributors must sign-off each commit by adding a `Signed-off-by: ...` 16 | line to commit messages to certify that they have the right to submit 17 | the code they are contributing to the project according to the 18 | [Developer Certificate of Origin (DCO)](https://developercertificate.org/). 19 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ## **What Is This?** 2 | 3 | This repository contains source code for demos mentioned in the official ROS 2 documentation [Tutorials](https://docs.ros.org/en/rolling/Tutorials.html). 4 | 5 | Each ROS 2 package consists of its own self-contained demonstration(s) with its respective `README.md` showing how things work. 6 | -------------------------------------------------------------------------------- /action_tutorials/README.md: -------------------------------------------------------------------------------- 1 | # ROS 2 Action Tutorials 2 | 3 | This tutorial demonstrates implementing ROS action servers and action clients. 4 | 5 | The action server in this case takes in an integer value between 0 and 45 named *order* and returns the Fibonacci sequence, a sequence with the form: 6 | $$F_0 = 0$$ 7 | $$F_1 = 1$$ 8 | $$F_{order}=F_{order-1} + F_{order-2}$$ 9 | 10 | The action server calculates each number in the sequence one at a time and returns a partial sequence as feedback at each iteration. 11 | If the action is cancelled before the entire sequence is calculated, the server stops calculating the sequence and no result is returned. 12 | The action client in this tutorial sends a goal to the action server with an order of 10. 13 | It logs each partial sequence returned as feedback. 14 | Once the action is finished executing, the action client logs the resulting sequence. 15 | 16 | ## Packages 17 | 18 | - [action_tutorials_cpp](./action_tutorials_cpp) implements the described action server and client using the rclcpp library in C++. 19 | - [action_tutorials_py](./action_tutorials_py) implements the described action server and client using the rclpy library in Python. 20 | -------------------------------------------------------------------------------- /action_tutorials/action_tutorials_cpp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.20) 2 | project(action_tutorials_cpp) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++17 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 17) 12 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 13 | endif() 14 | 15 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 16 | add_compile_options(-Wall -Wextra -Wpedantic) 17 | endif() 18 | 19 | # find dependencies 20 | find_package(ament_cmake REQUIRED) 21 | find_package(example_interfaces REQUIRED) 22 | find_package(rclcpp REQUIRED) 23 | find_package(rclcpp_action REQUIRED) 24 | find_package(rclcpp_components REQUIRED) 25 | 26 | add_library(action_tutorials SHARED 27 | src/fibonacci_action_client.cpp 28 | src/fibonacci_action_server.cpp) 29 | rclcpp_components_register_node(action_tutorials PLUGIN "action_tutorials_cpp::FibonacciActionClient" EXECUTABLE fibonacci_action_client) 30 | rclcpp_components_register_node(action_tutorials PLUGIN "action_tutorials_cpp::FibonacciActionServer" EXECUTABLE fibonacci_action_server) 31 | target_compile_definitions(action_tutorials 32 | PRIVATE "ACTION_TUTORIALS_CPP_BUILDING_DLL") 33 | target_include_directories(action_tutorials PRIVATE 34 | "$" 35 | "$") 36 | target_link_libraries(action_tutorials PRIVATE 37 | ${example_interfaces_TARGETS} 38 | rclcpp::rclcpp 39 | rclcpp_action::rclcpp_action 40 | rclcpp_components::component 41 | ) 42 | 43 | install(TARGETS action_tutorials 44 | ARCHIVE DESTINATION lib 45 | LIBRARY DESTINATION lib 46 | RUNTIME DESTINATION bin) 47 | 48 | if(BUILD_TESTING) 49 | find_package(ament_lint_auto REQUIRED) 50 | ament_lint_auto_find_test_dependencies() 51 | endif() 52 | 53 | ament_package() 54 | -------------------------------------------------------------------------------- /action_tutorials/action_tutorials_cpp/include/action_tutorials_cpp/visibility_control.h: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef ACTION_TUTORIALS_CPP__VISIBILITY_CONTROL_H_ 16 | #define ACTION_TUTORIALS_CPP__VISIBILITY_CONTROL_H_ 17 | 18 | #ifdef __cplusplus 19 | extern "C" 20 | { 21 | #endif 22 | 23 | // This logic was borrowed (then namespaced) from the examples on the gcc wiki: 24 | // https://gcc.gnu.org/wiki/Visibility 25 | 26 | #if defined _WIN32 || defined __CYGWIN__ 27 | #ifdef __GNUC__ 28 | #define ACTION_TUTORIALS_CPP_EXPORT __attribute__ ((dllexport)) 29 | #define ACTION_TUTORIALS_CPP_IMPORT __attribute__ ((dllimport)) 30 | #else 31 | #define ACTION_TUTORIALS_CPP_EXPORT __declspec(dllexport) 32 | #define ACTION_TUTORIALS_CPP_IMPORT __declspec(dllimport) 33 | #endif 34 | #ifdef ACTION_TUTORIALS_CPP_BUILDING_DLL 35 | #define ACTION_TUTORIALS_CPP_PUBLIC ACTION_TUTORIALS_CPP_EXPORT 36 | #else 37 | #define ACTION_TUTORIALS_CPP_PUBLIC ACTION_TUTORIALS_CPP_IMPORT 38 | #endif 39 | #define ACTION_TUTORIALS_CPP_PUBLIC_TYPE ACTION_TUTORIALS_CPP_PUBLIC 40 | #define ACTION_TUTORIALS_CPP_LOCAL 41 | #else 42 | #define ACTION_TUTORIALS_CPP_EXPORT __attribute__ ((visibility("default"))) 43 | #define ACTION_TUTORIALS_CPP_IMPORT 44 | #if __GNUC__ >= 4 45 | #define ACTION_TUTORIALS_CPP_PUBLIC __attribute__ ((visibility("default"))) 46 | #define ACTION_TUTORIALS_CPP_LOCAL __attribute__ ((visibility("hidden"))) 47 | #else 48 | #define ACTION_TUTORIALS_CPP_PUBLIC 49 | #define ACTION_TUTORIALS_CPP_LOCAL 50 | #endif 51 | #define ACTION_TUTORIALS_CPP_PUBLIC_TYPE 52 | #endif 53 | 54 | #ifdef __cplusplus 55 | } 56 | #endif 57 | 58 | #endif // ACTION_TUTORIALS_CPP__VISIBILITY_CONTROL_H_ 59 | -------------------------------------------------------------------------------- /action_tutorials/action_tutorials_cpp/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | action_tutorials_cpp 5 | 0.37.0 6 | C++ action tutorial cpp code 7 | 8 | Aditya Pande 9 | Audrow Nash 10 | 11 | Apache License 2.0 12 | 13 | Jacob Perron 14 | Mabel Zhang 15 | 16 | ament_cmake 17 | 18 | example_interfaces 19 | rclcpp 20 | rclcpp_action 21 | rclcpp_components 22 | 23 | ament_lint_auto 24 | ament_lint_common 25 | 26 | 27 | ament_cmake 28 | 29 | 30 | -------------------------------------------------------------------------------- /action_tutorials/action_tutorials_py/README.md: -------------------------------------------------------------------------------- 1 | # Action Server 2 | 3 | In the constructor for `FibonacciActionServer`, an action server is created with a callback that is called when a goal is accepted. 4 | ```python 5 | self._action_server = ActionServer( 6 | self, 7 | Fibonacci, 8 | 'fibonacci', 9 | self.execute_callback) 10 | ``` 11 | 12 | There are three types of callbacks: 13 | 14 | - A `goal_callback` can optionally be added to conditionally accept or reject the goal, however, by default the goal is accepted. 15 | - A `cancel_callback` can also optionally be added to conditionally accept or reject the cancel goal request, however, by default the cancel goal request is accepted. 16 | - The `execute_callback` calculates the Fibonacci sequence up to *order* and publishes partial sequences as feedback as each item is added to the sequence. 17 | 18 | The thread sleeps for 1 second between the calculation of each item in order to represent a long-running task. 19 | When execution is complete, the full sequence is returned to the action client. 20 | 21 | # Action Client 22 | 23 | In the constructor for `FibonacciActionClient`, an action client for the `fibonacci` action is created: 24 | 25 | ```python 26 | self._action_client = ActionClient(self, Fibonacci, 'fibonacci') 27 | ``` 28 | 29 | A goal of type `Fibonacci` is created with order 10. 30 | The goal is sent asynchronously with callbacks registered for the goal response and the feedback: 31 | 32 | ```python 33 | self._send_goal_future = self._action_client.send_goal_async( 34 | goal_msg, 35 | feedback_callback=self.feedback_callback) 36 | 37 | self._send_goal_future.add_done_callback(self.goal_response_callback) 38 | ``` 39 | 40 | Within the `goal_response_callback`, if the goal is accepted, the goal result is requested asynchronously. 41 | A callback is registered for receiving the result: 42 | ```python 43 | self._get_result_future = goal_handle.get_result_async() 44 | 45 | self._get_result_future.add_done_callback(self.get_result_callback) 46 | ``` 47 | 48 | There are two types of callbacks: 49 | 50 | - The `feedback_callback` logs the partial sequences as they are received. 51 | - The `get_result_callback` logs the full Fibonacci sequence. 52 | -------------------------------------------------------------------------------- /action_tutorials/action_tutorials_py/action_tutorials_py/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/demos/8f361a1ece2daae54016959f3ea9ab2e7692ddbb/action_tutorials/action_tutorials_py/action_tutorials_py/__init__.py -------------------------------------------------------------------------------- /action_tutorials/action_tutorials_py/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | action_tutorials_py 5 | 0.37.0 6 | Python action tutorial code 7 | 8 | Aditya Pande 9 | Audrow Nash 10 | 11 | Apache License 2.0 12 | 13 | Jacob Perron 14 | Mabel Zhang 15 | 16 | example_interfaces 17 | rclpy 18 | 19 | ament_copyright 20 | ament_flake8 21 | ament_pep257 22 | ament_xmllint 23 | python3-pytest 24 | 25 | 26 | ament_python 27 | 28 | 29 | -------------------------------------------------------------------------------- /action_tutorials/action_tutorials_py/resource/action_tutorials_py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/demos/8f361a1ece2daae54016959f3ea9ab2e7692ddbb/action_tutorials/action_tutorials_py/resource/action_tutorials_py -------------------------------------------------------------------------------- /action_tutorials/action_tutorials_py/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/action_tutorials_py 3 | [install] 4 | install_scripts=$base/lib/action_tutorials_py 5 | -------------------------------------------------------------------------------- /action_tutorials/action_tutorials_py/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | 3 | package_name = 'action_tutorials_py' 4 | 5 | setup( 6 | name=package_name, 7 | version='0.37.0', 8 | packages=[package_name], 9 | data_files=[ 10 | ('share/ament_index/resource_index/packages', 11 | ['resource/' + package_name]), 12 | ('share/' + package_name, ['package.xml']), 13 | ], 14 | install_requires=['setuptools'], 15 | zip_safe=True, 16 | author='Jacob Perron', 17 | author_email='jacob@openrobotics.org', 18 | maintainer='Aditya Pande, Audrow Nash, Michael Jeronimo', 19 | maintainer_email='aditya.pande@openrobotics.org, audrow@openrobotics.org, michael.jeronimo@openrobotics.org', # noqa: E501 20 | keywords=['ROS'], 21 | classifiers=[ 22 | 'Intended Audience :: Developers', 23 | 'License :: OSI Approved :: Apache Software License', 24 | 'Programming Language :: Python', 25 | 'Topic :: Software Development', 26 | ], 27 | description='Python action tutorials code.', 28 | license='Apache License, Version 2.0', 29 | tests_require=['pytest'], 30 | entry_points={ 31 | 'console_scripts': [ 32 | 'fibonacci_action_server = action_tutorials_py.fibonacci_action_server:main', 33 | 'fibonacci_action_client = action_tutorials_py.fibonacci_action_client:main', 34 | ], 35 | }, 36 | ) 37 | -------------------------------------------------------------------------------- /action_tutorials/action_tutorials_py/test/test_copyright.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_copyright.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.copyright 20 | @pytest.mark.linter 21 | def test_copyright(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found errors' 24 | -------------------------------------------------------------------------------- /action_tutorials/action_tutorials_py/test/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_flake8.main import main_with_errors 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc, errors = main_with_errors(argv=[]) 23 | assert rc == 0, \ 24 | 'Found %d code style errors / warnings:\n' % len(errors) + \ 25 | '\n'.join(errors) 26 | -------------------------------------------------------------------------------- /action_tutorials/action_tutorials_py/test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found code style errors / warnings' 24 | -------------------------------------------------------------------------------- /action_tutorials/action_tutorials_py/test/test_xmllint.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_xmllint.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.xmllint 21 | def test_xmllint() -> None: 22 | rc = main(argv=[]) 23 | assert rc == 0, 'Found errors' 24 | -------------------------------------------------------------------------------- /composition/Doxyfile: -------------------------------------------------------------------------------- 1 | # All settings not listed here will use the Doxygen default values. 2 | 3 | PROJECT_NAME = "composition" 4 | PROJECT_NUMBER = master 5 | PROJECT_BRIEF = "Examples for composing multiple nodes in a single process." 6 | 7 | INPUT = ./include 8 | RECURSIVE = YES 9 | OUTPUT_DIRECTORY = docs_output 10 | 11 | EXTRACT_ALL = YES 12 | SORT_MEMBER_DOCS = NO 13 | 14 | GENERATE_LATEX = NO 15 | GENERATE_XML = YES 16 | EXCLUDE_SYMBOLS = detail details 17 | 18 | ENABLE_PREPROCESSING = YES 19 | MACRO_EXPANSION = YES 20 | EXPAND_ONLY_PREDEF = YES 21 | PREDEFINED += COMPOSITION_PUBLIC 22 | -------------------------------------------------------------------------------- /composition/include/composition/client_component.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef COMPOSITION__CLIENT_COMPONENT_HPP_ 16 | #define COMPOSITION__CLIENT_COMPONENT_HPP_ 17 | 18 | #include "composition/visibility_control.h" 19 | #include "example_interfaces/srv/add_two_ints.hpp" 20 | #include "rclcpp/rclcpp.hpp" 21 | 22 | namespace composition 23 | { 24 | 25 | class Client : public rclcpp::Node 26 | { 27 | public: 28 | COMPOSITION_PUBLIC 29 | explicit Client(const rclcpp::NodeOptions & options); 30 | 31 | protected: 32 | void on_timer(); 33 | 34 | private: 35 | rclcpp::Client::SharedPtr client_; 36 | rclcpp::TimerBase::SharedPtr timer_; 37 | }; 38 | 39 | } // namespace composition 40 | 41 | #endif // COMPOSITION__CLIENT_COMPONENT_HPP_ 42 | -------------------------------------------------------------------------------- /composition/include/composition/listener_component.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef COMPOSITION__LISTENER_COMPONENT_HPP_ 16 | #define COMPOSITION__LISTENER_COMPONENT_HPP_ 17 | 18 | #include "composition/visibility_control.h" 19 | #include "rclcpp/rclcpp.hpp" 20 | #include "std_msgs/msg/string.hpp" 21 | 22 | namespace composition 23 | { 24 | 25 | class Listener : public rclcpp::Node 26 | { 27 | public: 28 | COMPOSITION_PUBLIC 29 | explicit Listener(const rclcpp::NodeOptions & options); 30 | 31 | private: 32 | rclcpp::Subscription::SharedPtr sub_; 33 | }; 34 | 35 | } // namespace composition 36 | 37 | #endif // COMPOSITION__LISTENER_COMPONENT_HPP_ 38 | -------------------------------------------------------------------------------- /composition/include/composition/node_like_listener_component.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef COMPOSITION__NODE_LIKE_LISTENER_COMPONENT_HPP_ 16 | #define COMPOSITION__NODE_LIKE_LISTENER_COMPONENT_HPP_ 17 | 18 | #include "composition/visibility_control.h" 19 | #include "rclcpp/rclcpp.hpp" 20 | #include "std_msgs/msg/string.hpp" 21 | 22 | namespace composition 23 | { 24 | 25 | class NodeLikeListener 26 | { 27 | public: 28 | COMPOSITION_PUBLIC 29 | explicit NodeLikeListener(const rclcpp::NodeOptions & options); 30 | 31 | COMPOSITION_PUBLIC 32 | rclcpp::node_interfaces::NodeBaseInterface::SharedPtr 33 | get_node_base_interface() const; 34 | 35 | private: 36 | rclcpp::Node::SharedPtr node_; 37 | rclcpp::Subscription::SharedPtr sub_; 38 | }; 39 | 40 | } // namespace composition 41 | 42 | #endif // COMPOSITION__NODE_LIKE_LISTENER_COMPONENT_HPP_ 43 | -------------------------------------------------------------------------------- /composition/include/composition/server_component.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef COMPOSITION__SERVER_COMPONENT_HPP_ 16 | #define COMPOSITION__SERVER_COMPONENT_HPP_ 17 | 18 | #include "composition/visibility_control.h" 19 | #include "example_interfaces/srv/add_two_ints.hpp" 20 | #include "rclcpp/rclcpp.hpp" 21 | 22 | namespace composition 23 | { 24 | 25 | class Server : public rclcpp::Node 26 | { 27 | public: 28 | COMPOSITION_PUBLIC 29 | explicit Server(const rclcpp::NodeOptions & options); 30 | 31 | private: 32 | rclcpp::Service::SharedPtr srv_; 33 | }; 34 | 35 | } // namespace composition 36 | 37 | #endif // COMPOSITION__SERVER_COMPONENT_HPP_ 38 | -------------------------------------------------------------------------------- /composition/include/composition/talker_component.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef COMPOSITION__TALKER_COMPONENT_HPP_ 16 | #define COMPOSITION__TALKER_COMPONENT_HPP_ 17 | 18 | #include "composition/visibility_control.h" 19 | #include "rclcpp/rclcpp.hpp" 20 | #include "std_msgs/msg/string.hpp" 21 | 22 | namespace composition 23 | { 24 | 25 | class Talker : public rclcpp::Node 26 | { 27 | public: 28 | COMPOSITION_PUBLIC 29 | explicit Talker(const rclcpp::NodeOptions & options); 30 | 31 | protected: 32 | void on_timer(); 33 | 34 | private: 35 | size_t count_; 36 | rclcpp::Publisher::SharedPtr pub_; 37 | rclcpp::TimerBase::SharedPtr timer_; 38 | }; 39 | 40 | } // namespace composition 41 | 42 | #endif // COMPOSITION__TALKER_COMPONENT_HPP_ 43 | -------------------------------------------------------------------------------- /composition/include/composition/visibility_control.h: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef COMPOSITION__VISIBILITY_CONTROL_H_ 16 | #define COMPOSITION__VISIBILITY_CONTROL_H_ 17 | 18 | #ifdef __cplusplus 19 | extern "C" 20 | { 21 | #endif 22 | 23 | // This logic was borrowed (then namespaced) from the examples on the gcc wiki: 24 | // https://gcc.gnu.org/wiki/Visibility 25 | 26 | #if defined _WIN32 || defined __CYGWIN__ 27 | #ifdef __GNUC__ 28 | #define COMPOSITION_EXPORT __attribute__ ((dllexport)) 29 | #define COMPOSITION_IMPORT __attribute__ ((dllimport)) 30 | #else 31 | #define COMPOSITION_EXPORT __declspec(dllexport) 32 | #define COMPOSITION_IMPORT __declspec(dllimport) 33 | #endif 34 | #ifdef COMPOSITION_BUILDING_DLL 35 | #define COMPOSITION_PUBLIC COMPOSITION_EXPORT 36 | #else 37 | #define COMPOSITION_PUBLIC COMPOSITION_IMPORT 38 | #endif 39 | #define COMPOSITION_PUBLIC_TYPE COMPOSITION_PUBLIC 40 | #define COMPOSITION_LOCAL 41 | #else 42 | #define COMPOSITION_EXPORT __attribute__ ((visibility("default"))) 43 | #define COMPOSITION_IMPORT 44 | #if __GNUC__ >= 4 45 | #define COMPOSITION_PUBLIC __attribute__ ((visibility("default"))) 46 | #define COMPOSITION_LOCAL __attribute__ ((visibility("hidden"))) 47 | #else 48 | #define COMPOSITION_PUBLIC 49 | #define COMPOSITION_LOCAL 50 | #endif 51 | #define COMPOSITION_PUBLIC_TYPE 52 | #endif 53 | 54 | #ifdef __cplusplus 55 | } 56 | #endif 57 | 58 | #endif // COMPOSITION__VISIBILITY_CONTROL_H_ 59 | -------------------------------------------------------------------------------- /composition/launch/composition_demo_launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Launch a talker and a listener in a component container.""" 16 | 17 | import launch 18 | from launch_ros.actions import ComposableNodeContainer 19 | from launch_ros.descriptions import ComposableNode 20 | 21 | 22 | def generate_launch_description(): 23 | """Generate launch description with multiple components.""" 24 | container = ComposableNodeContainer( 25 | name='my_container', 26 | namespace='', 27 | package='rclcpp_components', 28 | executable='component_container', 29 | composable_node_descriptions=[ 30 | ComposableNode( 31 | package='composition', 32 | plugin='composition::Talker', 33 | name='talker'), 34 | ComposableNode( 35 | package='composition', 36 | plugin='composition::Listener', 37 | name='listener') 38 | ], 39 | output='screen', 40 | ) 41 | 42 | return launch.LaunchDescription([container]) 43 | -------------------------------------------------------------------------------- /composition/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | composition 5 | 0.37.0 6 | Examples for composing multiple nodes in a single process. 7 | 8 | Aditya Pande 9 | Audrow Nash 10 | 11 | Apache License 2.0 12 | 13 | Dirk Thomas 14 | Mabel Zhang 15 | 16 | ament_cmake 17 | 18 | example_interfaces 19 | rclcpp 20 | rclcpp_components 21 | rcutils 22 | std_msgs 23 | 24 | example_interfaces 25 | launch_ros 26 | rclcpp 27 | rclcpp_components 28 | rcutils 29 | std_msgs 30 | 31 | ament_cmake_pytest 32 | ament_lint_auto 33 | ament_lint_common 34 | launch 35 | launch_testing 36 | launch_testing_ros 37 | launch_testing_ament_cmake 38 | rmw_implementation_cmake 39 | 40 | 41 | ament_cmake 42 | 43 | 44 | -------------------------------------------------------------------------------- /composition/src/listener_component.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "composition/listener_component.hpp" 16 | 17 | #include 18 | #include 19 | 20 | #include "rclcpp/rclcpp.hpp" 21 | #include "std_msgs/msg/string.hpp" 22 | 23 | namespace composition 24 | { 25 | 26 | // Create a Listener "component" that subclasses the generic rclcpp::Node base class. 27 | // Components get built into shared libraries and as such do not write their own main functions. 28 | // The process using the component's shared library will instantiate the class as a ROS node. 29 | Listener::Listener(const rclcpp::NodeOptions & options) 30 | : Node("listener", options) 31 | { 32 | // Create a callback function for when messages are received. 33 | // Variations of this function also exist using, for example, UniquePtr for zero-copy transport. 34 | auto callback = 35 | [this](std_msgs::msg::String::ConstSharedPtr msg) -> void 36 | { 37 | RCLCPP_INFO(this->get_logger(), "I heard: [%s]", msg->data.c_str()); 38 | std::flush(std::cout); 39 | }; 40 | 41 | // Create a subscription to the "chatter" topic which can be matched with one or more 42 | // compatible ROS publishers. 43 | // Note that not all publishers on the same topic with the same type will be compatible: 44 | // they must have compatible Quality of Service policies. 45 | sub_ = create_subscription("chatter", 10, callback); 46 | } 47 | 48 | } // namespace composition 49 | 50 | #include "rclcpp_components/register_node_macro.hpp" 51 | 52 | // Register the component with class_loader. 53 | // This acts as a sort of entry point, allowing the component to be discoverable when its library 54 | // is being loaded into a running process. 55 | RCLCPP_COMPONENTS_REGISTER_NODE(composition::Listener) 56 | -------------------------------------------------------------------------------- /composition/src/server_component.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "composition/server_component.hpp" 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | #include "example_interfaces/srv/add_two_ints.hpp" 22 | #include "rclcpp/rclcpp.hpp" 23 | 24 | namespace composition 25 | { 26 | 27 | Server::Server(const rclcpp::NodeOptions & options) 28 | : Node("Server", options) 29 | { 30 | auto handle_add_two_ints = 31 | [this]( 32 | const std::shared_ptr request, 33 | std::shared_ptr response 34 | ) -> void 35 | { 36 | RCLCPP_INFO( 37 | this->get_logger(), "Incoming request: [a: %" PRId64 ", b: %" PRId64 "]", 38 | request->a, request->b); 39 | std::flush(std::cout); 40 | response->sum = request->a + request->b; 41 | }; 42 | 43 | srv_ = create_service("add_two_ints", handle_add_two_ints); 44 | } 45 | 46 | } // namespace composition 47 | 48 | #include "rclcpp_components/register_node_macro.hpp" 49 | 50 | // Register the component with class_loader. 51 | // This acts as a sort of entry point, allowing the component to be discoverable when its library 52 | // is being loaded into a running process. 53 | RCLCPP_COMPONENTS_REGISTER_NODE(composition::Server) 54 | -------------------------------------------------------------------------------- /composition/src/talker_component.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "composition/talker_component.hpp" 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include "rclcpp/rclcpp.hpp" 23 | #include "std_msgs/msg/string.hpp" 24 | 25 | using namespace std::chrono_literals; 26 | 27 | namespace composition 28 | { 29 | 30 | // Create a Talker "component" that subclasses the generic rclcpp::Node base class. 31 | // Components get built into shared libraries and as such do not write their own main functions. 32 | // The process using the component's shared library will instantiate the class as a ROS node. 33 | Talker::Talker(const rclcpp::NodeOptions & options) 34 | : Node("talker", options), count_(0) 35 | { 36 | // Create a publisher of "std_mgs/String" messages on the "chatter" topic. 37 | pub_ = create_publisher("chatter", 10); 38 | 39 | // Use a timer to schedule periodic message publishing. 40 | timer_ = create_wall_timer(1s, [this]() {return this->on_timer();}); 41 | } 42 | 43 | void Talker::on_timer() 44 | { 45 | auto msg = std::make_unique(); 46 | msg->data = "Hello World: " + std::to_string(++count_); 47 | RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", msg->data.c_str()); 48 | std::flush(std::cout); 49 | 50 | // Put the message into a queue to be processed by the middleware. 51 | // This call is non-blocking. 52 | pub_->publish(std::move(msg)); 53 | } 54 | 55 | } // namespace composition 56 | 57 | #include "rclcpp_components/register_node_macro.hpp" 58 | 59 | // Register the component with class_loader. 60 | // This acts as a sort of entry point, allowing the component to be discoverable when its library 61 | // is being loaded into a running process. 62 | RCLCPP_COMPONENTS_REGISTER_NODE(composition::Talker) 63 | -------------------------------------------------------------------------------- /composition/test/composition_all.regex: -------------------------------------------------------------------------------- 1 | Publishing: 'Hello World: (\d+)'(\n.*)*I heard: \[Hello World: \1\] 2 | Incoming request: \[a: \d+, b: \d+\] 3 | Got result: \[\d+\] 4 | -------------------------------------------------------------------------------- /composition/test/composition_pubsub.regex: -------------------------------------------------------------------------------- 1 | Publishing: 'Hello World: (\d+)'(\n.*)*I heard: \[Hello World: \1\] 2 | -------------------------------------------------------------------------------- /composition/test/composition_srv.regex: -------------------------------------------------------------------------------- 1 | Incoming request: \[a: \d+, b: \d+\] 2 | Got result: \[\d+\] 3 | -------------------------------------------------------------------------------- /composition/test/test_dlopen_composition.py.in: -------------------------------------------------------------------------------- 1 | # Copyright 2016 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import unittest 16 | 17 | from launch import LaunchDescription 18 | from launch.actions import ExecuteProcess 19 | 20 | import launch_testing 21 | import launch_testing.actions 22 | import launch_testing.asserts 23 | import launch_testing_ros 24 | 25 | 26 | def generate_test_description(): 27 | launch_description = LaunchDescription() 28 | process_under_test = ExecuteProcess( 29 | cmd=[ 30 | '@DLOPEN_COMPOSITION_EXECUTABLE@', 31 | '@TALKER_LIBRARY@', 32 | '@LISTENER_LIBRARY@', 33 | '@SERVER_LIBRARY@', 34 | '@CLIENT_LIBRARY@' 35 | ], 36 | name='test_dlopen_composition', 37 | output='screen' 38 | ) 39 | launch_description.add_action(process_under_test) 40 | launch_description.add_action( 41 | launch_testing.actions.ReadyToTest() 42 | ) 43 | return launch_description, locals() 44 | 45 | 46 | class TestComposition(unittest.TestCase): 47 | 48 | def test_dlopen_composition(self, proc_output, process_under_test): 49 | """Test process' output against expectations.""" 50 | output_filter = launch_testing_ros.tools.basic_output_filter( 51 | filtered_rmw_implementation='@rmw_implementation@' 52 | ) 53 | proc_output.assertWaitFor( 54 | expected_output=launch_testing.tools.expected_output_from_file( 55 | path='@EXPECTED_OUTPUT_ALL@' 56 | ), process=process_under_test, output_filter=output_filter, timeout=10 57 | ) 58 | -------------------------------------------------------------------------------- /composition/test/test_linktime_composition.py.in: -------------------------------------------------------------------------------- 1 | # Copyright 2016 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import os 16 | 17 | import unittest 18 | from unittest.case import SkipTest 19 | 20 | from launch import LaunchDescription 21 | from launch.actions import ExecuteProcess 22 | 23 | import launch_testing 24 | import launch_testing.actions 25 | import launch_testing.asserts 26 | import launch_testing_ros 27 | 28 | 29 | def generate_test_description(): 30 | launch_description = LaunchDescription() 31 | process_under_test = ExecuteProcess( 32 | cmd=['@LINKTIME_COMPOSITION_EXECUTABLE@'], 33 | name='test_linktime_composition', 34 | output='screen' 35 | ) 36 | launch_description.add_action(process_under_test) 37 | launch_description.add_action( 38 | launch_testing.actions.ReadyToTest() 39 | ) 40 | return launch_description, locals() 41 | 42 | 43 | class TestComposition(unittest.TestCase): 44 | 45 | def test_linktime_composition(self, proc_output, process_under_test): 46 | """Test process' output against expectations.""" 47 | if os.name == 'nt': 48 | print('The link time registration of classes does not work on Windows') 49 | raise SkipTest 50 | output_filter = launch_testing_ros.tools.basic_output_filter( 51 | filtered_rmw_implementation='@rmw_implementation@' 52 | ) 53 | proc_output.assertWaitFor( 54 | expected_output=launch_testing.tools.expected_output_from_file( 55 | path='@EXPECTED_OUTPUT_ALL@' 56 | ), process=process_under_test, output_filter=output_filter, timeout=10 57 | ) 58 | -------------------------------------------------------------------------------- /composition/test/test_manual_composition.py.in: -------------------------------------------------------------------------------- 1 | # Copyright 2016 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import unittest 16 | 17 | from launch import LaunchDescription 18 | from launch.actions import ExecuteProcess 19 | 20 | import launch_testing 21 | import launch_testing.actions 22 | import launch_testing.asserts 23 | import launch_testing_ros 24 | from launch_testing_ros.actions import EnableRmwIsolation 25 | 26 | 27 | def generate_test_description(): 28 | launch_description = LaunchDescription([ 29 | EnableRmwIsolation(), 30 | ]) 31 | process_under_test = ExecuteProcess( 32 | cmd=['@MANUAL_COMPOSITION_EXECUTABLE@'], 33 | name='test_manual_composition', 34 | output='screen' 35 | ) 36 | launch_description.add_action(process_under_test) 37 | launch_description.add_action( 38 | launch_testing.actions.ReadyToTest() 39 | ) 40 | return launch_description, locals() 41 | 42 | 43 | class TestComposition(unittest.TestCase): 44 | 45 | def test_manual_composition(self, proc_output, process_under_test): 46 | """Test process' output against expectations.""" 47 | output_filter = launch_testing_ros.tools.basic_output_filter( 48 | filtered_rmw_implementation='@rmw_implementation@' 49 | ) 50 | proc_output.assertWaitFor( 51 | expected_output=launch_testing.tools.expected_output_from_file( 52 | path='@EXPECTED_OUTPUT_ALL@' 53 | ), process=process_under_test, output_filter=output_filter, timeout=10 54 | ) 55 | -------------------------------------------------------------------------------- /demo_nodes_cpp/img/allocator_tutorial.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/demos/8f361a1ece2daae54016959f3ea9ab2e7692ddbb/demo_nodes_cpp/img/allocator_tutorial.png -------------------------------------------------------------------------------- /demo_nodes_cpp/img/content_filtering_messaging.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/demos/8f361a1ece2daae54016959f3ea9ab2e7692ddbb/demo_nodes_cpp/img/content_filtering_messaging.png -------------------------------------------------------------------------------- /demo_nodes_cpp/img/even_parameters_node.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/demos/8f361a1ece2daae54016959f3ea9ab2e7692ddbb/demo_nodes_cpp/img/even_parameters_node.png -------------------------------------------------------------------------------- /demo_nodes_cpp/img/one_off_timer.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/demos/8f361a1ece2daae54016959f3ea9ab2e7692ddbb/demo_nodes_cpp/img/one_off_timer.png -------------------------------------------------------------------------------- /demo_nodes_cpp/img/parameter_blackboard.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/demos/8f361a1ece2daae54016959f3ea9ab2e7692ddbb/demo_nodes_cpp/img/parameter_blackboard.png -------------------------------------------------------------------------------- /demo_nodes_cpp/img/serialized_messaging.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/demos/8f361a1ece2daae54016959f3ea9ab2e7692ddbb/demo_nodes_cpp/img/serialized_messaging.png -------------------------------------------------------------------------------- /demo_nodes_cpp/img/server_client.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/demos/8f361a1ece2daae54016959f3ea9ab2e7692ddbb/demo_nodes_cpp/img/server_client.png -------------------------------------------------------------------------------- /demo_nodes_cpp/img/talker_listener.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/demos/8f361a1ece2daae54016959f3ea9ab2e7692ddbb/demo_nodes_cpp/img/talker_listener.png -------------------------------------------------------------------------------- /demo_nodes_cpp/include/demo_nodes_cpp/visibility_control.h: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef DEMO_NODES_CPP__VISIBILITY_CONTROL_H_ 16 | #define DEMO_NODES_CPP__VISIBILITY_CONTROL_H_ 17 | 18 | #ifdef __cplusplus 19 | extern "C" 20 | { 21 | #endif 22 | 23 | // This logic was borrowed (then namespaced) from the examples on the gcc wiki: 24 | // https://gcc.gnu.org/wiki/Visibility 25 | 26 | #if defined _WIN32 || defined __CYGWIN__ 27 | #ifdef __GNUC__ 28 | #define DEMO_NODES_CPP_EXPORT __attribute__ ((dllexport)) 29 | #define DEMO_NODES_CPP_IMPORT __attribute__ ((dllimport)) 30 | #else 31 | #define DEMO_NODES_CPP_EXPORT __declspec(dllexport) 32 | #define DEMO_NODES_CPP_IMPORT __declspec(dllimport) 33 | #endif 34 | #ifdef DEMO_NODES_CPP_BUILDING_DLL 35 | #define DEMO_NODES_CPP_PUBLIC DEMO_NODES_CPP_EXPORT 36 | #else 37 | #define DEMO_NODES_CPP_PUBLIC DEMO_NODES_CPP_IMPORT 38 | #endif 39 | #define DEMO_NODES_CPP_PUBLIC_TYPE DEMO_NODES_CPP_PUBLIC 40 | #define DEMO_NODES_CPP_LOCAL 41 | #else 42 | #define DEMO_NODES_CPP_EXPORT __attribute__ ((visibility("default"))) 43 | #define DEMO_NODES_CPP_IMPORT 44 | #if __GNUC__ >= 4 45 | #define DEMO_NODES_CPP_PUBLIC __attribute__ ((visibility("default"))) 46 | #define DEMO_NODES_CPP_LOCAL __attribute__ ((visibility("hidden"))) 47 | #else 48 | #define DEMO_NODES_CPP_PUBLIC 49 | #define DEMO_NODES_CPP_LOCAL 50 | #endif 51 | #define DEMO_NODES_CPP_PUBLIC_TYPE 52 | #endif 53 | 54 | #ifdef __cplusplus 55 | } 56 | #endif 57 | 58 | #endif // DEMO_NODES_CPP__VISIBILITY_CONTROL_H_ 59 | -------------------------------------------------------------------------------- /demo_nodes_cpp/launch/services/add_two_ints_async_launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2018 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Launch a add_two_ints_server and a (synchronous) add_two_ints_client.""" 16 | 17 | import launch 18 | import launch_ros.actions 19 | 20 | 21 | def generate_launch_description(): 22 | server = launch_ros.actions.Node( 23 | package='demo_nodes_cpp', executable='add_two_ints_server', output='screen') 24 | client = launch_ros.actions.Node( 25 | package='demo_nodes_cpp', executable='add_two_ints_client_async', output='screen') 26 | return launch.LaunchDescription([ 27 | server, 28 | client, 29 | # TODO(wjwwood): replace this with a `required=True|False` option on ExecuteProcess(). 30 | # Shutdown launch when client exits. 31 | launch.actions.RegisterEventHandler( 32 | event_handler=launch.event_handlers.OnProcessExit( 33 | target_action=client, 34 | on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())], 35 | )), 36 | ]) 37 | -------------------------------------------------------------------------------- /demo_nodes_cpp/launch/services/add_two_ints_async_launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /demo_nodes_cpp/launch/services/add_two_ints_launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2018 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Launch a add_two_ints_server and a (synchronous) add_two_ints_client.""" 16 | 17 | import launch 18 | import launch_ros.actions 19 | 20 | 21 | def generate_launch_description(): 22 | server = launch_ros.actions.Node( 23 | package='demo_nodes_cpp', executable='add_two_ints_server', output='screen') 24 | client = launch_ros.actions.Node( 25 | package='demo_nodes_cpp', executable='add_two_ints_client', output='screen') 26 | return launch.LaunchDescription([ 27 | server, 28 | client, 29 | # TODO(wjwwood): replace this with a `required=True|False` option on ExecuteProcess(). 30 | # Shutdown launch when client exits. 31 | launch.actions.RegisterEventHandler( 32 | event_handler=launch.event_handlers.OnProcessExit( 33 | target_action=client, 34 | on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())], 35 | )), 36 | ]) 37 | -------------------------------------------------------------------------------- /demo_nodes_cpp/launch/services/add_two_ints_launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /demo_nodes_cpp/launch/services/introspect_services_launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2023 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Launch the introspection service and client.""" 16 | 17 | import launch 18 | import launch_ros.actions 19 | 20 | 21 | def generate_launch_description(): 22 | server = launch_ros.actions.Node( 23 | package='demo_nodes_cpp', executable='introspection_service', output='screen') 24 | client = launch_ros.actions.Node( 25 | package='demo_nodes_cpp', executable='introspection_client', output='screen') 26 | return launch.LaunchDescription([ 27 | server, 28 | client, 29 | ]) 30 | -------------------------------------------------------------------------------- /demo_nodes_cpp/launch/topics/talker_listener_best_effort_launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2018 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Launch a talker and a best effort listener.""" 16 | 17 | from launch import LaunchDescription 18 | import launch_ros.actions 19 | 20 | 21 | def generate_launch_description(): 22 | return LaunchDescription([ 23 | launch_ros.actions.Node( 24 | package='demo_nodes_cpp', executable='talker', output='screen'), 25 | launch_ros.actions.Node( 26 | package='demo_nodes_cpp', executable='listener_best_effort', output='screen'), 27 | ]) 28 | -------------------------------------------------------------------------------- /demo_nodes_cpp/launch/topics/talker_listener_best_effort_launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /demo_nodes_cpp/launch/topics/talker_listener_best_effort_launch.yaml: -------------------------------------------------------------------------------- 1 | launch: 2 | - node: 3 | pkg: "demo_nodes_cpp" 4 | exec: "talker" 5 | output: "screen" 6 | - node: 7 | pkg: "demo_nodes_cpp" 8 | exec: "listener_best_effort" 9 | output: "screen" 10 | -------------------------------------------------------------------------------- /demo_nodes_cpp/launch/topics/talker_listener_launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2018 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Launch a talker and a listener.""" 16 | 17 | from launch import LaunchDescription 18 | import launch_ros.actions 19 | 20 | 21 | def generate_launch_description(): 22 | return LaunchDescription([ 23 | launch_ros.actions.Node( 24 | package='demo_nodes_cpp', executable='talker', output='screen'), 25 | launch_ros.actions.Node( 26 | package='demo_nodes_cpp', executable='listener', output='screen'), 27 | ]) 28 | -------------------------------------------------------------------------------- /demo_nodes_cpp/launch/topics/talker_listener_launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /demo_nodes_cpp/launch/topics/talker_listener_launch.yaml: -------------------------------------------------------------------------------- 1 | launch: 2 | - node: 3 | pkg: "demo_nodes_cpp" 4 | exec: "talker" 5 | output: "screen" 6 | - node: 7 | pkg: "demo_nodes_cpp" 8 | exec: "listener" 9 | output: "screen" 10 | -------------------------------------------------------------------------------- /demo_nodes_cpp/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | demo_nodes_cpp 5 | 0.37.0 6 | 7 | C++ nodes which were previously in the ros2/examples repository but are now just used for demo purposes. 8 | 9 | 10 | Aditya Pande 11 | Audrow Nash 12 | 13 | Apache License 2.0 14 | 15 | Mabel Zhang 16 | William Woodall 17 | 18 | ament_cmake 19 | 20 | example_interfaces 21 | rcl 22 | rclcpp 23 | rclcpp_components 24 | rcl_interfaces 25 | rcpputils 26 | rcutils 27 | rmw 28 | std_msgs 29 | 30 | example_interfaces 31 | launch_ros 32 | launch_xml 33 | rcl 34 | rclcpp 35 | rclcpp_components 36 | rcl_interfaces 37 | rcpputils 38 | rcutils 39 | rmw 40 | std_msgs 41 | 42 | ament_cmake_pytest 43 | ament_lint_auto 44 | ament_lint_common 45 | launch 46 | launch_testing 47 | launch_testing_ament_cmake 48 | launch_testing_ros 49 | 50 | 51 | ament_cmake 52 | 53 | 54 | -------------------------------------------------------------------------------- /demo_nodes_cpp/src/parameters/parameter_blackboard.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | 18 | #include "rcl_interfaces/srv/list_parameters.hpp" 19 | #include "rclcpp/rclcpp.hpp" 20 | #include "rclcpp_components/register_node_macro.hpp" 21 | 22 | #include "demo_nodes_cpp/visibility_control.h" 23 | 24 | namespace demo_nodes_cpp 25 | { 26 | 27 | class ParameterBlackboard : public rclcpp::Node 28 | { 29 | public: 30 | DEMO_NODES_CPP_PUBLIC 31 | explicit ParameterBlackboard( 32 | rclcpp::NodeOptions options 33 | ) 34 | : Node( 35 | "parameter_blackboard", 36 | options.allow_undeclared_parameters(true). 37 | automatically_declare_parameters_from_overrides(true)) 38 | { 39 | RCLCPP_INFO( 40 | this->get_logger(), 41 | "Parameter blackboard node named '%s' ready, and serving '%zu' parameters already!", 42 | this->get_fully_qualified_name(), this->list_parameters( 43 | {}, rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE).names.size()); 44 | } 45 | }; 46 | 47 | } // namespace demo_nodes_cpp 48 | 49 | RCLCPP_COMPONENTS_REGISTER_NODE(demo_nodes_cpp::ParameterBlackboard) 50 | -------------------------------------------------------------------------------- /demo_nodes_cpp/src/timers/one_off_timer.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2017 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include "rclcpp/rclcpp.hpp" 18 | #include "rclcpp_components/register_node_macro.hpp" 19 | 20 | #include "demo_nodes_cpp/visibility_control.h" 21 | 22 | using namespace std::chrono_literals; 23 | 24 | namespace demo_nodes_cpp 25 | { 26 | 27 | class OneOffTimerNode final : public rclcpp::Node 28 | { 29 | public: 30 | DEMO_NODES_CPP_PUBLIC 31 | explicit OneOffTimerNode(const rclcpp::NodeOptions & options) 32 | : Node("one_off_timer", options), count_(0) 33 | { 34 | periodic_timer_ = this->create_wall_timer( 35 | 2s, 36 | [this]() { 37 | RCLCPP_INFO(this->get_logger(), "in periodic_timer callback"); 38 | if (this->count_++ % 3 == 0) { 39 | RCLCPP_INFO(this->get_logger(), " resetting one off timer"); 40 | this->one_off_timer_ = this->create_wall_timer( 41 | 1s, [this]() { 42 | RCLCPP_INFO(this->get_logger(), "in one_off_timer callback"); 43 | this->one_off_timer_->cancel(); 44 | }); 45 | } else { 46 | RCLCPP_INFO(this->get_logger(), " not resetting one off timer"); 47 | } 48 | }); 49 | } 50 | 51 | private: 52 | rclcpp::TimerBase::SharedPtr periodic_timer_; 53 | rclcpp::TimerBase::SharedPtr one_off_timer_; 54 | size_t count_; 55 | }; 56 | 57 | } // namespace demo_nodes_cpp 58 | 59 | RCLCPP_COMPONENTS_REGISTER_NODE(demo_nodes_cpp::OneOffTimerNode) 60 | -------------------------------------------------------------------------------- /demo_nodes_cpp/src/timers/reuse_timer.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2017 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include "rclcpp/rclcpp.hpp" 18 | #include "rclcpp_components/register_node_macro.hpp" 19 | #include "demo_nodes_cpp/visibility_control.h" 20 | 21 | using namespace std::chrono_literals; 22 | 23 | namespace demo_nodes_cpp 24 | { 25 | 26 | class ReuseTimerNode final : public rclcpp::Node 27 | { 28 | public: 29 | DEMO_NODES_CPP_PUBLIC 30 | explicit ReuseTimerNode(const rclcpp::NodeOptions & options) 31 | : Node("reuse_timer", options), count_(0) 32 | { 33 | one_off_timer_ = this->create_wall_timer( 34 | 1s, 35 | [this]() { 36 | RCLCPP_INFO(this->get_logger(), "in one_off_timer callback"); 37 | this->one_off_timer_->cancel(); 38 | }); 39 | // cancel immediately to prevent it running the first time. 40 | one_off_timer_->cancel(); 41 | 42 | periodic_timer_ = this->create_wall_timer( 43 | 2s, 44 | [this]() { 45 | RCLCPP_INFO(this->get_logger(), "in periodic_timer callback"); 46 | if (this->count_++ % 3 == 0) { 47 | RCLCPP_INFO(this->get_logger(), " resetting one off timer"); 48 | this->one_off_timer_->reset(); 49 | } else { 50 | RCLCPP_INFO(this->get_logger(), " not resetting one off timer"); 51 | } 52 | }); 53 | } 54 | 55 | private: 56 | rclcpp::TimerBase::SharedPtr periodic_timer_; 57 | rclcpp::TimerBase::SharedPtr one_off_timer_; 58 | size_t count_; 59 | }; 60 | 61 | } // namespace demo_nodes_cpp 62 | 63 | RCLCPP_COMPONENTS_REGISTER_NODE(demo_nodes_cpp::ReuseTimerNode) 64 | -------------------------------------------------------------------------------- /demo_nodes_cpp/src/topics/listener.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2014 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "rclcpp/rclcpp.hpp" 16 | #include "rclcpp_components/register_node_macro.hpp" 17 | 18 | #include "std_msgs/msg/string.hpp" 19 | 20 | #include "demo_nodes_cpp/visibility_control.h" 21 | 22 | namespace demo_nodes_cpp 23 | { 24 | // Create a Listener class that subclasses the generic rclcpp::Node base class. 25 | // The main function below will instantiate the class as a ROS node. 26 | class Listener : public rclcpp::Node 27 | { 28 | public: 29 | DEMO_NODES_CPP_PUBLIC 30 | explicit Listener(const rclcpp::NodeOptions & options) 31 | : Node("listener", options) 32 | { 33 | // Create a callback function for when messages are received. 34 | // Variations of this function also exist using, for example UniquePtr for zero-copy transport. 35 | setvbuf(stdout, NULL, _IONBF, BUFSIZ); 36 | auto callback = 37 | [this](std_msgs::msg::String::ConstSharedPtr msg) -> void 38 | { 39 | RCLCPP_INFO(this->get_logger(), "I heard: [%s]", msg->data.c_str()); 40 | }; 41 | // Create a subscription to the topic which can be matched with one or more compatible ROS 42 | // publishers. 43 | // Note that not all publishers on the same topic with the same type will be compatible: 44 | // they must have compatible Quality of Service policies. 45 | sub_ = create_subscription("chatter", 10, callback); 46 | } 47 | 48 | private: 49 | rclcpp::Subscription::SharedPtr sub_; 50 | }; 51 | 52 | } // namespace demo_nodes_cpp 53 | 54 | RCLCPP_COMPONENTS_REGISTER_NODE(demo_nodes_cpp::Listener) 55 | -------------------------------------------------------------------------------- /demo_nodes_cpp/src/topics/listener_best_effort.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2015 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | 18 | #include "rclcpp/rclcpp.hpp" 19 | #include "rclcpp_components/register_node_macro.hpp" 20 | #include "std_msgs/msg/string.hpp" 21 | 22 | #include "demo_nodes_cpp/visibility_control.h" 23 | 24 | namespace demo_nodes_cpp 25 | { 26 | class ListenerBestEffort : public rclcpp::Node 27 | { 28 | public: 29 | DEMO_NODES_CPP_PUBLIC 30 | explicit ListenerBestEffort(const rclcpp::NodeOptions & options) 31 | : Node("listener", options) 32 | { 33 | setvbuf(stdout, NULL, _IONBF, BUFSIZ); 34 | auto callback = 35 | [this](std_msgs::msg::String::ConstSharedPtr msg) -> void 36 | { 37 | RCLCPP_INFO(this->get_logger(), "I heard: [%s]", msg->data.c_str()); 38 | }; 39 | 40 | sub_ = create_subscription("chatter", rclcpp::SensorDataQoS(), callback); 41 | } 42 | 43 | private: 44 | rclcpp::Subscription::SharedPtr sub_; 45 | }; 46 | 47 | } // namespace demo_nodes_cpp 48 | 49 | RCLCPP_COMPONENTS_REGISTER_NODE(demo_nodes_cpp::ListenerBestEffort) 50 | -------------------------------------------------------------------------------- /demo_nodes_cpp/test/add_two_ints_client.txt: -------------------------------------------------------------------------------- 1 | Result of add_two_ints: 5 2 | -------------------------------------------------------------------------------- /demo_nodes_cpp/test/add_two_ints_client_async.txt: -------------------------------------------------------------------------------- 1 | Result of add_two_ints: 5 2 | -------------------------------------------------------------------------------- /demo_nodes_cpp/test/add_two_ints_server.txt: -------------------------------------------------------------------------------- 1 | Incoming request 2 | a: 2 b: 3 3 | -------------------------------------------------------------------------------- /demo_nodes_cpp/test/content_filtering_publisher.txt: -------------------------------------------------------------------------------- 1 | Publishing: '-100.000000' 2 | Publishing: '-90.000000' 3 | Publishing: '-80.000000' 4 | Publishing: '-70.000000' 5 | Publishing: '-60.000000' 6 | Publishing: '-50.000000' 7 | Publishing: '-40.000000' 8 | Publishing: '-30.000000' 9 | Publishing: '-20.000000' 10 | Publishing: '-10.000000' 11 | Publishing: '0.000000' 12 | Publishing: '10.000000' 13 | Publishing: '20.000000' 14 | Publishing: '30.000000' 15 | Publishing: '40.000000' 16 | Publishing: '50.000000' 17 | Publishing: '60.000000' 18 | Publishing: '70.000000' 19 | Publishing: '80.000000' 20 | Publishing: '90.000000' 21 | Publishing: '100.000000' 22 | Publishing: '110.000000' 23 | Publishing: '120.000000' 24 | Publishing: '130.000000' 25 | Publishing: '140.000000' 26 | Publishing: '150.000000' 27 | -------------------------------------------------------------------------------- /demo_nodes_cpp/test/content_filtering_subscriber-rmw_connextdds.txt: -------------------------------------------------------------------------------- 1 | I receive an emergency temperature data: [-100.000000] 2 | I receive an emergency temperature data: [-90.000000] 3 | I receive an emergency temperature data: [-80.000000] 4 | I receive an emergency temperature data: [-70.000000] 5 | I receive an emergency temperature data: [-60.000000] 6 | I receive an emergency temperature data: [-50.000000] 7 | I receive an emergency temperature data: [-40.000000] 8 | I receive an emergency temperature data: [110.000000] 9 | I receive an emergency temperature data: [120.000000] 10 | I receive an emergency temperature data: [130.000000] 11 | I receive an emergency temperature data: [140.000000] 12 | I receive an emergency temperature data: [150.000000] 13 | -------------------------------------------------------------------------------- /demo_nodes_cpp/test/content_filtering_subscriber-rmw_fastrtps_cpp.txt: -------------------------------------------------------------------------------- 1 | I receive an emergency temperature data: [-100.000000] 2 | I receive an emergency temperature data: [-90.000000] 3 | I receive an emergency temperature data: [-80.000000] 4 | I receive an emergency temperature data: [-70.000000] 5 | I receive an emergency temperature data: [-60.000000] 6 | I receive an emergency temperature data: [-50.000000] 7 | I receive an emergency temperature data: [-40.000000] 8 | I receive an emergency temperature data: [110.000000] 9 | I receive an emergency temperature data: [120.000000] 10 | I receive an emergency temperature data: [130.000000] 11 | I receive an emergency temperature data: [140.000000] 12 | I receive an emergency temperature data: [150.000000] 13 | -------------------------------------------------------------------------------- /demo_nodes_cpp/test/content_filtering_subscriber.txt: -------------------------------------------------------------------------------- 1 | Content filter is not enabled since it's not supported 2 | -------------------------------------------------------------------------------- /demo_nodes_cpp/test/list_parameters.txt: -------------------------------------------------------------------------------- 1 | Setting parameters... 2 | Listing parameters... 3 | 4 | Parameter names: 5 | bar 6 | foo 7 | foo.first 8 | foo.second 9 | Parameter prefixes: 10 | foo 11 | -------------------------------------------------------------------------------- /demo_nodes_cpp/test/list_parameters_async.txt: -------------------------------------------------------------------------------- 1 | Setting parameters... 2 | Listing parameters... 3 | 4 | Parameter names: 5 | bar 6 | foo 7 | foo.first 8 | foo.second 9 | Parameter prefixes: 10 | foo 11 | -------------------------------------------------------------------------------- /demo_nodes_cpp/test/listener.regex: -------------------------------------------------------------------------------- 1 | I heard: \[Hello World: \d+\] 2 | -------------------------------------------------------------------------------- /demo_nodes_cpp/test/matched_event_detect.txt: -------------------------------------------------------------------------------- 1 | Create a new subscription. 2 | First subscription is connected. 3 | Create a new subscription. 4 | The changed number of connected subscription is 1 and current number of connected subscription is 2. 5 | Destroy a subscription. 6 | The changed number of connected subscription is -1 and current number of connected subscription is 1. 7 | Destroy a subscription. 8 | Last subscription is disconnected. 9 | Create a new publisher. 10 | First publisher is connected. 11 | Create a new publisher. 12 | The changed number of connected publisher is 1 and current number of connected publisher is 2. 13 | Destroy a publisher. 14 | The changed number of connected publisher is -1 and current number of connected publisher is 1. 15 | Destroy a publisher. 16 | Last publisher is disconnected. -------------------------------------------------------------------------------- /demo_nodes_cpp/test/parameter_events.txt: -------------------------------------------------------------------------------- 1 | 2 | Parameter event: 3 | new parameters: 4 | foo 5 | changed parameters: 6 | deleted parameters: 7 | 8 | 9 | Parameter event: 10 | new parameters: 11 | bar 12 | changed parameters: 13 | deleted parameters: 14 | 15 | 16 | Parameter event: 17 | new parameters: 18 | baz 19 | changed parameters: 20 | deleted parameters: 21 | 22 | 23 | Parameter event: 24 | new parameters: 25 | foobar 26 | changed parameters: 27 | deleted parameters: 28 | 29 | 30 | Parameter event: 31 | new parameters: 32 | changed parameters: 33 | foo 34 | deleted parameters: 35 | 36 | 37 | Parameter event: 38 | new parameters: 39 | changed parameters: 40 | bar 41 | deleted parameters: 42 | 43 | 44 | Parameter event: 45 | new parameters: 46 | changed parameters: 47 | baz 48 | deleted parameters: 49 | 50 | 51 | Parameter event: 52 | new parameters: 53 | changed parameters: 54 | foobar 55 | deleted parameters: 56 | 57 | 58 | Parameter event: 59 | new parameters: 60 | changed parameters: 61 | foo 62 | deleted parameters: 63 | 64 | 65 | Parameter event: 66 | new parameters: 67 | changed parameters: 68 | bar 69 | deleted parameters: 70 | 71 | -------------------------------------------------------------------------------- /demo_nodes_cpp/test/parameter_events_async.txt: -------------------------------------------------------------------------------- 1 | 2 | Parameter event: 3 | new parameters: 4 | foo 5 | changed parameters: 6 | deleted parameters: 7 | 8 | 9 | Parameter event: 10 | new parameters: 11 | bar 12 | changed parameters: 13 | deleted parameters: 14 | 15 | 16 | Parameter event: 17 | new parameters: 18 | baz 19 | changed parameters: 20 | deleted parameters: 21 | 22 | 23 | Parameter event: 24 | new parameters: 25 | foobar 26 | changed parameters: 27 | deleted parameters: 28 | 29 | 30 | Parameter event: 31 | new parameters: 32 | changed parameters: 33 | foo 34 | deleted parameters: 35 | 36 | 37 | Parameter event: 38 | new parameters: 39 | changed parameters: 40 | bar 41 | deleted parameters: 42 | 43 | 44 | Parameter event: 45 | new parameters: 46 | changed parameters: 47 | baz 48 | deleted parameters: 49 | 50 | 51 | Parameter event: 52 | new parameters: 53 | changed parameters: 54 | foobar 55 | deleted parameters: 56 | 57 | 58 | Parameter event: 59 | new parameters: 60 | changed parameters: 61 | foo 62 | deleted parameters: 63 | 64 | 65 | Parameter event: 66 | new parameters: 67 | changed parameters: 68 | bar 69 | deleted parameters: 70 | 71 | -------------------------------------------------------------------------------- /demo_nodes_cpp/test/set_and_get_parameters.txt: -------------------------------------------------------------------------------- 1 | 2 | Parameter name: foo 3 | Parameter value (integer): 2 4 | Parameter name: baz 5 | Parameter value (double): 1.450000 6 | Parameter name: foobarbaz 7 | Parameter value (bool_array): [true, false] 8 | Parameter name: toto 9 | Parameter value (byte_array): [0xff, 0x7f] 10 | -------------------------------------------------------------------------------- /demo_nodes_cpp/test/set_and_get_parameters_async.txt: -------------------------------------------------------------------------------- 1 | 2 | Parameter name: foo 3 | Parameter value (integer): 2 4 | Parameter name: baz 5 | Parameter value (double): 1.450000 6 | Parameter name: foobarbaz 7 | Parameter value (bool_array): [true, false] 8 | Parameter name: toto 9 | Parameter value (byte_array): [0xff, 0x7f] 10 | -------------------------------------------------------------------------------- /demo_nodes_cpp/test/talker.txt: -------------------------------------------------------------------------------- 1 | Publishing: 'Hello World: 1' 2 | -------------------------------------------------------------------------------- /demo_nodes_cpp/test/use_logger_service.txt: -------------------------------------------------------------------------------- 1 | Output with default logger level: 2 | Output 1 with INFO logger level. 3 | Output 1 with WARN logger level. 4 | Output 1 with ERROR logger level. 5 | Current logger level: 0 6 | Output with debug logger level: 7 | Output 2 with DEBUG logger level. 8 | Output 2 with INFO logger level. 9 | Output 2 with WARN logger level. 10 | Output 2 with ERROR logger level. 11 | Current logger level: 10 12 | Output with warn logger level: 13 | Output 3 with WARN logger level. 14 | Output 3 with ERROR logger level. 15 | Current logger level: 30 16 | Output with error logger level: 17 | Output 4 with ERROR logger level. 18 | Current logger level: 40 19 | -------------------------------------------------------------------------------- /demo_nodes_cpp_native/include/demo_nodes_cpp_native/visibility_control.h: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef DEMO_NODES_CPP_NATIVE__VISIBILITY_CONTROL_H_ 16 | #define DEMO_NODES_CPP_NATIVE__VISIBILITY_CONTROL_H_ 17 | 18 | #ifdef __cplusplus 19 | extern "C" 20 | { 21 | #endif 22 | 23 | // This logic was borrowed (then namespaced) from the examples on the gcc wiki: 24 | // https://gcc.gnu.org/wiki/Visibility 25 | 26 | #if defined _WIN32 || defined __CYGWIN__ 27 | #ifdef __GNUC__ 28 | #define DEMO_NODES_CPP_NATIVE_EXPORT __attribute__ ((dllexport)) 29 | #define DEMO_NODES_CPP_NATIVE_IMPORT __attribute__ ((dllimport)) 30 | #else 31 | #define DEMO_NODES_CPP_NATIVE_EXPORT __declspec(dllexport) 32 | #define DEMO_NODES_CPP_NATIVE_IMPORT __declspec(dllimport) 33 | #endif 34 | #ifdef DEMO_NODES_CPP_NATIVE_BUILDING_DLL 35 | #define DEMO_NODES_CPP_NATIVE_PUBLIC DEMO_NODES_CPP_NATIVE_EXPORT 36 | #else 37 | #define DEMO_NODES_CPP_NATIVE_PUBLIC DEMO_NODES_CPP_NATIVE_IMPORT 38 | #endif 39 | #define DEMO_NODES_CPP_NATIVE_PUBLIC_TYPE DEMO_NODES_CPP_NATIVE_PUBLIC 40 | #define DEMO_NODES_CPP_NATIVE_LOCAL 41 | #else 42 | #define DEMO_NODES_CPP_NATIVE_EXPORT __attribute__ ((visibility("default"))) 43 | #define DEMO_NODES_CPP_NATIVE_IMPORT 44 | #if __GNUC__ >= 4 45 | #define DEMO_NODES_CPP_NATIVE_PUBLIC __attribute__ ((visibility("default"))) 46 | #define DEMO_NODES_CPP_NATIVE_LOCAL __attribute__ ((visibility("hidden"))) 47 | #else 48 | #define DEMO_NODES_CPP_NATIVE_PUBLIC 49 | #define DEMO_NODES_CPP_NATIVE_LOCAL 50 | #endif 51 | #define DEMO_NODES_CPP_NATIVE_PUBLIC_TYPE 52 | #endif 53 | 54 | #ifdef __cplusplus 55 | } 56 | #endif 57 | 58 | #endif // DEMO_NODES_CPP_NATIVE__VISIBILITY_CONTROL_H_ 59 | -------------------------------------------------------------------------------- /demo_nodes_cpp_native/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | demo_nodes_cpp_native 5 | 0.37.0 6 | 7 | C++ nodes which access the native handles of the rmw implementation. 8 | 9 | 10 | Aditya Pande 11 | Audrow Nash 12 | 13 | Apache License 2.0 14 | 15 | Dirk Thomas 16 | Mabel Zhang 17 | 18 | ament_cmake 19 | 20 | rclcpp 21 | rclcpp_components 22 | rmw_fastrtps_cpp 23 | std_msgs 24 | 25 | ament_cmake_pytest 26 | ament_lint_auto 27 | ament_lint_common 28 | launch 29 | launch_testing 30 | launch_testing_ament_cmake 31 | launch_testing_ros 32 | 33 | 34 | ament_cmake 35 | 36 | 37 | -------------------------------------------------------------------------------- /demo_nodes_cpp_native/test/talker.regex: -------------------------------------------------------------------------------- 1 | eprosima::fastdds::dds::DomainParticipant \* [0-9]{2,} 2 | eprosima::fastdds::dds::DataWriter \* [0-9]{2,} 3 | Publishing: 'Hello World: 1' 4 | -------------------------------------------------------------------------------- /demo_nodes_cpp_native/test/test_executables_tutorial.py.in: -------------------------------------------------------------------------------- 1 | # generated from demo_nodes_cpp/test/test_executables_tutorial.py.in 2 | # generated code does not contain a copyright notice 3 | 4 | import unittest 5 | 6 | from launch import LaunchDescription 7 | from launch.actions import ExecuteProcess 8 | 9 | import launch_testing 10 | import launch_testing.actions 11 | import launch_testing.asserts 12 | import launch_testing_ros 13 | 14 | 15 | def generate_test_description(): 16 | launch_description = LaunchDescription() 17 | processes_under_test = [ 18 | ExecuteProcess(cmd=[executable], name='test_executable_' + str(i), output='screen') 19 | for i, executable in enumerate('@DEMO_NODES_CPP_EXECUTABLE@'.split(';')) 20 | ] 21 | for process in processes_under_test: 22 | launch_description.add_action(process) 23 | launch_description.add_action( 24 | launch_testing.actions.ReadyToTest() 25 | ) 26 | return launch_description, locals() 27 | 28 | 29 | class TestExecutablesTutorial(unittest.TestCase): 30 | 31 | def test_executables_output(self, proc_output, processes_under_test): 32 | """Test all processes output against expectations.""" 33 | from launch_testing.tools.output import get_default_filtered_prefixes 34 | output_filter = launch_testing_ros.tools.basic_output_filter( 35 | filtered_prefixes=get_default_filtered_prefixes() + [ 36 | 'service not available, waiting again...' 37 | ], 38 | filtered_rmw_implementation='rmw_fastrtps_cpp' 39 | ) 40 | output_files = '@DEMO_NODES_CPP_EXPECTED_OUTPUT@'.split(';') 41 | for process, output_file in zip(processes_under_test, output_files): 42 | proc_output.assertWaitFor( 43 | expected_output=launch_testing.tools.expected_output_from_file( 44 | path=output_file 45 | ), process=process, output_filter=output_filter, timeout=10 46 | ) 47 | 48 | 49 | @launch_testing.post_shutdown_test() 50 | class TestExecutablesTutorialAfterShutdown(unittest.TestCase): 51 | 52 | def test_last_executable_exit_code(self, proc_info, processes_under_test): 53 | """Test last process exit code.""" 54 | launch_testing.asserts.assertExitCodes( 55 | proc_info, process=processes_under_test[-1] 56 | ) 57 | -------------------------------------------------------------------------------- /demo_nodes_py/demo_nodes_py/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/demos/8f361a1ece2daae54016959f3ea9ab2e7692ddbb/demo_nodes_py/demo_nodes_py/__init__.py -------------------------------------------------------------------------------- /demo_nodes_py/demo_nodes_py/events/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/demos/8f361a1ece2daae54016959f3ea9ab2e7692ddbb/demo_nodes_py/demo_nodes_py/events/__init__.py -------------------------------------------------------------------------------- /demo_nodes_py/demo_nodes_py/logging/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/demos/8f361a1ece2daae54016959f3ea9ab2e7692ddbb/demo_nodes_py/demo_nodes_py/logging/__init__.py -------------------------------------------------------------------------------- /demo_nodes_py/demo_nodes_py/parameters/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/demos/8f361a1ece2daae54016959f3ea9ab2e7692ddbb/demo_nodes_py/demo_nodes_py/parameters/__init__.py -------------------------------------------------------------------------------- /demo_nodes_py/demo_nodes_py/parameters/params.yaml: -------------------------------------------------------------------------------- 1 | /parameter_blackboard: 2 | ros__parameters: 3 | other_int_parameter: 0 4 | int_parameter: 12 5 | string_parameter: I smell the blood of an Englishman 6 | other_string_parameter: One fish, two fish, Red fish, blue fish 7 | bool_parameter: False 8 | -------------------------------------------------------------------------------- /demo_nodes_py/demo_nodes_py/services/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/demos/8f361a1ece2daae54016959f3ea9ab2e7692ddbb/demo_nodes_py/demo_nodes_py/services/__init__.py -------------------------------------------------------------------------------- /demo_nodes_py/demo_nodes_py/services/add_two_ints_client.py: -------------------------------------------------------------------------------- 1 | # Copyright 2016 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from example_interfaces.srv import AddTwoInts 16 | 17 | import rclpy 18 | from rclpy.executors import ExternalShutdownException 19 | 20 | 21 | def main(args=None): 22 | try: 23 | with rclpy.init(args=args): 24 | node = rclpy.create_node('add_two_ints_client') 25 | 26 | cli = node.create_client(AddTwoInts, 'add_two_ints') 27 | while not cli.wait_for_service(timeout_sec=1.0): 28 | print('service not available, waiting again...') 29 | req = AddTwoInts.Request() 30 | req.a = 2 31 | req.b = 3 32 | future = cli.call_async(req) 33 | rclpy.spin_until_future_complete(node, future) 34 | if future.result() is not None: 35 | node.get_logger().info('Result of add_two_ints: %d' % future.result().sum) 36 | else: 37 | node.get_logger().error('Exception while calling service: %r' % future.exception()) 38 | except (KeyboardInterrupt, ExternalShutdownException): 39 | pass 40 | 41 | 42 | if __name__ == '__main__': 43 | main() 44 | -------------------------------------------------------------------------------- /demo_nodes_py/demo_nodes_py/services/add_two_ints_client_async.py: -------------------------------------------------------------------------------- 1 | # Copyright 2016 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from example_interfaces.srv import AddTwoInts 16 | 17 | import rclpy 18 | from rclpy.executors import ExternalShutdownException 19 | 20 | 21 | def main(args=None): 22 | try: 23 | with rclpy.init(args=args): 24 | node = rclpy.create_node('add_two_ints_client') 25 | 26 | cli = node.create_client(AddTwoInts, 'add_two_ints') 27 | 28 | req = AddTwoInts.Request() 29 | req.a = 2 30 | req.b = 3 31 | while not cli.wait_for_service(timeout_sec=1.0): 32 | print('service not available, waiting again...') 33 | future = cli.call_async(req) 34 | 35 | logger = node.get_logger() 36 | 37 | while rclpy.ok(): 38 | rclpy.spin_once(node) 39 | if future.done(): 40 | if future.result() is not None: 41 | logger.info('Result of add_two_ints: %d' % future.result().sum) 42 | else: 43 | logger.error('Exception while calling service: %r' % future.exception()) 44 | break 45 | except (KeyboardInterrupt, ExternalShutdownException): 46 | pass 47 | 48 | 49 | if __name__ == '__main__': 50 | main() 51 | -------------------------------------------------------------------------------- /demo_nodes_py/demo_nodes_py/services/add_two_ints_server.py: -------------------------------------------------------------------------------- 1 | # Copyright 2016 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from example_interfaces.srv import AddTwoInts 16 | 17 | import rclpy 18 | from rclpy.executors import ExternalShutdownException 19 | from rclpy.node import Node 20 | 21 | 22 | class AddTwoIntsServer(Node): 23 | 24 | def __init__(self): 25 | super().__init__('add_two_ints_server') 26 | self.srv = self.create_service(AddTwoInts, 'add_two_ints', self.add_two_ints_callback) 27 | 28 | def add_two_ints_callback(self, request, response): 29 | response.sum = request.a + request.b 30 | self.get_logger().info('Incoming request\na: %d b: %d' % (request.a, request.b)) 31 | 32 | return response 33 | 34 | 35 | def main(args=None): 36 | try: 37 | with rclpy.init(args=args): 38 | node = AddTwoIntsServer() 39 | 40 | rclpy.spin(node) 41 | except (KeyboardInterrupt, ExternalShutdownException): 42 | pass 43 | 44 | 45 | if __name__ == '__main__': 46 | main() 47 | -------------------------------------------------------------------------------- /demo_nodes_py/demo_nodes_py/topics/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/demos/8f361a1ece2daae54016959f3ea9ab2e7692ddbb/demo_nodes_py/demo_nodes_py/topics/__init__.py -------------------------------------------------------------------------------- /demo_nodes_py/demo_nodes_py/topics/listener.py: -------------------------------------------------------------------------------- 1 | # Copyright 2016 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import rclpy 16 | from rclpy.executors import ExternalShutdownException 17 | from rclpy.node import Node 18 | 19 | from std_msgs.msg import String 20 | 21 | 22 | class Listener(Node): 23 | 24 | def __init__(self): 25 | super().__init__('listener') 26 | self.sub = self.create_subscription(String, 'chatter', self.chatter_callback, 10) 27 | 28 | def chatter_callback(self, msg): 29 | self.get_logger().info('I heard: [%s]' % msg.data) 30 | 31 | 32 | def main(args=None): 33 | try: 34 | with rclpy.init(args=args): 35 | node = Listener() 36 | rclpy.spin(node) 37 | except (KeyboardInterrupt, ExternalShutdownException): 38 | pass 39 | 40 | 41 | if __name__ == '__main__': 42 | main() 43 | -------------------------------------------------------------------------------- /demo_nodes_py/demo_nodes_py/topics/listener_serialized.py: -------------------------------------------------------------------------------- 1 | # Copyright 2018 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import rclpy 16 | from rclpy.executors import ExternalShutdownException 17 | from rclpy.node import Node 18 | 19 | from std_msgs.msg import String 20 | 21 | 22 | class SerializedSubscriber(Node): 23 | 24 | def __init__(self): 25 | super().__init__('serialized_subscriber') 26 | self.subscription = self.create_subscription( 27 | String, 28 | 'chatter', 29 | self.listener_callback, 30 | 10, 31 | raw=True) # We're subscribing to the serialized bytes, not std_msgs.msg.String 32 | 33 | def listener_callback(self, msg): 34 | self.get_logger().info('I heard: "%s"' % msg) 35 | 36 | 37 | def main(args=None): 38 | try: 39 | with rclpy.init(args=args): 40 | serialized_subscriber = SerializedSubscriber() 41 | 42 | rclpy.spin(serialized_subscriber) 43 | except (KeyboardInterrupt, ExternalShutdownException): 44 | pass 45 | 46 | 47 | if __name__ == '__main__': 48 | main() 49 | -------------------------------------------------------------------------------- /demo_nodes_py/demo_nodes_py/topics/talker.py: -------------------------------------------------------------------------------- 1 | # Copyright 2016 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import rclpy 16 | from rclpy.executors import ExternalShutdownException 17 | from rclpy.node import Node 18 | 19 | from std_msgs.msg import String 20 | 21 | 22 | class Talker(Node): 23 | 24 | def __init__(self): 25 | super().__init__('talker') 26 | self.i = 0 27 | self.pub = self.create_publisher(String, 'chatter', 10) 28 | timer_period = 1.0 29 | self.tmr = self.create_timer(timer_period, self.timer_callback) 30 | 31 | def timer_callback(self): 32 | msg = String() 33 | msg.data = 'Hello World: {0}'.format(self.i) 34 | self.i += 1 35 | self.get_logger().info('Publishing: "{0}"'.format(msg.data)) 36 | self.pub.publish(msg) 37 | 38 | 39 | def main(args=None): 40 | try: 41 | with rclpy.init(args=args): 42 | node = Talker() 43 | 44 | rclpy.spin(node) 45 | except (KeyboardInterrupt, ExternalShutdownException): 46 | pass 47 | 48 | 49 | if __name__ == '__main__': 50 | main() 51 | -------------------------------------------------------------------------------- /demo_nodes_py/img/qos_listener_talker.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/demos/8f361a1ece2daae54016959f3ea9ab2e7692ddbb/demo_nodes_py/img/qos_listener_talker.png -------------------------------------------------------------------------------- /demo_nodes_py/img/serialized_subscriber.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/demos/8f361a1ece2daae54016959f3ea9ab2e7692ddbb/demo_nodes_py/img/serialized_subscriber.png -------------------------------------------------------------------------------- /demo_nodes_py/img/server_client.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/demos/8f361a1ece2daae54016959f3ea9ab2e7692ddbb/demo_nodes_py/img/server_client.png -------------------------------------------------------------------------------- /demo_nodes_py/img/set_parameters_callback.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/demos/8f361a1ece2daae54016959f3ea9ab2e7692ddbb/demo_nodes_py/img/set_parameters_callback.png -------------------------------------------------------------------------------- /demo_nodes_py/img/talker_listener.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/demos/8f361a1ece2daae54016959f3ea9ab2e7692ddbb/demo_nodes_py/img/talker_listener.png -------------------------------------------------------------------------------- /demo_nodes_py/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | demo_nodes_py 5 | 0.37.0 6 | 7 | Python nodes which were previously in the ros2/examples repository but are now just used for demo purposes. 8 | 9 | 10 | Aditya Pande 11 | Audrow Nash 12 | 13 | Apache License 2.0 14 | 15 | Esteve Fernandez 16 | Mabel Zhang 17 | Michael Carroll 18 | Mikael Arguedas 19 | 20 | ament_index_python 21 | example_interfaces 22 | rclpy 23 | rcl_interfaces 24 | std_msgs 25 | 26 | ament_copyright 27 | ament_flake8 28 | ament_pep257 29 | ament_xmllint 30 | python3-pytest 31 | 32 | 33 | ament_python 34 | 35 | 36 | -------------------------------------------------------------------------------- /demo_nodes_py/resource/demo_nodes_py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/demos/8f361a1ece2daae54016959f3ea9ab2e7692ddbb/demo_nodes_py/resource/demo_nodes_py -------------------------------------------------------------------------------- /demo_nodes_py/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/demo_nodes_py 3 | [install] 4 | install_scripts=$base/lib/demo_nodes_py 5 | -------------------------------------------------------------------------------- /demo_nodes_py/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import find_packages 2 | from setuptools import setup 3 | 4 | package_name = 'demo_nodes_py' 5 | 6 | setup( 7 | name=package_name, 8 | version='0.37.0', 9 | packages=find_packages(exclude=['test']), 10 | data_files=[ 11 | ('share/ament_index/resource_index/packages', ['resource/' + package_name]), 12 | ('share/' + package_name, ['package.xml', 'demo_nodes_py/parameters/params.yaml']), 13 | ], 14 | install_requires=['setuptools'], 15 | zip_safe=True, 16 | author='Esteve Fernandez', 17 | author_email='esteve@osrfoundation.org', 18 | maintainer='Aditya Pande, Audrow Nash, Michael Jeronimo', 19 | maintainer_email='aditya.pande@openrobotics.org, audrow@openrobotics.org, michael.jeronimo@openrobotics.org', # noqa: E501 20 | keywords=['ROS'], 21 | classifiers=[ 22 | 'Intended Audience :: Developers', 23 | 'License :: OSI Approved :: Apache Software License', 24 | 'Programming Language :: Python', 25 | 'Topic :: Software Development', 26 | ], 27 | description=( 28 | 'Python nodes which were previously in the ros2/examples repository ' 29 | 'but are now just used for demo purposes.' 30 | ), 31 | license='Apache License, Version 2.0', 32 | tests_require=['pytest'], 33 | entry_points={ 34 | 'console_scripts': [ 35 | 'listener = demo_nodes_py.topics.listener:main', 36 | 'talker = demo_nodes_py.topics.talker:main', 37 | 'listener_qos = demo_nodes_py.topics.listener_qos:main', 38 | 'talker_qos = demo_nodes_py.topics.talker_qos:main', 39 | 'listener_serialized = demo_nodes_py.topics.listener_serialized:main', 40 | 'add_two_ints_client = demo_nodes_py.services.add_two_ints_client:main', 41 | 'add_two_ints_client_async = demo_nodes_py.services.add_two_ints_client_async:main', 42 | 'add_two_ints_server = demo_nodes_py.services.add_two_ints_server:main', 43 | 'async_param_client = demo_nodes_py.parameters.async_param_client:main', 44 | 'set_parameters_callback = demo_nodes_py.parameters.set_parameters_callback:main', 45 | 'introspection = demo_nodes_py.services.introspection:main', 46 | 'matched_event_detect = demo_nodes_py.events.matched_event_detect:main', 47 | 'use_logger_service = demo_nodes_py.logging.use_logger_service:main', 48 | ], 49 | }, 50 | ) 51 | -------------------------------------------------------------------------------- /demo_nodes_py/test/test_copyright.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_copyright.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.copyright 20 | @pytest.mark.linter 21 | def test_copyright(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found errors' 24 | -------------------------------------------------------------------------------- /demo_nodes_py/test/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_flake8.main import main_with_errors 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc, errors = main_with_errors(argv=[]) 23 | assert rc == 0, \ 24 | 'Found %d code style errors / warnings:\n' % len(errors) + \ 25 | '\n'.join(errors) 26 | -------------------------------------------------------------------------------- /demo_nodes_py/test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found code style errors / warnings' 24 | -------------------------------------------------------------------------------- /demo_nodes_py/test/test_xmllint.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_xmllint.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.xmllint 21 | def test_xmllint() -> None: 22 | rc = main(argv=[]) 23 | assert rc == 0, 'Found errors' 24 | -------------------------------------------------------------------------------- /dummy_robot/dummy_map_server/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.20) 2 | 3 | project(dummy_map_server) 4 | 5 | # Default to C++17 6 | if(NOT CMAKE_CXX_STANDARD) 7 | set(CMAKE_CXX_STANDARD 17) 8 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 9 | endif() 10 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 11 | add_compile_options(-Wall -Wextra -Wpedantic) 12 | endif() 13 | 14 | find_package(ament_cmake REQUIRED) 15 | find_package(nav_msgs REQUIRED) 16 | find_package(rclcpp REQUIRED) 17 | find_package(rmw REQUIRED) 18 | 19 | add_executable(dummy_map_server src/dummy_map_server.cpp) 20 | target_link_libraries(dummy_map_server PRIVATE 21 | rclcpp::rclcpp 22 | ${nav_msgs_TARGETS} 23 | ) 24 | 25 | install(TARGETS dummy_map_server 26 | DESTINATION lib/${PROJECT_NAME}) 27 | 28 | if(BUILD_TESTING) 29 | find_package(ament_lint_auto REQUIRED) 30 | ament_lint_auto_find_test_dependencies() 31 | endif() 32 | 33 | ament_package() 34 | -------------------------------------------------------------------------------- /dummy_robot/dummy_map_server/img/occupancy_grid.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/demos/8f361a1ece2daae54016959f3ea9ab2e7692ddbb/dummy_robot/dummy_map_server/img/occupancy_grid.png -------------------------------------------------------------------------------- /dummy_robot/dummy_map_server/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | dummy_map_server 5 | 0.37.0 6 | 7 | dummy map server node 8 | 9 | 10 | Aditya Pande 11 | Audrow Nash 12 | 13 | Apache License 2.0 14 | 15 | Karsten Knese 16 | Mabel Zhang 17 | 18 | ament_cmake 19 | 20 | nav_msgs 21 | rclcpp 22 | 23 | nav_msgs 24 | rclcpp 25 | 26 | ament_cmake_gtest 27 | ament_lint_auto 28 | ament_lint_common 29 | 30 | 31 | ament_cmake 32 | 33 | 34 | -------------------------------------------------------------------------------- /dummy_robot/dummy_robot_bringup/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.20) 2 | 3 | project(dummy_robot_bringup) 4 | 5 | find_package(ament_cmake REQUIRED) 6 | 7 | install(DIRECTORY launch 8 | DESTINATION share/${PROJECT_NAME}) 9 | 10 | if(BUILD_TESTING) 11 | find_package(ament_lint_auto REQUIRED) 12 | ament_lint_auto_find_test_dependencies() 13 | endif() 14 | 15 | ament_package() 16 | -------------------------------------------------------------------------------- /dummy_robot/dummy_robot_bringup/launch/dummy_robot_bringup_launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2018 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import os 16 | 17 | from launch import LaunchDescription 18 | from launch.substitutions import FileContent 19 | from launch_ros.actions import Node 20 | from launch_ros.substitutions import FindPackageShare 21 | 22 | 23 | def generate_launch_description(): 24 | pkg_share = FindPackageShare('dummy_robot_bringup').find('dummy_robot_bringup') 25 | urdf_file = os.path.join(pkg_share, 'launch', 'single_rrbot.urdf') 26 | robot_desc = FileContent(urdf_file) 27 | rsp_params = {'robot_description': robot_desc} 28 | 29 | return LaunchDescription([ 30 | Node(package='dummy_map_server', executable='dummy_map_server', output='screen'), 31 | Node(package='robot_state_publisher', executable='robot_state_publisher', 32 | output='screen', parameters=[rsp_params]), 33 | Node(package='dummy_sensors', executable='dummy_joint_states', output='screen'), 34 | Node(package='dummy_sensors', executable='dummy_laser', output='screen') 35 | ]) 36 | -------------------------------------------------------------------------------- /dummy_robot/dummy_robot_bringup/launch/dummy_robot_bringup_launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /dummy_robot/dummy_robot_bringup/launch/dummy_robot_bringup_launch.yaml: -------------------------------------------------------------------------------- 1 | launch: 2 | - node: 3 | pkg: dummy_map_server 4 | exec: dummy_map_server 5 | output: screen 6 | - node: 7 | pkg: robot_state_publisher 8 | exec: robot_state_publisher 9 | output: screen 10 | param: 11 | - name: robot_description 12 | value: $(file-content '$(find-pkg-share dummy_robot_bringup)/launch/single_rrbot.urdf') 13 | - node: 14 | pkg: dummy_sensors 15 | exec: dummy_joint_states 16 | output: screen 17 | - node: 18 | pkg: dummy_sensors 19 | exec: dummy_laser 20 | output: screen 21 | -------------------------------------------------------------------------------- /dummy_robot/dummy_robot_bringup/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | dummy_robot_bringup 5 | 0.37.0 6 | 7 | dummy robot bringup 8 | 9 | 10 | Aditya Pande 11 | Audrow Nash 12 | 13 | Apache License 2.0 14 | 15 | Karsten Knese 16 | Mabel Zhang 17 | 18 | ament_cmake 19 | 20 | ament_index_python 21 | dummy_map_server 22 | dummy_sensors 23 | launch 24 | launch_ros 25 | robot_state_publisher 26 | 27 | ament_cmake_gtest 28 | ament_lint_auto 29 | ament_lint_common 30 | 31 | 32 | ament_cmake 33 | 34 | 35 | -------------------------------------------------------------------------------- /dummy_robot/dummy_sensors/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.20) 2 | 3 | project(dummy_sensors) 4 | 5 | # Default to C++17 6 | if(NOT CMAKE_CXX_STANDARD) 7 | set(CMAKE_CXX_STANDARD 17) 8 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 9 | endif() 10 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 11 | add_compile_options(-Wall -Wextra -Wpedantic) 12 | endif() 13 | 14 | find_package(ament_cmake REQUIRED) 15 | find_package(rclcpp REQUIRED) 16 | find_package(rmw REQUIRED) 17 | find_package(sensor_msgs REQUIRED) 18 | 19 | add_executable(dummy_laser src/dummy_laser.cpp) 20 | target_link_libraries(dummy_laser PRIVATE 21 | rclcpp::rclcpp 22 | ${sensor_msgs_TARGETS} 23 | ) 24 | 25 | add_executable(dummy_joint_states src/dummy_joint_states.cpp) 26 | target_link_libraries(dummy_joint_states PRIVATE 27 | rclcpp::rclcpp 28 | ${sensor_msgs_TARGETS} 29 | ) 30 | install(TARGETS 31 | dummy_joint_states 32 | dummy_laser 33 | DESTINATION lib/${PROJECT_NAME}) 34 | 35 | if(BUILD_TESTING) 36 | find_package(ament_lint_auto REQUIRED) 37 | ament_lint_auto_find_test_dependencies() 38 | endif() 39 | 40 | ament_package() 41 | -------------------------------------------------------------------------------- /dummy_robot/dummy_sensors/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | dummy_sensors 5 | 0.37.0 6 | 7 | dummy sensor nodes 8 | 9 | 10 | Aditya Pande 11 | Audrow Nash 12 | 13 | Apache License 2.0 14 | 15 | Karsten Knese 16 | Mabel Zhang 17 | 18 | ament_cmake 19 | 20 | rclcpp 21 | sensor_msgs 22 | 23 | rclcpp 24 | sensor_msgs 25 | 26 | ament_cmake_gtest 27 | ament_lint_auto 28 | ament_lint_common 29 | 30 | 31 | ament_cmake 32 | 33 | 34 | -------------------------------------------------------------------------------- /dummy_robot/dummy_sensors/src/dummy_joint_states.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2017 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include "rclcpp/clock.hpp" 21 | #include "rclcpp/rclcpp.hpp" 22 | #include "rclcpp/time_source.hpp" 23 | 24 | #include "sensor_msgs/msg/joint_state.hpp" 25 | 26 | int main(int argc, char * argv[]) 27 | { 28 | rclcpp::init(argc, argv); 29 | 30 | auto node = rclcpp::Node::make_shared("dummy_joint_states"); 31 | 32 | auto joint_state_pub = node->create_publisher("joint_states", 10); 33 | 34 | rclcpp::WallRate loop_rate(50); 35 | 36 | sensor_msgs::msg::JointState msg; 37 | msg.name.push_back("single_rrbot_joint1"); 38 | msg.name.push_back("single_rrbot_joint2"); 39 | msg.position.push_back(0.0); 40 | msg.position.push_back(0.0); 41 | 42 | rclcpp::TimeSource ts(node); 43 | rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_ROS_TIME); 44 | ts.attachClock(clock); 45 | 46 | auto counter = 0.0; 47 | auto joint_value = 0.0; 48 | while (rclcpp::ok()) { 49 | counter += 0.1; 50 | joint_value = std::sin(counter); 51 | 52 | for (size_t i = 0; i < msg.name.size(); ++i) { 53 | msg.position[i] = joint_value; 54 | } 55 | 56 | msg.header.stamp = clock->now(); 57 | 58 | joint_state_pub->publish(msg); 59 | rclcpp::spin_some(node); 60 | loop_rate.sleep(); 61 | } 62 | 63 | rclcpp::shutdown(); 64 | 65 | return 0; 66 | } 67 | -------------------------------------------------------------------------------- /image_tools/Doxyfile: -------------------------------------------------------------------------------- 1 | # All settings not listed here will use the Doxygen default values. 2 | 3 | PROJECT_NAME = "image_tools" 4 | PROJECT_NUMBER = master 5 | PROJECT_BRIEF = "Tools to capture and play back images to and from DDS subscriptions and publications." 6 | 7 | INPUT = ./include 8 | RECURSIVE = YES 9 | OUTPUT_DIRECTORY = docs_output 10 | 11 | EXTRACT_ALL = YES 12 | SORT_MEMBER_DOCS = NO 13 | 14 | GENERATE_LATEX = NO 15 | GENERATE_XML = YES 16 | EXCLUDE_SYMBOLS = detail details 17 | 18 | ENABLE_PREPROCESSING = YES 19 | MACRO_EXPANSION = YES 20 | EXPAND_ONLY_PREDEF = YES 21 | PREDEFINED += IMAGE_TOOLS_PUBLIC 22 | -------------------------------------------------------------------------------- /image_tools/README.md: -------------------------------------------------------------------------------- 1 | ## **What Is This?** 2 | 3 | This demo provides simple utilities to connect to a workstation default camera device and display it in a window like below: 4 | 5 | ![](img/result.png) 6 | 7 | ## **Build** 8 | 9 | ```bash 10 | colcon build --packages-select image_tools 11 | ``` 12 | 13 | ## **Run** 14 | 15 | In `image_tools` ROS 2 package, two executables are provided, namely `cam2image` and `showimage` with different functions. 16 | 17 | ## **1 - cam2image** 18 | Running this executable connects to your workstation's default camera device's video stream and publishes the images on '/image' and '/flipimage' topics using a ROS 2 publisher. 19 | 20 | ```bash 21 | # Open new terminal 22 | ros2 run image_tools cam2image 23 | ``` 24 | 25 | Note that `cam2image` provides many useful command-line options. Run `ros2 run image_tools cam2image --help` to see the list of options available. 26 | > 27 | > Eg. If a camera device is not available, run `ros2 run image_tools cam2image --ros-args -p burger_mode:=true`. 28 | 29 | ## **2 - showimage** 30 | Running this executable creates a ROS 2 node, `showimage`, which subscribes to the `sensor_msg/msg/Image` topic, `/image` and displays the images in a window. 31 | 32 | ```bash 33 | # Open new terminal 34 | # Run showimage ROS 2 node to display the cam2image sensor_msg/msg/Image messages. 35 | ros2 run image_tools showimage 36 | ``` 37 | -------------------------------------------------------------------------------- /image_tools/doc/qos-best-effort.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/demos/8f361a1ece2daae54016959f3ea9ab2e7692ddbb/image_tools/doc/qos-best-effort.png -------------------------------------------------------------------------------- /image_tools/img/result.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/demos/8f361a1ece2daae54016959f3ea9ab2e7692ddbb/image_tools/img/result.png -------------------------------------------------------------------------------- /image_tools/include/image_tools/visibility_control.h: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef IMAGE_TOOLS__VISIBILITY_CONTROL_H_ 16 | #define IMAGE_TOOLS__VISIBILITY_CONTROL_H_ 17 | 18 | #ifdef __cplusplus 19 | extern "C" 20 | { 21 | #endif 22 | 23 | // This logic was borrowed (then namespaced) from the examples on the gcc wiki: 24 | // https://gcc.gnu.org/wiki/Visibility 25 | 26 | #if defined _WIN32 || defined __CYGWIN__ 27 | #ifdef __GNUC__ 28 | #define IMAGE_TOOLS_EXPORT __attribute__ ((dllexport)) 29 | #define IMAGE_TOOLS_IMPORT __attribute__ ((dllimport)) 30 | #else 31 | #define IMAGE_TOOLS_EXPORT __declspec(dllexport) 32 | #define IMAGE_TOOLS_IMPORT __declspec(dllimport) 33 | #endif 34 | #ifdef IMAGE_TOOLS_BUILDING_DLL 35 | #define IMAGE_TOOLS_PUBLIC IMAGE_TOOLS_EXPORT 36 | #else 37 | #define IMAGE_TOOLS_PUBLIC IMAGE_TOOLS_IMPORT 38 | #endif 39 | #define IMAGE_TOOLS_PUBLIC_TYPE IMAGE_TOOLS_PUBLIC 40 | #define IMAGE_TOOLS_LOCAL 41 | #else 42 | #define IMAGE_TOOLS_EXPORT __attribute__ ((visibility("default"))) 43 | #define IMAGE_TOOLS_IMPORT 44 | #if __GNUC__ >= 4 45 | #define IMAGE_TOOLS_PUBLIC __attribute__ ((visibility("default"))) 46 | #define IMAGE_TOOLS_LOCAL __attribute__ ((visibility("hidden"))) 47 | #else 48 | #define IMAGE_TOOLS_PUBLIC 49 | #define IMAGE_TOOLS_LOCAL 50 | #endif 51 | #define IMAGE_TOOLS_PUBLIC_TYPE 52 | #endif 53 | 54 | #ifdef __cplusplus 55 | } 56 | #endif 57 | 58 | #endif // IMAGE_TOOLS__VISIBILITY_CONTROL_H_ 59 | -------------------------------------------------------------------------------- /image_tools/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | image_tools 5 | 0.37.0 6 | Tools to capture and play back images to and from DDS subscriptions and publications. 7 | 8 | Aditya Pande 9 | Audrow Nash 10 | 11 | Apache License 2.0 12 | 13 | Dirk Thomas 14 | Mabel Zhang 15 | 16 | ament_cmake 17 | libopencv-dev 18 | rclcpp 19 | rclcpp_components 20 | sensor_msgs 21 | std_msgs 22 | 23 | libopencv-dev 24 | rclcpp 25 | rclcpp_components 26 | sensor_msgs 27 | std_msgs 28 | 29 | ament_cmake_pytest 30 | ament_lint_auto 31 | ament_lint_common 32 | launch 33 | launch_ros 34 | launch_testing 35 | launch_testing_ament_cmake 36 | launch_testing_ros 37 | rmw_implementation_cmake 38 | 39 | 40 | ament_cmake 41 | 42 | 43 | -------------------------------------------------------------------------------- /image_tools/src/policy_maps.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef POLICY_MAPS_HPP_ 15 | #define POLICY_MAPS_HPP_ 16 | 17 | #include 18 | #include 19 | 20 | namespace image_tools 21 | { 22 | 23 | static 24 | std::map name_to_reliability_policy_map = { 25 | {"reliable", RMW_QOS_POLICY_RELIABILITY_RELIABLE}, 26 | {"best_effort", RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT} 27 | }; 28 | 29 | static 30 | std::map name_to_history_policy_map = { 31 | {"keep_last", RMW_QOS_POLICY_HISTORY_KEEP_LAST}, 32 | {"keep_all", RMW_QOS_POLICY_HISTORY_KEEP_ALL} 33 | }; 34 | 35 | } // namespace image_tools 36 | 37 | #endif // POLICY_MAPS_HPP_ 38 | -------------------------------------------------------------------------------- /image_tools/test/cam2image.txt: -------------------------------------------------------------------------------- 1 | Publishing image #1 2 | Publishing image #2 3 | -------------------------------------------------------------------------------- /image_tools/test/showimage.regex: -------------------------------------------------------------------------------- 1 | Received image #\w+ 2 | -------------------------------------------------------------------------------- /intra_process_demo/img/image_pipeline_all_in_one.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/demos/8f361a1ece2daae54016959f3ea9ab2e7692ddbb/intra_process_demo/img/image_pipeline_all_in_one.png -------------------------------------------------------------------------------- /intra_process_demo/img/image_pipeline_all_in_one_rqtgraph.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/demos/8f361a1ece2daae54016959f3ea9ab2e7692ddbb/intra_process_demo/img/image_pipeline_all_in_one_rqtgraph.png -------------------------------------------------------------------------------- /intra_process_demo/img/image_pipeline_all_separately.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/demos/8f361a1ece2daae54016959f3ea9ab2e7692ddbb/intra_process_demo/img/image_pipeline_all_separately.png -------------------------------------------------------------------------------- /intra_process_demo/img/image_pipeline_with_two_image_views.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/demos/8f361a1ece2daae54016959f3ea9ab2e7692ddbb/intra_process_demo/img/image_pipeline_with_two_image_views.png -------------------------------------------------------------------------------- /intra_process_demo/img/image_pipeline_with_two_image_views_rqtgraph.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/demos/8f361a1ece2daae54016959f3ea9ab2e7692ddbb/intra_process_demo/img/image_pipeline_with_two_image_views_rqtgraph.png -------------------------------------------------------------------------------- /intra_process_demo/img/two_node_pipeline.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/demos/8f361a1ece2daae54016959f3ea9ab2e7692ddbb/intra_process_demo/img/two_node_pipeline.png -------------------------------------------------------------------------------- /intra_process_demo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | intra_process_demo 5 | 0.37.0 6 | Demonstrations of intra process communication. 7 | 8 | Aditya Pande 9 | Audrow Nash 10 | 11 | Apache License 2.0 12 | 13 | Mabel Zhang 14 | William Woodall 15 | 16 | ament_cmake 17 | 18 | libopencv-dev 19 | rclcpp 20 | sensor_msgs 21 | std_msgs 22 | 23 | libopencv-dev 24 | rclcpp 25 | sensor_msgs 26 | 27 | ament_cmake_pytest 28 | ament_lint_auto 29 | ament_lint_common 30 | launch 31 | launch_testing 32 | launch_testing_ros 33 | launch_testing_ament_cmake 34 | rmw_implementation_cmake 35 | 36 | ament_cmake 37 | 38 | -------------------------------------------------------------------------------- /intra_process_demo/src/image_pipeline/camera_node.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2015 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include "image_pipeline/camera_node.hpp" 18 | 19 | #include "rclcpp/rclcpp.hpp" 20 | 21 | int main(int argc, char ** argv) 22 | { 23 | rclcpp::init(argc, argv); 24 | 25 | std::shared_ptr camera_node = nullptr; 26 | try { 27 | camera_node = std::make_shared("image"); 28 | } catch (const std::exception & e) { 29 | fprintf(stderr, "%s Exiting..\n", e.what()); 30 | return 1; 31 | } 32 | 33 | rclcpp::spin(camera_node); 34 | 35 | rclcpp::shutdown(); 36 | 37 | return 0; 38 | } 39 | -------------------------------------------------------------------------------- /intra_process_demo/src/image_pipeline/image_pipeline_all_in_one.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2015 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include "rclcpp/rclcpp.hpp" 18 | 19 | #include "image_pipeline/camera_node.hpp" 20 | #include "image_pipeline/image_view_node.hpp" 21 | #include "image_pipeline/watermark_node.hpp" 22 | 23 | int main(int argc, char * argv[]) 24 | { 25 | rclcpp::init(argc, argv); 26 | rclcpp::executors::SingleThreadedExecutor executor; 27 | 28 | // Connect the nodes as a pipeline: camera_node -> watermark_node -> image_view_node 29 | std::shared_ptr camera_node = nullptr; 30 | try { 31 | camera_node = std::make_shared("image"); 32 | } catch (const std::exception & e) { 33 | fprintf(stderr, "%s Exiting ..\n", e.what()); 34 | return 1; 35 | } 36 | auto watermark_node = 37 | std::make_shared("image", "watermarked_image", "Hello world!"); 38 | auto image_view_node = std::make_shared("watermarked_image"); 39 | 40 | executor.add_node(camera_node); 41 | executor.add_node(watermark_node); 42 | executor.add_node(image_view_node); 43 | 44 | executor.spin(); 45 | 46 | rclcpp::shutdown(); 47 | 48 | return 0; 49 | } 50 | -------------------------------------------------------------------------------- /intra_process_demo/src/image_pipeline/image_pipeline_with_two_image_view.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2015 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include "rclcpp/rclcpp.hpp" 18 | 19 | #include "image_pipeline/camera_node.hpp" 20 | #include "image_pipeline/image_view_node.hpp" 21 | #include "image_pipeline/watermark_node.hpp" 22 | 23 | int main(int argc, char * argv[]) 24 | { 25 | rclcpp::init(argc, argv); 26 | rclcpp::executors::SingleThreadedExecutor executor; 27 | 28 | // Connect the nodes as a pipeline: camera_node -> watermark_node -> image_view_node 29 | // And the extra image view as a fork: \-> image_view_node2 30 | std::shared_ptr camera_node = nullptr; 31 | try { 32 | camera_node = std::make_shared("image"); 33 | } catch (const std::exception & e) { 34 | fprintf(stderr, "%s Exiting..\n", e.what()); 35 | return 1; 36 | } 37 | auto watermark_node = 38 | std::make_shared("image", "watermarked_image", "Hello world!"); 39 | auto image_view_node = std::make_shared("watermarked_image"); 40 | auto image_view_node2 = std::make_shared("watermarked_image", "image_view_node2"); 41 | 42 | executor.add_node(camera_node); 43 | executor.add_node(watermark_node); 44 | executor.add_node(image_view_node); 45 | executor.add_node(image_view_node2); 46 | 47 | executor.spin(); 48 | 49 | rclcpp::shutdown(); 50 | 51 | return 0; 52 | } 53 | -------------------------------------------------------------------------------- /intra_process_demo/src/image_pipeline/image_view_node.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2015 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include "image_pipeline/image_view_node.hpp" 18 | 19 | #include "rclcpp/rclcpp.hpp" 20 | 21 | int main(int argc, char ** argv) 22 | { 23 | rclcpp::init(argc, argv); 24 | auto image_view_node = std::make_shared("watermarked_image"); 25 | rclcpp::spin(image_view_node); 26 | 27 | rclcpp::shutdown(); 28 | 29 | return 0; 30 | } 31 | -------------------------------------------------------------------------------- /intra_process_demo/src/image_pipeline/watermark_node.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2015 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include "image_pipeline/watermark_node.hpp" 18 | 19 | #include "rclcpp/rclcpp.hpp" 20 | 21 | int main(int argc, char ** argv) 22 | { 23 | rclcpp::init(argc, argv); 24 | auto watermark_node = 25 | std::make_shared("image", "watermarked_image", "Hello world!"); 26 | rclcpp::spin(watermark_node); 27 | 28 | rclcpp::shutdown(); 29 | 30 | return 0; 31 | } 32 | -------------------------------------------------------------------------------- /intra_process_demo/test/cyclic_pipeline.regex: -------------------------------------------------------------------------------- 1 | Incrementing and sending with value: (\d+), and address: (0x([A-Z]|\d)+)(\n.*)*Received message with value: \1, and address: \2 2 | sleeping for 1 second... 3 | done. 4 | -------------------------------------------------------------------------------- /intra_process_demo/test/test_executables_demo.py.in: -------------------------------------------------------------------------------- 1 | # generated from intra_process_demo/test/test_executables_demo.py.in 2 | # generated code does not contain a copyright notice 3 | 4 | import unittest 5 | 6 | from launch import LaunchDescription 7 | from launch.actions import ExecuteProcess 8 | 9 | import launch_testing 10 | import launch_testing.actions 11 | import launch_testing.asserts 12 | from launch_testing_ros.actions import EnableRmwIsolation 13 | 14 | 15 | def generate_test_description(): 16 | launch_description = LaunchDescription([ 17 | EnableRmwIsolation(), 18 | ]) 19 | processes_under_test = [ 20 | ExecuteProcess( 21 | cmd=[executable, 'test_executable'], 22 | name='test_executable_' + str(i), 23 | output='screen' 24 | ) for i, executable in enumerate('@RCLCPP_DEMOS_EXECUTABLE@'.split(';')) 25 | ] 26 | for process in processes_under_test: 27 | launch_description.add_action(process) 28 | launch_description.add_action( 29 | launch_testing.actions.ReadyToTest() 30 | ) 31 | return launch_description, locals() 32 | 33 | 34 | class TestExecutablesDemo(unittest.TestCase): 35 | 36 | def test_executables_output(self, proc_output, processes_under_test): 37 | """Test all processes output against expectations.""" 38 | output_files = '@RCLCPP_DEMOS_EXPECTED_OUTPUT@'.split(';') 39 | for process, output_file in zip(processes_under_test, output_files): 40 | proc_output.assertWaitFor( 41 | expected_output=launch_testing.tools.expected_output_from_file( 42 | path=output_file 43 | ), process=process, stream='stdout' 44 | ) 45 | -------------------------------------------------------------------------------- /intra_process_demo/test/two_node_pipeline.regex: -------------------------------------------------------------------------------- 1 | Published message with value: (\d+), and address: (0x([A-Z]|\d)+)(\n.*)* Received message with value: \1, and address: \2 2 | -------------------------------------------------------------------------------- /lifecycle/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.20) 2 | 3 | project(lifecycle) 4 | 5 | # Default to C++17 6 | if(NOT CMAKE_CXX_STANDARD) 7 | set(CMAKE_CXX_STANDARD 17) 8 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 9 | endif() 10 | 11 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 12 | add_compile_options(-Wall -Wextra -Wpedantic) 13 | endif() 14 | 15 | find_package(ament_cmake REQUIRED) 16 | find_package(rclcpp_lifecycle REQUIRED) 17 | find_package(lifecycle_msgs REQUIRED) 18 | find_package(rclcpp REQUIRED) 19 | find_package(std_msgs REQUIRED) 20 | 21 | ### demos 22 | add_executable(lifecycle_talker 23 | src/lifecycle_talker.cpp) 24 | target_link_libraries(lifecycle_talker PRIVATE 25 | ${lifecycle_msgs_TARGETS} 26 | rclcpp::rclcpp 27 | rclcpp_lifecycle::rclcpp_lifecycle 28 | ${std_msgs_TARGETS} 29 | ) 30 | add_executable(lifecycle_listener 31 | src/lifecycle_listener.cpp) 32 | target_link_libraries(lifecycle_listener PRIVATE 33 | ${lifecycle_msgs_TARGETS} 34 | rclcpp::rclcpp 35 | ${std_msgs_TARGETS} 36 | ) 37 | add_executable(lifecycle_service_client 38 | src/lifecycle_service_client.cpp) 39 | target_link_libraries(lifecycle_service_client PRIVATE 40 | ${lifecycle_msgs_TARGETS} 41 | rclcpp::rclcpp 42 | ) 43 | 44 | install(TARGETS 45 | lifecycle_talker 46 | lifecycle_listener 47 | lifecycle_service_client 48 | DESTINATION lib/${PROJECT_NAME}) 49 | 50 | if(BUILD_TESTING) 51 | find_package(ament_lint_auto REQUIRED) 52 | ament_lint_auto_find_test_dependencies() 53 | 54 | find_package(ros_testing REQUIRED) 55 | add_ros_test( 56 | test/test_lifecycle.py 57 | TIMEOUT 60 58 | ) 59 | endif() 60 | 61 | install(DIRECTORY 62 | launch 63 | DESTINATION share/${PROJECT_NAME}/ 64 | ) 65 | 66 | ament_package() 67 | -------------------------------------------------------------------------------- /lifecycle/launch/lifecycle_demo_launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2018 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from launch import LaunchDescription 16 | from launch.actions import Shutdown 17 | from launch_ros.actions import LifecycleNode 18 | from launch_ros.actions import Node 19 | 20 | 21 | def generate_launch_description(): 22 | return LaunchDescription([ 23 | LifecycleNode(package='lifecycle', executable='lifecycle_talker', 24 | name='lc_talker', namespace='', output='screen'), 25 | Node(package='lifecycle', executable='lifecycle_listener', output='screen'), 26 | Node(package='lifecycle', executable='lifecycle_service_client', output='screen', 27 | on_exit=Shutdown()), 28 | ]) 29 | -------------------------------------------------------------------------------- /lifecycle/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | lifecycle 5 | 0.37.0 6 | Package containing demos for lifecycle implementation 7 | 8 | Aditya Pande 9 | Audrow Nash 10 | 11 | Apache License 2.0 12 | 13 | Karsten Knese 14 | Mabel Zhang 15 | 16 | ament_cmake 17 | 18 | lifecycle_msgs 19 | rclcpp 20 | rclcpp_lifecycle 21 | std_msgs 22 | 23 | ament_lint_auto 24 | ament_lint_common 25 | ros_testing 26 | 27 | 28 | ament_cmake 29 | 30 | 31 | -------------------------------------------------------------------------------- /lifecycle_py/README.rst: -------------------------------------------------------------------------------- 1 | See `lifecycle/README.rst <../lifecycle/README.rst>`_. 2 | You can follow the same instructions and replace: 3 | 4 | .. code-block:: python 5 | 6 | ros2 run lifecycle lifecycle_talker 7 | 8 | with: 9 | 10 | .. code-block:: python 11 | 12 | ros2 run lifecycle_py lifecycle_talker 13 | 14 | 15 | You still need to use the ``lifecycle_listener`` and ``lifecycle_service_client`` executables in the ``lifecycle`` package, as those demos were not ported to python. 16 | -------------------------------------------------------------------------------- /lifecycle_py/launch/lifecycle_demo_launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2018 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from launch import LaunchDescription 16 | from launch_ros.actions import LifecycleNode 17 | from launch_ros.actions import Node 18 | 19 | 20 | def generate_launch_description(): 21 | return LaunchDescription([ 22 | LifecycleNode(package='lifecycle_py', executable='lifecycle_talker', 23 | name='lc_talker', namespace='', output='screen'), 24 | Node(package='lifecycle', executable='lifecycle_listener', output='screen'), 25 | Node(package='lifecycle', executable='lifecycle_service_client', output='screen') 26 | ]) 27 | -------------------------------------------------------------------------------- /lifecycle_py/lifecycle_py/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/demos/8f361a1ece2daae54016959f3ea9ab2e7692ddbb/lifecycle_py/lifecycle_py/__init__.py -------------------------------------------------------------------------------- /lifecycle_py/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | lifecycle_py 5 | 0.37.0 6 | Package containing demos for rclpy lifecycle implementation 7 | 8 | Aditya Pande 9 | Audrow Nash 10 | 11 | Apache License 2.0 12 | 13 | Ivan Santiago Paunovic 14 | 15 | rclpy 16 | lifecycle_msgs 17 | std_msgs 18 | 19 | ament_copyright 20 | ament_flake8 21 | ament_pep257 22 | ament_xmllint 23 | lifecycle 24 | ros_testing 25 | 26 | 27 | ament_python 28 | 29 | 30 | -------------------------------------------------------------------------------- /lifecycle_py/resource/lifecycle_py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/demos/8f361a1ece2daae54016959f3ea9ab2e7692ddbb/lifecycle_py/resource/lifecycle_py -------------------------------------------------------------------------------- /lifecycle_py/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/lifecycle_py 3 | [install] 4 | install_scripts=$base/lib/lifecycle_py 5 | -------------------------------------------------------------------------------- /lifecycle_py/setup.py: -------------------------------------------------------------------------------- 1 | import glob 2 | import os 3 | 4 | from setuptools import find_packages 5 | from setuptools import setup 6 | 7 | package_name = 'lifecycle_py' 8 | 9 | setup( 10 | name=package_name, 11 | version='0.37.0', 12 | packages=find_packages(exclude=['test']), 13 | data_files=[ 14 | ('share/ament_index/resource_index/packages', 15 | ['resource/' + package_name]), 16 | ('share/' + package_name, ['package.xml']), 17 | ('share/lifecycle_py/launch', 18 | glob.glob(os.path.join('launch', '*_launch.py'))), 19 | ], 20 | install_requires=['setuptools'], 21 | zip_safe=True, 22 | author='Ivan Santiago Paunovic', 23 | author_email='ivanpauno@ekumenlabs.com', 24 | maintainer='Aditya Pande, Audrow Nash, Michael Jeronimo', 25 | maintainer_email='aditya.pande@openrobotics.org, audrow@openrobotics.org, michael.jeronimo@openrobotics.org', # noqa: E501 26 | keywords=['ROS'], 27 | classifiers=[ 28 | 'Intended Audience :: Developers', 29 | 'License :: OSI Approved :: Apache Software License', 30 | 'Programming Language :: Python', 31 | 'Topic :: Software Development', 32 | ], 33 | description=( 34 | 'Python lifecycle node demo' 35 | ), 36 | license='Apache License, Version 2.0', 37 | tests_require=['pytest'], 38 | entry_points={ 39 | 'console_scripts': [ 40 | 'lifecycle_talker = lifecycle_py.talker:main', 41 | ], 42 | }, 43 | ) 44 | -------------------------------------------------------------------------------- /lifecycle_py/test/test_copyright.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_copyright.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.copyright 20 | @pytest.mark.linter 21 | def test_copyright(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found errors' 24 | -------------------------------------------------------------------------------- /lifecycle_py/test/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_flake8.main import main_with_errors 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc, errors = main_with_errors(argv=[]) 23 | assert rc == 0, \ 24 | 'Found %d code style errors / warnings:\n' % len(errors) + \ 25 | '\n'.join(errors) 26 | -------------------------------------------------------------------------------- /lifecycle_py/test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found code style errors / warnings' 24 | -------------------------------------------------------------------------------- /lifecycle_py/test/test_xmllint.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_xmllint.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.xmllint 21 | def test_xmllint() -> None: 22 | rc = main(argv=[]) 23 | assert rc == 0, 'Found errors' 24 | -------------------------------------------------------------------------------- /logging_demo/Doxyfile: -------------------------------------------------------------------------------- 1 | # All settings not listed here will use the Doxygen default values. 2 | 3 | PROJECT_NAME = "logging_demo" 4 | PROJECT_NUMBER = master 5 | PROJECT_BRIEF = "Examples for using and configuring loggers." 6 | 7 | INPUT = ./include 8 | RECURSIVE = YES 9 | OUTPUT_DIRECTORY = docs_output 10 | 11 | EXTRACT_ALL = YES 12 | SORT_MEMBER_DOCS = NO 13 | 14 | GENERATE_LATEX = NO 15 | GENERATE_XML = YES 16 | EXCLUDE_SYMBOLS = detail details 17 | 18 | ENABLE_PREPROCESSING = YES 19 | MACRO_EXPANSION = YES 20 | EXPAND_ONLY_PREDEF = YES 21 | PREDEFINED += LOGGING_DEMO_PUBLIC 22 | -------------------------------------------------------------------------------- /logging_demo/include/logging_demo/logger_config_component.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2017 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef LOGGING_DEMO__LOGGER_CONFIG_COMPONENT_HPP_ 16 | #define LOGGING_DEMO__LOGGER_CONFIG_COMPONENT_HPP_ 17 | 18 | #include 19 | 20 | #include "logging_demo/srv/config_logger.hpp" 21 | #include "logging_demo/visibility_control.h" 22 | 23 | #include "rclcpp/rclcpp.hpp" 24 | 25 | namespace logging_demo 26 | { 27 | 28 | class LoggerConfig : public rclcpp::Node 29 | { 30 | public: 31 | LOGGING_DEMO_PUBLIC 32 | explicit LoggerConfig(rclcpp::NodeOptions options); 33 | 34 | LOGGING_DEMO_PUBLIC 35 | void 36 | handle_logger_config_request( 37 | const std::shared_ptr request, 38 | std::shared_ptr response); 39 | 40 | private: 41 | rclcpp::Service::SharedPtr srv_; 42 | }; 43 | 44 | } // namespace logging_demo 45 | 46 | #endif // LOGGING_DEMO__LOGGER_CONFIG_COMPONENT_HPP_ 47 | -------------------------------------------------------------------------------- /logging_demo/include/logging_demo/logger_usage_component.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2017 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef LOGGING_DEMO__LOGGER_USAGE_COMPONENT_HPP_ 16 | #define LOGGING_DEMO__LOGGER_USAGE_COMPONENT_HPP_ 17 | 18 | #include 19 | 20 | #include "logging_demo/visibility_control.h" 21 | #include "rclcpp/rclcpp.hpp" 22 | #include "std_msgs/msg/string.hpp" 23 | 24 | namespace logging_demo 25 | { 26 | 27 | class LoggerUsage : public rclcpp::Node 28 | { 29 | public: 30 | LOGGING_DEMO_PUBLIC 31 | explicit LoggerUsage(rclcpp::NodeOptions options); 32 | 33 | protected: 34 | void on_timer(); 35 | 36 | private: 37 | size_t count_; 38 | rclcpp::Publisher::SharedPtr pub_; 39 | rclcpp::TimerBase::SharedPtr one_shot_timer_, timer_; 40 | std::function debug_function_to_evaluate_; 41 | }; 42 | 43 | bool is_divisor_of_twelve(size_t val, rclcpp::Logger logger); 44 | } // namespace logging_demo 45 | 46 | #endif // LOGGING_DEMO__LOGGER_USAGE_COMPONENT_HPP_ 47 | -------------------------------------------------------------------------------- /logging_demo/include/logging_demo/visibility_control.h: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef LOGGING_DEMO__VISIBILITY_CONTROL_H_ 16 | #define LOGGING_DEMO__VISIBILITY_CONTROL_H_ 17 | 18 | #ifdef __cplusplus 19 | extern "C" 20 | { 21 | #endif 22 | 23 | // This logic was borrowed (then namespaced) from the examples on the gcc wiki: 24 | // https://gcc.gnu.org/wiki/Visibility 25 | 26 | #if defined _WIN32 || defined __CYGWIN__ 27 | #ifdef __GNUC__ 28 | #define LOGGING_DEMO_EXPORT __attribute__ ((dllexport)) 29 | #define LOGGING_DEMO_IMPORT __attribute__ ((dllimport)) 30 | #else 31 | #define LOGGING_DEMO_EXPORT __declspec(dllexport) 32 | #define LOGGING_DEMO_IMPORT __declspec(dllimport) 33 | #endif 34 | #ifdef LOGGING_DEMO_BUILDING_DLL 35 | #define LOGGING_DEMO_PUBLIC LOGGING_DEMO_EXPORT 36 | #else 37 | #define LOGGING_DEMO_PUBLIC LOGGING_DEMO_IMPORT 38 | #endif 39 | #define LOGGING_DEMO_PUBLIC_TYPE LOGGING_DEMO_PUBLIC 40 | #define LOGGING_DEMO_LOCAL 41 | #else 42 | #define LOGGING_DEMO_EXPORT __attribute__ ((visibility("default"))) 43 | #define LOGGING_DEMO_IMPORT 44 | #if __GNUC__ >= 4 45 | #define LOGGING_DEMO_PUBLIC __attribute__ ((visibility("default"))) 46 | #define LOGGING_DEMO_LOCAL __attribute__ ((visibility("hidden"))) 47 | #else 48 | #define LOGGING_DEMO_PUBLIC 49 | #define LOGGING_DEMO_LOCAL 50 | #endif 51 | #define LOGGING_DEMO_PUBLIC_TYPE 52 | #endif 53 | 54 | #ifdef __cplusplus 55 | } 56 | #endif 57 | 58 | #endif // LOGGING_DEMO__VISIBILITY_CONTROL_H_ 59 | -------------------------------------------------------------------------------- /logging_demo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | logging_demo 5 | 0.37.0 6 | Examples for using and configuring loggers. 7 | 8 | Aditya Pande 9 | Audrow Nash 10 | 11 | Apache License 2.0 12 | 13 | D. Hood 14 | Mabel Zhang 15 | Scott K Logan 16 | 17 | ament_cmake 18 | rosidl_default_generators 19 | 20 | rclcpp 21 | rclcpp_components 22 | rcutils 23 | rosidl_default_generators 24 | std_msgs 25 | 26 | rclcpp 27 | rclcpp_components 28 | rcutils 29 | rosidl_default_runtime 30 | std_msgs 31 | 32 | ament_cmake_pytest 33 | ament_lint_auto 34 | ament_lint_common 35 | launch 36 | launch_testing 37 | launch_testing_ament_cmake 38 | launch_testing_ros 39 | rmw_implementation_cmake 40 | 41 | rosidl_interface_packages 42 | 43 | 44 | ament_cmake 45 | 46 | 47 | -------------------------------------------------------------------------------- /logging_demo/src/logging_demo_main.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2017 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include "logging_demo/logger_config_component.hpp" 18 | #include "logging_demo/logger_usage_component.hpp" 19 | #include "rclcpp/rclcpp.hpp" 20 | 21 | int main(int argc, char * argv[]) 22 | { 23 | // Force flush of the stdout buffer. 24 | setvbuf(stdout, NULL, _IONBF, BUFSIZ); 25 | 26 | rclcpp::init(argc, argv); 27 | rclcpp::executors::SingleThreadedExecutor exec; 28 | rclcpp::NodeOptions options; 29 | 30 | // Create a node that processes logger configuration requests 31 | auto logger_config = std::make_shared(options); 32 | exec.add_node(logger_config); 33 | // Create a node that has examples of different logger usage 34 | auto logger_usage = std::make_shared(options); 35 | exec.add_node(logger_usage); 36 | 37 | exec.spin(); 38 | 39 | rclcpp::shutdown(); 40 | return 0; 41 | } 42 | -------------------------------------------------------------------------------- /logging_demo/srv/ConfigLogger.srv: -------------------------------------------------------------------------------- 1 | string logger_name 2 | string level 3 | --- 4 | bool success 5 | -------------------------------------------------------------------------------- /logging_demo/test/logging_demo_main_debug_severity.txt: -------------------------------------------------------------------------------- 1 | [DEBUG] [logger_usage_demo]: Count divides into 12 (function evaluated to true) 2 | -------------------------------------------------------------------------------- /logging_demo/test/logging_demo_main_default_severity.txt: -------------------------------------------------------------------------------- 1 | [INFO] [logger_usage_demo]: Timer callback called (this will only log once) 2 | [INFO] [logger_usage_demo]: Publishing: 'Current count: 0' 3 | [INFO] [logger_usage_demo]: Publishing: 'Current count: 1' 4 | -------------------------------------------------------------------------------- /pendulum_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | pendulum_control 5 | 0.37.0 6 | Demonstrates ROS 2's realtime capabilities with a simulated inverted pendulum. 7 | 8 | Aditya Pande 9 | Audrow Nash 10 | 11 | Apache License 2.0 12 | 13 | Jackie Kay 14 | Mabel Zhang 15 | Michael Carroll 16 | Mikael Arguedas 17 | 18 | ament_cmake 19 | 20 | rclcpp 21 | pendulum_msgs 22 | rttest 23 | tlsf_cpp 24 | 25 | rclcpp 26 | pendulum_msgs 27 | rttest 28 | tlsf_cpp 29 | 30 | ament_cmake_pytest 31 | ament_lint_auto 32 | ament_lint_common 33 | launch 34 | launch_testing 35 | launch_testing_ament_cmake 36 | launch_testing_ros 37 | rmw_implementation_cmake 38 | ros2run 39 | 40 | 41 | ament_cmake 42 | 43 | 44 | -------------------------------------------------------------------------------- /pendulum_control/src/pendulum_teleop.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2015 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include "rclcpp/rclcpp.hpp" 22 | 23 | #include "rttest/utils.hpp" 24 | 25 | #include "pendulum_msgs/msg/joint_command.hpp" 26 | 27 | using namespace std::chrono_literals; 28 | 29 | // Non real-time safe node for publishing a user-specified pendulum setpoint exactly once 30 | 31 | int main(int argc, char * argv[]) 32 | { 33 | rclcpp::init(argc, argv); 34 | 35 | double command = M_PI / 2; 36 | if (argc < 2) { 37 | fprintf( 38 | stderr, 39 | "Command argument not specified. Setting command to 90 degrees (PI/2 radians).\n"); 40 | } else { 41 | command = atof(argv[1]); 42 | } 43 | 44 | auto teleop_node = rclcpp::Node::make_shared("pendulum_teleop"); 45 | 46 | auto qos = rclcpp::QoS(rclcpp::KeepLast(10)).transient_local().reliable(); 47 | 48 | auto pub = 49 | teleop_node->create_publisher("pendulum_setpoint", qos); 50 | 51 | auto msg = std::make_unique(); 52 | msg->position = command; 53 | 54 | rclcpp::sleep_for(500ms); 55 | pub->publish(std::move(msg)); 56 | rclcpp::spin_some(teleop_node); 57 | printf("Teleop message published.\n"); 58 | rclcpp::sleep_for(1s); 59 | printf("Teleop node exited.\n"); 60 | 61 | rclcpp::shutdown(); 62 | 63 | return 0; 64 | } 65 | -------------------------------------------------------------------------------- /pendulum_control/test/execute_with_delay.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import argparse 16 | import sys 17 | import time 18 | 19 | from ros2run.api import run_executable 20 | 21 | 22 | def main(): 23 | parser = argparse.ArgumentParser(description='Delay and execute an executable.') 24 | parser.add_argument('delay', metavar='T', type=float, help='Start delay in ms') 25 | parser.add_argument('executable', metavar='exec', type=str, nargs='+', 26 | help='Executable to execute, with a variable number of arguments.') 27 | args = parser.parse_args() 28 | 29 | delay_time = args.delay * 0.001 30 | time.sleep(delay_time) 31 | # use ros2run api to handle KeyboardInterrupt 32 | return run_executable(path=args.executable[0], argv=args.executable[1:]) 33 | 34 | 35 | if __name__ == '__main__': 36 | sys.exit(main()) 37 | -------------------------------------------------------------------------------- /pendulum_control/test/pendulum_demo.regex: -------------------------------------------------------------------------------- 1 | Initial major pagefaults: \d+ 2 | Initial minor pagefaults: \d+ 3 | rttest statistics: 4 | - Minor pagefaults: \d+ 5 | - Major pagefaults: \d+ 6 | Latency \(time after deadline was missed\): 7 | - Min: \d+ ns 8 | - Max: \d+ ns 9 | - Mean: \d+.\d+ ns 10 | - Standard deviation: \d+(\.\d+)?(e[\+\-]\d+)? 11 | \s+ 12 | PendulumMotor received \d+ messages 13 | PendulumController received \d+ messages 14 | -------------------------------------------------------------------------------- /pendulum_control/test/pendulum_demo_teleop.regex: -------------------------------------------------------------------------------- 1 | Pendulum set to: \d+\.\d+ 2 | -------------------------------------------------------------------------------- /pendulum_control/test/pendulum_logger.regex: -------------------------------------------------------------------------------- 1 | Commanded motor angle: \d+.\d+ 2 | Actual motor angle: \d+.\d+ 3 | Current latency: \d+ ns 4 | Mean latency: \d+.\d+ ns 5 | Min latency: \d+ ns 6 | Max latency: \d+ ns 7 | Minor pagefaults during execution: \d+ 8 | Major pagefaults during execution: \d+ 9 | -------------------------------------------------------------------------------- /pendulum_control/test/pendulum_teleop.txt: -------------------------------------------------------------------------------- 1 | Teleop node exited. 2 | -------------------------------------------------------------------------------- /pendulum_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.20) 2 | 3 | project(pendulum_msgs) 4 | 5 | # Default to C++17 6 | if(NOT CMAKE_CXX_STANDARD) 7 | set(CMAKE_CXX_STANDARD 17) 8 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 9 | endif() 10 | 11 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 12 | add_compile_options(-Wall -Wextra -Wpedantic) 13 | endif() 14 | 15 | find_package(ament_cmake REQUIRED) 16 | find_package(builtin_interfaces REQUIRED) 17 | find_package(rosidl_default_generators REQUIRED) 18 | 19 | rosidl_generate_interfaces(pendulum_msgs 20 | "msg/JointState.msg" 21 | "msg/JointCommand.msg" 22 | "msg/RttestResults.msg" 23 | DEPENDENCIES builtin_interfaces 24 | ) 25 | 26 | ament_package() 27 | -------------------------------------------------------------------------------- /pendulum_msgs/README.md: -------------------------------------------------------------------------------- 1 | ## **What Is This?** 2 | 3 | The **pendulum_msgs** ROS 2 package is a dependency of **pendulum_control** ROS 2 package. 4 | It contains `JointCommand.msg`, `JointState.msg` and `RttestResults.msg` 5 | 6 | Please refer to [pendulum_control](https://github.com/ros2/demos/tree/rolling/pendulum_control) for more details. 7 | 8 | ### **JointCommand.msg** 9 | 10 | ```msg 11 | float64 position 12 | ``` 13 | 14 | ### **JointState.msg** 15 | 16 | ```msg 17 | float64 position 18 | float64 velocity 19 | float64 effort 20 | ``` 21 | 22 | ### **RttestResults.msg** 23 | 24 | ```msg 25 | builtin_interfaces/Time stamp 26 | 27 | JointCommand command 28 | JointState state 29 | 30 | uint64 cur_latency 31 | float64 mean_latency 32 | uint64 min_latency 33 | uint64 max_latency 34 | uint64 minor_pagefaults 35 | uint64 major_pagefaults 36 | ``` 37 | 38 | 39 | ## References 40 | 41 | - Real-time Jitter Measurements under ROS 2: The Inverted Pendulum Case: https://www.researchgate.net/publication/350353690_Real-time_Jitter_Measurements_under_ROS2_the_Inverted_Pendulum_case 42 | -------------------------------------------------------------------------------- /pendulum_msgs/msg/JointCommand.msg: -------------------------------------------------------------------------------- 1 | float64 position 2 | -------------------------------------------------------------------------------- /pendulum_msgs/msg/JointState.msg: -------------------------------------------------------------------------------- 1 | float64 position 2 | float64 velocity 3 | float64 effort 4 | -------------------------------------------------------------------------------- /pendulum_msgs/msg/RttestResults.msg: -------------------------------------------------------------------------------- 1 | builtin_interfaces/Time stamp 2 | 3 | JointCommand command 4 | JointState state 5 | 6 | uint64 cur_latency 7 | float64 mean_latency 8 | uint64 min_latency 9 | uint64 max_latency 10 | uint64 minor_pagefaults 11 | uint64 major_pagefaults 12 | -------------------------------------------------------------------------------- /pendulum_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | pendulum_msgs 5 | 0.37.0 6 | Custom messages for real-time pendulum control. 7 | 8 | Aditya Pande 9 | Audrow Nash 10 | 11 | Apache License 2.0 12 | 13 | Jackie Kay 14 | Mabel Zhang 15 | Mikael Arguedas 16 | 17 | ament_cmake 18 | 19 | builtin_interfaces 20 | rosidl_default_generators 21 | builtin_interfaces 22 | rosidl_default_runtime 23 | 24 | rosidl_interface_packages 25 | 26 | 27 | ament_cmake 28 | 29 | 30 | -------------------------------------------------------------------------------- /pytest.ini: -------------------------------------------------------------------------------- 1 | [pytest] 2 | junit_family=xunit2 3 | -------------------------------------------------------------------------------- /quality_of_service_demo/rclcpp/include/quality_of_service_demo/visibility_control.h: -------------------------------------------------------------------------------- 1 | // Copyright 2020 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef QUALITY_OF_SERVICE_DEMO__VISIBILITY_CONTROL_H_ 16 | #define QUALITY_OF_SERVICE_DEMO__VISIBILITY_CONTROL_H_ 17 | 18 | #ifdef __cplusplus 19 | extern "C" 20 | { 21 | #endif 22 | 23 | // This logic was borrowed (then namespaced) from the examples on the gcc wiki: 24 | // https://gcc.gnu.org/wiki/Visibility 25 | 26 | #if defined _WIN32 || defined __CYGWIN__ 27 | #ifdef __GNUC__ 28 | #define QUALITY_OF_SERVICE_DEMO_EXPORT __attribute__ ((dllexport)) 29 | #define QUALITY_OF_SERVICE_DEMO_IMPORT __attribute__ ((dllimport)) 30 | #else 31 | #define QUALITY_OF_SERVICE_DEMO_EXPORT __declspec(dllexport) 32 | #define QUALITY_OF_SERVICE_DEMO_IMPORT __declspec(dllimport) 33 | #endif 34 | #ifdef QUALITY_OF_SERVICE_DEMO_BUILDING_DLL 35 | #define QUALITY_OF_SERVICE_DEMO_PUBLIC QUALITY_OF_SERVICE_DEMO_EXPORT 36 | #else 37 | #define QUALITY_OF_SERVICE_DEMO_PUBLIC QUALITY_OF_SERVICE_DEMO_IMPORT 38 | #endif 39 | #define QUALITY_OF_SERVICE_DEMO_PUBLIC_TYPE QUALITY_OF_SERVICE_DEMO_PUBLIC 40 | #define QUALITY_OF_SERVICE_DEMO_LOCAL 41 | #else 42 | #define QUALITY_OF_SERVICE_DEMO_EXPORT __attribute__ ((visibility("default"))) 43 | #define QUALITY_OF_SERVICE_DEMO_IMPORT 44 | #if __GNUC__ >= 4 45 | #define QUALITY_OF_SERVICE_DEMO_PUBLIC __attribute__ ((visibility("default"))) 46 | #define QUALITY_OF_SERVICE_DEMO_LOCAL __attribute__ ((visibility("hidden"))) 47 | #else 48 | #define QUALITY_OF_SERVICE_DEMO_PUBLIC 49 | #define QUALITY_OF_SERVICE_DEMO_LOCAL 50 | #endif 51 | #define QUALITY_OF_SERVICE_DEMO_PUBLIC_TYPE 52 | #endif 53 | 54 | #ifdef __cplusplus 55 | } 56 | #endif 57 | 58 | #endif // QUALITY_OF_SERVICE_DEMO__VISIBILITY_CONTROL_H_ 59 | -------------------------------------------------------------------------------- /quality_of_service_demo/rclcpp/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | quality_of_service_demo_cpp 5 | 0.37.0 6 | C++ Demo applications for Quality of Service features 7 | 8 | Aditya Pande 9 | Audrow Nash 10 | 11 | Apache License 2.0 12 | 13 | Amazon ROS Contributions 14 | Emerson Knapp 15 | Mabel Zhang 16 | 17 | ament_cmake 18 | 19 | example_interfaces 20 | rclcpp 21 | rclcpp_components 22 | rcutils 23 | rmw 24 | rmw_implementation_cmake 25 | sensor_msgs 26 | std_msgs 27 | 28 | example_interfaces 29 | launch_ros 30 | rclcpp 31 | rclcpp_components 32 | rcutils 33 | rmw 34 | sensor_msgs 35 | std_msgs 36 | 37 | ament_lint_auto 38 | ament_lint_common 39 | launch 40 | launch_testing 41 | 42 | 43 | ament_cmake 44 | 45 | 46 | -------------------------------------------------------------------------------- /quality_of_service_demo/rclcpp/params_file/example_qos_overrides.yaml: -------------------------------------------------------------------------------- 1 | qos_overrides_talker: # node name 2 | ros__parameters: # All parameters are nested in this key 3 | qos_overrides: # Parameter group for all qos overrides 4 | /qos_overrides_chatter: # Parameter group for the topic 5 | publisher: # Profile for publishers in the topic 6 | # publisher_my_id: # Profile for publishers in the topic with id="my_id" 7 | reliability: reliable 8 | depth: 9 9 | # depth: 11 # Uncomment this line to make the validation callback fail 10 | qos_overrides_listener: 11 | ros__parameters: 12 | qos_overrides: 13 | /qos_overrides_chatter: 14 | subscription: # Profile for subscriptions in the topic 15 | # subscription_my_id: # Profile for subscriptions in the topic with id="my_id" 16 | reliability: reliable 17 | depth: 9 18 | -------------------------------------------------------------------------------- /quality_of_service_demo/rclcpp/params_file/example_qos_overrides_with_wildcard.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | qos_overrides: 4 | /qos_overrides_chatter: 5 | publisher: 6 | reliability: reliable 7 | depth: 9 8 | subscription: 9 | reliability: reliable 10 | depth: 9 11 | -------------------------------------------------------------------------------- /quality_of_service_demo/rclcpp/src/utils.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Amazon.com, Inc. or its affiliates. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef UTILS_HPP_ 16 | #define UTILS_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | #include "rclcpp/qos.hpp" 22 | #include "rmw/types.h" 23 | 24 | /// Convert rmw_time_t to seconds (represented by a floating point number). 25 | double 26 | rmw_time_to_seconds(const rmw_time_t & time); 27 | 28 | /// Print the given QoS settings to stdout. 29 | void 30 | print_qos(const rclcpp::QoS & qos); 31 | 32 | void 33 | install_ctrl_handler(std::function ctrl_handler); 34 | 35 | class KeyboardReader final 36 | { 37 | public: 38 | KeyboardReader(); 39 | 40 | char readOne(); 41 | 42 | ~KeyboardReader(); 43 | 44 | private: 45 | class KeyboardReaderImpl; 46 | 47 | std::unique_ptr pimpl_; 48 | }; 49 | 50 | #endif // UTILS_HPP_ 51 | -------------------------------------------------------------------------------- /quality_of_service_demo/rclpy/.gitignore: -------------------------------------------------------------------------------- 1 | __pycache__/ 2 | -------------------------------------------------------------------------------- /quality_of_service_demo/rclpy/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | quality_of_service_demo_py 5 | 0.37.0 6 | Python Demo applications for Quality of Service features 7 | 8 | Aditya Pande 9 | Audrow Nash 10 | 11 | Apache License 2.0 12 | 13 | Amazon ROS Contributions 14 | Emerson Knapp 15 | Mabel Zhang 16 | 17 | rclpy 18 | sensor_msgs 19 | std_msgs 20 | 21 | ament_copyright 22 | ament_flake8 23 | ament_pep257 24 | ament_xmllint 25 | python3-pytest 26 | 27 | 28 | ament_python 29 | 30 | 31 | -------------------------------------------------------------------------------- /quality_of_service_demo/rclpy/quality_of_service_demo_py/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/demos/8f361a1ece2daae54016959f3ea9ab2e7692ddbb/quality_of_service_demo/rclpy/quality_of_service_demo_py/__init__.py -------------------------------------------------------------------------------- /quality_of_service_demo/rclpy/quality_of_service_demo_py/qos_overrides_listener.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import sys 16 | 17 | import rclpy 18 | from rclpy.executors import ExternalShutdownException 19 | from rclpy.node import Node 20 | from rclpy.qos_overriding_options import QosCallbackResult 21 | from rclpy.qos_overriding_options import QoSOverridingOptions 22 | 23 | from std_msgs.msg import String 24 | 25 | 26 | class Listener(Node): 27 | 28 | def __init__(self): 29 | super().__init__('qos_overrides_listener') 30 | self.sub = self.create_subscription( 31 | String, 'qos_overrides_chatter', self.chatter_callback, 10, 32 | qos_overriding_options=QoSOverridingOptions.with_default_policies( 33 | callback=self.qos_callback, 34 | # entity_id='my_custom_id', # Use this if you want a custo qos override id. 35 | )) 36 | 37 | def chatter_callback(self, msg): 38 | self.get_logger().info('I heard: [%s]' % msg.data) 39 | 40 | def qos_callback(self, qos): 41 | result = QosCallbackResult() 42 | if qos.depth <= 10: 43 | result.successful = True 44 | return result 45 | result.successful = False 46 | result.reason = 'expected qos depth less than 10' 47 | return result 48 | 49 | 50 | def main(args=None): 51 | try: 52 | with rclpy.init(args=args): 53 | node = Listener() 54 | rclpy.spin(node) 55 | except (KeyboardInterrupt, ExternalShutdownException): 56 | pass 57 | 58 | return 0 59 | 60 | 61 | if __name__ == '__main__': 62 | sys.exit(main()) 63 | -------------------------------------------------------------------------------- /quality_of_service_demo/rclpy/quality_of_service_demo_py/qos_overrides_talker.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import sys 16 | 17 | import rclpy 18 | from rclpy.executors import ExternalShutdownException 19 | from rclpy.node import Node 20 | from rclpy.qos_overriding_options import QosCallbackResult 21 | from rclpy.qos_overriding_options import QoSOverridingOptions 22 | 23 | from std_msgs.msg import String 24 | 25 | 26 | class Talker(Node): 27 | 28 | def __init__(self): 29 | super().__init__('qos_overrides_talker') 30 | self.i = 0 31 | self.pub = self.create_publisher( 32 | String, 'qos_overrides_chatter', 10, 33 | qos_overriding_options=QoSOverridingOptions.with_default_policies( 34 | callback=self.qos_callback, 35 | # entity_id='my_custom_id', # Use this if you want a custo qos override id. 36 | )) 37 | timer_period = 1.0 38 | self.tmr = self.create_timer(timer_period, self.timer_callback) 39 | 40 | def timer_callback(self): 41 | msg = String() 42 | msg.data = 'Hello World: {0}'.format(self.i) 43 | self.i += 1 44 | self.get_logger().info('Publishing: "{0}"'.format(msg.data)) 45 | self.pub.publish(msg) 46 | 47 | def qos_callback(self, qos): 48 | result = QosCallbackResult() 49 | if qos.depth <= 10: 50 | result.successful = True 51 | return result 52 | result.successful = False 53 | result.reason = 'expected qos depth less than 10' 54 | return result 55 | 56 | 57 | def main(args=None): 58 | try: 59 | with rclpy.init(args=args): 60 | node = Talker() 61 | 62 | rclpy.spin(node) 63 | except (KeyboardInterrupt, ExternalShutdownException): 64 | pass 65 | 66 | return 0 67 | 68 | 69 | if __name__ == '__main__': 70 | sys.exit(main()) 71 | -------------------------------------------------------------------------------- /quality_of_service_demo/rclpy/resource/quality_of_service_demo_py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/demos/8f361a1ece2daae54016959f3ea9ab2e7692ddbb/quality_of_service_demo/rclpy/resource/quality_of_service_demo_py -------------------------------------------------------------------------------- /quality_of_service_demo/rclpy/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/quality_of_service_demo_py 3 | [install] 4 | install_scripts=$base/lib/quality_of_service_demo_py 5 | -------------------------------------------------------------------------------- /quality_of_service_demo/rclpy/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | 3 | package_name = 'quality_of_service_demo_py' 4 | 5 | setup( 6 | name=package_name, 7 | version='0.37.0', 8 | packages=[package_name], 9 | data_files=[ 10 | ('share/ament_index/resource_index/packages', ['resource/' + package_name]), 11 | ('share/' + package_name, ['package.xml']), 12 | ], 13 | install_requires=['setuptools'], 14 | zip_safe=True, 15 | author='Emerson Knapp', 16 | maintainer='Aditya Pande, Audrow Nash, Michael Jeronimo', 17 | maintainer_email='aditya.pande@openrobotics.org, audrow@openrobotics.org, michael.jeronimo@openrobotics.org', # noqa: E501 18 | keywords=['ROS'], 19 | classifiers=[ 20 | 'Intended Audience :: Developers', 21 | 'License :: OSI Approved :: Apache Software License', 22 | 'Programming Language :: Python', 23 | 'Topic :: Software Development', 24 | ], 25 | description='Python nodes to demonstrate ROS 2 QoS policies.', 26 | license='Apache License, Version 2.0', 27 | tests_require=['pytest'], 28 | entry_points={ 29 | 'console_scripts': [ 30 | 'deadline = quality_of_service_demo_py.deadline:main', 31 | 'incompatible_qos = quality_of_service_demo_py.incompatible_qos:main', 32 | 'lifespan = quality_of_service_demo_py.lifespan:main', 33 | 'liveliness = quality_of_service_demo_py.liveliness:main', 34 | 'message_lost_listener = quality_of_service_demo_py.message_lost_listener:main', 35 | 'qos_overrides_listener = quality_of_service_demo_py.qos_overrides_listener:main', 36 | 'qos_overrides_talker = quality_of_service_demo_py.qos_overrides_talker:main', 37 | ], 38 | }, 39 | ) 40 | -------------------------------------------------------------------------------- /quality_of_service_demo/rclpy/test/test_linters.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 Amazon.com, Inc. or its affiliates. All Rights Reserved. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import ament_copyright.main 16 | import ament_flake8.main 17 | import ament_pep257.main 18 | 19 | import pytest 20 | 21 | 22 | @pytest.mark.copyright 23 | @pytest.mark.linter 24 | def test_copyright(): 25 | # Test is called from package root 26 | rc = ament_copyright.main.main(argv=['.']) 27 | assert rc == 0, 'Found copyright errors' 28 | 29 | 30 | @pytest.mark.flake8 31 | @pytest.mark.linter 32 | def test_flake8(): 33 | # Test is called from package root 34 | rc = ament_flake8.main.main(argv=['.']) 35 | assert rc == 0, 'Found flake8 errors' 36 | 37 | 38 | @pytest.mark.linter 39 | @pytest.mark.pep257 40 | def test_pep257(): 41 | # Test is called from package root 42 | rc = ament_pep257.main.main(argv=['.']) 43 | assert rc == 0, 'Found PEP257 code style error / warnings' 44 | -------------------------------------------------------------------------------- /quality_of_service_demo/rclpy/test/test_xmllint.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_xmllint.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.xmllint 21 | def test_xmllint() -> None: 22 | rc = main(argv=[]) 23 | assert rc == 0, 'Found errors' 24 | -------------------------------------------------------------------------------- /topic_monitor/doc/message_size_comparison.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/demos/8f361a1ece2daae54016959f3ea9ab2e7692ddbb/topic_monitor/doc/message_size_comparison.png -------------------------------------------------------------------------------- /topic_monitor/doc/reliability_comparison.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/demos/8f361a1ece2daae54016959f3ea9ab2e7692ddbb/topic_monitor/doc/reliability_comparison.png -------------------------------------------------------------------------------- /topic_monitor/launch/depth_demo_launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2018 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Launch data publishers with different depth configruations.""" 16 | 17 | from launch import LaunchDescription 18 | import launch.actions 19 | from launch_ros.substitutions import ExecutableInPackage 20 | 21 | 22 | def create_data_publisher_action(size, depth): 23 | name = '{0}_depth_{1}'.format(size, depth) 24 | payload = 0 if size == 'small' else 100000 25 | executable = ExecutableInPackage(package='topic_monitor', executable='data_publisher') 26 | 27 | return launch.actions.ExecuteProcess( 28 | cmd=[executable, name, '--depth', str(depth), '--payload-size', str(payload)], 29 | output='screen', 30 | ) 31 | 32 | 33 | def generate_launch_description(): 34 | return LaunchDescription([ 35 | create_data_publisher_action('small', 1), 36 | create_data_publisher_action('small', 50), 37 | create_data_publisher_action('large', 1), 38 | create_data_publisher_action('large', 50), 39 | ]) 40 | -------------------------------------------------------------------------------- /topic_monitor/launch/fragmentation_demo_launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2018 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Launch data publishers with different payload sizes.""" 16 | 17 | from launch import LaunchDescription 18 | import launch.actions 19 | from launch_ros.substitutions import ExecutableInPackage 20 | 21 | 22 | def generate_launch_description(): 23 | executable = ExecutableInPackage(package='topic_monitor', executable='data_publisher') 24 | return LaunchDescription([ 25 | launch.actions.ExecuteProcess( 26 | cmd=[executable, 'small', '--payload-size', '1', '--period', '4'], 27 | output='screen'), 28 | launch.actions.ExecuteProcess( 29 | cmd=[executable, 'medium', '--payload-size', '50000', '--period', '4'], 30 | output='screen'), 31 | launch.actions.ExecuteProcess( 32 | cmd=[executable, 'large', '--payload-size', '100000', '--period', '4'], 33 | output='screen'), 34 | launch.actions.ExecuteProcess( 35 | cmd=[executable, 'xlarge', '--payload-size', '150000', '--period', '4'], 36 | output='screen'), 37 | ]) 38 | -------------------------------------------------------------------------------- /topic_monitor/launch/reliability_demo_launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2018 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Launch data publishers with different reliability configruations.""" 16 | 17 | from launch import LaunchDescription 18 | import launch.actions 19 | from launch_ros.substitutions import ExecutableInPackage 20 | 21 | 22 | def generate_launch_description(): 23 | executable = ExecutableInPackage(package='topic_monitor', executable='data_publisher') 24 | return LaunchDescription([ 25 | launch.actions.ExecuteProcess( 26 | cmd=[executable, 'sensor', '--best-effort'], output='screen'), 27 | launch.actions.ExecuteProcess( 28 | cmd=[executable, 'critical'], output='screen'), 29 | ]) 30 | -------------------------------------------------------------------------------- /topic_monitor/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | topic_monitor 5 | 0.37.0 6 | Package containing tools for monitoring ROS 2 topics. 7 | 8 | Aditya Pande 9 | Audrow Nash 10 | 11 | Apache License 2.0 12 | 13 | D. Hood 14 | Mabel Zhang 15 | Scott K Logan 16 | 17 | rclpy 18 | 19 | launch 20 | launch_ros 21 | rclpy 22 | std_msgs 23 | 24 | ament_flake8 25 | ament_pep257 26 | ament_xmllint 27 | python3-pytest 28 | 29 | 30 | ament_python 31 | 32 | 33 | -------------------------------------------------------------------------------- /topic_monitor/resource/topic_monitor: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/demos/8f361a1ece2daae54016959f3ea9ab2e7692ddbb/topic_monitor/resource/topic_monitor -------------------------------------------------------------------------------- /topic_monitor/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/topic_monitor 3 | [install] 4 | install_scripts=$base/lib/topic_monitor 5 | -------------------------------------------------------------------------------- /topic_monitor/setup.py: -------------------------------------------------------------------------------- 1 | import glob 2 | import os 3 | 4 | from setuptools import find_packages 5 | from setuptools import setup 6 | 7 | package_name = 'topic_monitor' 8 | 9 | setup( 10 | name=package_name, 11 | version='0.37.0', 12 | packages=find_packages(exclude=['test']), 13 | data_files=[ 14 | ('share/ament_index/resource_index/packages', 15 | ['resource/' + package_name]), 16 | ('share/' + package_name, ['package.xml']), 17 | ('share/topic_monitor/launch', 18 | glob.glob(os.path.join('launch', '*_launch.py'))), 19 | ], 20 | install_requires=[ 21 | 'launch', 22 | 'setuptools', 23 | ], 24 | zip_safe=True, 25 | maintainer='Aditya Pande, Audrow Nash, Michael Jeronimo', 26 | maintainer_email='aditya.pande@openrobotics.org, audrow@openrobotics.org, michael.jeronimo@openrobotics.org', # noqa: E501 27 | keywords=['ROS'], 28 | classifiers=[ 29 | 'Intended Audience :: Developers', 30 | 'License :: OSI Approved :: Apache Software License', 31 | 'Programming Language :: Python', 32 | 'Topic :: Software Development', 33 | ], 34 | description='Package containing tools for monitoring ROS 2 topics.', 35 | license='Apache License, Version 2.0', 36 | tests_require=['pytest'], 37 | entry_points={ 38 | 'console_scripts': [ 39 | 'data_publisher = topic_monitor.scripts.data_publisher:main', 40 | 'topic_monitor = topic_monitor.scripts.topic_monitor:main', 41 | ], 42 | }, 43 | ) 44 | -------------------------------------------------------------------------------- /topic_monitor/test/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_flake8.main import main_with_errors 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc, errors = main_with_errors(argv=[]) 23 | assert rc == 0, \ 24 | 'Found %d code style errors / warnings:\n' % len(errors) + \ 25 | '\n'.join(errors) 26 | -------------------------------------------------------------------------------- /topic_monitor/test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=[]) 23 | assert rc == 0, 'Found code style errors / warnings' 24 | -------------------------------------------------------------------------------- /topic_monitor/test/test_xmllint.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_xmllint.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.xmllint 21 | def test_xmllint() -> None: 22 | rc = main(argv=[]) 23 | assert rc == 0, 'Found errors' 24 | -------------------------------------------------------------------------------- /topic_monitor/topic_monitor/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/demos/8f361a1ece2daae54016959f3ea9ab2e7692ddbb/topic_monitor/topic_monitor/__init__.py -------------------------------------------------------------------------------- /topic_monitor/topic_monitor/scripts/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros2/demos/8f361a1ece2daae54016959f3ea9ab2e7692ddbb/topic_monitor/topic_monitor/scripts/__init__.py -------------------------------------------------------------------------------- /topic_statistics_demo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.20) 2 | 3 | project(topic_statistics_demo) 4 | 5 | # Default to C++17 6 | if(NOT CMAKE_CXX_STANDARD) 7 | set(CMAKE_CXX_STANDARD 17) 8 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 9 | endif() 10 | 11 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 12 | add_compile_options(-Wall -Wextra -Wpedantic) 13 | endif() 14 | 15 | find_package(ament_cmake REQUIRED) 16 | find_package(rclcpp REQUIRED) 17 | find_package(rcutils) 18 | find_package(sensor_msgs REQUIRED) 19 | find_package(statistics_msgs REQUIRED) 20 | 21 | include_directories(include) 22 | # TODO(sloretz) stop exporting old-style CMake variables in the future 23 | ament_export_include_directories("include/${PROJECT_NAME}") 24 | 25 | add_executable(display_topic_statistics 26 | src/imu_talker_listener_nodes.cpp 27 | src/display_topic_statistics.cpp 28 | src/string_talker_listener_nodes.cpp 29 | src/topic_statistics_listener.cpp) 30 | target_link_libraries(display_topic_statistics 31 | rclcpp::rclcpp 32 | rcutils::rcutils 33 | ${sensor_msgs_TARGETS} 34 | ${statistics_msgs_TARGETS}) 35 | install(TARGETS display_topic_statistics DESTINATION lib/${PROJECT_NAME}) 36 | 37 | if(BUILD_TESTING) 38 | find_package(ament_lint_auto REQUIRED) 39 | ament_lint_auto_find_test_dependencies() 40 | endif() 41 | 42 | ament_package() 43 | -------------------------------------------------------------------------------- /topic_statistics_demo/README.md: -------------------------------------------------------------------------------- 1 | # Topic Statistics Demo 2 | 3 | The demo application in this package demonstrates [Topic Statistics](https://docs.ros.org/en/rolling/Concepts/About-Topic-Statistics.html) feature in ROS 2. 4 | The application creates ROS 2 nodes to publish messages to topics, subscribes to the statistics topic and displays the statistics data received. 5 | 6 | The demo application in this package `display_topic_statistics` creates the following ROS 2 nodes: 7 | 1. Talker and Listener nodes to generate message traffic 8 | 2. Statistics listener node to display generated statistics 9 | 10 | The application requires an argument `message_type` - the type of message chatter to generate. 11 | Possible values are `string` and `imu`. 12 | 13 | The application also accepts the following optional arguments to configure the Listener node's subscription: 14 | 1. `--publish-topic`: Topic to which topic statistics are published. Default topic is `/statistics`. 15 | 2. `--publish-period`: Publish period for publication of statistics. Default value is 5s. 16 | 17 | Once the application starts, the talker node will publish messages on a topic that the listener node has subscribed to. 18 | The listener's subscription will generate topic statistics upon receiving messages. 19 | Statistics are published to the statistics topic at a pre-determined frequency. 20 | The statistics listener node listens to these statistics and prints them for the user to see. 21 | -------------------------------------------------------------------------------- /topic_statistics_demo/include/topic_statistics_demo/topic_statistics_listener.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef TOPIC_STATISTICS_DEMO__TOPIC_STATISTICS_LISTENER_HPP_ 16 | #define TOPIC_STATISTICS_DEMO__TOPIC_STATISTICS_LISTENER_HPP_ 17 | 18 | #include 19 | 20 | #include "rclcpp/rclcpp.hpp" 21 | #include "statistics_msgs/msg/metrics_message.hpp" 22 | 23 | namespace topic_stats_demo 24 | { 25 | constexpr char STATISTICS_TOPIC_NAME[] = "statistics"; 26 | } 27 | 28 | class TopicStatisticsListener : public rclcpp::Node 29 | { 30 | public: 31 | /// Standard Constructor. 32 | /** 33 | * \param[in] topic_name Topic to which statistics are published. 34 | */ 35 | explicit TopicStatisticsListener( 36 | const std::string & topic_name = topic_stats_demo::STATISTICS_TOPIC_NAME); 37 | 38 | /// Initialize the listener node. 39 | void initialize(); 40 | 41 | /// Return string representation of a MetricsMessage. 42 | /** 43 | * \param[in] results Statistics heard form the subscribed topic. 44 | * \return String representation of the input statistics. 45 | */ 46 | std::string MetricsMessageToString(const statistics_msgs::msg::MetricsMessage & results); 47 | 48 | /// Instantiate a Subscription to the statistics topic. 49 | void start_listening(); 50 | 51 | private: 52 | rclcpp::SubscriptionOptions subscription_options_; 53 | rclcpp::Subscription::SharedPtr subscription_ = nullptr; 54 | 55 | const std::string topic_name_; 56 | }; 57 | 58 | #endif // TOPIC_STATISTICS_DEMO__TOPIC_STATISTICS_LISTENER_HPP_ 59 | -------------------------------------------------------------------------------- /topic_statistics_demo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | topic_statistics_demo 5 | 0.37.0 6 | C++ demo application for topic statistics feature. 7 | 8 | Aditya Pande 9 | Audrow Nash 10 | 11 | Apache License 2.0 12 | 13 | Amazon ROS Contributions 14 | Mabel Zhang 15 | 16 | ament_cmake 17 | 18 | rclcpp 19 | rcutils 20 | sensor_msgs 21 | statistics_msgs 22 | 23 | rclcpp 24 | rcutils 25 | sensor_msgs 26 | statistics_msgs 27 | 28 | ament_lint_auto 29 | ament_lint_common 30 | 31 | 32 | ament_cmake 33 | 34 | 35 | -------------------------------------------------------------------------------- /topic_statistics_demo/src/topic_statistics_listener.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | 18 | #include "topic_statistics_demo/topic_statistics_listener.hpp" 19 | 20 | using statistics_msgs::msg::MetricsMessage; 21 | const char * STATISTIC_TYPES[] = {"unknown", "avg", "min", "max", "std_dev", "sample_count"}; 22 | 23 | TopicStatisticsListener::TopicStatisticsListener(const std::string & topic_name) 24 | : Node("statistics_listener"), 25 | topic_name_(topic_name) {} 26 | 27 | void TopicStatisticsListener::initialize() 28 | { 29 | RCLCPP_INFO(get_logger(), "TopicStatisticsListener starting up"); 30 | start_listening(); 31 | } 32 | 33 | void TopicStatisticsListener::start_listening() 34 | { 35 | if (!subscription_) { 36 | subscription_ = create_subscription( 37 | topic_name_, 38 | 10, /* QoS history_depth */ 39 | [this](statistics_msgs::msg::MetricsMessage::ConstSharedPtr msg) -> void 40 | { 41 | RCLCPP_INFO(get_logger(), "Statistics heard:\n%s", MetricsMessageToString(*msg).c_str()); 42 | }, 43 | subscription_options_); 44 | } 45 | } 46 | 47 | std::string TopicStatisticsListener::MetricsMessageToString(const MetricsMessage & results) 48 | { 49 | std::stringstream ss; 50 | ss << "Metric name: " << results.metrics_source << 51 | " source: " << results.measurement_source_name << 52 | " unit: " << results.unit; 53 | ss << "\nWindow start: " << results.window_start.nanosec << " end: " << 54 | results.window_stop.nanosec; 55 | 56 | for (const auto & statistic : results.statistics) { 57 | ss << "\n" << 58 | STATISTIC_TYPES[statistic.data_type] << 59 | ": " << 60 | std::to_string(statistic.data); 61 | } 62 | 63 | return ss.str(); 64 | } 65 | --------------------------------------------------------------------------------