├── docker ├── docker_build.sh ├── docker_launch.sh └── Dockerfile ├── test ├── test_runner.cpp ├── factorial_test.cpp └── hello_world_test.cpp ├── .gitmodules ├── tools ├── compile_debug_launch.sh ├── compile_launch.sh ├── compile_clean.sh ├── compile.sh ├── install.sh ├── compile_test.sh └── launch_test_nodes.sh ├── codecov.yml ├── include └── ornis │ ├── config.hpp │ ├── topic_printer.hpp │ ├── ros_interface_node.hpp │ ├── topic_string_viewer.hpp │ ├── stream_interface.hpp │ ├── topic_plotter.hpp │ ├── topic_monitor.hpp │ ├── node_monitor.hpp │ ├── topic_visualiser.hpp │ ├── service_monitor.hpp │ ├── channel_interface.hpp │ ├── monitor_interface.hpp │ ├── topic_streamer.hpp │ ├── helper_functions.hpp │ ├── object_controller.hpp │ ├── monitor.hpp │ ├── options.hpp │ ├── introspection_functions.hpp │ ├── ui.hpp │ ├── msg_tree.hpp │ └── ui_helpers.hpp ├── src ├── ros_interface_node.cpp ├── main.cpp ├── topic_printer.cpp ├── monitor_interface.cpp ├── topic_string_viewer.cpp ├── topic_monitor.cpp ├── topic_plotter.cpp ├── topic_streamer.cpp ├── options.cpp ├── node_monitor.cpp ├── service_monitor.cpp ├── object_controller.cpp └── introspection_functions.cpp ├── .gitlab-ci.yml ├── appveyor.yml ├── .gitignore ├── package.xml ├── python ├── test_service.py ├── twist_publisher.py ├── topic_streamer_double.py └── random_number_publishers.py ├── .github └── workflows │ └── default-action.yml ├── .clang-format ├── .travis.yml ├── README.md ├── CMakeLists.txt └── LICENSE /docker/docker_build.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env sh 2 | docker build -t ornis_docker . 3 | -------------------------------------------------------------------------------- /test/test_runner.cpp: -------------------------------------------------------------------------------- 1 | #define DOCTEST_CONFIG_IMPLEMENT_WITH_MAIN 2 | #include "doctest.h" 3 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "external/notcurses"] 2 | path = external/notcurses 3 | url = https://github.com/dankamongmen/notcurses 4 | -------------------------------------------------------------------------------- /tools/compile_debug_launch.sh: -------------------------------------------------------------------------------- 1 | ./compile.sh && clear && valgrind --log-file="ornis.log" --track-origins=yes ../../build/ornis/ornis 2 | -------------------------------------------------------------------------------- /codecov.yml: -------------------------------------------------------------------------------- 1 | ignore: 2 | - "test/" 3 | - "third_party/**/*" 4 | 5 | comment: 6 | layout: "header, diff, files" 7 | behavior: default 8 | require_changes: true 9 | -------------------------------------------------------------------------------- /tools/compile_launch.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | cd "$(dirname "$0")" 4 | cd ../../ 5 | (colcon build --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -DCMAKE_BUILD_TYPE=Debug --symlink-install && ./build/ornis/ornis ) 6 | cd ornis 7 | rm compile_commands.json 8 | cp ../build/compile_commands.json . 9 | -------------------------------------------------------------------------------- /tools/compile_clean.sh: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | cd "$(dirname "$0")" 3 | cd ../../ 4 | rm -r build/* 5 | rm -r install/* 6 | colcon build --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -DCMAKE_BUILD_TYPE=Debug --symlink-install 7 | rm ornis/compile_commands.json 8 | cp build/compile_commands.json ornis/compile_commands.json 9 | -------------------------------------------------------------------------------- /docker/docker_launch.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env sh 2 | 3 | docker container stop ornis_docker 4 | docker container rm ornis_docker 5 | docker run --network=host --ipc=host --pid=host --privileged --ulimit nofile=1024:2056 --env="DISPLAY" -it -v /dev/shm:/dev/shm -v ~/ros2_ws/ornis_ws:/home/ornis/ornis_ws:Z --name ornis_docker ornis_docker 6 | -------------------------------------------------------------------------------- /include/ornis/config.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #ifndef CONFIG_HPP_ 3 | #define CONFIG_HPP_ 4 | 5 | #define VERSION_MAJOR "@Project-Name_VERSION_MAJOR@" 6 | #define VERSION_MINOR "@Project-Name_VERSION_MINOR@" 7 | #define VERSION_PATCH "@Project-Name_VERSION_PATCH@" 8 | #define VERSION "@Project-Name_VERSION@" 9 | 10 | #endif // !CONFIG_HPP_ 11 | -------------------------------------------------------------------------------- /tools/compile.sh: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | set -euo pipefail 3 | cd "$(dirname "$0")" 4 | cd ../.. 5 | colcon build --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -DCMAKE_BUILD_TYPE=Debug -DCMAKE_CXX_OUTPUT_EXTENSION_REPLACE=ON --symlink-install --event-handlers console_direct+ 6 | cd ornis 7 | rm compile_commands.json 8 | cp ../build/compile_commands.json . 9 | -------------------------------------------------------------------------------- /tools/install.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | if [[ -z "$PREFIX" ]]; then 4 | PREFIX="/usr/local" 5 | # $var is empty, do what you want 6 | fi 7 | 8 | 9 | echo "Installing ornis to $PREFIX/bin" 10 | 11 | cd "$(dirname "$0")" 12 | 13 | if ./compile.sh ; then 14 | echo "Copying file" 15 | sudo cp ../../build/ornis/ornis $PREFIX/bin 16 | fi 17 | echo "Installation complete!" 18 | -------------------------------------------------------------------------------- /docker/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:humble-ros-base 2 | 3 | # Add ornis user 4 | RUN useradd -ms /bin/bash ornis 5 | WORKDIR /home/ornis/ornis_ws/ornis/ 6 | 7 | RUN apt-get update \ 8 | && apt-get upgrade -y \ 9 | && apt-get clean 10 | 11 | # install ORNIS deps 12 | RUN apt-get install -y build-essential cmake doctest-dev libavdevice-dev libdeflate-dev libgpm-dev \ 13 | libncurses-dev libqrcodegen-dev libswscale-dev libunistring-dev pandoc pkg-config \ 14 | ros-humble-rmw-fastrtps-dynamic-cpp git 15 | 16 | user ornis 17 | -------------------------------------------------------------------------------- /include/ornis/topic_printer.hpp: -------------------------------------------------------------------------------- 1 | #ifndef TOPIC_PRINTER_H_ 2 | #define TOPIC_PRINTER_H_ 3 | 4 | #include 5 | #include "ornis/topic_visualiser.hpp" 6 | 7 | class TopicPrinter : public TopicVisualiser 8 | { 9 | public: 10 | TopicPrinter(ncpp::Plane* plane, uint height, uint width, std::vector entry_path, 11 | const Options::color_scheme theme); 12 | ~TopicPrinter(); 13 | 14 | void renderData(const rosidl_typesupport_introspection_cpp::MessageMembers* members, uint8_t* data); 15 | 16 | private: 17 | uint longest_string_; 18 | }; 19 | 20 | #endif // TOPIC_PRINTER_H_ 21 | -------------------------------------------------------------------------------- /src/ros_interface_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "ornis/ros_interface_node.hpp" 5 | 6 | namespace rclcpp 7 | { 8 | class NodeOptions; 9 | } // namespace rclcpp 10 | 11 | RosInterfaceNode::RosInterfaceNode(const std::string& node_name, const rclcpp::NodeOptions& options) 12 | : Node(node_name), spin_(true) 13 | { 14 | (void)options; 15 | } 16 | 17 | void RosInterfaceNode::spin() 18 | { 19 | rclcpp::Rate rate(1); 20 | while (spin_) 21 | { 22 | rate.sleep(); 23 | } 24 | rclcpp::shutdown(); 25 | } 26 | 27 | RosInterfaceNode::~RosInterfaceNode() 28 | { 29 | } 30 | -------------------------------------------------------------------------------- /tools/compile_test.sh: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | 3 | cd "$(dirname "$0")" 4 | cd ../../ 5 | rm -r build/* 6 | rm -r install/* 7 | colcon build --cmake-clean-cache --cmake-args " -DFIX_INCLUDES=ON" " -DCHECK_INCLUDES=ON" " -DUSE_POC=OFF" " -DCMAKE_CXX_OUTPUT_EXTENSION_REPLACE=ON" " -DCMAKE_BUILD_TYPE=Debug" " -DCMAKE_EXPORT_COMPILE_COMMANDS=ON" --symlink-install --event-handlers console_direct+ 8 | rm ornis/compile_commands.json 9 | cp build/compile_commands.json ornis/compile_commands.json 10 | cd ../../ 11 | # Until I figure out how to get the ament 12 | # linters to ignore the external/ directory, don't use colcon test 13 | # colcon test --event-handlers console_cohesion+ 14 | -------------------------------------------------------------------------------- /.gitlab-ci.yml: -------------------------------------------------------------------------------- 1 | image: registry.gitlab.com/juraph/ornis:latest 2 | 3 | workflow: 4 | # Only run on master branch and merge requests 5 | rules: 6 | - if: $CI_MERGE_REQUEST_ID 7 | - if: $CI_COMMIT_BRANCH == "master" 8 | 9 | variables: 10 | PROJECT_BASE_DIR: $CI_PROJECT_DIR 11 | 12 | stages: 13 | - build 14 | 15 | build: 16 | stage: build 17 | before_script: 18 | - apt-get update -y 19 | 20 | script: 21 | - . /opt/ros/humble/setup.sh 22 | - git submodule update --init --recursive 23 | - colcon build --event-handlers console_direct+ 24 | variables: 25 | GIT_STRATEGY: clone 26 | artifacts: 27 | expire_in: 30 days 28 | reports: 29 | junit: $CI_PROJECT_DIR/test-results.xml 30 | -------------------------------------------------------------------------------- /include/ornis/ros_interface_node.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ROS_INTERFACE_NODE_H_ 2 | #define ROS_INTERFACE_NODE_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | namespace rclcpp 10 | { 11 | class NodeOptions; 12 | } // namespace rclcpp 13 | 14 | class RosInterfaceNode : public rclcpp::Node 15 | { 16 | public: 17 | explicit RosInterfaceNode(const rclcpp::NodeOptions& options) : Node("ornis", options) 18 | { 19 | } 20 | 21 | // Constructor 22 | RosInterfaceNode(const std::string& node_name, const rclcpp::NodeOptions& options); 23 | 24 | ~RosInterfaceNode(); 25 | 26 | void spin(); 27 | 28 | std::atomic spin_; 29 | }; 30 | 31 | #endif // ROS_INTERFACE_NODE_H_ 32 | -------------------------------------------------------------------------------- /appveyor.yml: -------------------------------------------------------------------------------- 1 | version: "{branch}-build-{build}" 2 | 3 | branches: 4 | only: 5 | - master 6 | 7 | os: 8 | - Visual Studio 2017 9 | 10 | init: 11 | - git config --global core.autocrlf input 12 | 13 | install: 14 | - git submodule update --init --recursive 15 | 16 | platform: 17 | - Win32 18 | - x64 19 | 20 | configuration: 21 | - Debug 22 | - Release 23 | 24 | before_build: 25 | - mkdir build && cd build 26 | - cmake -A%PLATFORM% -DBUILD_TESTS=ON -DGEN_DOCS=OFF .. 27 | 28 | build_script: 29 | - cmake --build . --config %CONFIGURATION% -- %MSBUILD_ARGS% # Build the code 30 | - ctest -C %CONFIGURATION% # Run tests 31 | - cmake --build . --config %CONFIGURATION% --target INSTALL -- %MSBUILD_ARGS% # Install the necessary files 32 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Created by https://www.gitignore.io/api/c++,cmake 2 | 3 | ### C++ ### 4 | # Prerequisites 5 | *.d 6 | 7 | # Compiled Object files 8 | *.slo 9 | *.lo 10 | *.o 11 | *.obj 12 | 13 | # Precompiled Headers 14 | *.gch 15 | *.pch 16 | 17 | # Compiled Dynamic libraries 18 | *.so 19 | *.dylib 20 | *.dll 21 | 22 | # Fortran module files 23 | *.mod 24 | *.smod 25 | 26 | # Compiled Static libraries 27 | *.lai 28 | *.la 29 | *.a 30 | *.lib 31 | 32 | # Executables 33 | *.exe 34 | *.out 35 | *.app 36 | 37 | ### CMake ### 38 | CMakeCache.txt 39 | CMakeFiles 40 | CMakeScripts 41 | Testing 42 | Makefile 43 | cmake_install.cmake 44 | install_manifest.txt 45 | compile_commands.json 46 | CTestTestfile.cmake 47 | build 48 | 49 | 50 | # End of https://www.gitignore.io/api/c++,cmake 51 | -------------------------------------------------------------------------------- /include/ornis/topic_string_viewer.hpp: -------------------------------------------------------------------------------- 1 | #ifndef TOPIC_STRING_VIEWER_H_ 2 | #define TOPIC_STRING_VIEWER_H_ 3 | 4 | #include 5 | 6 | #include "ornis/topic_visualiser.hpp" 7 | 8 | class TopicStringViewer : public TopicVisualiser 9 | { 10 | public: 11 | TopicStringViewer(ncpp::Plane* plane, uint height, uint width, std::vector entry_path, 12 | const Options::color_scheme& theme); 13 | ~TopicStringViewer(); 14 | 15 | void renderData(const rosidl_typesupport_introspection_cpp::MessageMembers* members, uint8_t* data); 16 | 17 | private: 18 | void drawStrings(); 19 | 20 | DataBuffer data_buffer_; 21 | 22 | uint longest_string_; 23 | 24 | float r_step_, b_step_, g_step_; 25 | }; 26 | 27 | #endif // TOPIC_STRING_VIEWER_H_ 28 | -------------------------------------------------------------------------------- /test/factorial_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "doctest.h" 4 | 5 | TEST_CASE("function factorial") 6 | { 7 | SUBCASE("normal arguments") 8 | { 9 | CHECK(factorial(2) == 2); 10 | CHECK(factorial(3) == 6); 11 | CHECK(factorial(4) == 24); 12 | CHECK(factorial(5) == 120); 13 | } 14 | 15 | SUBCASE("somewhat large numbers") 16 | { 17 | CHECK(factorial(10) == 3628800); 18 | CHECK(factorial(12) == 479001600); 19 | } 20 | 21 | SUBCASE("negative numbers") 22 | { 23 | CHECK(factorial(-1) == 1); 24 | CHECK(factorial(-2) == 1); 25 | } 26 | 27 | SUBCASE("zero and one") 28 | { 29 | CHECK(factorial(0) == 1); 30 | CHECK(factorial(1) == 1); 31 | } 32 | } 33 | -------------------------------------------------------------------------------- /test/hello_world_test.cpp: -------------------------------------------------------------------------------- 1 | #include "doctest.h" 2 | 3 | #include 4 | 5 | #include // Needed in OSX builds for some weird reason. 6 | // See https://github.com/onqtam/doctest/issues/126#issuecomment-381742003 7 | #include // This isn't needed but it clarifies intent. 8 | 9 | TEST_CASE("class HelloWorld") 10 | { 11 | HelloWorld helloWorld; 12 | 13 | SUBCASE("function hello()") 14 | { 15 | CHECK(helloWorld.hello() == "Hello"); 16 | } 17 | SUBCASE("function world()") 18 | { 19 | CHECK(helloWorld.world() == "World"); 20 | } 21 | SUBCASE("function generateRandomNumber()") 22 | { 23 | CHECK(helloWorld.generateRandomNumber() == 4000); 24 | } 25 | SUBCASE("function headerFunction()") 26 | { 27 | CHECK(helloWorld.headerFunction(6) == 3); 28 | // Intentionally incomplete code coverage 29 | } 30 | } 31 | -------------------------------------------------------------------------------- /include/ornis/stream_interface.hpp: -------------------------------------------------------------------------------- 1 | #ifndef STREAM_INTERFACE_H_ 2 | #define STREAM_INTERFACE_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include // IWYU pragma: keep 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | class StreamChannel 16 | { 17 | public: 18 | explicit StreamChannel(const std::string& topic_name) : topic_name_(topic_name), stream_open_(false) 19 | { 20 | } 21 | 22 | StreamChannel(){}; 23 | ~StreamChannel(){}; 24 | 25 | std::mutex access_mutex_; 26 | std::condition_variable condition_variable_; 27 | 28 | // name of topic being streamed 29 | std::string topic_name_; 30 | 31 | // Flag for whether the stream is currently open in the UI. 32 | // Object controller checks this flag for whether to kill the stream thread 33 | std::atomic stream_open_; 34 | 35 | // Plane that gets written to directly from the streamer 36 | std::shared_ptr stream_plane_; 37 | }; 38 | 39 | #endif // STREAM_INTERFACE_H_ 40 | -------------------------------------------------------------------------------- /include/ornis/topic_plotter.hpp: -------------------------------------------------------------------------------- 1 | #ifndef TOPIC_PLOTTER_H_ 2 | #define TOPIC_PLOTTER_H_ 3 | 4 | #include 5 | 6 | #include "ornis/options.hpp" 7 | #include "ornis/topic_visualiser.hpp" 8 | 9 | class TopicPlotter : public TopicVisualiser 10 | { 11 | public: 12 | TopicPlotter(ncpp::Plane* plane, uint height, uint width, std::vector entry_path, 13 | const Options::color_scheme theme); 14 | ~TopicPlotter(); 15 | 16 | void renderData(const rosidl_typesupport_introspection_cpp::MessageMembers* members, uint8_t* data); 17 | 18 | private: 19 | void drawPlot(); 20 | void drawSlice(const uint64_t& curr_point, const uint64_t& next_point, const uint64_t& horizontal_loc); 21 | 22 | DataBuffer data_buffer_; // Storage of actual message data 23 | 24 | unsigned long timestep_; 25 | DataBuffer scaled_data_buffer_; // Storage of scaled message data. 26 | // Gets rescaled upon recieving a datapoint beyond previous bounds 27 | 28 | double highest_value_, lowest_value_; 29 | 30 | uint entry_offset_; 31 | }; 32 | 33 | #endif // TOPIC_PLOTTER_H_ 34 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ornis 5 | 0.0.0 6 | TODO: Package description 7 | juraph 8 | TODO: License declaration 9 | 10 | rcl 11 | rclcpp 12 | rcl_action 13 | rcl_interfaces 14 | ament_index_cpp 15 | 16 | rosidl_runtime_cpp 17 | rosidl_generator_cpp 18 | rosidl_typesupport_cpp 19 | rosidl_typesupport_introspection_cpp 20 | 21 | ament_cmake 22 | 23 | ament_lint_auto 24 | ament_cmake_clang_format 25 | ament_cmake_cppcheck 26 | 27 | 28 | 29 | 30 | 31 | ament_cmake 32 | 33 | 34 | -------------------------------------------------------------------------------- /python/test_service.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | from example_interfaces.srv import AddTwoInts 3 | 4 | import rclpy 5 | from rclpy.node import Node 6 | 7 | 8 | class MinimalService(Node): 9 | 10 | def __init__(self): 11 | super().__init__('minimal_service') 12 | self.int_srv = self.create_service(AddTwoInts, 'add_two_ints', self.add_two_ints_callback) 13 | 14 | def add_two_ints_callback(self, request, response): 15 | self.get_logger().info('[Int] Incoming request\na: %d b: %d' % (request.a, request.b)) 16 | response.sum = request.a + request.b 17 | return response 18 | 19 | def add_two_floats_callback(self, request, response): 20 | self.get_logger().info('[Float] Incoming request\na: %f b: %f' % (request.a, request.b)) 21 | response.sum = request.a + request.b 22 | return response 23 | 24 | def add_two_strings_callback(self, request, response): 25 | print('[String] Incoming request') 26 | print(request) 27 | response.sum.data = request.a.data + request.b.data 28 | return response 29 | 30 | 31 | def main(args=None): 32 | rclpy.init(args=args) 33 | 34 | minimal_service = MinimalService() 35 | 36 | rclpy.spin(minimal_service) 37 | 38 | rclpy.shutdown() 39 | 40 | 41 | if __name__ == '__main__': 42 | main() 43 | -------------------------------------------------------------------------------- /include/ornis/topic_monitor.hpp: -------------------------------------------------------------------------------- 1 | #ifndef TOPIC_MONITOR_H_ 2 | #define TOPIC_MONITOR_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include // IWYU pragma: keep 9 | 10 | #include "ornis/monitor.hpp" 11 | #include "ornis/ros_interface_node.hpp" 12 | 13 | class RosInterfaceNode; 14 | 15 | class TopicMonitor : public Monitor 16 | { 17 | public: 18 | TopicMonitor(std::shared_ptr ros_interface_node); 19 | ~TopicMonitor(); 20 | 21 | void getEntryInfo(const std::string& entry_name, const std::string& entry_details, 22 | std::map>& entry_info); 23 | 24 | void getInteractionForm(const std::string& entry_details, msg_tree::MsgTree& form); 25 | 26 | void getInteractionTree(const std::string message_type, msg_tree::MsgTree& message_tree); 27 | 28 | void interact(const std::string& entry_name, const std::string& entry_details, const msg_tree::MsgTree& request, 29 | msg_tree::MsgTree& response); 30 | 31 | private: 32 | static constexpr auto ros2_list_string_ = "ros2 topic list"; 33 | static constexpr auto ros2_info_string_ = "ros2 topic info "; 34 | 35 | void spin(); 36 | void updateValue(); 37 | 38 | std::thread* thread_; 39 | 40 | std::shared_ptr ros_interface_node_; 41 | }; 42 | 43 | #endif // TOPIC_MONITOR_H_ 44 | -------------------------------------------------------------------------------- /tools/launch_test_nodes.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | (ros2 topic pub /turtle1/cmd_vel geometry_msgs/msg/Twist "{linear: {x: 2.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 1.8}}" -n test1) > /dev/null 2>&1 & 4 | (ros2 topic pub /turtle2/cmd_vel geometry_msgs/msg/Twist "{linear: {x: 2.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 1.8}}" -n test2) > /dev/null 2>&1 & 5 | (ros2 topic pub /turtle3/cmd_vel geometry_msgs/msg/Twist "{linear: {x: 2.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 1.8}}" -n test3) > /dev/null 2>&1 & 6 | (ros2 topic pub /turtle4/cmd_vel geometry_msgs/msg/Twist "{linear: {x: 2.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 1.8}}" -n test4) > /dev/null 2>&1 & 7 | (ros2 topic pub /turtle5/cmd_vel geometry_msgs/msg/Twist "{linear: {x: 2.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 1.8}}" -n test5) > /dev/null 2>&1 & 8 | (ros2 topic pub /test6 std_msgs/Float32 123.3) > /dev/null 2>&1 & 9 | (ros2 topic pub /test7 std_msgs/Float32 123.3) > /dev/null 2>&1 & 10 | (ros2 topic pub /test8 std_msgs/Float32 123.3) > /dev/null 2>&1 & 11 | (ros2 topic pub /test9 std_msgs/Float32 123.3) > /dev/null 2>&1 & 12 | (ros2 topic pub /test10 std_msgs/Int 5) > /dev/null 2>&1 & 13 | (ros2 topic pub /test11 std_msgs/Int 5) > /dev/null 2>&1 & 14 | (ros2 topic pub /test12 std_msgs/Int 5) > /dev/null 2>&1 & 15 | (ros2 topic pub /test13 std_msgs/Int 5) > /dev/null 2>&1 & 16 | (ros2 topic pub /test14 std_msgs/Int 5) > /dev/null 2>&1 17 | -------------------------------------------------------------------------------- /include/ornis/node_monitor.hpp: -------------------------------------------------------------------------------- 1 | #ifndef NODE_MONITOR_H_ 2 | #define NODE_MONITOR_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include // IWYU pragma: keep 10 | 11 | #include "ornis/monitor.hpp" 12 | 13 | class RosInterfaceNode; 14 | 15 | class NodeMonitor : public Monitor 16 | { 17 | public: 18 | NodeMonitor(std::shared_ptr ros_interface_node); 19 | ~NodeMonitor(); 20 | 21 | void getEntryInfo(const std::string& entry_name, const std::string& entry_details, 22 | std::map>& entry_info); 23 | 24 | void getInteractionForm(const std::string& entry_details, msg_tree::MsgTree& form); 25 | 26 | void interact(const std::string& entry_name, const std::string& entry_details, const msg_tree::MsgTree& request, 27 | msg_tree::MsgTree& response); 28 | 29 | private: 30 | static constexpr auto ros2_list_string_ = "ros2 node list"; 31 | static constexpr auto ros2_info_string_ = "ros2 node info "; 32 | 33 | void spin(); 34 | void updateValue(); 35 | 36 | void namesAndTypesToMap(const std::string& entry_name, const rcl_names_and_types_t& names_and_types, 37 | std::map>& entry_map); 38 | 39 | std::thread* thread_; 40 | 41 | std::shared_ptr ros_interface_node_; 42 | }; 43 | 44 | #endif // NODE_MONITOR_H_ 45 | -------------------------------------------------------------------------------- /include/ornis/topic_visualiser.hpp: -------------------------------------------------------------------------------- 1 | #ifndef TOPIC_VISUALISER_H_ 2 | #define TOPIC_VISUALISER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "ncpp/Plane.hh" 9 | #include "ornis/options.hpp" 10 | 11 | template 12 | class DataBuffer 13 | { 14 | public: 15 | DataBuffer(const int n) : n_(n), filled_(false) 16 | { 17 | buffer.resize(n_); 18 | } 19 | 20 | void step(T xn) 21 | { 22 | buffer[i_] = xn; 23 | i_++; 24 | if (i_ == n_) 25 | { 26 | filled_ = true; 27 | i_ = 0; 28 | } 29 | } 30 | 31 | std::vector buffer; 32 | 33 | const size_t n_; // Size of buffer 34 | size_t i_ = 0; // index currently being addressed 35 | bool filled_; // Flag indicating the buffer has been completely filled 36 | }; 37 | 38 | class TopicVisualiser 39 | { 40 | public: 41 | TopicVisualiser(ncpp::Plane* plane, uint height, uint width, std::vector entry_path, const Options::color_scheme& theme) 42 | : height_(height), width_(width), entry_path_(entry_path), plane_(plane), theme_(theme) 43 | { 44 | } 45 | virtual ~TopicVisualiser() 46 | { 47 | } 48 | 49 | virtual void renderData(const rosidl_typesupport_introspection_cpp::MessageMembers* members, uint8_t* data) = 0; 50 | 51 | uint height_, width_; 52 | 53 | std::vector entry_path_; 54 | 55 | ncpp::Plane* plane_; 56 | 57 | const Options::color_scheme theme_; 58 | }; 59 | 60 | #endif // TOPIC_VISUALISER_H_ 61 | -------------------------------------------------------------------------------- /include/ornis/service_monitor.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SERVICE_MONITOR_H_ 2 | #define SERVICE_MONITOR_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include // IWYU pragma: keep 9 | 10 | #include "ornis/monitor.hpp" 11 | 12 | // TODO Rename/Clean up this struct. 13 | struct serviceInfo 14 | { 15 | const rosidl_service_type_support_t* type_support; 16 | const rosidl_message_type_support_t* request_type_support; 17 | const rosidl_message_type_support_t* response_type_support; 18 | }; 19 | 20 | class ServiceMonitor : public Monitor 21 | { 22 | public: 23 | ServiceMonitor(std::shared_ptr ros_interface_node); 24 | ~ServiceMonitor(); 25 | 26 | void getEntryInfo(const std::string& entry_name, const std::string& entry_details, 27 | std::map>& entry_info); 28 | 29 | void getInteractionForm(const std::string& entry_details, msg_tree::MsgTree& form); 30 | 31 | void interact(const std::string& entry_name, const std::string& entry_details, const msg_tree::MsgTree& request, 32 | msg_tree::MsgTree& response); 33 | 34 | private: 35 | static constexpr auto ros2_info_string_ = "ros2 service info "; 36 | 37 | void spin(); 38 | void updateValue(); 39 | 40 | void getInteractionTrees(const std::string service_type, msg_tree::MsgTree& request, msg_tree::MsgTree& response); 41 | 42 | std::thread* thread_; 43 | std::shared_ptr ros_interface_node_; 44 | 45 | serviceInfo service_info_; 46 | }; 47 | 48 | #endif // SERVICE_MONITOR_H_ 49 | -------------------------------------------------------------------------------- /include/ornis/channel_interface.hpp: -------------------------------------------------------------------------------- 1 | #ifndef CHANNEL_INTERFACE_H_ 2 | #define CHANNEL_INTERFACE_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include "ornis/msg_tree.hpp" 14 | 15 | class Channel 16 | { 17 | public: 18 | Channel() : request_pending_(false), ui_data_current_(false){}; 19 | ~Channel(){}; 20 | 21 | enum class RequestEnum 22 | { 23 | monitorEntryInformation = 0, 24 | monitorEntryInteraction = 1, 25 | monitorEntryInteractionResult = 2, 26 | topicStreamer = 3, 27 | closeStream = 4 28 | }; 29 | 30 | std::mutex access_mutex_; 31 | // Structures to facilitate UI requesting information from 32 | // Object controller 33 | RequestEnum request_type_; 34 | std::string response_string_; 35 | std::atomic request_pending_; 36 | std::condition_variable condition_variable_; 37 | std::map request_details_; 38 | 39 | std::map> response_map_; 40 | std::pair* request_response_trees_; 41 | 42 | // Structures to facilitate Object controller storing updated information 43 | // for UI to grab when ready 44 | std::map>> latest_monitor_data_; 45 | // Flag to allow the UI to check if the monitor data has been updated since 46 | // last check. 47 | std::atomic ui_data_current_; 48 | }; 49 | 50 | #endif // CHANNEL_INTERFACE_H_ 51 | -------------------------------------------------------------------------------- /python/twist_publisher.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import rclpy 4 | from rclpy.node import Node 5 | from rclpy.qos import qos_profile_sensor_data 6 | from geometry_msgs.msg import TwistStamped 7 | import math, random 8 | 9 | 10 | class MinimalPublisher(Node): 11 | 12 | def __init__(self): 13 | super().__init__('twist_publisher') 14 | 15 | self.twist_publisher_ = self.create_publisher(TwistStamped, 'twist_pub', qos_profile_sensor_data) 16 | timer_period = 0.1 # Start with 10hz 17 | self.timer = self.create_timer(timer_period, self.timer_callback) 18 | self.i = 0 19 | self.scale = 1000 * (random.random() - 0.5) 20 | 21 | def timer_callback(self): 22 | 23 | # msg_seed = self.scale * math.sin(self.i / 5) 24 | twist_msg = TwistStamped() 25 | twist_msg.header.stamp = self.get_clock().now().to_msg() 26 | twist_msg.twist.linear.x = 100 * random.random() 27 | twist_msg.twist.linear.y = 100 * random.random() 28 | twist_msg.twist.linear.z = 100 * random.random() 29 | 30 | twist_msg.twist.angular.x = random.random() 31 | twist_msg.twist.angular.y = random.random() 32 | twist_msg.twist.angular.z = random.random() 33 | self.twist_publisher_.publish(twist_msg) 34 | # self.get_logger().info('Publishing: "%s"' % twist_msg) 35 | self.i += 1 36 | 37 | 38 | def main(args=None): 39 | rclpy.init(args=args) 40 | 41 | minimal_publisher = MinimalPublisher() 42 | 43 | rclpy.spin(minimal_publisher) 44 | 45 | # Destroy the node explicitly 46 | # (optional - otherwise it will be done automatically 47 | # when the garbage collector destroys the node object) 48 | minimal_publisher.destroy_node() 49 | rclpy.shutdown() 50 | 51 | 52 | if __name__ == '__main__': 53 | main() 54 | -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | // Config contains application information such as VERSION 6 | #include "ornis/config.hpp" 7 | #include "ornis/object_controller.hpp" 8 | 9 | ObjectController object_controller; 10 | 11 | void intHandler(int dum) 12 | { 13 | std::cout << "[Ornis] Recieved Signal, shutting down: " << dum << '\n'; 14 | object_controller.ui_.screen_loop_ = false; 15 | rclcpp::shutdown(); 16 | exit(0); 17 | } 18 | 19 | int main(int argc, char* argv[]) 20 | { 21 | signal(SIGINT, intHandler); 22 | 23 | // Print version information 24 | if (argc == 2 && std::string{ argv[1] } == "--version") 25 | { 26 | std::cout << "Project Name version " << VERSION << "\n"; 27 | std::cout << "Copyright information here\n"; 28 | std::cout << "More copyright details.\n"; 29 | return 0; 30 | } 31 | 32 | int obj_ret = object_controller.spin(); 33 | 34 | if (obj_ret == -1) 35 | { 36 | (&object_controller)->~ObjectController(); 37 | new (&object_controller) ObjectController(); 38 | obj_ret = object_controller.spin(); 39 | } 40 | if (obj_ret == 1) 41 | { 42 | std::cerr << "Failed to get typesupport for std_msgs. Ensure your ros workspace is sourced\n"; 43 | } 44 | else if (obj_ret == 2) 45 | { 46 | std::cerr << "Hello there, it looks like you're using fastrtps without dynamic typesupport, \n \ 47 | You can launch ORNIS with this functionality using: $RMW_IMPLEMENTATION=rmw_fastrtps_dynamic_cpp ornis\n \ 48 | (You may also need to install the package: \n " 49 | << "#apt install ros-" << std::getenv("ROS_DISTRO") << "-rmw-fastrtps-dynamic-cpp)\n \ 50 | Maybe a handy alias: \"alias ornis=RMW_IMPLEMENTATION=rmw_fastrtps_dynamic_cpp ornis\" in the ~/.bashrc?\ 51 | \n Exiting...\n"; 52 | } 53 | 54 | exit(0); 55 | } 56 | -------------------------------------------------------------------------------- /python/topic_streamer_double.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import rclpy 4 | from rclpy.node import Node 5 | from rclpy.qos import qos_profile_sensor_data 6 | from std_msgs.msg import Float32 7 | import math, random 8 | 9 | # def callback(data): 10 | # rospy.loginfo(rospy.get_caller_id() + "I heard %f", data.data) 11 | 12 | # def listener(): 13 | 14 | # rospy.init_node('listener', anonymous=True) 15 | 16 | # rospy.Subscriber("number", Float32, callback) 17 | 18 | # # spin() simply keeps python from exiting until this node is stopped 19 | # rospy.spin() 20 | 21 | class MinimalPublisher(Node): 22 | 23 | def __init__(self): 24 | super().__init__('sin_wave_pub') 25 | 26 | self.float_publisher_ = self.create_publisher(Float32, 'sin_wave', qos_profile_sensor_data) 27 | timer_period = 0.1 # Start with 10hz 28 | self.timer = self.create_timer(timer_period, self.timer_callback) 29 | self.i = 0 30 | self.scale = 1000 * (random.random() - 0.5) 31 | 32 | def timer_callback(self): 33 | 34 | # msg_seed = self.scale * math.sin(self.i / 5) 35 | msg_seed = self.scale * math.sin(self.i/5) 36 | float_msg = Float32() 37 | float_msg.data = msg_seed 38 | self.float_publisher_.publish(float_msg) 39 | self.get_logger().info('Publishing: "%s"' % float_msg.data) 40 | self.i += 1 41 | 42 | 43 | def main(args=None): 44 | rclpy.init(args=args) 45 | 46 | minimal_publisher = MinimalPublisher() 47 | 48 | rclpy.spin(minimal_publisher) 49 | 50 | # Destroy the node explicitly 51 | # (optional - otherwise it will be done automatically 52 | # when the garbage collector destroys the node object) 53 | minimal_publisher.destroy_node() 54 | rclpy.shutdown() 55 | 56 | 57 | if __name__ == '__main__': 58 | main() 59 | -------------------------------------------------------------------------------- /.github/workflows/default-action.yml: -------------------------------------------------------------------------------- 1 | 2 | name: ORNIS 3 | 4 | on: 5 | push: 6 | branches: [ master ] 7 | pull_request: 8 | branches: [ master ] 9 | 10 | jobs: 11 | test_docker: # On Linux, iterates on all ROS 1 and ROS 2 distributions. 12 | runs-on: ubuntu-latest 13 | strategy: 14 | matrix: 15 | ros_distribution: 16 | - foxy 17 | - humble 18 | - rolling 19 | include: 20 | # Foxy Fitzroy (June 2020 - May 2023) 21 | - docker_image: ubuntu:focal 22 | ros_distribution: foxy 23 | ros_version: 2 24 | # Humble Hawksbill (May 2022 - May 2027) 25 | - docker_image: ubuntu:jammy 26 | ros_distribution: humble 27 | ros_version: 2 28 | # Rolling Ridley (No End-Of-Life) 29 | - docker_image: ubuntu:jammy 30 | ros_distribution: rolling 31 | ros_version: 2 32 | 33 | container: 34 | image: ${{ matrix.docker_image }} 35 | steps: 36 | - name: Setup ROS environment 37 | uses: ros-tooling/setup-ros@v0.6 38 | with: 39 | required-ros-distributions: ${{ matrix.ros_distribution }} 40 | - name: Install Notcurses deps 41 | run: | 42 | sudo apt update 43 | sudo apt install -y \ 44 | build-essential \ 45 | cmake \ 46 | doctest-dev \ 47 | ffmpeg \ 48 | libavdevice-dev \ 49 | libdeflate-dev \ 50 | libncurses-dev \ 51 | libqrcodegen-dev \ 52 | libswscale-dev \ 53 | libunistring-dev \ 54 | pandoc \ 55 | pkg-config \ 56 | python3-cffi \ 57 | python3-dev \ 58 | python3-setuptools 59 | - name: Build ORNIS 60 | if: ${{ matrix.ros_version == 2 }} 61 | uses: ros-tooling/action-ros-ci@v0.2 62 | with: 63 | target-ros2-distro: ${{ matrix.ros_distribution }} 64 | skip-tests: true 65 | -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | BasedOnStyle: Google 3 | AccessModifierOffset: -2 4 | ConstructorInitializerIndentWidth: 2 5 | AlignEscapedNewlinesLeft: false 6 | AlignTrailingComments: true 7 | AllowAllParametersOfDeclarationOnNextLine: false 8 | AllowShortIfStatementsOnASingleLine: false 9 | AllowShortLoopsOnASingleLine: false 10 | AllowShortFunctionsOnASingleLine: None 11 | AlwaysBreakTemplateDeclarations: true 12 | AlwaysBreakBeforeMultilineStrings: true 13 | BreakBeforeBinaryOperators: false 14 | BreakBeforeTernaryOperators: false 15 | BreakConstructorInitializersBeforeComma: true 16 | BinPackParameters: true 17 | ColumnLimit: 120 18 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 19 | DerivePointerBinding: false 20 | PointerBindsToType: true 21 | ExperimentalAutoDetectBinPacking: false 22 | IndentCaseLabels: true 23 | MaxEmptyLinesToKeep: 1 24 | NamespaceIndentation: None 25 | ObjCSpaceBeforeProtocolList: true 26 | PenaltyBreakBeforeFirstCallParameter: 19 27 | PenaltyBreakComment: 60 28 | PenaltyBreakString: 1 29 | PenaltyBreakFirstLessLess: 1000 30 | PenaltyExcessCharacter: 1000 31 | PenaltyReturnTypeOnItsOwnLine: 90 32 | SpacesBeforeTrailingComments: 2 33 | Cpp11BracedListStyle: false 34 | Standard: Auto 35 | IndentWidth: 2 36 | TabWidth: 2 37 | UseTab: Never 38 | IndentFunctionDeclarationAfterType: false 39 | SpacesInParentheses: false 40 | SpacesInAngles: false 41 | SpaceInEmptyParentheses: false 42 | SpacesInCStyleCastParentheses: false 43 | SpaceAfterControlStatementKeyword: true 44 | SpaceBeforeAssignmentOperators: true 45 | ContinuationIndentWidth: 4 46 | SortIncludes: false 47 | SpaceAfterCStyleCast: false 48 | 49 | # Configure each individual brace in BraceWrapping 50 | BreakBeforeBraces: Custom 51 | 52 | # Control of individual brace wrapping cases 53 | BraceWrapping: { 54 | AfterClass: 'true' 55 | AfterControlStatement: 'true' 56 | AfterEnum : 'true' 57 | AfterFunction : 'true' 58 | AfterNamespace : 'true' 59 | AfterStruct : 'true' 60 | AfterUnion : 'true' 61 | BeforeCatch : 'true' 62 | BeforeElse : 'true' 63 | IndentBraces : 'false' 64 | } 65 | ... 66 | -------------------------------------------------------------------------------- /include/ornis/monitor_interface.hpp: -------------------------------------------------------------------------------- 1 | #ifndef MONITOR_INTERFACE_H_ 2 | #define MONITOR_INTERFACE_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include // IWYU pragma: keep 16 | #include 17 | #include "ornis/options.hpp" 18 | 19 | inline bool operator==(const ncselector_item A, const ncselector_item B) 20 | { 21 | return (strcmp(A.desc, B.desc) == 0) && strcmp(A.option, B.option) == 0; 22 | } 23 | 24 | class MonitorInterface 25 | { 26 | public: 27 | MonitorInterface(const std::string& monitor_name, const std::string& selector_title); 28 | ~MonitorInterface(); 29 | void initialiseInterface(const int& x, const int& y, const ncpp::Plane* std_plane, 30 | const Options::color_scheme& theme); 31 | 32 | unsigned getLines() const 33 | { 34 | return lines_; 35 | } 36 | 37 | void updateEntries(std::vector& new_vector, std::vector& add_values, 38 | std::vector& delete_values); 39 | 40 | ncpp::Plane* get_plane() const 41 | { 42 | return selector_->get_plane(); 43 | } 44 | void addLine() 45 | { 46 | ++lines_; 47 | } 48 | 49 | std::vector getEntries() 50 | { 51 | return entries_; 52 | } 53 | int getIdx() const 54 | { 55 | return idx_; 56 | } 57 | unsigned getRGB() const 58 | { 59 | return rgb_; 60 | } 61 | 62 | const std::string monitor_name_; 63 | 64 | std::shared_ptr selector_; 65 | 66 | // Plane that gets displayed in place of the selector when the Ui 67 | // wants to display a smaller version of the plane. 68 | std::shared_ptr minimised_plane_; 69 | 70 | private: 71 | int lines_; 72 | unsigned rgb_; 73 | int idx_; 74 | 75 | std::vector entries_; 76 | 77 | const std::string selector_title_; 78 | }; 79 | 80 | #endif // MONITOR_INTERFACE_H_ 81 | -------------------------------------------------------------------------------- /include/ornis/topic_streamer.hpp: -------------------------------------------------------------------------------- 1 | #ifndef TOPIC_STREAMER_H_ 2 | #define TOPIC_STREAMER_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include // IWYU pragma: keep 11 | 12 | #include "ornis/options.hpp" 13 | #include "ornis/stream_interface.hpp" 14 | #include "ornis/topic_visualiser.hpp" 15 | 16 | class StreamChannel; 17 | 18 | /* 19 | * A note on the streamer, Notcurses itself is thread-safe. This allows the 20 | * streamer to freely access and write to the plane provided by the UI, as long 21 | * as the UI promises to not access it at the same time, which it won't without 22 | * destroying the streamer first. 23 | * TODO: Check if the UI is allowed to move the plane while the stream is open 24 | */ 25 | class TopicStreamer 26 | { 27 | public: 28 | TopicStreamer(const std::string& topic_name, const std::string& topic_entry, const std::string& topic_type, 29 | const std::string& entry_type, const std::string& entry_path, 30 | std::shared_ptr& interface_channel, std::shared_ptr ros_interface_node, 31 | rcl_context_t context, const Options::color_scheme& theme); 32 | ~TopicStreamer(); 33 | 34 | void closeStream(); 35 | 36 | private: 37 | void updateValue(); 38 | void streamEntry(std::string& stream_frame); 39 | void waitUntilUiReady(); 40 | void initialise(); 41 | void callback(rcl_subscription_t& subscription, const rosidl_message_type_support_t* type_support); 42 | 43 | std::thread* thread_; 44 | 45 | const std::string topic_name_; 46 | // TODO: May be able to remove topic_entry 47 | const std::string topic_entry_; 48 | const std::string topic_type_; 49 | const std::string entry_type_; 50 | const std::string entry_path_; 51 | 52 | std::vector offset_; 53 | 54 | std::shared_ptr interface_channel_; 55 | 56 | std::shared_ptr ros_interface_node_; 57 | 58 | rcl_context_t context_; 59 | 60 | std::atomic stream_open_; 61 | 62 | std::unique_ptr topic_visualiser_; 63 | 64 | const Options::color_scheme theme_; 65 | }; 66 | 67 | #endif // TOPIC_STREAMER_H_ 68 | -------------------------------------------------------------------------------- /include/ornis/helper_functions.hpp: -------------------------------------------------------------------------------- 1 | #ifndef HELPER_FUNCTIONS_H_ 2 | #define HELPER_FUNCTIONS_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace helper_functions 9 | { 10 | inline bool getNthIndex(const std::string& string, const char& target_char, const size_t& target_count, size_t& index) 11 | { 12 | size_t count = 0; 13 | size_t final_find = 0; 14 | for (uint i = 0; i < string.length(); i++) 15 | { 16 | if (string[i] == target_char) 17 | { 18 | count++; 19 | final_find = i; 20 | if (count == target_count) 21 | { 22 | index = i; 23 | return true; 24 | } 25 | } 26 | } 27 | index = final_find; 28 | return false; 29 | } 30 | 31 | inline void getAllIndexes(const std::string& string, const char& target_char, std::vector& index) 32 | { 33 | for (uint i = 0; i < string.length(); i++) 34 | { 35 | if (string[i] == target_char) 36 | { 37 | index.push_back(i); 38 | } 39 | } 40 | } 41 | 42 | inline bool getDataFromRequestString(std::vector& data_strings, const std::string& request_string) 43 | { 44 | std::vector data_start_char_indexes, data_end_char_indexes; 45 | getAllIndexes(request_string, ':', data_start_char_indexes); 46 | getAllIndexes(request_string, '\n', data_end_char_indexes); 47 | 48 | size_t end_index = 0; 49 | for (size_t start_index = 0; start_index < data_start_char_indexes.size(); ++start_index) 50 | { 51 | // Get first index of data_end that is greater than data_end_char 52 | while (true) 53 | { 54 | // If the end char (Usually a newline) occurs before the data start, it doens't belong to the data, so discard 55 | if (data_end_char_indexes[end_index] < data_start_char_indexes[start_index]) 56 | { 57 | end_index++; 58 | } 59 | else 60 | { 61 | data_strings.push_back( 62 | request_string.substr(data_start_char_indexes[start_index] + 1, 63 | data_end_char_indexes[end_index] - data_start_char_indexes[start_index] - 1)); 64 | end_index++; 65 | break; 66 | } 67 | } 68 | } 69 | return true; 70 | } 71 | } // namespace helper_functions 72 | 73 | #endif // HELPER_FUNCTIONS_H_ 74 | -------------------------------------------------------------------------------- /include/ornis/object_controller.hpp: -------------------------------------------------------------------------------- 1 | #ifndef OBJECT_CONTROLLER_H_ 2 | #define OBJECT_CONTROLLER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "ornis/channel_interface.hpp" 13 | #include "ornis/node_monitor.hpp" 14 | #include "ornis/service_monitor.hpp" 15 | #include "ornis/stream_interface.hpp" 16 | #include "ornis/topic_monitor.hpp" 17 | #include "ornis/topic_streamer.hpp" 18 | #include "ornis/ui.hpp" 19 | 20 | class Channel; 21 | class Monitor; 22 | class RosInterfaceNode; 23 | class StreamChannel; 24 | class TopicStreamer; 25 | 26 | class ObjectController 27 | { 28 | public: 29 | // Constructor 30 | ObjectController(); 31 | 32 | ~ObjectController(); 33 | 34 | // Initialises object controller. Creates loop 35 | int spin(); 36 | // Communication channel between the object controller and the user interface 37 | std::shared_ptr interface_channel_; 38 | 39 | std::atomic_bool spin_; 40 | 41 | Ui ui_; 42 | 43 | private: 44 | // Initialises the user interface 45 | bool initialiseUserInterface(); 46 | // Initialise the ros itnerface thread 47 | void initialiseRosInterface(); 48 | // Initialises all controllers 49 | void initialiseMonitors(); 50 | // Updates all monitors 51 | void updateMonitors(); 52 | // Check if the UI is wating for any information 53 | void checkUiRequests(); 54 | // Dedicated ui loop function 55 | uint8_t spinUi(); 56 | // Timed callback for collecting general ros statistics 57 | void heartBeat(); 58 | 59 | std::map>> previous_monitor_info_; 60 | 61 | std::map> monitor_map_; 62 | 63 | // Stream interface map. 64 | std::map> stream_interface_map_; 65 | 66 | // Stream thread map 67 | std::map> stream_map_; 68 | 69 | // Ros interface node 70 | std::shared_ptr ros_interface_node_; 71 | 72 | // RCL context. Gets passed to topic streamer. Will likely end up being 73 | // copied to all of the monitors at some point. 74 | rcl_context_t context_; 75 | }; 76 | 77 | #endif // OBJECT_CONTROLLER_H_ 78 | -------------------------------------------------------------------------------- /python/random_number_publishers.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import rclpy 3 | from rclpy.node import Node 4 | from rclpy.qos import qos_profile_sensor_data 5 | 6 | import random 7 | 8 | from std_msgs.msg import Float32, Int32, String 9 | from example_interfaces.msg import MultiFormat 10 | 11 | 12 | class MinimalPublisher(Node): 13 | 14 | def __init__(self): 15 | super().__init__('random_number_pub') 16 | 17 | self.float_publisher_ = self.create_publisher(Float32, 'random_float', qos_profile_sensor_data) 18 | self.int_publisher_ = self.create_publisher(Int32, 'random_int', qos_profile_sensor_data) 19 | self.string_publisher_ = self.create_publisher(String, 'random_string', qos_profile_sensor_data) 20 | self.multi_publisher_ = self.create_publisher(MultiFormat, 'multi_format', qos_profile_sensor_data) 21 | timer_period = 0.2 # Start with 10hz 22 | self.timer = self.create_timer(timer_period, self.timer_callback) 23 | self.i = 0 24 | 25 | def timer_callback(self): 26 | msg_seed = 100 * random.random() 27 | float_msg = Float32() 28 | float_msg.data = msg_seed 29 | int_msg = Int32() 30 | int_msg.data = int(msg_seed * 100) 31 | string_msg = String() 32 | string_msg.data = "Message no: " + str(self.i) + ", val: " + str(int_msg) + " \ o.0 /" 33 | multi_msg = MultiFormat() 34 | multi_msg.float_data.data = msg_seed 35 | multi_msg.int_data.data = int_msg.data 36 | multi_msg.string_data.data = string_msg.data 37 | self.float_publisher_.publish(float_msg) 38 | self.int_publisher_.publish(int_msg) 39 | self.string_publisher_.publish(string_msg) 40 | self.multi_publisher_.publish(multi_msg) 41 | self.get_logger().info('Publishing: "%s"' % float_msg.data) 42 | self.i += 1 43 | 44 | 45 | def main(args=None): 46 | rclpy.init(args=args) 47 | 48 | minimal_publisher = MinimalPublisher() 49 | 50 | rclpy.spin(minimal_publisher) 51 | 52 | # Destroy the node explicitly 53 | # (optional - otherwise it will be done automatically 54 | # when the garbage collector destroys the node object) 55 | minimal_publisher.destroy_node() 56 | rclpy.shutdown() 57 | 58 | 59 | if __name__ == '__main__': 60 | main() 61 | -------------------------------------------------------------------------------- /src/topic_printer.cpp: -------------------------------------------------------------------------------- 1 | #include "ornis/topic_printer.hpp" 2 | 3 | #include "ornis/introspection_functions.hpp" 4 | #include "ornis/ui_helpers.hpp" 5 | 6 | TopicPrinter::TopicPrinter(ncpp::Plane* plane, uint height, uint width, std::vector entry_path, 7 | const Options::color_scheme theme) 8 | : TopicVisualiser(plane, height, width, entry_path, theme), longest_string_(0) 9 | { 10 | plane_->resize(height_, width_); 11 | const auto& fg = std::get<1>(theme); 12 | const auto& bg = std::get<2>(theme); 13 | uint64_t bgchannels = NCCHANNELS_INITIALIZER(fg.r, fg.g, fg.b, bg.r, bg.g, bg.b); 14 | ncchannels_set_fg_alpha(&bgchannels, NCALPHA_OPAQUE); 15 | ncchannels_set_bg_alpha(&bgchannels, NCALPHA_OPAQUE); 16 | plane_->set_channels(bgchannels); 17 | plane->move_top(); 18 | } 19 | 20 | TopicPrinter::~TopicPrinter() 21 | { 22 | } 23 | 24 | void TopicPrinter::renderData(const rosidl_typesupport_introspection_cpp::MessageMembers* members, uint8_t* data) 25 | { 26 | const rosidl_typesupport_introspection_cpp::MessageMembers* desired_member; 27 | uint8_t* member_data = nullptr; 28 | if (!entry_path_.empty()) 29 | { 30 | rosidl_typesupport_introspection_cpp::MessageMember member; 31 | introspection::getMessageMember(entry_path_, members, data, member, &member_data); 32 | desired_member = static_cast(member.members_->data); 33 | } 34 | else 35 | { 36 | desired_member = members; 37 | member_data = data; 38 | } 39 | 40 | char topic_char[100]; 41 | std::string prefix = desired_member->message_namespace_; 42 | prefix.replace(prefix.find("::"), sizeof("::") - 1, "/"); 43 | prefix += "/"; 44 | const auto prefix_char = prefix.c_str(); 45 | 46 | strcpy(topic_char, prefix_char); 47 | strcat(topic_char, desired_member->message_name_); 48 | 49 | msg_tree::msg_contents topic_contents = { .data_type_ = "", .entry_name_ = "topic", .entry_data_ = "" }; 50 | 51 | auto typesupport = 52 | introspection::getMessageTypeSupport(topic_char, rosidl_typesupport_introspection_cpp::typesupport_identifier); 53 | 54 | msg_tree::MsgTree topic_msg(topic_contents); 55 | topic_msg.recursivelyCreateTree(topic_msg.getRoot(), typesupport, member_data); 56 | ui_helpers::writeDetailedTreeToTitledPlane(*plane_, topic_char, topic_msg, theme_); 57 | } 58 | -------------------------------------------------------------------------------- /include/ornis/monitor.hpp: -------------------------------------------------------------------------------- 1 | #ifndef MONITOR_H_ 2 | #define MONITOR_H_ 3 | 4 | // Template class for monitoring an object. 5 | // Monitor is utilised by the object controller to collect data 6 | // MonitorInterface is dedicated to providing the Ui with methods of 7 | // visualising the data. 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include // IWYU pragma: keep 19 | #include 20 | 21 | #include "ornis/msg_tree.hpp" 22 | 23 | class Monitor 24 | { 25 | public: 26 | Monitor() : spin_(true), last_read_current_(false) 27 | { 28 | } 29 | virtual ~Monitor() 30 | { 31 | } 32 | 33 | virtual void getEntryInfo(const std::string& entry_name, const std::string& entry_details, 34 | std::map>& entry_info) = 0; 35 | 36 | virtual void getInteractionForm(const std::string& entry_details, msg_tree::MsgTree& form) = 0; 37 | 38 | virtual void interact(const std::string& entry_name, const std::string& entry_details, 39 | const msg_tree::MsgTree& request, msg_tree::MsgTree& response) = 0; 40 | 41 | bool getValue(std::vector>& value) 42 | { 43 | if (last_read_current_.load()) 44 | { 45 | return false; 46 | } 47 | std::unique_lock lk(data_mutex_); 48 | value = latest_value_; 49 | last_read_current_ = true; 50 | return true; 51 | } 52 | 53 | std::atomic spin_; 54 | 55 | protected: 56 | // Function to interface with the command line (eg: ros2 topic info) 57 | std::string callConsole(const std::string& cmd) 58 | { 59 | std::string result = ""; 60 | const std::string cmd_pipe = cmd + " 2>&1"; 61 | FILE* pipe = popen(cmd_pipe.c_str(), "r"); 62 | if (!pipe) 63 | throw std::runtime_error("popen() failed!"); 64 | try 65 | { 66 | char buffer[128]; 67 | while (fgets(buffer, sizeof buffer, pipe) != NULL) 68 | { 69 | result += buffer; 70 | } 71 | } 72 | catch (...) 73 | { 74 | pclose(pipe); 75 | throw; 76 | } 77 | return result; 78 | } 79 | 80 | std::mutex data_mutex_; 81 | std::vector> latest_value_; 82 | std::atomic last_read_current_; 83 | 84 | private: 85 | virtual void spin() = 0; 86 | virtual void updateValue() = 0; 87 | }; 88 | 89 | #endif // MONITOR_H_ 90 | -------------------------------------------------------------------------------- /include/ornis/options.hpp: -------------------------------------------------------------------------------- 1 | #ifndef OPTIONS_H_ 2 | #define OPTIONS_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace Options 11 | { 12 | struct rgb 13 | { 14 | int r; 15 | int g; 16 | int b; 17 | }; 18 | 19 | enum class CommandEnum 20 | { 21 | noAction, // No action required after command 22 | reboot // UI re-initialisation required 23 | }; 24 | 25 | // {Color Name, fg, bg, highlight, lowlight} 26 | using color_scheme = std::tuple; 27 | 28 | class OptionsMenu 29 | { 30 | public: 31 | OptionsMenu(); 32 | ~OptionsMenu(); 33 | 34 | void initialise(const int& x, const int& y, const ncpp::Plane* std_plane); 35 | 36 | ncpp::Plane* get_plane() const 37 | { 38 | return selector_->get_plane(); 39 | } 40 | 41 | CommandEnum handleInput(const ncinput& input); 42 | 43 | void loadConfiguration(); 44 | 45 | std::shared_ptr selector_; 46 | 47 | // Plane that gets displayed in place of the selector when the Ui 48 | // wants to display a smaller version of the plane. 49 | std::shared_ptr minimised_plane_; 50 | 51 | // Currently selected theme. 52 | color_scheme current_scheme_; 53 | 54 | private: 55 | enum class MenuEnum 56 | { 57 | baseOptions, 58 | colourMenu 59 | }; 60 | 61 | MenuEnum current_menu_; 62 | 63 | void transitionMenu(const MenuEnum& new_state); 64 | void selectColour(); 65 | 66 | void createDefaultConfiguration(); 67 | 68 | // Currently saves configuration to 69 | // ~/.config/ornis/config 70 | void saveConfiguration(); 71 | 72 | unsigned rgb_; 73 | 74 | constexpr static ncselector_item home_options_[] = { { "Color scheme", "" }, { nullptr, nullptr } }; 75 | constexpr static ncselector_item colour_menu_[] = { { "Bluejay", "(Default) A tasteful white on blue" }, 76 | { "Nord", "(Dark) Vikings or something" }, 77 | { "Monokai", "Just a great theme" }, 78 | { "Choom", "Night City..." }, 79 | { "Zombie", "Probably a metaphor for something" }, 80 | { "VSCode", "Babby's first editor lmao" }, 81 | { nullptr, nullptr } }; 82 | 83 | // {Color Name, fg, bg, highlight, lowlight} 84 | const std::vector available_colour_list = { 85 | { "Bluejay", { 255, 255, 255 }, { 32, 51, 70 }, { 173, 126, 77 }, { 204, 145, 109 } }, 86 | { "Nord", { 143, 188, 187 }, { 46, 52, 64 }, { 180, 142, 173 }, { 94, 129, 172 } }, 87 | { "Monokai", { 248, 248, 242 }, { 39, 40, 34 }, { 253, 151, 31 }, { 174, 129, 255 } }, 88 | { "Choom", { 158, 55, 159 }, { 55, 56, 84 }, { 232, 106, 240 }, { 123, 179, 255 } }, 89 | { "Zombie", { 185, 165, 137 }, { 38, 34, 29 }, { 120, 32, 32 }, { 80, 80, 60 } }, 90 | { "VSCode", { 255, 255, 255 }, { 39, 39, 39 }, { 30, 178, 228 }, { 55, 228, 158 } } 91 | }; 92 | 93 | // Application configuration, currently very simple, and 94 | // option storage reflects that 95 | std::map current_configuration_; 96 | }; 97 | 98 | } // namespace Options 99 | 100 | #endif // OPTIONS_H_ 101 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | language: cpp 2 | 3 | sudo: required 4 | 5 | branches: 6 | only: 7 | - master 8 | - coverity_scan 9 | 10 | env: 11 | global: 12 | # The next declaration is the encrypted COVERITY_SCAN_TOKEN, created 13 | # via the "travis encrypt" command using the project repo's public key 14 | secure: "veAdPtCJipVm1byxjBdwEAmFrTkvPeQTFh2+F+HGoCpzKJloQA3r7TcUSvkw3VKCSdc7fim6YKei3w57N9lR8wjlOdM+HScWims4i9n9t7zFQCbsaOuxxK7nmoFDuRx/9HPoi4px7famfhqRbYDQKcX4wX4XWeO5Ic364oGfXSc5loKSiu40ruseEeSAAyf96QYRRYvfbg2ifQTPTc1mMrE7KXDtyRQPnIWMHE3vFGdmcekqjRuVotUHbkoTZOElnA5dfXIctfJfDIv6ZMF1fLOcqoGqQrBiGqNtZvGZf0vGjmqwURZZOCkmIeBroyQSlneB6uhkes52vjbYMs+3NF7Y7D1kyPFiMaoK4WfjlQ5Z9k6JoBupkOhMLwyklxkks/scZXuI+H0V285fl2E5tdiowCBXn68J5yn1RjiAOLOS20oExLhPwd5cNsUKOmrWbNBlvEq37MKTVyBfu0h2gCVF3xo+H3b2RLzgn39hKppSK0iReVEfOmzhk6yP5QUBZUjemruJeZFp0CG3On8EdUQgIY9gOsFBoAsYkm6ahwvNSdL3Am8zpQMBBjS/WqJ3nNaPKQWqqL4eWG2y4cItjrRiIlczNh+LsJEP5vescEdiF+fx0hpCBy0B7LrOLUn7bGcPSOPV3XmqPesK5HaDE1bwgZ1ZjebdioTHfk+YHEs=" 15 | 16 | matrix: 17 | include: 18 | # Coverity Scan (Linux Debug) 19 | - os: linux 20 | compiler: gcc 21 | before_install: 22 | - echo -n | openssl s_client -connect https://scan.coverity.com:443 | sed -ne '/-BEGIN CERTIFICATE-/,/-END CERTIFICATE-/p' | sudo tee -a /etc/ssl/certs/ca- 23 | addons: 24 | apt: 25 | sources: 26 | - ubuntu-toolchain-r-test 27 | packages: 28 | - g++-5 29 | - cmake 30 | coverity_scan: 31 | project: 32 | name: "arnavb/cpp14-project-template" 33 | description: "A cross-platform C++14 project template" 34 | notification_email: arnavborborah11@gmail.com 35 | build_command_prepend: "cov-configure --comptype gcc --compiler g++-5 --template && cmake -DCMAKE_CXX_COMPILER=g++-5 -DGEN_DOCS=OFF ." 36 | build_command: "cmake --build ." 37 | branch_pattern: coverity_scan 38 | if: branch = coverity_scan 39 | before_script: echo "Coverity Scan" 40 | script: echo "Coverity Scan" 41 | # Code coverage (Linux Debug) 42 | - os: linux 43 | compiler: gcc 44 | addons: 45 | apt: 46 | sources: 47 | - ubuntu-toolchain-r-test 48 | packages: 49 | - g++-5 50 | - cmake 51 | - lcov 52 | before_script: 53 | - mkdir -p build && cd build 54 | - cmake -DCMAKE_BUILD_TYPE=Debug -DCMAKE_CXX_COMPILER=g++-5 -DBUILD_TESTS=ON -DENABLE_COVERAGE=ON -DGEN_DOCS=OFF .. 55 | script: 56 | - cmake --build . -- -j2 57 | - cmake --build . --target coverage 58 | after_success: 59 | - bash <(curl -s https://codecov.io/bash) || echo "Codecov did not collect coverage reports" 60 | if: branch != coverity_scan 61 | # Linux Release 62 | - os: linux 63 | compiler: gcc 64 | addons: 65 | apt: 66 | sources: 67 | - ubuntu-toolchain-r-test 68 | packages: 69 | - g++-5 70 | - cmake 71 | env: 72 | - MATRIX_EVAL="COMPILER=g++-5 && BUILD_TYPE=Release" 73 | if: branch != coverity_scan 74 | # OSX Debug 75 | - os: osx 76 | osx_image: xcode9.2 77 | compiler: clang 78 | env: 79 | - MATRIX_EVAL="COMPILER=clang++ && BUILD_TYPE=Debug" 80 | if: branch != coverity_scan 81 | # OSX Release 82 | - os: osx 83 | osx_image: xcode9.2 84 | compiler: clang 85 | env: 86 | - MATRIX_EVAL="COMPILER=clang++ && BUILD_TYPE=Release" 87 | if: branch != coverity_scan 88 | 89 | before_install: 90 | - eval "${MATRIX_EVAL}" 91 | 92 | before_script: 93 | - mkdir -p build && cd build 94 | - cmake -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DCMAKE_CXX_COMPILER=$COMPILER -DBUILD_TESTS=ON -DGEN_DOCS=OFF .. 95 | 96 | script: 97 | - cmake --build . -- -j2 # Build the code 98 | - ctest --output-on-failure 99 | - sudo cmake --build . --target install # Install the necessary files 100 | -------------------------------------------------------------------------------- /src/monitor_interface.cpp: -------------------------------------------------------------------------------- 1 | #include "ornis/monitor_interface.hpp" 2 | #include "ornis/options.hpp" 3 | 4 | MonitorInterface::MonitorInterface(const std::string& monitor_name, const std::string& selector_title) 5 | : monitor_name_(monitor_name), selector_title_(selector_title) 6 | { 7 | } 8 | 9 | MonitorInterface::~MonitorInterface() 10 | { 11 | } 12 | 13 | void MonitorInterface::initialiseInterface(const int& x, const int& y, const ncpp::Plane* std_plane, 14 | const Options::color_scheme& theme) 15 | { 16 | const auto &fg = std::get<1>(theme); 17 | const auto &bg = std::get<2>(theme); 18 | const auto &hl = std::get<3>(theme); 19 | const auto &ll = std::get<4>(theme); 20 | 21 | ncpp::Plane selector_plane = ncpp::Plane(std_plane, 2, 2, x, y); 22 | 23 | uint64_t bgchannels = NCCHANNELS_INITIALIZER(fg.r, fg.g, fg.b, bg.r, bg.g, bg.b); 24 | ncchannels_set_fg_alpha(&bgchannels, NCALPHA_BLEND); 25 | ncchannels_set_bg_alpha(&bgchannels, NCALPHA_BLEND); 26 | selector_plane.set_base("", 0, bgchannels); 27 | 28 | // Blank item array for selector, which needs an items object upon creation 29 | ncselector_item items[] = { 30 | { 31 | nullptr, 32 | nullptr, 33 | }, 34 | }; 35 | 36 | // Set up interface selector. 37 | struct ncselector_options sopts; 38 | sopts.maxdisplay = 10; 39 | sopts.items = items; 40 | sopts.defidx = 0; 41 | sopts.boxchannels = NCCHANNELS_INITIALIZER(fg.r, fg.g, fg.b, 0, 0, 0); 42 | sopts.opchannels = NCCHANNELS_INITIALIZER(hl.r, hl.g, hl.b, bg.r, bg.g, bg.b); 43 | sopts.descchannels = NCCHANNELS_INITIALIZER(ll.r, ll.g, ll.b, bg.r, bg.g, bg.b); 44 | sopts.titlechannels = NCCHANNELS_INITIALIZER(fg.r, fg.g, fg.b, 0x0e, 0x0e, 0x0e); 45 | 46 | ncchannels_set_bg_alpha(&sopts.boxchannels, NCALPHA_TRANSPARENT); 47 | ncchannels_set_bg_alpha(&sopts.titlechannels, NCALPHA_TRANSPARENT); 48 | 49 | sopts.title = selector_title_.c_str(); 50 | 51 | selector_ = std::make_shared(selector_plane, &sopts); 52 | 53 | // Create minimised plane (With a border and some text, out of view of the window) 54 | minimised_plane_ = std::make_shared(std_plane, monitor_name_.size() + 2, 3, -10, 0); 55 | 56 | uint64_t channel = NCCHANNELS_INITIALIZER(fg.r, fg.g, fg.b, bg.r, bg.g, bg.b); 57 | minimised_plane_->set_base("", 0, channel); 58 | minimised_plane_->perimeter_rounded(0, channel, 0); 59 | 60 | // configure minimised plane 61 | for (uint i = 0; i < monitor_name_.size(); i++) 62 | { 63 | minimised_plane_->putc(i + 1, 1, monitor_name_[i]); 64 | } 65 | } 66 | 67 | void MonitorInterface::updateEntries(std::vector& new_vector, std::vector& add_values, 68 | std::vector& delete_values) 69 | { 70 | if (entries_.empty()) 71 | { // If we have no entries currently in selector, add all in new vector 72 | entries_ = new_vector; 73 | add_values = new_vector; 74 | lines_ = new_vector.size(); 75 | return; 76 | } 77 | else if (new_vector.empty()) 78 | { // If new entry list is empty, delete all entries in selector 79 | delete_values = entries_; 80 | lines_ = 0; 81 | return; 82 | } 83 | 84 | // TODO: Add a check for if the new vector is equal to the current entry list, 85 | // Maybe convert to a hash for a quick compare? 86 | // Populate add_values for new values in the new_vector 87 | const auto t_vec = entries_; 88 | 89 | // Populate add values for items not found in list 90 | std::copy_if( 91 | new_vector.begin(), new_vector.end(), std::back_inserter(add_values), 92 | [&t_vec](const ncselector_item& item) { return (std::find(t_vec.begin(), t_vec.end(), item) == t_vec.end()); }); 93 | 94 | // Populate delete_values for values no longer in the vector list 95 | std::copy_if(t_vec.begin(), t_vec.end(), std::back_inserter(delete_values), 96 | [&new_vector](const ncselector_item& item) { 97 | return (std::find(new_vector.begin(), new_vector.end(), item) == new_vector.end()); 98 | }); 99 | 100 | lines_ = new_vector.size(); 101 | entries_ = new_vector; 102 | } 103 | -------------------------------------------------------------------------------- /include/ornis/introspection_functions.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | ** (Blatantly) Stolen from Rosbag2's message type introspection system: 3 | ** https://github.com/ros2/rosbag2/blob/dashing/rosbag2/src/rosbag2/typesupport_helpers.cpp 4 | */ 5 | 6 | #ifndef INTROSPECTION_FUNCTIONS_H_ 7 | #define INTROSPECTION_FUNCTIONS_H_ 8 | 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | namespace introspection 26 | { 27 | std::string getTypeSupportLibraryPath(const std::string& package_name, const std::string& typesupport_identifier); 28 | 29 | const std::tuple extractTypeIdentifier(const std::string& full_type); 30 | 31 | const std::pair extractTypeAndPackage(const std::string& full_type); 32 | 33 | const rosidl_service_type_support_t* getServiceTypeSupport(const std::string& type, 34 | const std::string& typesupport_identifier); 35 | 36 | const rosidl_message_type_support_t* getMessageTypeSupport(const std::string& type, 37 | const std::string& typesupport_identifier); 38 | 39 | void messageDataToString(const rosidl_typesupport_introspection_cpp::MessageMember& member_info, 40 | const uint8_t* member_data, std::string& message_data); 41 | 42 | void messageDataToDouble(const rosidl_typesupport_introspection_cpp::MessageMember& member_info, 43 | const uint8_t* member_data, double& message_data); 44 | 45 | void messageTypeToString(const rosidl_typesupport_introspection_cpp::MessageMember& member_info, 46 | std::string& message_type); 47 | 48 | void readMessageAsTreeString(std::vector& output, uint8_t* message_data, 49 | const rosidl_typesupport_introspection_cpp::MessageMembers* members, int indent = 0); 50 | 51 | std::string readMessageAsString(uint8_t* message_data, 52 | const rosidl_typesupport_introspection_cpp::MessageMembers* members); 53 | 54 | double readMessageAsDouble(uint8_t* message_data, const rosidl_typesupport_introspection_cpp::MessageMembers* members); 55 | 56 | void writeDataToMessage(uint8_t* message_data, const rosidl_typesupport_introspection_cpp::MessageMembers* members, 57 | const std::vector& data); 58 | 59 | bool populateMessage(uint8_t* message_data, const rosidl_typesupport_introspection_cpp::MessageMembers* members, 60 | const std::string& data); 61 | 62 | const std::string sanitiseNumericData(const std::string &data); 63 | 64 | void stringToMessageData(uint8_t* message_data, const rosidl_typesupport_introspection_cpp::MessageMember& member, 65 | std::string& data); 66 | 67 | std::vector getEntryOffset(std::vector entry_path, const std::string& member_type_id, 68 | const rosidl_typesupport_introspection_cpp::MessageMembers* message_members); 69 | 70 | void getMessageMember(const std::vector& offsets, 71 | const rosidl_typesupport_introspection_cpp::MessageMembers* message_members, 72 | rosidl_typesupport_introspection_cpp::MessageMember& found_member); 73 | 74 | void getMessageMember(const std::vector& offsets, 75 | const rosidl_typesupport_introspection_cpp::MessageMembers* message_members, uint8_t* data, 76 | rosidl_typesupport_introspection_cpp::MessageMember& found_member, uint8_t** found_data); 77 | 78 | bool parsableAsNumeric(const rosidl_typesupport_introspection_cpp::MessageMember& msg_info); 79 | 80 | } // namespace introspection 81 | 82 | #endif // INTROSPECTION_FUNCTIONS_H_ 83 | -------------------------------------------------------------------------------- /src/topic_string_viewer.cpp: -------------------------------------------------------------------------------- 1 | #include "ornis/topic_string_viewer.hpp" 2 | 3 | #include "ornis/introspection_functions.hpp" 4 | 5 | TopicStringViewer::TopicStringViewer(ncpp::Plane* plane, uint height, uint width, std::vector entry_path, 6 | const Options::color_scheme& theme) 7 | : TopicVisualiser(plane, height, width, entry_path, theme), data_buffer_(height - 2), longest_string_(0) 8 | { 9 | plane_ = plane; 10 | const auto &fg = std::get<1>(theme); 11 | const auto &bg = std::get<2>(theme); 12 | uint64_t bgchannels = NCCHANNELS_INITIALIZER(fg.r, fg.g, fg.b, bg.r, bg.g, bg.b); 13 | ncchannels_set_fg_alpha(&bgchannels, NCALPHA_OPAQUE); 14 | ncchannels_set_bg_alpha(&bgchannels, NCALPHA_OPAQUE); 15 | 16 | // Step values for fading string history 17 | r_step_ = (fg.r - bg.r) / height_; 18 | g_step_ = (fg.g - bg.g) / height_; 19 | b_step_ = (fg.b - bg.b) / height_; 20 | 21 | plane_->set_channels(bgchannels); 22 | plane->move_top(); 23 | } 24 | 25 | TopicStringViewer::~TopicStringViewer() 26 | { 27 | } 28 | 29 | void TopicStringViewer::drawStrings() 30 | { 31 | plane_->erase(); 32 | 33 | uint64_t channel = plane_->get_channels(); 34 | plane_->perimeter_rounded(0, channel, 0); 35 | 36 | // Polyfill to prevent transparent background 37 | ncpp::Cell c(' '); 38 | plane_->polyfill(2, 2, c); 39 | 40 | const auto &fg = std::get<1>(theme_); 41 | const auto &bg = std::get<2>(theme_); 42 | // Draw most recent string at bottom 43 | if (!data_buffer_.filled_) 44 | { 45 | for (size_t i = 0; i <= data_buffer_.i_; i++) 46 | { 47 | uint64_t bgchannels = NCCHANNELS_INITIALIZER((int)(fg.r - r_step_ * (data_buffer_.i_ - i)), 48 | (int)(fg.g - g_step_ * (data_buffer_.i_ - i)), 49 | (int)(fg.b - b_step_ * (data_buffer_.i_ - i)), bg.r, bg.g, bg.b); 50 | plane_->set_channels(bgchannels); 51 | plane_->putstr(height_ - 1 - data_buffer_.i_ + i, 1, data_buffer_.buffer[i].c_str()); 52 | bgchannels = NCCHANNELS_INITIALIZER(fg.r, fg.g, fg.b, bg.r, bg.g, bg.b); 53 | plane_->set_channels(bgchannels); 54 | } 55 | } 56 | else 57 | { 58 | for (size_t i = data_buffer_.i_; i < data_buffer_.buffer.size(); i++) 59 | { 60 | uint64_t bgchannels = NCCHANNELS_INITIALIZER((int)(bg.r + r_step_ * (i - data_buffer_.i_ + 2)), 61 | (int)(bg.g + g_step_ * (i - data_buffer_.i_ + 2)), 62 | (int)(bg.b + b_step_ * (i - data_buffer_.i_ + 2)), bg.r, bg.g, bg.b); 63 | plane_->set_channels(bgchannels); 64 | plane_->putstr(1 + i - data_buffer_.i_, 1, data_buffer_.buffer[i].c_str()); 65 | bgchannels = NCCHANNELS_INITIALIZER(fg.r, fg.g, fg.b, bg.r, bg.g, bg.b); 66 | plane_->set_channels(bgchannels); 67 | } 68 | for (size_t i = 0; i < data_buffer_.i_; i++) 69 | { 70 | uint64_t bgchannels = NCCHANNELS_INITIALIZER((int)(fg.r - r_step_ * (data_buffer_.i_ - i)), 71 | (int)(fg.g - g_step_ * (data_buffer_.i_ - i)), 72 | (int)(fg.b - b_step_ * (data_buffer_.i_ - i)), bg.r, bg.g, bg.b); 73 | plane_->set_channels(bgchannels); 74 | plane_->putstr(height_ - 1 + i - data_buffer_.i_, 1, data_buffer_.buffer[i].c_str()); 75 | bgchannels = NCCHANNELS_INITIALIZER(fg.r, fg.g, fg.b, bg.r, bg.g, bg.b); 76 | plane_->set_channels(bgchannels); 77 | } 78 | } 79 | } 80 | 81 | void TopicStringViewer::renderData(const rosidl_typesupport_introspection_cpp::MessageMembers* members, uint8_t* data) 82 | { 83 | rosidl_typesupport_introspection_cpp::MessageMember member; 84 | uint8_t* member_data = nullptr; 85 | if (!entry_path_.empty()) 86 | { 87 | introspection::getMessageMember(entry_path_, members, data, member, &member_data); 88 | } 89 | else 90 | { 91 | member = members->members_[0]; 92 | member_data = &data[member.offset_]; 93 | } 94 | 95 | std::string message_string; 96 | introspection::messageDataToString(member, member_data, message_string); 97 | data_buffer_.step(message_string); 98 | 99 | // If the newest datapoint is outside current bounds 100 | if (message_string.length() > longest_string_) 101 | { 102 | longest_string_ = message_string.length(); 103 | // We add 2 to ensure the plane borders don't overlap with the string 104 | plane_->resize(height_, longest_string_ + 2); 105 | width_ = longest_string_ + 2; 106 | } 107 | 108 | drawStrings(); 109 | } 110 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ORNIS (Open.Ros.Notcurses.Interface.System) 2 | 3 | ![ORNIS build](https://github.com/juraph-dev/ornis/actions/workflows/default-action.yml/badge.svg) 4 | 5 | ## What is it? 6 | 7 | A waste of time (A Terminal User Interface for ROS2 (a ROS2 TUI)). 8 | 9 | ## Current features 10 | * View currently active topics/nodes/services 11 | * Get information on currently active topics/nodes/services 12 | * Support for service calls 13 | * Streaming topics 14 | * \*Kind of\* Supports mouse workflows 15 | 16 | ## Videos 17 | Small demo video of topic selection and service calls. 18 | 19 | 20 | https://user-images.githubusercontent.com/28330806/230512415-6c214e54-f59b-49c7-a2fa-0de811d996ee.mp4 21 | 22 | 23 | ### Prerequisites 24 | [Notcurses](https://github.com/dankamongmen/notcurses) \ 25 | [ROS2 Humble or above](https://docs.ros.org/en/humble/index.html) \ 26 | 27 | FastRTPS's default configuration doesn't work, so you're going to need to do the following if you're using it: 28 | ```sh 29 | apt-get install ros-${distro}-rmw-fastrtps-dynamic-cpp 30 | echo 'alias ornis="RMW_IMPLEMENTATION=rmw_fastrtps_dynamic_cpp ornis"' >> ~/.bashrc 31 | ``` 32 | CycloneDDS works out of the box, so no extra config required. 33 | 34 | Windows and mac also aren't currently supported. Mac might work, feel free to try it and let me know how it goes. 35 | 36 | ## Installation 37 | You're going to need Notcurses. You can either initialise it as a submodule to this repo, and compile alongside ORNIS, or you can install it [using your favourite package manager](https://repology.org/project/notcurses/versions). 38 | Ensure you have ROS2 (Humble or newer) installed and sourced. 39 | 40 | Install notcurses deps 41 | ```sh 42 | apt-get install build-essential cmake doctest-dev libavdevice-dev libdeflate-dev libgpm-dev libncurses-dev libqrcodegen-dev libswscale-dev libunistring-dev pandoc pkg-config 43 | ``` 44 | Build ornis 45 | ``` sh 46 | mkdir ornis_ws && cd ornis_ws 47 | git clone https://github.com/juraph-dev/ornis.git --depth 1 48 | cd ornis 49 | git submodule update --init 50 | cd tools && ./compile.sh 51 | ``` 52 | ## Usage 53 | Ensure ornis has been sourced by sourcing its setup.bash from your .bashrc 54 | ``` sh 55 | cd ornis_ws 56 | echo 'source '$(pwd)'/install/setup.bash' >> ~/.bashrc 57 | source ~/.bashrc 58 | ornis 59 | ``` 60 | From here on out, you can just run ```ornis```, then trust the bar at the top that tells you how to use it. You can probably click things too, if you're one of those kinds of people. 61 | 62 | ## Future roadmap 63 | I still have a ton of things I'd like to implement. To name a few: 64 | 65 | 1. Proper topic visualisation, ala [rosshow](https://github.com/dheera/rosshow). At the moment, you only get the raw data displayed, I'd like to provide a meaningful and intuitive way to actually see the data being thrown around. 66 | 2. Action support. I don't personally use Actions frequently, so it wasn't a very high priority for me. It'll be pretty easy for me to add it, the hard part of ORNIS has already been layed out, I just need to get around to doing it. 67 | 3. Add support for multiple topic streams at once. The foundation is already there to allow the user to, for example: view a topic, while sending out a service call. 68 | 4. I want to add a "graph view" for all of the currently launched nodes. In the same vein as [Graphviz](https://graphviz.org/gallery/), with nodes linked by publishers/subscribers. I can see it being very useful (And fun) to be able to get an easy to digest visualisation of the node structure currently running on the robot. 69 | 70 | ## Ackowledgements 71 | [ros2_introspection](https://github.com/facontidavide/ros2_introspection) I blatantly stole a ton of code from here \ 72 | [dynamic message introspection tools](https://github.com/osrf/dynamic_message_introspection) And here \ 73 | [rosshow](https://github.com/dheera/rosshow) I actually started ORNIS _before_ I found rosshow (Promise), but once I saw it, it really fueled the fire, having shown me the potential of clever data visualisation. It's a pretty great piece of software. \ 74 | [Notcurses](https://github.com/dankamongmen/notcurses), the library powering the visuals of ORNIS. [Nick Black](https://github.com/dankamongmen) (The creator) is a super interesting dude, and I've enjoyed reading his [guidebook](https://nick-black.com/htp-notcurses.pdf), which is how I was introduced to Notcurses. \ 75 | [Arnav Borborah](https://github.com/arnavb/cpp14-project-template) Their cpp14 template is pretty great, allowed me to quickly get ORNIS up and running with barely any hassle. Having a template to look at how things like CI and test modules should be layed out was also extremely useful (Even though I don't use either, ha) 76 | 77 | ## Further reading 78 | I wrote a long winded posted about [ORNIS on my blog](https://juraph.com/ornis/introducing_ornis), for if you have absolutely nothing better to do with your time. 79 | 80 | -------------------------------------------------------------------------------- /src/topic_monitor.cpp: -------------------------------------------------------------------------------- 1 | #include "ornis/topic_monitor.hpp" 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include // IWYU pragma: keep 12 | #include 13 | #include 14 | 15 | #include "ornis/ros_interface_node.hpp" 16 | 17 | TopicMonitor::TopicMonitor(std::shared_ptr ros_interface_node) 18 | : ros_interface_node_(std::move(ros_interface_node)) 19 | { 20 | thread_ = new std::thread([this]() { spin(); }); 21 | } 22 | TopicMonitor::~TopicMonitor() 23 | { 24 | spin_ = false; 25 | if (thread_ != nullptr) 26 | { 27 | thread_->join(); 28 | delete thread_; 29 | } 30 | } 31 | 32 | void TopicMonitor::spin() 33 | { 34 | while (spin_) 35 | { 36 | updateValue(); 37 | std::this_thread::sleep_for(std::chrono::milliseconds(500)); 38 | } 39 | } 40 | 41 | void TopicMonitor::getEntryInfo(const std::string& entry_name, const std::string& entry_details, 42 | std::map>& entry_info) 43 | { 44 | (void)entry_details; 45 | rcl_allocator_t allocator = rcl_get_default_allocator(); 46 | rcl_topic_endpoint_info_array_t topic_publishers = rcl_get_zero_initialized_topic_endpoint_info_array(); 47 | rcl_topic_endpoint_info_array_t topic_subscribers = rcl_get_zero_initialized_topic_endpoint_info_array(); 48 | 49 | int ret = rcl_get_publishers_info_by_topic(ros_interface_node_.get(), &allocator, entry_name.c_str(), false, 50 | &topic_publishers); 51 | 52 | if (ret != RCL_RET_OK) 53 | { 54 | std::cerr << "Topic monitor failed to get publisher info! \n"; 55 | } 56 | 57 | ret = rcl_get_subscriptions_info_by_topic(ros_interface_node_.get(), &allocator, entry_name.c_str(), false, 58 | &topic_subscribers); 59 | 60 | if (ret != RCL_RET_OK) 61 | { 62 | std::cerr << "Topic monitor failed to get subscriber info! \n"; 63 | } 64 | 65 | for (size_t i = 0; i < topic_publishers.size; i++) 66 | { 67 | entry_info["Publishers"].push_back(topic_publishers.info_array[i].node_name); 68 | } 69 | 70 | if (!topic_subscribers.size) 71 | { 72 | entry_info["Subscribers"].push_back("None"); 73 | } 74 | else 75 | { 76 | for (size_t i = 0; i < topic_subscribers.size; i++) 77 | { 78 | entry_info["Subscribers"].push_back(topic_subscribers.info_array[i].node_name); 79 | } 80 | } 81 | } 82 | 83 | void TopicMonitor::getInteractionForm(const std::string& entry_details, msg_tree::MsgTree& form) 84 | { 85 | msg_tree::msg_contents blank_contents = { .data_type_ = "", .entry_name_ = "", .entry_data_ = "" }; 86 | msg_tree::MsgTree message_tree(blank_contents); 87 | getInteractionTree(entry_details, form); 88 | } 89 | 90 | void TopicMonitor::getInteractionTree(const std::string message_type, msg_tree::MsgTree& message_tree) 91 | { 92 | const rosidl_message_type_support_t* message_type_support = 93 | introspection::getMessageTypeSupport(message_type, rosidl_typesupport_introspection_cpp::typesupport_identifier); 94 | 95 | message_tree.recursivelyCreateTree(message_tree.getRoot(), message_type_support); 96 | } 97 | 98 | void TopicMonitor::interact(const std::string& entry_name, const std::string& entry_details, 99 | const msg_tree::MsgTree& request, msg_tree::MsgTree& response) 100 | { 101 | (void)entry_name; 102 | (void)entry_details; 103 | (void)entry_name; 104 | (void)request; 105 | (void)response; 106 | 107 | // response = "Not yet implemented :("; 108 | } 109 | 110 | void TopicMonitor::updateValue() 111 | { 112 | rcl_allocator_t allocator = rcl_get_default_allocator(); 113 | rcl_names_and_types_t topic_names_and_types{}; 114 | 115 | int ret = rcl_get_topic_names_and_types(ros_interface_node_.get(), &allocator, false, &topic_names_and_types); 116 | 117 | if (ret != RCL_RET_OK) 118 | { 119 | std::cerr << "Topic monitor failed to get topic names and types!\n"; 120 | } 121 | 122 | std::vector> topic_info; 123 | topic_info.reserve(topic_names_and_types.names.size); 124 | 125 | for (size_t i = 0; i < topic_names_and_types.names.size; ++i) 126 | { 127 | const std::string service_name = topic_names_and_types.names.data[i]; 128 | for (size_t j = 0; j < topic_names_and_types.types[i].size; ++j) 129 | { 130 | topic_info.push_back(std::pair{ service_name, topic_names_and_types.types[i].data[j] }); 131 | } 132 | } 133 | ret = rcl_names_and_types_fini(&topic_names_and_types); 134 | if (ret != RCL_RET_OK) 135 | { 136 | std::cerr << "Topic monitor failed to finish topic names and types!\n"; 137 | } 138 | 139 | if (latest_value_ == topic_info) 140 | { 141 | return; 142 | } 143 | std::unique_lock lk(data_mutex_); 144 | latest_value_ = topic_info; 145 | last_read_current_ = false; 146 | } 147 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(ornis) 3 | 4 | set(Project-Name_VERSION_MAJOR 0) 5 | set(Project-Name_VERSION_MINOR 1) 6 | set(Project-Name_VERSION_PATCH 0) 7 | set(Project-Name_VERSION "${Project-Name_VERSION_MAJOR}.${Project-Name_VERSION_MINOR}.${Project-Name_VERSION_PATCH}") 8 | 9 | set(PROJECT_NAME "ornis") 10 | set(PROJECT_DESCRIPTION "\"Open Ros2 Notcurses Interface System\"") 11 | 12 | # Options 13 | # option(BUILD_TESTS "Build test executable" OFF) 14 | # option(GEN_DOCS "Generate documentation" OFF) 15 | # option(ENABLE_COVERAGE "Enable code coverage" OFF) 16 | 17 | # if (CHECK_INCLUDES) 18 | # find_program(iwyu_path NAMES include-what-you-use iwyu REQUIRED) 19 | # endif(CHECK_INCLUDES) 20 | option(CHECK_INCLUDES "use iwyu to check for bad includes" OFF) 21 | option(FIX_INCLUDES "use iwyu to fix bad includes" OFF) 22 | 23 | 24 | # if (ENABLE_COVERAGE) 25 | # include(CodeCoverage) 26 | # append_coverage_compiler_flags() 27 | # set(COVERAGE_EXCLUDES "/usr/include/\\*;${CMAKE_SOURCE_DIR}/src/main.cpp;${CMAKE_SOURCE_DIR}/test/*;${CMAKE_SOURCE_DIR}/third_party/doctest/*") 28 | 29 | # setup_target_for_coverage(NAME coverage EXECUTABLE ctest DEPENDENCIES coverage) 30 | # endif(ENABLE_COVERAGE) 31 | 32 | set(CMAKE_THREAD_PREFER_PTHREAD TRUE) 33 | set(THREADS_PREFER_PTHREAD_FLAG TRUE) 34 | 35 | # Default to C99 36 | if(NOT CMAKE_C_STANDARD) 37 | set(CMAKE_C_STANDARD 99) 38 | endif() 39 | 40 | # Default to C++17 41 | if(NOT CMAKE_CXX_STANDARD) 42 | set(CMAKE_CXX_STANDARD 17) 43 | endif() 44 | 45 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 46 | add_compile_options(-Wall -Wextra ) 47 | endif() 48 | 49 | # Set subdirectory variables 50 | # option(USE_POC "Build small, uninstalled proof-of-concept binaries" OFF) # Doesn't work 51 | set(NCPP_DIR "external/notcurses") 52 | # Use Notcurses 53 | add_subdirectory(${NCPP_DIR}) 54 | 55 | # find dependencies 56 | find_package(ament_index_cpp REQUIRED) 57 | find_package(Threads REQUIRED) 58 | find_package(ament_cmake REQUIRED) 59 | find_package(rclcpp REQUIRED) 60 | find_package(rcl REQUIRED) 61 | find_package(rcl_action REQUIRED) 62 | find_package(rcl_interfaces REQUIRED) 63 | find_package(rosidl_runtime_cpp REQUIRED) 64 | find_package(rosidl_generator_cpp REQUIRED) 65 | find_package(rosidl_typesupport_cpp REQUIRED) 66 | find_package(rosidl_typesupport_introspection_cpp REQUIRED) 67 | find_package(notcurses 3.0.0 CONFIG) 68 | 69 | # Perform this after find_package to prevent iwyu from running on our external dir 70 | IF (FIX_INCLUDES) 71 | find_program(IWYU_PATH NAMES include-what-you-use iwyu) 72 | IF(NOT IWYU_PATH) 73 | message(FATAL_ERROR "Could not find the program include-what-you-use") 74 | ENDIF() 75 | set(IWYU_PATH_OPTS 76 | ${IWYU_PATH}) 77 | set(CMAKE_CXX_INCLUDE_WHAT_YOU_USE ${IWYU_PATH_OPTS}) 78 | set(CMAKE_C_INCLUDE_WHAT_YOU_USE ${IWYU_PATH_OPTS}) 79 | ELSEIF (CHECK_INCLUDES) 80 | find_program(IWYU_PATH NAMES include-what-you-use iwyu) 81 | IF(NOT IWYU_PATH) 82 | message(FATAL_ERROR "Could not find the program include-what-you-use") 83 | ENDIF() 84 | set(IWYU_PATH_OPTS 85 | ${IWYU_PATH} 86 | -Xiwyu) 87 | set(CMAKE_CXX_INCLUDE_WHAT_YOU_USE ${IWYU_PATH_OPTS}) 88 | set(CMAKE_C_INCLUDE_WHAT_YOU_USE ${IWYU_PATH_OPTS}) 89 | ENDIF(FIX_INCLUDES) 90 | 91 | file(GLOB LIBRARY_SOURCES 92 | src/*.cpp 93 | ) 94 | 95 | list(REMOVE_ITEM LIBRARY_SOURCES "main.cpp") 96 | 97 | add_library(${PROJECT_NAME}-lib ${LIBRARY_SOURCES}) 98 | target_include_directories(${PROJECT_NAME}-lib PUBLIC ${PROJECT_SOURCE_DIR}/include 99 | ${CMAKE_BINARY_DIR}/include ${NCPP_DIR}/include 100 | ${rclcpp_INLUDE_DIR} rosidl_generator_cpp 101 | rosidl_typesupport_introspection_cpp 102 | rosidl_typesupport_cpp) 103 | 104 | add_executable(${PROJECT_NAME} src/main.cpp) # The main executables 105 | 106 | target_link_libraries(ornis-lib PUBLIC 107 | notcurses++ 108 | Threads::Threads 109 | ament_index_cpp::ament_index_cpp 110 | rclcpp::rclcpp 111 | rcl::rcl 112 | rcl_action::rcl_action 113 | ) # Link our sources to the executable 114 | 115 | target_link_libraries(ornis PUBLIC 116 | ornis-lib 117 | notcurses++ 118 | Threads::Threads 119 | rclcpp::rclcpp 120 | rcl::rcl 121 | rcl_action::rcl_action 122 | ament_index_cpp::ament_index_cpp 123 | ) # Link our sources to the executable 124 | 125 | # Install the built library and executable into the appropriate directory 126 | install(TARGETS ${PROJECT_NAME} DESTINATION bin) 127 | install(TARGETS ${PROJECT_NAME}-lib 128 | ARCHIVE DESTINATION lib 129 | LIBRARY DESTINATION lib 130 | RUNTIME DESTINATION bin 131 | ) 132 | 133 | file(GLOB_RECURSE AMENT_LINT_AUTO_FILE_EXCLUDE 134 | # Don't lint external submodules 135 | external/* 136 | ) 137 | 138 | if(BUILD_TESTING) 139 | find_package(ament_lint_auto REQUIRED) 140 | # the following line skips the linter which checks for copyrights 141 | # uncomment the line when a copyright and license is not present in all source files 142 | #set(ament_cmake_copyright_FOUND TRUE) 143 | # the following line skips cpplint (only works in a git repo) 144 | # uncomment the line when this package is not in a git repo 145 | #set(ament_cmake_cpplint_FOUND TRUE) 146 | ament_lint_auto_find_test_dependencies() 147 | endif() 148 | 149 | ament_package() 150 | -------------------------------------------------------------------------------- /include/ornis/ui.hpp: -------------------------------------------------------------------------------- 1 | #ifndef UI_H_ 2 | #define UI_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include "ncpp/Selector.hh" 11 | #include 12 | #include // IWYU pragma: keep 13 | #include 14 | #include 15 | #include 16 | 17 | #include "notcurses/notcurses.h" 18 | #include "ornis/msg_tree.hpp" 19 | #include "ornis/options.hpp" 20 | 21 | class Channel; 22 | class MonitorInterface; 23 | class StreamChannel; 24 | namespace ncpp 25 | { 26 | class NotCurses; 27 | } 28 | 29 | class Ui 30 | { 31 | public: 32 | Ui(); 33 | ~Ui(); 34 | bool initialise(std::shared_ptr interface_channel, 35 | std::map>& stream_map); 36 | 37 | // Re-draw flag, for updated value, or changed console dimensions 38 | bool redraw_flag_; 39 | std::atomic_bool screen_loop_; 40 | // If the UI wants to restart, for example to change colour scheme, 41 | // param is checked by, and acted upon by the object controller. 42 | std::atomic_bool reboot_required_; 43 | 44 | Options::color_scheme current_scheme_; 45 | 46 | // Stores the width of the terminal at startup, and when recieving sigwinch. Used for scaling the ui 47 | uint term_width_; 48 | uint term_height_; 49 | 50 | private: 51 | // Class to for storing the current ui state 52 | // TODO: Rename monitorInteraction. That's a silly name. 53 | enum class UiDisplayingEnum 54 | { 55 | monitors, 56 | selectedMonitor, 57 | monitorEntry, 58 | monitorInteraction, 59 | monitorSelection, 60 | monitorInteractionResult, 61 | streamingTopic, 62 | optionsMenu 63 | }; 64 | enum class UiLayoutEnum 65 | { 66 | Vertical, 67 | Horizontal, 68 | HorizontalClipped 69 | }; 70 | 71 | struct UserHelpStrings 72 | { 73 | static constexpr auto home_layout_prompt_ = "Press s/t/m/o, or use your mouse to select an option"; 74 | static constexpr auto selected_monitor_prompt = 75 | "Press Enter for entry information, or Esc/q/mouseclick the background to go back"; 76 | static constexpr auto streamable_entry_prompt = "Press Enter to stream topic, Esc/q to go back"; 77 | static constexpr auto interactable_entry_prompt = "Press Enter to interact, Esc/q to go back"; 78 | static constexpr auto standard_entry_prompt = "Esc/q to go back"; 79 | static constexpr auto stream_prompt = "Esc/q to to go back"; 80 | static constexpr auto options_prompt = ""; 81 | static constexpr auto interaction_request_prompt = 82 | "Type to enter data, Enter to send, Tab to change fields, Esc to give up"; 83 | }; 84 | 85 | UserHelpStrings userHelpStrings_; 86 | 87 | void updateMonitor(std::vector> updated_values, 88 | const std::unique_ptr& interface); 89 | 90 | void renderMonitorInfo(MonitorInterface* interface); 91 | bool renderMonitors(); 92 | void renderOptions(); 93 | void renderHomeLayout(); 94 | void renderSelectedMonitor(); 95 | void renderMonitorSelection(MonitorInterface* interface); 96 | void renderMonitorInteraction(MonitorInterface* interface); 97 | void renderMonitorInteractionResult(MonitorInterface* interface); 98 | 99 | std::shared_ptr createStreamPlane(); 100 | void openStream(const std::string& topic_name, const std::string& entry_path, 101 | const msg_tree::msg_contents& message_contents); 102 | void refreshUi(); 103 | 104 | void handleInputOptions(const ncinput& input); 105 | void handleInputMonitors(const ncinput& input); 106 | void handleInputSelected(const ncinput& input); 107 | void handleInputMonitorEntry(const ncinput& input); 108 | void handleInputStreaming(const ncinput& input); 109 | void handleInputMonitorSelection(const ncinput& input); 110 | void handleInputMonitorInteraction(const ncinput& input); 111 | void handleInputMonitorInteractionResult(const ncinput& input); 112 | 113 | void transitionUiState(const UiDisplayingEnum& desired_state); 114 | void resizeUi(const uint& rows, const uint& cols); 115 | void closeStream(const std::string& stream_name); 116 | Ui::UiLayoutEnum calcMonitorLayout(); 117 | 118 | // Moves a each plane in the vector to their corresponding x/y locations in 119 | // the tuple. 120 | void movePlanesAnimated(const std::vector>& planes_locations); 121 | 122 | bool checkEventOnPlane(const ncinput& input, const ncpp::Plane* plane) const; 123 | bool offerInputMonitor(MonitorInterface* interface, const ncinput& input); 124 | 125 | void updateColours(); 126 | 127 | std::thread* ui_thread_; 128 | 129 | std::map>> monitor_data_; 130 | 131 | // Notcurses core 132 | std::unique_ptr notcurses_core_; 133 | 134 | // Channel to handle sending information to/from the object controller 135 | std::shared_ptr interface_channel_; 136 | 137 | // Monitor interface map 138 | std::map> interface_map_; 139 | 140 | // Stream map 141 | std::map>* stream_map_; 142 | 143 | // Popup Information planes 144 | std::unique_ptr monitor_info_plane_; 145 | 146 | UiDisplayingEnum ui_displaying_; 147 | 148 | // Storage for the monitor that has been selected to be interacted with by the 149 | // user 150 | std::string selected_monitor_; 151 | 152 | // Popup Information planes 153 | std::shared_ptr notcurses_stdplane_; 154 | 155 | // Storage string for user interactions with the back-end 156 | // TODO Rename this to be currently_selected_, as it's also used by the topic selection, 157 | // (Also change to size_t) 158 | uint currently_editing_index_; 159 | // TODO Ditto 160 | msg_tree::MsgTreeNode* msg_node_being_edited_; 161 | 162 | std::shared_ptr> currently_active_trees_; 163 | 164 | // Number of frames to use for anmations 165 | // TODO: re-write as a parameter for the user to configure 166 | int transition_time_ = 50; 167 | 168 | // Help menu options 169 | Options::OptionsMenu options_; 170 | }; 171 | 172 | #endif // UI_H_ 173 | -------------------------------------------------------------------------------- /src/topic_plotter.cpp: -------------------------------------------------------------------------------- 1 | #include "ornis/topic_plotter.hpp" 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | #include "ornis/introspection_functions.hpp" 9 | #include "ornis/ui_helpers.hpp" 10 | 11 | TopicPlotter::TopicPlotter(ncpp::Plane* plane, uint height, uint width, std::vector entry_path, 12 | const Options::color_scheme theme) 13 | : TopicVisualiser(plane, height, width, entry_path, theme) 14 | , data_buffer_(width - 3) 15 | , scaled_data_buffer_(width - 3) 16 | , entry_offset_(0) 17 | { 18 | const auto& fg = std::get<1>(theme); 19 | const auto& bg = std::get<2>(theme); 20 | timestep_ = 0; 21 | plane_->resize(height_, width_); 22 | uint64_t bgchannels = NCCHANNELS_INITIALIZER(fg.r, fg.g, fg.b, bg.r, bg.g, bg.b); 23 | ncchannels_set_fg_alpha(&bgchannels, NCALPHA_OPAQUE); 24 | ncchannels_set_bg_alpha(&bgchannels, NCALPHA_OPAQUE); 25 | plane_->set_channels(bgchannels); 26 | plane->move_top(); 27 | } 28 | 29 | TopicPlotter::~TopicPlotter() 30 | { 31 | } 32 | 33 | void TopicPlotter::drawPlot() 34 | { 35 | plane_->erase(); 36 | 37 | uint64_t channel = plane_->get_channels(); 38 | plane_->perimeter_rounded(0, channel, 0); 39 | 40 | // Polyfill to prevent transparent background 41 | ncpp::Cell c(' '); 42 | plane_->polyfill(2, 2, c); 43 | 44 | // Horizontal plane 45 | const char horz_line = '-'; 46 | for (uint i = 1; i < width_ - 1; i++) 47 | { 48 | plane_->putc(height_ - 2, i, horz_line); 49 | } 50 | 51 | for (int i = 9; i >= 0; i--) 52 | { 53 | char axis_str[32]; 54 | const double axis_val = (double)timestep_ - ((10 - i) * (int)width_ / 10); 55 | sprintf(axis_str, "%.6g", axis_val < 0 ? 0 : axis_val); 56 | plane_->putstr(height_ - 2, i * width_ / 10, axis_str); 57 | } 58 | 59 | // vertical plane 60 | const char vert_line = '|'; 61 | for (uint i = 1; i < height_ - 2; i++) 62 | { 63 | plane_->putc(i, 1, vert_line); 64 | } 65 | 66 | // If not filled, simply draw from right to left, continously through the buffer 67 | if (!scaled_data_buffer_.filled_) 68 | { 69 | for (size_t i = 0; i < scaled_data_buffer_.i_ - 1; i++) 70 | { 71 | drawSlice(scaled_data_buffer_.buffer[i], scaled_data_buffer_.buffer[i + 1], width_ - scaled_data_buffer_.i_ + i); 72 | } 73 | } 74 | 75 | else 76 | { // If buffer is filled, fill from current buffer index to end (Left side of graph) 77 | for (size_t i = scaled_data_buffer_.i_; i < scaled_data_buffer_.buffer.size() - 1; i++) 78 | { 79 | drawSlice(scaled_data_buffer_.buffer[i], scaled_data_buffer_.buffer[i + 1], i - scaled_data_buffer_.i_ + 2); 80 | } // Then from start to current buffer index (Right side of graph) 81 | for (size_t i = 0; i < (scaled_data_buffer_.i_ == 0 ? scaled_data_buffer_.i_ : scaled_data_buffer_.i_ - 1); i++) 82 | { 83 | drawSlice(scaled_data_buffer_.buffer[i], scaled_data_buffer_.buffer[i + 1], 84 | width_ - 1 - scaled_data_buffer_.i_ + i); 85 | } 86 | // Fill gap between the two ends of the vector 87 | drawSlice(scaled_data_buffer_.buffer.back(), scaled_data_buffer_.buffer[0], width_ - 2 - scaled_data_buffer_.i_); 88 | } 89 | 90 | // Draw vertical axis steps last, to prevent graph from overlapping with numbers 91 | const double step = (highest_value_ - lowest_value_) / 4; 92 | for (int i = 4; i >= 0; i--) 93 | { 94 | const double axis_val = lowest_value_ + ((4 - i) * step); 95 | char axis_str[32]; 96 | sprintf(axis_str, "%.6g", axis_val); 97 | plane_->putstr(i * height_ / 5 + 1, 1, axis_str); 98 | } 99 | } 100 | 101 | void TopicPlotter::drawSlice(const uint64_t& curr_point, const uint64_t& next_point, const uint64_t& horizontal_loc) 102 | { 103 | const int diff = next_point - curr_point; 104 | 105 | if (diff == 0) 106 | { // Straight horizontal line 107 | plane_->putc(curr_point, horizontal_loc, "─"); 108 | } 109 | else if (abs(diff) == 1) 110 | { // Single step 111 | if (diff > 0) 112 | { 113 | plane_->putc(curr_point, horizontal_loc, "╮"); 114 | plane_->putc(next_point, horizontal_loc, "╰"); 115 | } 116 | else 117 | { 118 | plane_->putc(curr_point, horizontal_loc, "╯"); 119 | plane_->putc(next_point, horizontal_loc, "╭"); 120 | } 121 | } 122 | else 123 | { // Greater than single step 124 | if (diff > 0) 125 | { 126 | ui_helpers::drawVertLine(plane_, curr_point + 1, next_point - 1, horizontal_loc, "│"); 127 | plane_->putc(curr_point, horizontal_loc, "╮"); 128 | plane_->putc(next_point, horizontal_loc, "╰"); 129 | } 130 | else 131 | { 132 | ui_helpers::drawVertLine(plane_, curr_point - 1, next_point + 1, horizontal_loc, "│"); 133 | plane_->putc(curr_point, horizontal_loc, "╯"); 134 | plane_->putc(next_point, horizontal_loc, "╭"); 135 | } 136 | } 137 | } 138 | 139 | void TopicPlotter::renderData(const rosidl_typesupport_introspection_cpp::MessageMembers* members, uint8_t* data) 140 | { 141 | rosidl_typesupport_introspection_cpp::MessageMember member; 142 | uint8_t* member_data = nullptr; 143 | if (!entry_path_.empty()) 144 | { 145 | introspection::getMessageMember(entry_path_, members, data, member, &member_data); 146 | } 147 | else 148 | { 149 | member = members->members_[0]; 150 | member_data = &data[member.offset_]; 151 | } 152 | 153 | double message_double; 154 | introspection::messageDataToDouble(member, member_data, message_double); 155 | 156 | // const double message_double = introspection::readMessageAsDouble(data, members); 157 | data_buffer_.step(message_double); 158 | 159 | // If the newest datapoint is outside current bounds 160 | bool rescale_required = false; 161 | if (message_double > highest_value_) 162 | { 163 | highest_value_ = message_double; 164 | rescale_required = true; 165 | } 166 | else if (message_double < lowest_value_) 167 | { 168 | lowest_value_ = message_double; 169 | rescale_required = true; 170 | } 171 | 172 | if (rescale_required) 173 | { 174 | // If we need to rescale the data, do so before adding latest data point 175 | for (size_t i = 0; i < data_buffer_.buffer.size(); i++) 176 | { 177 | // We subtract the scale from height_, as the plane is inexed with 0,0 from the top left, meaning 178 | // we need to invert the data 179 | scaled_data_buffer_.buffer[i] = 180 | height_ - (height_ - 3) * (data_buffer_.buffer[i] - lowest_value_) / (highest_value_ - lowest_value_) - 2; 181 | } 182 | } 183 | 184 | const int scaled_data_point = 185 | height_ - (height_ - 3) * (message_double - lowest_value_) / (highest_value_ - lowest_value_) - 2; 186 | scaled_data_buffer_.step(scaled_data_point); 187 | 188 | drawPlot(); 189 | timestep_++; 190 | } 191 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | CC0 1.0 Universal 2 | 3 | Statement of Purpose 4 | 5 | The laws of most jurisdictions throughout the world automatically confer 6 | exclusive Copyright and Related Rights (defined below) upon the creator and 7 | subsequent owner(s) (each and all, an "owner") of an original work of 8 | authorship and/or a database (each, a "Work"). 9 | 10 | Certain owners wish to permanently relinquish those rights to a Work for the 11 | purpose of contributing to a commons of creative, cultural and scientific 12 | works ("Commons") that the public can reliably and without fear of later 13 | claims of infringement build upon, modify, incorporate in other works, reuse 14 | and redistribute as freely as possible in any form whatsoever and for any 15 | purposes, including without limitation commercial purposes. These owners may 16 | contribute to the Commons to promote the ideal of a free culture and the 17 | further production of creative, cultural and scientific works, or to gain 18 | reputation or greater distribution for their Work in part through the use and 19 | efforts of others. 20 | 21 | For these and/or other purposes and motivations, and without any expectation 22 | of additional consideration or compensation, the person associating CC0 with a 23 | Work (the "Affirmer"), to the extent that he or she is an owner of Copyright 24 | and Related Rights in the Work, voluntarily elects to apply CC0 to the Work 25 | and publicly distribute the Work under its terms, with knowledge of his or her 26 | Copyright and Related Rights in the Work and the meaning and intended legal 27 | effect of CC0 on those rights. 28 | 29 | 1. Copyright and Related Rights. A Work made available under CC0 may be 30 | protected by copyright and related or neighboring rights ("Copyright and 31 | Related Rights"). Copyright and Related Rights include, but are not limited 32 | to, the following: 33 | 34 | i. the right to reproduce, adapt, distribute, perform, display, communicate, 35 | and translate a Work; 36 | 37 | ii. moral rights retained by the original author(s) and/or performer(s); 38 | 39 | iii. publicity and privacy rights pertaining to a person's image or likeness 40 | depicted in a Work; 41 | 42 | iv. rights protecting against unfair competition in regards to a Work, 43 | subject to the limitations in paragraph 4(a), below; 44 | 45 | v. rights protecting the extraction, dissemination, use and reuse of data in 46 | a Work; 47 | 48 | vi. database rights (such as those arising under Directive 96/9/EC of the 49 | European Parliament and of the Council of 11 March 1996 on the legal 50 | protection of databases, and under any national implementation thereof, 51 | including any amended or successor version of such directive); and 52 | 53 | vii. other similar, equivalent or corresponding rights throughout the world 54 | based on applicable law or treaty, and any national implementations thereof. 55 | 56 | 2. Waiver. To the greatest extent permitted by, but not in contravention of, 57 | applicable law, Affirmer hereby overtly, fully, permanently, irrevocably and 58 | unconditionally waives, abandons, and surrenders all of Affirmer's Copyright 59 | and Related Rights and associated claims and causes of action, whether now 60 | known or unknown (including existing as well as future claims and causes of 61 | action), in the Work (i) in all territories worldwide, (ii) for the maximum 62 | duration provided by applicable law or treaty (including future time 63 | extensions), (iii) in any current or future medium and for any number of 64 | copies, and (iv) for any purpose whatsoever, including without limitation 65 | commercial, advertising or promotional purposes (the "Waiver"). Affirmer makes 66 | the Waiver for the benefit of each member of the public at large and to the 67 | detriment of Affirmer's heirs and successors, fully intending that such Waiver 68 | shall not be subject to revocation, rescission, cancellation, termination, or 69 | any other legal or equitable action to disrupt the quiet enjoyment of the Work 70 | by the public as contemplated by Affirmer's express Statement of Purpose. 71 | 72 | 3. Public License Fallback. Should any part of the Waiver for any reason be 73 | judged legally invalid or ineffective under applicable law, then the Waiver 74 | shall be preserved to the maximum extent permitted taking into account 75 | Affirmer's express Statement of Purpose. In addition, to the extent the Waiver 76 | is so judged Affirmer hereby grants to each affected person a royalty-free, 77 | non transferable, non sublicensable, non exclusive, irrevocable and 78 | unconditional license to exercise Affirmer's Copyright and Related Rights in 79 | the Work (i) in all territories worldwide, (ii) for the maximum duration 80 | provided by applicable law or treaty (including future time extensions), (iii) 81 | in any current or future medium and for any number of copies, and (iv) for any 82 | purpose whatsoever, including without limitation commercial, advertising or 83 | promotional purposes (the "License"). The License shall be deemed effective as 84 | of the date CC0 was applied by Affirmer to the Work. Should any part of the 85 | License for any reason be judged legally invalid or ineffective under 86 | applicable law, such partial invalidity or ineffectiveness shall not 87 | invalidate the remainder of the License, and in such case Affirmer hereby 88 | affirms that he or she will not (i) exercise any of his or her remaining 89 | Copyright and Related Rights in the Work or (ii) assert any associated claims 90 | and causes of action with respect to the Work, in either case contrary to 91 | Affirmer's express Statement of Purpose. 92 | 93 | 4. Limitations and Disclaimers. 94 | 95 | a. No trademark or patent rights held by Affirmer are waived, abandoned, 96 | surrendered, licensed or otherwise affected by this document. 97 | 98 | b. Affirmer offers the Work as-is and makes no representations or warranties 99 | of any kind concerning the Work, express, implied, statutory or otherwise, 100 | including without limitation warranties of title, merchantability, fitness 101 | for a particular purpose, non infringement, or the absence of latent or 102 | other defects, accuracy, or the present or absence of errors, whether or not 103 | discoverable, all to the greatest extent permissible under applicable law. 104 | 105 | c. Affirmer disclaims responsibility for clearing rights of other persons 106 | that may apply to the Work or any use thereof, including without limitation 107 | any person's Copyright and Related Rights in the Work. Further, Affirmer 108 | disclaims responsibility for obtaining any necessary consents, permissions 109 | or other rights required for any use of the Work. 110 | 111 | d. Affirmer understands and acknowledges that Creative Commons is not a 112 | party to this document and has no duty or obligation with respect to this 113 | CC0 or use of the Work. 114 | 115 | For more information, please see 116 | 117 | -------------------------------------------------------------------------------- /src/topic_streamer.cpp: -------------------------------------------------------------------------------- 1 | #include "ornis/topic_streamer.hpp" 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include "ncpp/Plane.hh" 12 | #include "notcurses/notcurses.h" 13 | #include "ornis/introspection_functions.hpp" 14 | #include "ornis/stream_interface.hpp" 15 | #include "ornis/topic_plotter.hpp" 16 | #include "ornis/topic_printer.hpp" 17 | #include "ornis/topic_string_viewer.hpp" 18 | #include "ornis/ui_helpers.hpp" 19 | 20 | using namespace std::chrono_literals; 21 | 22 | TopicStreamer::TopicStreamer(const std::string& topic_name, const std::string& topic_entry, 23 | const std::string& topic_type, const std::string& entry_type, 24 | const std::string& entry_path, std::shared_ptr& interface_channel, 25 | std::shared_ptr ros_interface_node, rcl_context_t context, 26 | const Options::color_scheme& theme) 27 | 28 | : topic_name_(topic_name) 29 | , topic_entry_(topic_entry) 30 | , topic_type_(topic_type) 31 | , entry_type_(entry_type) 32 | , entry_path_(entry_path) 33 | , ros_interface_node_(std::move(ros_interface_node)) 34 | , context_(context) 35 | , theme_(theme) 36 | { 37 | stream_open_.store(true); 38 | interface_channel_ = interface_channel; 39 | 40 | thread_ = new std::thread([this]() { initialise(); }); 41 | } 42 | TopicStreamer::~TopicStreamer() 43 | { 44 | stream_open_ = false; 45 | if (thread_ != nullptr) 46 | { 47 | thread_->join(); 48 | delete thread_; 49 | } 50 | } 51 | void TopicStreamer::closeStream() 52 | { 53 | interface_channel_->stream_plane_->erase(); 54 | stream_open_.store(false); 55 | interface_channel_->stream_open_.store(false); 56 | } 57 | 58 | void TopicStreamer::callback(rcl_subscription_t& subscription, const rosidl_message_type_support_t* type_support) 59 | { 60 | const auto members = static_cast(type_support->data); 61 | 62 | rcutils_allocator_t allocator = rcutils_get_default_allocator(); 63 | 64 | uint8_t* request_data = static_cast(allocator.allocate(members->size_of_, allocator.state)); 65 | 66 | // Initialise the memory that is expected to be used by the message 67 | members->init_function(request_data, rosidl_runtime_cpp::MessageInitialization::ALL); 68 | 69 | rmw_message_info_t info; 70 | 71 | // Grab the waiting message 72 | auto rc = rcl_take(&subscription, request_data, &info, NULL); 73 | 74 | if (rc == RCL_RET_OK) 75 | { 76 | topic_visualiser_->renderData(members, request_data); 77 | } 78 | else 79 | { 80 | // TODO: Test to make sure this fail string actually writes 81 | const std::string error = "Failed to read message! Error: " + std::to_string(rc); 82 | ui_helpers::writeStringToPlane(*interface_channel_->stream_plane_, error, theme_); 83 | } 84 | } 85 | 86 | void TopicStreamer::waitUntilUiReady() 87 | { 88 | while (!interface_channel_->stream_open_.load()) 89 | { 90 | } 91 | } 92 | 93 | void TopicStreamer::initialise() 94 | { 95 | waitUntilUiReady(); 96 | 97 | // FIXME Should take ownership of the stream plane, instead of leaving it in the channel. Once 98 | // Ornis moves to support multiple streams, this will cause issues. 99 | 100 | // Make the stream plane pretty 101 | ui_helpers::writeStringToTitledPlane(*interface_channel_->stream_plane_, topic_name_, "Waiting for message", theme_); 102 | 103 | const auto type_support = introspection::getMessageTypeSupport( 104 | topic_type_.c_str(), rosidl_typesupport_introspection_cpp::typesupport_identifier); 105 | 106 | auto* members = static_cast(type_support->data); 107 | 108 | std::vector entry_path_vec; 109 | // Split string into std::vector 110 | size_t last = 0; 111 | size_t next = 0; 112 | while ((next = entry_path_.find('/', last)) != std::string::npos) 113 | { 114 | const auto t_string = entry_path_.substr(last, next - last); 115 | if (!t_string.empty()) 116 | { 117 | entry_path_vec.push_back(t_string); 118 | } 119 | last = next + 1; 120 | } 121 | entry_path_vec.push_back(entry_path_.substr(last)); 122 | // We also trim out the first entry, as it's simply the message name 123 | entry_path_vec.erase(entry_path_vec.begin()); 124 | 125 | if (!entry_path_vec.empty()) 126 | { 127 | offset_ = introspection::getEntryOffset(entry_path_vec, entry_type_, members); 128 | } 129 | // If we are grabbing the whole message, we obviously have neither an entry path, nor an offset 130 | // visualise message as a plain string 131 | if (entry_type_ == "Msg" || entry_type_.empty()) 132 | { 133 | topic_visualiser_ = 134 | std::make_unique(TopicPrinter(interface_channel_->stream_plane_.get(), 20, 80, offset_, theme_)); 135 | } 136 | else 137 | { 138 | // Requested a single subelement. Attempt to visualise accordingly. 139 | rosidl_typesupport_introspection_cpp::MessageMember found_member; 140 | introspection::getMessageMember(offset_, members, found_member); 141 | if (introspection::parsableAsNumeric(found_member)) 142 | { 143 | topic_visualiser_ = 144 | std::make_unique(TopicPlotter(interface_channel_->stream_plane_.get(), 20, 80, offset_, theme_)); 145 | } 146 | else 147 | { 148 | topic_visualiser_ = std::make_unique( 149 | TopicStringViewer(interface_channel_->stream_plane_.get(), 20, 80, offset_, theme_)); 150 | } 151 | } 152 | 153 | // TODO: Investigate swapping profiles at runtime 154 | // There are some get_qos functions available. Investigate. 155 | rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; 156 | 157 | rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); 158 | 159 | rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); 160 | rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); 161 | subscription_options.qos = qos_profile; 162 | 163 | auto ret = rcl_subscription_init(&subscription, ros_interface_node_.get(), type_support, topic_name_.c_str(), 164 | &subscription_options); 165 | 166 | ret = rcl_wait_set_init(&wait_set, 1, 0, 0, 0, 0, 0, &context_, rcl_get_default_allocator()); 167 | size_t index; 168 | 169 | while (stream_open_.load()) 170 | { 171 | ret = rcl_wait_set_clear(&wait_set); 172 | ret = rcl_wait_set_add_subscription(&wait_set, &subscription, &index); 173 | ret = rcl_wait(&wait_set, RCL_MS_TO_NS(10000)); 174 | 175 | if (ret == RCL_RET_TIMEOUT) 176 | { 177 | ui_helpers::writeStringToTitledPlane(*interface_channel_->stream_plane_, topic_name_, 178 | "Timed out waiting for message!", theme_); 179 | std::this_thread::sleep_for(std::chrono::seconds(2)); 180 | continue; 181 | } 182 | if (wait_set.subscriptions[0]) 183 | { 184 | callback(subscription, type_support); 185 | } 186 | } 187 | 188 | // Clean up 189 | ret = rcl_subscription_fini(&subscription, ros_interface_node_.get()); 190 | ret = rcl_wait_set_fini(&wait_set); 191 | 192 | interface_channel_->stream_plane_->erase(); 193 | interface_channel_->stream_plane_.reset(); 194 | } 195 | -------------------------------------------------------------------------------- /src/options.cpp: -------------------------------------------------------------------------------- 1 | #include "ornis/options.hpp" 2 | #include "ncpp/Palette.hh" 3 | #include "notcurses/nckeys.h" 4 | #include "notcurses/notcurses.h" 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace Options 11 | { 12 | 13 | OptionsMenu::OptionsMenu() 14 | { 15 | } 16 | OptionsMenu::~OptionsMenu() 17 | { 18 | } 19 | 20 | void OptionsMenu::initialise(const int& x, const int& y, const ncpp::Plane* std_plane) 21 | { 22 | (void)y; // Unused for now. 23 | 24 | std::unique_ptr options_plane = std::make_unique(std_plane, 2, 2, -100, -100); 25 | 26 | const auto &fg = std::get<1>(current_scheme_); 27 | const auto &bg = std::get<2>(current_scheme_); 28 | const auto &hl = std::get<3>(current_scheme_); 29 | const auto &ll = std::get<4>(current_scheme_); 30 | uint64_t bgchannels = NCCHANNELS_INITIALIZER(fg.r, fg.g, fg.b, bg.r, bg.g, bg.b); 31 | ncchannels_set_fg_alpha(&bgchannels, NCALPHA_BLEND); 32 | ncchannels_set_bg_alpha(&bgchannels, NCALPHA_BLEND); 33 | 34 | // Give options plane the same background color as main plane 35 | options_plane->set_base("", 0, bgchannels); 36 | options_plane->set_channels(bgchannels); 37 | 38 | struct ncselector_options sopts; 39 | sopts.maxdisplay = 10; 40 | sopts.items = home_options_; 41 | sopts.defidx = 0; 42 | sopts.footer = ""; 43 | 44 | sopts.boxchannels = NCCHANNELS_INITIALIZER(fg.r, fg.g, fg.b, 0, 0, 0); 45 | sopts.opchannels = NCCHANNELS_INITIALIZER(hl.r, hl.g, hl.b, bg.r, bg.g, bg.b); 46 | sopts.descchannels = NCCHANNELS_INITIALIZER(ll.r, ll.g, ll.b, bg.r, bg.g, bg.b); 47 | sopts.titlechannels = NCCHANNELS_INITIALIZER(fg.r, fg.g, fg.b, 0x0e, 0x0e, 0x0e); 48 | 49 | ncchannels_set_bg_alpha(&sopts.boxchannels, NCALPHA_TRANSPARENT); 50 | ncchannels_set_bg_alpha(&sopts.titlechannels, NCALPHA_TRANSPARENT); 51 | 52 | const std::string options_title = "[o]ptions"; 53 | 54 | sopts.title = options_title.c_str(); 55 | 56 | selector_ = std::make_shared(options_plane.get(), &sopts); 57 | 58 | minimised_plane_ = 59 | std::make_shared(std_plane, 3, options_title.size() + 2, 1, x / 2 - (options_title.size() + 2) / 2); 60 | 61 | uint64_t channel = NCCHANNELS_INITIALIZER(fg.r, fg.g, fg.b, bg.r, bg.g, bg.b); 62 | minimised_plane_->set_base("", 0, channel); 63 | minimised_plane_->perimeter_rounded(0, channel, 0); 64 | 65 | for (uint i = 0; i < options_title.size(); i++) 66 | { 67 | minimised_plane_->putc(1, i + 1, options_title[i]); 68 | } 69 | } 70 | 71 | CommandEnum OptionsMenu::handleInput(const ncinput& input) 72 | { 73 | // Currently, only input is a selector 74 | // Handle transition (Enter) 75 | if (input.id == NCKEY_ENTER) 76 | { 77 | switch (current_menu_) 78 | { 79 | case MenuEnum::baseOptions: { 80 | transitionMenu(MenuEnum::colourMenu); 81 | return CommandEnum::noAction; 82 | } 83 | case MenuEnum::colourMenu: { 84 | selectColour(); 85 | transitionMenu(MenuEnum::baseOptions); 86 | return CommandEnum::reboot; 87 | } 88 | } 89 | } 90 | return CommandEnum::noAction; 91 | } 92 | 93 | void OptionsMenu::selectColour() 94 | { 95 | const auto currently_selected = selector_->get_selected(); 96 | size_t desired_index = 0; 97 | for (size_t i = 0; i < available_colour_list.size(); ++i) 98 | { 99 | if (strcmp(colour_menu_[i].option, currently_selected) == 0) 100 | { 101 | desired_index = i; 102 | break; 103 | } 104 | } 105 | // Should write to current config here 106 | current_configuration_["Theme"] = std::get<0>(available_colour_list[desired_index]); 107 | // Save Configuration: 108 | saveConfiguration(); 109 | } 110 | 111 | void OptionsMenu::transitionMenu(const MenuEnum& new_state) 112 | { 113 | switch (new_state) 114 | { 115 | case MenuEnum::colourMenu: { 116 | for (const auto& t : colour_menu_) 117 | { 118 | if (t.option != nullptr) 119 | { 120 | selector_->additem(&t); 121 | } 122 | for (const auto& t : home_options_) 123 | { 124 | if (t.option != nullptr) 125 | { 126 | selector_->delitem(t.option); 127 | } 128 | } 129 | } 130 | current_menu_ = MenuEnum::colourMenu; 131 | break; 132 | } 133 | case MenuEnum::baseOptions: { 134 | for (const auto& t : home_options_) 135 | { 136 | if (t.option != nullptr) 137 | { 138 | selector_->additem(&t); 139 | } 140 | for (const auto& t : colour_menu_) 141 | { 142 | if (t.option != nullptr) 143 | { 144 | selector_->delitem(t.option); 145 | } 146 | } 147 | } 148 | current_menu_ = MenuEnum::baseOptions; 149 | break; 150 | } 151 | default: { 152 | std::cerr << "Attempted to change options menu without valid state!\n"; 153 | } 154 | } 155 | } 156 | 157 | void OptionsMenu::createDefaultConfiguration() 158 | { 159 | current_configuration_["Theme"] = "Native"; 160 | saveConfiguration(); 161 | } 162 | 163 | void OptionsMenu::loadConfiguration() 164 | { 165 | const char* home_dir = std::getenv("HOME"); 166 | if (home_dir == nullptr) 167 | { 168 | std::cerr << "Error: could not get home directory\n"; 169 | return; 170 | } 171 | 172 | std::string config_dir = std::string(home_dir) + "/.config/ornis/config"; 173 | 174 | // If the configuration doesn't exist, create a default. 175 | if (!std::filesystem::exists(config_dir)) 176 | { 177 | createDefaultConfiguration(); 178 | } 179 | 180 | std::ifstream inputFile(config_dir); 181 | 182 | if (inputFile.is_open()) 183 | { 184 | std::string line; 185 | while (std::getline(inputFile, line)) 186 | { 187 | size_t pos = line.find(":"); 188 | if (pos != std::string::npos) 189 | { 190 | std::string key = line.substr(0, pos); 191 | std::string value = line.substr(pos + 1); 192 | current_configuration_[key] = value; 193 | } 194 | } 195 | inputFile.close(); 196 | } 197 | else 198 | { 199 | // Else, load default config 200 | std::cerr << "Error: could not open file \"" 201 | << "~/.config/ornis/config" 202 | << "\"\n"; 203 | } 204 | 205 | size_t desired_index = 0; 206 | for (size_t i = 0; i < available_colour_list.size(); ++i) 207 | { 208 | if (strcmp(colour_menu_[i].option, current_configuration_["Theme"].c_str()) == 0) 209 | { 210 | desired_index = i; 211 | break; 212 | } 213 | } 214 | current_scheme_ = available_colour_list[desired_index]; 215 | } 216 | 217 | void OptionsMenu::saveConfiguration() 218 | { 219 | const char* home_dir = std::getenv("HOME"); 220 | if (home_dir == nullptr) 221 | { 222 | std::cerr << "Error: could not get home directory\n"; 223 | return; 224 | } 225 | 226 | std::string config_dir = std::string(home_dir) + "/.config/ornis"; 227 | 228 | if (!std::filesystem::is_directory(config_dir)) 229 | { 230 | // Directory does not exist, create it 231 | try 232 | { 233 | std::filesystem::create_directories(config_dir); 234 | } 235 | catch (const std::filesystem::filesystem_error& e) 236 | { 237 | std::cerr << "Error: could not create directory \"" << config_dir << "\": " << e.what() << "\n"; 238 | } 239 | } 240 | 241 | std::ofstream outputFile(config_dir + "/config"); 242 | 243 | // Iterate through and save configuration 244 | for (const auto& option : current_configuration_) 245 | { 246 | outputFile << option.first << ":" << option.second << "\n"; 247 | } 248 | 249 | outputFile.close(); 250 | } 251 | } // namespace Options 252 | -------------------------------------------------------------------------------- /src/node_monitor.cpp: -------------------------------------------------------------------------------- 1 | #include "ornis/node_monitor.hpp" 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include // IWYU pragma: keep 12 | #include 13 | #include 14 | 15 | #include "ornis/ros_interface_node.hpp" 16 | 17 | NodeMonitor::NodeMonitor(std::shared_ptr ros_interface_node) 18 | : ros_interface_node_(std::move(ros_interface_node)) 19 | { 20 | thread_ = new std::thread([this]() { spin(); }); 21 | } 22 | NodeMonitor::~NodeMonitor() 23 | { 24 | spin_ = false; 25 | if (thread_ != nullptr) 26 | { 27 | thread_->join(); 28 | delete thread_; 29 | } 30 | } 31 | 32 | void NodeMonitor::spin() 33 | { 34 | while (spin_.load()) 35 | { 36 | updateValue(); 37 | std::this_thread::sleep_for(std::chrono::milliseconds(500)); 38 | } 39 | } 40 | 41 | void NodeMonitor::getInteractionForm(const std::string& entry_details, msg_tree::MsgTree& form) 42 | { 43 | (void)entry_details; 44 | form.getRoot()->setValue("Not yet implemented"); 45 | return; 46 | } 47 | 48 | void NodeMonitor::interact(const std::string& entry_name, const std::string& entry_details, 49 | const msg_tree::MsgTree& request, msg_tree::MsgTree& response) 50 | { 51 | (void)entry_name; 52 | (void)entry_details; 53 | (void)entry_name; 54 | (void)request; 55 | (void)response; 56 | // response = "Not yet implemented :("; 57 | } 58 | 59 | void NodeMonitor::getEntryInfo(const std::string& entry_name, const std::string& entry_details, 60 | std::map>& entry_info) 61 | { 62 | (void)entry_details; 63 | rcl_allocator_t allocator = rcl_get_default_allocator(); 64 | rcl_names_and_types_t published_topics = rcl_get_zero_initialized_names_and_types(); 65 | rcl_names_and_types_t subscribed_topics = rcl_get_zero_initialized_names_and_types(); 66 | rcl_names_and_types_t service_servers = rcl_get_zero_initialized_names_and_types(); 67 | rcl_names_and_types_t service_clients = rcl_get_zero_initialized_names_and_types(); 68 | rcl_names_and_types_t action_servers = rcl_get_zero_initialized_names_and_types(); 69 | rcl_names_and_types_t action_clients = rcl_get_zero_initialized_names_and_types(); 70 | 71 | constexpr auto blank_namespace = ""; 72 | constexpr auto error_string = "Failed! Ret code: "; 73 | // Publishers 74 | auto ret = rcl_get_publisher_names_and_types_by_node(ros_interface_node_.get(), &allocator, false, entry_name.c_str(), 75 | blank_namespace, &published_topics); 76 | if (ret == 0) 77 | { 78 | namesAndTypesToMap("Publishers", published_topics, entry_info); 79 | } 80 | else 81 | { 82 | entry_info["Publishers"] = { error_string + std::to_string(ret) }; 83 | } 84 | 85 | // Subscribers 86 | ret = rcl_get_subscriber_names_and_types_by_node(ros_interface_node_.get(), &allocator, false, entry_name.c_str(), 87 | blank_namespace, &subscribed_topics); 88 | 89 | if (ret == 0) 90 | { 91 | namesAndTypesToMap("Subscribers", subscribed_topics, entry_info); 92 | } 93 | else 94 | { 95 | entry_info["Subscribers"] = { error_string + std::to_string(ret) }; 96 | } 97 | 98 | // Service servers 99 | ret = rcl_get_service_names_and_types_by_node(ros_interface_node_.get(), &allocator, entry_name.c_str(), 100 | blank_namespace, &service_servers); 101 | 102 | if (ret == 0) 103 | { 104 | namesAndTypesToMap("Service servers", service_servers, entry_info); 105 | } 106 | else 107 | { 108 | entry_info["Service servers"] = { error_string + std::to_string(ret) }; 109 | } 110 | 111 | // Service clients 112 | ret = rcl_get_client_names_and_types_by_node(ros_interface_node_.get(), &allocator, entry_name.c_str(), 113 | blank_namespace, &service_clients); 114 | 115 | if (ret == 0) 116 | { 117 | namesAndTypesToMap("Service clients", service_clients, entry_info); 118 | } 119 | else 120 | { 121 | entry_info["Service clients"] = { error_string + std::to_string(ret) }; 122 | } 123 | 124 | // Action clients 125 | ret = rcl_action_get_client_names_and_types_by_node(ros_interface_node_.get(), &allocator, entry_name.c_str(), 126 | blank_namespace, &action_clients); 127 | 128 | if (ret == 0) 129 | { 130 | namesAndTypesToMap("Action clients", action_clients, entry_info); 131 | } 132 | else 133 | { 134 | entry_info["Action clients"] = { error_string + std::to_string(ret) }; 135 | } 136 | 137 | // Action servers 138 | ret = rcl_action_get_server_names_and_types_by_node(ros_interface_node_.get(), &allocator, entry_name.c_str(), 139 | blank_namespace, &action_servers); 140 | 141 | if (ret == 0) 142 | { 143 | namesAndTypesToMap("Action servers", action_servers, entry_info); 144 | } 145 | else 146 | { 147 | entry_info["Action servers"] = { error_string + std::to_string(ret) }; 148 | } 149 | 150 | ret = rcl_names_and_types_fini(&published_topics); 151 | ret = rcl_names_and_types_fini(&subscribed_topics); 152 | ret = rcl_names_and_types_fini(&service_servers); 153 | ret = rcl_names_and_types_fini(&service_clients); 154 | ret = rcl_names_and_types_fini(&action_servers); 155 | ret = rcl_names_and_types_fini(&action_clients); 156 | } 157 | 158 | void NodeMonitor::namesAndTypesToMap(const std::string& entry_name, const rcl_names_and_types_t& names_and_types, 159 | std::map>& entry_map) 160 | { 161 | if (names_and_types.names.size == 0) 162 | { 163 | entry_map[entry_name].push_back("None"); 164 | } 165 | else 166 | { 167 | for (size_t i = 0; i < names_and_types.names.size; ++i) 168 | { 169 | rcutils_string_array_t* topic_types = &names_and_types.types[i]; 170 | const std::string name = names_and_types.names.data[i]; 171 | for (size_t j = 0; j < topic_types->size; ++j) 172 | { 173 | entry_map[entry_name].push_back(name + ": " + std::string(topic_types->data[j])); 174 | } 175 | } 176 | } 177 | } 178 | 179 | void NodeMonitor::updateValue() 180 | { 181 | rcl_allocator_t allocator = rcl_get_default_allocator(); 182 | rcutils_string_array_t node_names{}; 183 | rcutils_string_array_t node_namespaces{}; 184 | 185 | int ret = rcl_get_node_names(ros_interface_node_.get(), allocator, &node_names, &node_namespaces); 186 | 187 | if (ret != RCL_RET_OK) 188 | { 189 | std::cerr << "Failed to get rcl_node_names!\n"; 190 | } 191 | 192 | // TODO: Figure out if there is anything useful you can use to populate the information with 193 | // For now, initialise nodes with empty info 194 | std::vector> nodes; 195 | nodes.reserve(node_names.size); 196 | for (size_t i = 0; i < node_names.size; ++i) 197 | { 198 | nodes.push_back(std::pair{ node_names.data[i], "" }); 199 | } 200 | 201 | if (latest_value_ != nodes) 202 | { 203 | std::unique_lock lk(data_mutex_); 204 | latest_value_ = nodes; 205 | last_read_current_ = false; 206 | } 207 | 208 | ret = rcutils_string_array_fini(&node_names); 209 | if (ret != RCL_RET_OK) 210 | { 211 | std::cerr << "Failed to finish node names string array!\n"; 212 | } 213 | ret = rcutils_string_array_fini(&node_namespaces); 214 | if (ret != RCL_RET_OK) 215 | { 216 | std::cerr << "Failed to finish node namespaces string array!\n"; 217 | } 218 | } 219 | -------------------------------------------------------------------------------- /src/service_monitor.cpp: -------------------------------------------------------------------------------- 1 | #include "ornis/service_monitor.hpp" 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include // IWYU pragma: keep 13 | #include 14 | #include 15 | 16 | #include "ornis/helper_functions.hpp" 17 | #include "ornis/introspection_functions.hpp" 18 | 19 | ServiceMonitor::ServiceMonitor(std::shared_ptr ros_interface_node) 20 | : ros_interface_node_(std::move(ros_interface_node)) 21 | { 22 | thread_ = new std::thread([this]() { spin(); }); 23 | } 24 | ServiceMonitor::~ServiceMonitor() 25 | { 26 | spin_ = false; 27 | if (thread_ != nullptr) 28 | { 29 | thread_->join(); 30 | delete thread_; 31 | } 32 | } 33 | 34 | void ServiceMonitor::spin() 35 | { 36 | while (spin_) 37 | { 38 | updateValue(); 39 | std::this_thread::sleep_for(std::chrono::milliseconds(500)); 40 | } 41 | } 42 | 43 | void ServiceMonitor::getEntryInfo(const std::string& entry_name, const std::string& entry_details, 44 | std::map>& entry_info) 45 | { 46 | (void)entry_name; 47 | 48 | msg_tree::msg_contents req_contents = { .data_type_ = "", .entry_name_ = "Request", .entry_data_ = "" }; 49 | 50 | msg_tree::msg_contents res_contents = { .data_type_ = "", .entry_name_ = "Response", .entry_data_ = "" }; 51 | msg_tree::MsgTree req_tree(req_contents), res_tree(res_contents); 52 | getInteractionTrees(entry_details, req_tree, res_tree); 53 | 54 | std::string response_string, request_string; 55 | 56 | // Show the request, and response messages 57 | res_tree.getRoot()->toString(response_string); 58 | req_tree.getRoot()->toString(request_string); 59 | 60 | entry_info["Response"].push_back(response_string); 61 | entry_info["Request"].push_back(request_string); 62 | } 63 | 64 | void ServiceMonitor::getInteractionTrees(const std::string service_type, msg_tree::MsgTree& request, 65 | msg_tree::MsgTree& response) 66 | { 67 | // Use namespace to shorten up some of the longer names 68 | using rosidl_typesupport_introspection_cpp::MessageMember; 69 | using rosidl_typesupport_introspection_cpp::MessageMembers; 70 | using rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE; 71 | using rosidl_typesupport_introspection_cpp::ServiceMembers; 72 | 73 | service_info_.type_support = 74 | introspection::getServiceTypeSupport(service_type, rosidl_typesupport_introspection_cpp::typesupport_identifier); 75 | 76 | const auto* service_members = static_cast(service_info_.type_support->data); 77 | 78 | // Need to create prefix from service_namespace, which needs to be converted from: 79 | // example_interfaces::srv to example_interfaces/srv/ 80 | std::string prefix = service_members->service_namespace_; 81 | prefix.replace(prefix.find("::"), sizeof("::") - 1, "/"); 82 | prefix += "/"; 83 | const auto prefix_char = prefix.c_str(); 84 | 85 | char request_char[100]; 86 | char response_char[100]; 87 | 88 | strcpy(request_char, prefix_char); 89 | strcpy(response_char, prefix_char); 90 | 91 | strcat(request_char, service_members->request_members_->message_name_); 92 | strcat(response_char, service_members->response_members_->message_name_); 93 | 94 | service_info_.request_type_support = 95 | introspection::getMessageTypeSupport(request_char, rosidl_typesupport_introspection_cpp::typesupport_identifier); 96 | 97 | service_info_.response_type_support = 98 | introspection::getMessageTypeSupport(response_char, rosidl_typesupport_introspection_cpp::typesupport_identifier); 99 | 100 | request.recursivelyCreateTree(request.getRoot(), service_info_.request_type_support); 101 | response.recursivelyCreateTree(response.getRoot(), service_info_.response_type_support); 102 | } 103 | 104 | void ServiceMonitor::getInteractionForm(const std::string& entry_details, msg_tree::MsgTree& form) 105 | { 106 | msg_tree::msg_contents blank_contents = { .data_type_ = "", .entry_name_ = "", .entry_data_ = "" }; 107 | msg_tree::MsgTree blank_tree(blank_contents); 108 | getInteractionTrees(entry_details, form, blank_tree); 109 | } 110 | 111 | void ServiceMonitor::interact(const std::string& entry_name, const std::string& entry_details, 112 | const msg_tree::MsgTree& request, msg_tree::MsgTree& response) 113 | { 114 | (void)entry_details; 115 | 116 | // Set up service message (Need to convert from string to rcl service msg) 117 | const char* service_name = entry_name.c_str(); 118 | const rosidl_service_type_support_t* type_support = service_info_.type_support; 119 | auto* request_members = static_cast( 120 | service_info_.request_type_support->data); 121 | 122 | // Allocate space to store the message 123 | rcutils_allocator_t allocator = rcutils_get_default_allocator(); 124 | uint8_t* request_data = static_cast(allocator.allocate(request_members->size_of_, allocator.state)); 125 | 126 | // Initialise the members 127 | request_members->init_function(request_data, rosidl_runtime_cpp::MessageInitialization::ALL); 128 | 129 | request.writeTreeToMessage(request_data, request_members); 130 | 131 | // Set up client 132 | rcl_client_t client = rcl_get_zero_initialized_client(); 133 | rcl_client_options_t client_ops = rcl_client_get_default_options(); 134 | auto ret = rcl_client_init(&client, ros_interface_node_.get(), type_support, service_name, &client_ops); 135 | 136 | if (ret != RCL_RET_OK) 137 | { 138 | std::cerr << "failed, error code: " << ret << " \n"; 139 | } 140 | 141 | // Sequence number of the request (Populated in rcl_send_request) 142 | int64_t sequence_number; 143 | 144 | auto* response_members = static_cast( 145 | service_info_.response_type_support->data); 146 | 147 | uint8_t* response_data = static_cast(allocator.allocate(response_members->size_of_, allocator.state)); 148 | 149 | rmw_service_info_t req_header; 150 | rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); 151 | ret = rcl_wait_set_init(&wait_set, 0, 0, 0, 1, 0, 0, ros_interface_node_->context, rcl_get_default_allocator()); 152 | 153 | // Send request 154 | ret = rcl_send_request(&client, request_data, &sequence_number); 155 | if (ret != RMW_RET_OK) 156 | { 157 | // response = "Failed to recieve response from service! error: "; 158 | // response += ret; 159 | // response += '\n'; 160 | return; 161 | } 162 | 163 | size_t index; 164 | while (true) 165 | { 166 | ret = rcl_wait_set_clear(&wait_set); 167 | ret = rcl_wait_set_add_client(&wait_set, &client, &index); 168 | ret = rcl_wait(&wait_set, RCL_MS_TO_NS(100)); 169 | if (ret == RCL_RET_TIMEOUT) 170 | { 171 | break; 172 | } 173 | if (wait_set.clients[0]) 174 | { 175 | ret = rcl_take_response_with_info(&client, &req_header, response_data); 176 | if (ret != RMW_RET_OK) 177 | { 178 | // response = "Failed to recieve response from service! error: "; 179 | // response += ret; 180 | // response += '\n'; 181 | return; 182 | } 183 | } 184 | } 185 | 186 | msg_tree::msg_contents response_contents = { .data_type_ = "", .entry_name_ = "response", .entry_data_ = "" }; 187 | 188 | response.recursivelyCreateTree(response.getRoot(), service_info_.response_type_support, response_data); 189 | // Clean up 190 | } 191 | 192 | void ServiceMonitor::updateValue() 193 | { 194 | rcl_allocator_t allocator = rcl_get_default_allocator(); 195 | rcl_names_and_types_t service_names_and_types{}; 196 | 197 | int ret = rcl_get_service_names_and_types(ros_interface_node_.get(), &allocator, &service_names_and_types); 198 | 199 | if (ret != RCL_RET_OK) 200 | { 201 | std::cerr << "Failed to update service monitor!\n"; 202 | } 203 | 204 | std::vector> service_info; 205 | 206 | for (size_t i = 0; i < service_names_and_types.names.size; ++i) 207 | { 208 | const std::string service_name = service_names_and_types.names.data[i]; 209 | for (size_t j = 0; j < service_names_and_types.types[i].size; ++j) 210 | { 211 | service_info.push_back( 212 | std::pair{ service_name, service_names_and_types.types[i].data[j] }); 213 | } 214 | } 215 | ret = rcl_names_and_types_fini(&service_names_and_types); 216 | if (ret != RCL_RET_OK) 217 | { 218 | std::cerr << "Failed to destroy service rcl_names_and_types object!\n"; 219 | } 220 | 221 | if (latest_value_ == service_info) 222 | { 223 | return; 224 | } 225 | std::unique_lock lk(data_mutex_); 226 | latest_value_ = service_info; 227 | last_read_current_ = false; 228 | } 229 | -------------------------------------------------------------------------------- /src/object_controller.cpp: -------------------------------------------------------------------------------- 1 | #include "ornis/object_controller.hpp" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include // IWYU pragma: keep 18 | 19 | #include 20 | 21 | #include "ornis/channel_interface.hpp" 22 | #include "ornis/introspection_functions.hpp" 23 | #include "ornis/monitor.hpp" 24 | #include "ornis/node_monitor.hpp" 25 | #include "ornis/ros_interface_node.hpp" 26 | #include "ornis/service_monitor.hpp" 27 | #include "ornis/stream_interface.hpp" 28 | #include "ornis/topic_monitor.hpp" 29 | #include "ornis/topic_streamer.hpp" 30 | 31 | ObjectController::ObjectController() : spin_(true) 32 | { 33 | interface_channel_ = std::make_shared(); 34 | interface_channel_->request_pending_ = false; 35 | } 36 | 37 | ObjectController::~ObjectController() 38 | { 39 | // Destroy monitors 40 | for (const auto& monitor : monitor_map_) 41 | { 42 | monitor.second->spin_ = false; 43 | } 44 | 45 | // TODO: Think about whether the object controller needs to handle destroying 46 | // any open streams 47 | rclcpp::shutdown(); 48 | } 49 | 50 | bool ObjectController::initialiseUserInterface() 51 | { 52 | if (!ui_.initialise(interface_channel_, stream_interface_map_)) 53 | { 54 | return true; 55 | } 56 | return false; 57 | } 58 | 59 | void ObjectController::initialiseMonitors() 60 | { 61 | monitor_map_["nodes"] = std::unique_ptr(new NodeMonitor(ros_interface_node_)); 62 | monitor_map_["topics"] = std::unique_ptr(new TopicMonitor(ros_interface_node_)); 63 | monitor_map_["services"] = std::unique_ptr(new ServiceMonitor(ros_interface_node_)); 64 | } 65 | 66 | void ObjectController::updateMonitors() 67 | { 68 | bool have_updated = false; 69 | 70 | std::map>> monitor_info; 71 | for (const auto& monitor : monitor_map_) 72 | { 73 | if (monitor.second->getValue(monitor_info[monitor.first])) 74 | { 75 | have_updated = true; 76 | previous_monitor_info_[monitor.first] = monitor_info[monitor.first]; 77 | } 78 | } 79 | 80 | if (!have_updated) 81 | { 82 | return; 83 | } 84 | 85 | std::unique_lock lk(interface_channel_->access_mutex_); 86 | for (auto& monitor_data : monitor_info) 87 | { 88 | if (monitor_data.second.size()) 89 | { 90 | interface_channel_->latest_monitor_data_[monitor_data.first] = monitor_data.second; 91 | } 92 | } 93 | 94 | interface_channel_->ui_data_current_ = false; 95 | } 96 | 97 | void ObjectController::checkUiRequests() 98 | { 99 | // FIXME: The contents of these switch cases to their own dedicated functions 100 | if (interface_channel_->request_pending_.load()) 101 | { 102 | // Check to see what the type of request is 103 | switch (interface_channel_->request_type_) 104 | { 105 | case (Channel::RequestEnum::monitorEntryInformation): { 106 | // Ensure response string empty 107 | interface_channel_->response_string_.clear(); 108 | 109 | // HACK: Horrible manual check for service call, as we cannot easily get 110 | // the selector desc from the ui. Manually grab the first service's type, 111 | // and just use that until you write a proper interface. All monitors have 112 | // infrastructure to recieve entry details, however only the service 113 | // monitor uses this. 114 | std::string entry_details; 115 | if (interface_channel_->request_details_["monitor_name"] == "services") 116 | { 117 | const std::string service_name = interface_channel_->request_details_["monitor_entry"]; 118 | const auto it = 119 | std::find_if(previous_monitor_info_["services"].begin(), previous_monitor_info_["services"].end(), 120 | [&service_name](const std::pair& service) { 121 | return service.first == service_name; 122 | }); 123 | entry_details = it->second; 124 | } 125 | // Ensure response map is clear before sending 126 | interface_channel_->response_map_.clear(); 127 | 128 | monitor_map_[interface_channel_->request_details_["monitor_name"]]->getEntryInfo( 129 | interface_channel_->request_details_["monitor_entry"], entry_details, interface_channel_->response_map_); 130 | 131 | std::unique_lock lk(interface_channel_->access_mutex_); 132 | interface_channel_->request_pending_ = false; 133 | interface_channel_->condition_variable_.notify_all(); 134 | break; 135 | } 136 | case (Channel::RequestEnum::monitorEntryInteraction): { 137 | // Ensure response string empty 138 | interface_channel_->response_string_.clear(); 139 | 140 | // HACK: Horrible manual check for service call, as we cannot easily get 141 | // the selector desc from the ui. Manually grab the first service's type, 142 | // and just use that until you write a proper interface. All monitors have 143 | // infrastructure to recieve entry details, however only the service 144 | // monitor uses this. 145 | std::string entry_details; 146 | const auto type_name = interface_channel_->request_details_["monitor_entry"]; 147 | const auto it = std::find_if( 148 | previous_monitor_info_[interface_channel_->request_details_["monitor_name"]].begin(), 149 | previous_monitor_info_[interface_channel_->request_details_["monitor_name"]].end(), 150 | [&type_name](const std::pair& type) { return type.first == type_name; }); 151 | entry_details = it->second; 152 | 153 | monitor_map_[interface_channel_->request_details_["monitor_name"]]->getInteractionForm( 154 | entry_details, interface_channel_->request_response_trees_->first); 155 | 156 | std::unique_lock lk(interface_channel_->access_mutex_); 157 | interface_channel_->request_pending_ = false; 158 | interface_channel_->condition_variable_.notify_all(); 159 | break; 160 | } 161 | case (Channel::RequestEnum::monitorEntryInteractionResult): { 162 | std::string entry_details; 163 | const auto type_name = interface_channel_->request_details_["monitor_entry"]; 164 | const auto it = std::find_if( 165 | previous_monitor_info_[interface_channel_->request_details_["monitor_name"]].begin(), 166 | previous_monitor_info_[interface_channel_->request_details_["monitor_name"]].end(), 167 | [&type_name](const std::pair& type) { return type.first == type_name; }); 168 | entry_details = it->second; 169 | 170 | monitor_map_[interface_channel_->request_details_["monitor_name"]]->interact( 171 | interface_channel_->request_details_["monitor_entry"], entry_details, 172 | interface_channel_->request_response_trees_->first, interface_channel_->request_response_trees_->second); 173 | 174 | std::unique_lock lk(interface_channel_->access_mutex_); 175 | interface_channel_->request_pending_ = false; 176 | interface_channel_->condition_variable_.notify_all(); 177 | break; 178 | } 179 | case (Channel::RequestEnum::topicStreamer): { 180 | std::unique_lock lk(interface_channel_->access_mutex_); 181 | const auto& topic_name = interface_channel_->request_details_["topic_name"]; 182 | const auto& entry_type = interface_channel_->request_details_["entry_type"]; 183 | const auto& topic_entry = interface_channel_->request_details_["topic_entry"]; 184 | const auto& entry_path = interface_channel_->request_details_["entry_path"]; 185 | 186 | const auto it = std::find_if( 187 | previous_monitor_info_[interface_channel_->request_details_["monitor_name"]].begin(), 188 | previous_monitor_info_[interface_channel_->request_details_["monitor_name"]].end(), 189 | [&topic_name](const std::pair& type) { return type.first == topic_name; }); 190 | const std::string topic_type = it->second; 191 | 192 | // Currently, the interface map isn't used for anything, but will likely 193 | // be used in the future if the streamer needs to make ui scaling requests 194 | // (Both to and from) 195 | stream_interface_map_[topic_name] = std::make_shared(topic_name); 196 | // Create the stream thread 197 | stream_map_[topic_name] = std::make_shared(topic_name, topic_entry, topic_type, entry_type, 198 | entry_path, stream_interface_map_[topic_name], 199 | ros_interface_node_, context_, ui_.current_scheme_); 200 | interface_channel_->request_pending_ = false; 201 | interface_channel_->condition_variable_.notify_all(); 202 | break; 203 | } 204 | case (Channel::RequestEnum::closeStream): { 205 | std::unique_lock lk(interface_channel_->access_mutex_); 206 | const auto& stream_name = interface_channel_->request_details_["stream_name"]; 207 | stream_map_[stream_name]->closeStream(); 208 | stream_map_.erase(stream_name); 209 | stream_interface_map_.erase(stream_name); 210 | interface_channel_->request_pending_ = false; 211 | interface_channel_->condition_variable_.notify_all(); 212 | break; 213 | } 214 | default: { 215 | break; 216 | } 217 | } 218 | } 219 | } 220 | 221 | int ObjectController::spin() 222 | { 223 | char** argv = NULL; 224 | rcl_init_options_t options = rcl_get_zero_initialized_init_options(); 225 | 226 | rcl_ret_t ret = rcl_init_options_init(&options, rcl_get_default_allocator()); 227 | 228 | context_ = rcl_get_zero_initialized_context(); 229 | 230 | ret = rcl_init(0, argv, &options, &context_); 231 | 232 | if (ret != RCL_RET_OK) 233 | { 234 | std::cerr << "Failed rcl init: " << ret << '\n'; 235 | } 236 | 237 | rcl_node_options_t node_options = rcl_node_get_default_options(); 238 | ros_interface_node_ = std::make_shared(rcl_get_zero_initialized_node()); 239 | ret = rcl_node_init(ros_interface_node_.get(), "ornis", "", &context_, &node_options); 240 | 241 | if (ret != RCL_RET_OK) 242 | { 243 | std::cerr << "Failed to initialise rcl node, error: " << ret << '\n'; 244 | return -1; 245 | } 246 | 247 | if (!strcmp("rmw_fastrtps_cpp", rmw_get_implementation_identifier())) 248 | { 249 | rclcpp::shutdown(); 250 | ret = rcl_node_options_fini(&node_options); 251 | return 2; 252 | } 253 | 254 | const auto ts = introspection::getMessageTypeSupport("std_msgs/msg/String", 255 | rosidl_typesupport_introspection_cpp::typesupport_identifier); 256 | 257 | if (!ts) 258 | { 259 | rclcpp::shutdown(); 260 | ret = rcl_node_options_fini(&node_options); 261 | return 1; 262 | } 263 | 264 | while (spinUi() != 1) 265 | ; 266 | 267 | rclcpp::shutdown(); 268 | return rcl_node_options_fini(&node_options); 269 | } 270 | 271 | uint8_t ObjectController::spinUi() 272 | { 273 | if (initialiseUserInterface()) 274 | { 275 | initialiseMonitors(); 276 | } 277 | else 278 | { 279 | return 1; 280 | } 281 | 282 | while (ui_.screen_loop_) 283 | { 284 | updateMonitors(); 285 | // Check if the monitor has any pending data requests 286 | checkUiRequests(); 287 | 288 | if (ui_.reboot_required_) 289 | { 290 | (&ui_)->~Ui(); 291 | new (&ui_) Ui(); 292 | 293 | monitor_map_["nodes"].reset(); 294 | monitor_map_["topics"].reset(); 295 | monitor_map_["services"].reset(); 296 | return 0; 297 | } 298 | 299 | using namespace std::chrono_literals; 300 | std::this_thread::sleep_for(0.1s); 301 | } 302 | return 1; 303 | } 304 | -------------------------------------------------------------------------------- /include/ornis/msg_tree.hpp: -------------------------------------------------------------------------------- 1 | #ifndef MSG_TREE_H_ 2 | #define MSG_TREE_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include "ornis/introspection_functions.hpp" 11 | #pragma once 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | namespace msg_tree 19 | { 20 | struct msg_contents 21 | { 22 | // Type of entry (Double, String, Array etc) 23 | const std::string data_type_; 24 | 25 | const std::string entry_name_; 26 | // Contents of entry. Non-const as user will fill out sometimes 27 | std::string entry_data_; 28 | 29 | bool operator==(msg_contents const& rhs) const 30 | { 31 | return this->data_type_ == rhs.data_type_ && this->entry_name_ == rhs.entry_name_; 32 | } 33 | 34 | // OStream operator 35 | friend std::ostream& operator<<(std::ostream& os, const msg_contents& msg) 36 | { 37 | os << "(" << msg.data_type_ << ") " << msg.entry_name_ << ": " << msg.entry_data_; 38 | return os; 39 | } 40 | }; 41 | 42 | class MsgTreeNode 43 | { 44 | public: 45 | MsgTreeNode() 46 | { 47 | } 48 | 49 | MsgTreeNode(const msg_contents& new_msg, const MsgTreeNode* parent) : msg_contents_(new_msg), parent_(parent) 50 | { 51 | } 52 | 53 | virtual ~MsgTreeNode() 54 | { 55 | } 56 | 57 | MsgTreeNode* addChild(const msg_contents& t) 58 | { 59 | this->children_.push_back(MsgTreeNode(t, this)); 60 | return &this->children_.back(); 61 | } 62 | 63 | void setValue(const std::string& entry_data) 64 | { 65 | this->msg_contents_.entry_data_ = entry_data; 66 | } 67 | 68 | void setEditingStatus(const bool& edit_status) 69 | { 70 | being_edited_ = edit_status; 71 | } 72 | 73 | bool getEditingStatus() const 74 | { 75 | return being_edited_; 76 | } 77 | 78 | msg_contents& getValue() 79 | { 80 | return this->msg_contents_; 81 | } 82 | 83 | const msg_contents& getValue() const 84 | { 85 | return this->msg_contents_; 86 | } 87 | 88 | std::vector& getChildren() 89 | { 90 | return this->children_; 91 | } 92 | 93 | const std::vector& getChildren() const 94 | { 95 | return this->children_; 96 | } 97 | 98 | bool isEditable() const 99 | { 100 | return this->children_.size() == 0; 101 | } 102 | 103 | const MsgTreeNode& getChild(size_t index) const 104 | { 105 | return this->children_[index]; 106 | } 107 | 108 | MsgTreeNode& getChild(size_t index) 109 | { 110 | return this->children_[index]; 111 | } 112 | 113 | const MsgTreeNode* getParent() 114 | { 115 | return parent_; 116 | } 117 | 118 | bool isLeaf() const 119 | { 120 | return children_.empty(); 121 | } 122 | 123 | size_t leafCount() const 124 | { 125 | return children_.size(); 126 | } 127 | 128 | void toString(std::string& output, int indent = 0) const 129 | { 130 | const std::string indent_str(indent, ' '); 131 | if (indent != 0) 132 | { 133 | output.append("\n"); 134 | } 135 | output.append(indent_str + this->msg_contents_.entry_name_); 136 | 137 | for (const auto& child : children_) 138 | { 139 | child.toString(output, indent + 2); 140 | } 141 | } 142 | 143 | void print(const int depth = 0) const 144 | { 145 | for (int i = 0; i < depth; ++i) 146 | { 147 | if (i != depth - 1) 148 | std::cout << " "; 149 | else 150 | std::cout << "╰── "; 151 | } 152 | std::cout << this->msg_contents_ << std::endl; 153 | for (uint i = 0; i < this->children_.size(); ++i) 154 | { 155 | this->children_.at(i).print(depth + 1); 156 | } 157 | } 158 | 159 | MsgTreeNode* getNthEditableNode(const uint& n) 160 | { 161 | uint search_index = 1; 162 | return findNthEditableNode(n, search_index); 163 | } 164 | 165 | MsgTreeNode* findNthEditableNode(const uint& n, uint& search_index) 166 | { 167 | MsgTreeNode* editable_leaf = nullptr; 168 | if (isEditable()) 169 | { 170 | if (search_index == n) 171 | { 172 | return this; 173 | } 174 | else 175 | { 176 | search_index++; 177 | } 178 | } 179 | else 180 | { 181 | for (auto& child : children_) 182 | { 183 | editable_leaf = child.findNthEditableNode(n, search_index); 184 | if (editable_leaf != nullptr) 185 | { 186 | return editable_leaf; 187 | } 188 | } 189 | } 190 | return editable_leaf; 191 | } 192 | 193 | std::string getPathNthNode(const uint& n) 194 | { 195 | uint search_index = 1; 196 | return findPathNthNode(n, search_index); 197 | } 198 | 199 | std::string findPathNthNode(const uint& n, uint& search_index) 200 | { 201 | std::string path = msg_contents_.entry_name_; 202 | if (path[0] != '/') 203 | { 204 | path.insert(path.begin(), '/'); 205 | } 206 | if (search_index == n) 207 | { 208 | return path; 209 | } 210 | search_index++; 211 | for (auto& child : children_) 212 | { 213 | const auto subpath = child.findPathNthNode(n, search_index); 214 | if (!subpath.empty()) 215 | { 216 | path.append(subpath); 217 | return path; 218 | } 219 | } 220 | return ""; 221 | } 222 | 223 | MsgTreeNode* getNthNode(const uint& n) 224 | { 225 | if (n == 0) 226 | { 227 | return this; 228 | } 229 | uint search_index = 1; 230 | return findNthNode(n, search_index); 231 | } 232 | 233 | MsgTreeNode* findNthNode(const uint& n, uint& search_index) 234 | { 235 | MsgTreeNode* leaf = nullptr; 236 | if (search_index == n) 237 | { 238 | return this; 239 | } 240 | search_index++; 241 | for (auto& child : children_) 242 | { 243 | leaf = child.findNthNode(n, search_index); 244 | if (leaf != nullptr) 245 | { 246 | return leaf; 247 | } 248 | } 249 | return leaf; 250 | } 251 | 252 | void writeNodeToMessage(uint8_t* message_data, const rosidl_typesupport_introspection_cpp::MessageMembers* members) 253 | { 254 | for (size_t i = 0; i < members->member_count_; i++) 255 | { 256 | const rosidl_typesupport_introspection_cpp::MessageMember& member = members->members_[i]; 257 | uint8_t* member_data = &message_data[member.offset_]; 258 | // Perform a check for if we're dealing with a ros message type, and recurse if we are 259 | if (member.type_id_ == rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE) 260 | { 261 | const auto sub_members = 262 | static_cast(member.members_->data); 263 | children_[i].writeNodeToMessage(member_data, sub_members); 264 | } 265 | else 266 | { 267 | introspection::stringToMessageData(message_data, member, children_[i].msg_contents_.entry_data_); 268 | } 269 | } 270 | } 271 | 272 | private: 273 | msg_contents msg_contents_; 274 | std::vector children_; 275 | const MsgTreeNode* parent_; 276 | 277 | // If the user is editing this node 278 | // TODO: Rename to "currently_selected" or something similar 279 | bool being_edited_ = false; 280 | }; 281 | 282 | class MsgTree 283 | { 284 | public: 285 | MsgTree(const msg_contents& msg_contents_, const rosidl_message_type_support_t* type_data) 286 | : node_count_(0), editable_node_count_(0), base_(new MsgTreeNode(msg_contents_, nullptr)) 287 | { 288 | recursivelyCreateTree(base_.get(), type_data); 289 | } 290 | 291 | // If constructed with no typesupport, no tree is created. Used for if tree is constructed 292 | // after root node is created. 293 | MsgTree(const msg_contents& msg_contents_) 294 | : node_count_(0), editable_node_count_(0), base_(new MsgTreeNode(msg_contents_, nullptr)) 295 | { 296 | } 297 | 298 | ~MsgTree() 299 | { 300 | } 301 | 302 | const MsgTreeNode* getRoot() const 303 | { 304 | return base_.get(); 305 | } 306 | MsgTreeNode* getRoot() 307 | { 308 | return base_.get(); 309 | } 310 | 311 | const MsgTreeNode* getFirstEditableNode() const 312 | { 313 | return base_->getNthEditableNode(1); 314 | } 315 | 316 | void recursivelyCreateTree(MsgTreeNode* target_node, const rosidl_message_type_support_t* type_data) 317 | { 318 | using rosidl_typesupport_introspection_cpp::MessageMember; 319 | using rosidl_typesupport_introspection_cpp::MessageMembers; 320 | using rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE; 321 | const auto* members = static_cast(type_data->data); 322 | 323 | // To prevent potential duplication, Remove Node's children if any exist 324 | if (!target_node->getChildren().empty()) 325 | { 326 | target_node->getChildren().clear(); 327 | } 328 | target_node->getChildren().reserve(members->member_count_); 329 | 330 | for (size_t i = 0; i < members->member_count_; i++) 331 | { 332 | const MessageMember& member = members->members_[i]; 333 | std::string new_node_type; 334 | introspection::messageTypeToString(member, new_node_type); 335 | const msg_contents msg_data = { .data_type_ = new_node_type, .entry_name_ = member.name_, .entry_data_ = "" }; 336 | 337 | MsgTreeNode* new_node = target_node->addChild(msg_data); 338 | 339 | if (member.is_array_) 340 | { 341 | const msg_contents msg_array_data = { .data_type_ = "Array", .entry_name_ = "[]", .entry_data_ = "" }; 342 | new_node->getChildren().reserve(1); 343 | new_node->addChild(msg_array_data); 344 | editable_node_count_++; // An array is editable 345 | node_count_++; 346 | } 347 | else if (member.type_id_ == ROS_TYPE_MESSAGE) 348 | { 349 | recursivelyCreateTree(new_node, member.members_); 350 | } 351 | else 352 | { 353 | // Node has no chldren, probably editable 354 | editable_node_count_++; 355 | } 356 | node_count_++; 357 | } 358 | } 359 | 360 | void recursivelyCreateTree(MsgTreeNode* target_node, const rosidl_message_type_support_t* type_data, 361 | uint8_t* message_data) 362 | { 363 | using rosidl_typesupport_introspection_cpp::MessageMember; 364 | using rosidl_typesupport_introspection_cpp::MessageMembers; 365 | using rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE; 366 | const auto* members = static_cast(type_data->data); 367 | 368 | // To prevent potential duplication, Remove Node's children if any exist 369 | if (!target_node->getChildren().empty()) 370 | { 371 | target_node->getChildren().clear(); 372 | } 373 | target_node->getChildren().reserve(members->member_count_); 374 | 375 | for (size_t i = 0; i < members->member_count_; i++) 376 | { 377 | const MessageMember& member = members->members_[i]; 378 | std::string new_node_type; 379 | introspection::messageTypeToString(member, new_node_type); 380 | const msg_contents msg_data = { .data_type_ = new_node_type, .entry_name_ = member.name_, .entry_data_ = "" }; 381 | 382 | MsgTreeNode* new_node = target_node->addChild(msg_data); 383 | 384 | if (member.is_array_) 385 | { 386 | new_node->getValue().entry_data_ += "["; 387 | const uint32_t array_size = static_cast(member.size_function(message_data + member.offset_)); 388 | for (uint32_t i = 0; i < array_size; ++i) 389 | { 390 | std::string array_element_data; 391 | const uint8_t* raw_array_element_data = 392 | static_cast(member.get_function(message_data + member.offset_, i)); 393 | introspection::messageDataToString(member, raw_array_element_data, array_element_data); 394 | new_node->getValue().entry_data_ += array_element_data; 395 | if (i + 1 != array_size) 396 | { 397 | new_node->getValue().entry_data_ += ", "; 398 | } 399 | } 400 | new_node->getValue().entry_data_ += ']'; 401 | node_count_++; 402 | editable_node_count_++; // an array is editable 403 | } 404 | else if (member.type_id_ == ROS_TYPE_MESSAGE) 405 | { 406 | uint8_t* sub_member_data = &message_data[member.offset_]; 407 | recursivelyCreateTree(new_node, member.members_, sub_member_data); 408 | } 409 | else 410 | { 411 | // Node has no chldren, probably editable 412 | editable_node_count_++; 413 | introspection::messageDataToString(member, &message_data[member.offset_], new_node->getValue().entry_data_); 414 | } 415 | node_count_++; 416 | } 417 | } 418 | 419 | void writeTreeToMessage(uint8_t* message_data, 420 | const rosidl_typesupport_introspection_cpp::MessageMembers* members) const 421 | { 422 | base_->writeNodeToMessage(message_data, members); 423 | } 424 | 425 | uint node_count_; 426 | uint editable_node_count_; 427 | 428 | private: 429 | std::unique_ptr base_; 430 | }; 431 | 432 | } // namespace msg_tree 433 | #endif // MSG_TREE_H_ 434 | -------------------------------------------------------------------------------- /include/ornis/ui_helpers.hpp: -------------------------------------------------------------------------------- 1 | #ifndef UI_HELPERS_H_ 2 | #define UI_HELPERS_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include "ncpp/Palette.hh" 12 | #include "ncpp/Plane.hh" 13 | #include "ncpp/Selector.hh" 14 | #include "notcurses/nckeys.h" 15 | #include "notcurses/notcurses.h" 16 | #include "ornis/options.hpp" 17 | #include "ornis/msg_tree.hpp" 18 | 19 | namespace ui_helpers 20 | { 21 | inline void sizePlaneToString(ncpp::Plane& plane, const std::string& content, const Options::color_scheme &color_scheme) 22 | { 23 | const auto bg = std::get<2>(color_scheme); 24 | 25 | int row = 1; 26 | int col = 1; 27 | int longest_col = 0; 28 | 29 | // iterate through string twice, once to find what size 30 | // to resize the plane to, second to place the characters on the plane. 31 | // It's ugly, but much more efficient than dynamically resizing the 32 | // plane as we iterate through the string. 33 | for (const char& c : content) 34 | { 35 | if (c == '\n') 36 | { 37 | row++; 38 | col = 1; 39 | } 40 | else 41 | { 42 | col++; 43 | longest_col = col > longest_col ? col : longest_col; 44 | } 45 | } 46 | // If we haven't found an endline char, artificially add a single row, to prevent 47 | // single line strings from being overwritten by the border. We also add one to the 48 | // longest col, to account for what would usually be read as the invisible \n 49 | if (row == 1) 50 | { 51 | row++; 52 | longest_col++; 53 | } 54 | // Add one to longest col to account for border 55 | plane.resize(row + 2, longest_col + 1); 56 | 57 | // Fill plane, ensures we don't have a transparent background 58 | ncpp::Cell c(' '); 59 | c.set_bg_rgb8(bg.r, bg.g, bg.b); 60 | c.set_fg_rgb8(bg.r, bg.g, bg.b); 61 | plane.polyfill(row, longest_col, c); 62 | } 63 | 64 | inline void sizePlaneToMap(ncpp::Plane& plane, const std::string& title, 65 | const std::map>& content_map, 66 | const Options::color_scheme &color_scheme) 67 | { 68 | const auto bg = std::get<2>(color_scheme); 69 | size_t row = 1; 70 | size_t col = 1; 71 | size_t longest_col = 0; 72 | 73 | for (const auto& entry : content_map) 74 | { 75 | row++; 76 | // Check length of map key (Which is used as the divider name) 77 | longest_col = entry.first.size() > longest_col ? entry.first.size() : longest_col; 78 | for (const auto& entry_string : entry.second) 79 | { 80 | col = 1; 81 | for (const char& c : entry_string) 82 | { 83 | if (c == '\n') 84 | { 85 | row++; 86 | col = 1; 87 | } 88 | else 89 | { 90 | col++; 91 | longest_col = col > longest_col ? col : longest_col; 92 | } 93 | } 94 | row++; 95 | } 96 | } 97 | 98 | // If we haven't found an endline char, artificially add a single row, to prevent 99 | // single line strings from being overwritten by the border. We also add one to the 100 | // longest col, to account for what would usually be read as the invisible \n 101 | if (row == 1) 102 | { 103 | row++; 104 | longest_col++; 105 | } 106 | 107 | longest_col = title.size() > longest_col ? title.size() : longest_col; 108 | 109 | // Add one to longest col to account for border 110 | plane.resize(row + 1, longest_col + 2); 111 | 112 | // Fill plane, ensures we don't have a transparent background 113 | ncpp::Cell c(' '); 114 | c.set_bg_rgb8(bg.r, bg.g, bg.b); 115 | c.set_fg_rgb8(bg.r, bg.g, bg.b); 116 | plane.polyfill(row, longest_col, c); 117 | } 118 | 119 | inline void sizeTree(const msg_tree::MsgTreeNode& node, size_t& rows, size_t& longest_col) 120 | { 121 | const auto t = &node.getValue(); 122 | const std::string node_line = "(" + t->data_type_ + ") " + t->entry_name_ + ": " + t->entry_data_ + " "; 123 | const size_t child_length = node_line.length(); 124 | longest_col = child_length > longest_col ? child_length : longest_col; 125 | for (const auto& child : node.getChildren()) 126 | { 127 | rows++; 128 | sizeTree(child, rows, longest_col); 129 | } 130 | } 131 | 132 | inline void sizePlaneToTree(ncpp::Plane& plane, const std::string& title, const msg_tree::MsgTree& tree, 133 | const Options::color_scheme &color_scheme, const bool skip_root = true) 134 | { 135 | const auto bg = std::get<2>(color_scheme); 136 | size_t rows = 1; 137 | size_t longest_col = 0; 138 | 139 | sizeTree(*tree.getRoot(), rows, longest_col); 140 | 141 | longest_col = title.size() > longest_col ? title.size() : longest_col; 142 | 143 | // Add one to longest col to account for border 144 | const uint extra_rows = skip_root ? 1 : 2; 145 | plane.resize(rows + extra_rows, longest_col + 4); 146 | 147 | // Fill plane, ensures we don't have a transparent background 148 | ncpp::Cell c(' '); 149 | c.set_bg_rgb8(bg.r, bg.g, bg.b); 150 | c.set_fg_rgb8(bg.r, bg.g, bg.b); 151 | plane.polyfill(rows, longest_col, c); 152 | } 153 | 154 | // Overload to allow for functions to specify how much white space they want around the string 155 | inline void sizePlaneToString(ncpp::Plane& plane, const std::string& content, const int& row_buffer, 156 | const int& col_buffer, const Options::color_scheme &color_scheme) 157 | { 158 | const auto bg = std::get<2>(color_scheme); 159 | int row = 1; 160 | int col = 1; 161 | int longest_col = 0; 162 | 163 | // iterate through string twice, once to find what size 164 | // to resize the plane to, second to place the characters on the plane. 165 | // It's ugly, but much more efficient than dynamically resizing the 166 | // plane as we iterate through the string. 167 | for (const char& c : content) 168 | { 169 | if (c == '\n') 170 | { 171 | row++; 172 | col = 1; 173 | } 174 | else 175 | { 176 | col++; 177 | longest_col = col > longest_col ? col : longest_col; 178 | } 179 | } 180 | // If we haven't found an endline char, artificially add a single row, to prevent 181 | // single line strings from being overwritten by the border. We also add one to the 182 | // longest col, to account for what would usually be read as the invisible \n 183 | if (row == 1) 184 | { 185 | row++; 186 | longest_col++; 187 | } 188 | row += row_buffer; 189 | longest_col += col_buffer; 190 | // Add one to longest col to account for border 191 | plane.resize(row + 2, longest_col + 1); 192 | 193 | // Fill plane, ensures we don't have a transparent background 194 | ncpp::Cell c(' '); 195 | c.set_bg_rgb8(bg.r, bg.g, bg.b); 196 | c.set_fg_rgb8(bg.r, bg.g, bg.b); 197 | plane.polyfill(row, longest_col, c); 198 | } 199 | 200 | inline void writeStringToPlane(ncpp::Plane& plane, const std::string& content, const Options::color_scheme &color_scheme) 201 | { 202 | plane.erase(); 203 | plane.move_top(); 204 | 205 | ui_helpers::sizePlaneToString(plane, content, color_scheme); 206 | 207 | int row = 1; 208 | int col = 1; 209 | 210 | ncpp::Cell c(' '); 211 | nccell cell = NCCELL_TRIVIAL_INITIALIZER; 212 | for (const char& c : content) 213 | { 214 | if (c == '\n') 215 | { 216 | row++; 217 | col = 1; 218 | } 219 | else 220 | { 221 | nccell_load(plane.to_ncplane(), &cell, &c); 222 | plane.putc(row, col, c); 223 | nccell_release(plane.to_ncplane(), &cell); 224 | col++; 225 | } 226 | } 227 | 228 | uint64_t channel = plane.get_channels(); 229 | plane.perimeter_rounded(0, channel, 0); 230 | } 231 | 232 | // Identical to previous, but renders a cursor at index 233 | inline void writeStringToPlane(ncpp::Plane& plane, const std::string& content, const int& cursor_index, const Options::color_scheme &color_scheme) 234 | { 235 | // Cursor index is a location in string. If -1, place at end of string 236 | plane.erase(); 237 | plane.move_top(); 238 | // As we have a cursor (And therefore expect to be editing the string), we add a horizontal buffer 239 | ui_helpers::sizePlaneToString(plane, content, 0, 2, color_scheme); 240 | 241 | int string_index = 0; 242 | int row = 1; 243 | int col = 1; 244 | 245 | nccell cell = NCCELL_TRIVIAL_INITIALIZER; 246 | for (const char& c : content) 247 | { 248 | if (c == '\n') 249 | { 250 | row++; 251 | col = 1; 252 | } 253 | else 254 | { 255 | nccell_load(plane.to_ncplane(), &cell, &c); 256 | plane.putc(row, col, c); 257 | nccell_release(plane.to_ncplane(), &cell); 258 | col++; 259 | } 260 | if (string_index == cursor_index) 261 | { 262 | nccell_load(plane.to_ncplane(), &cell, &c); 263 | plane.putc(row, col, "┃"); 264 | nccell_release(plane.to_ncplane(), &cell); 265 | } 266 | string_index++; 267 | } 268 | 269 | uint64_t channel = plane.get_channels(); 270 | plane.perimeter_rounded(0, channel, 0); 271 | } 272 | 273 | inline void writeStringToTitledPlane(ncpp::Plane& plane, const std::string& title, const std::string& content, const Options::color_scheme &color_scheme) 274 | { 275 | plane.erase(); 276 | plane.move_top(); 277 | 278 | // Add some white space to the front and end of the title string. 279 | title.size() > content.size() ? ui_helpers::sizePlaneToString(plane, title, color_scheme) : 280 | ui_helpers::sizePlaneToString(plane, content, color_scheme); 281 | 282 | int row = 1; 283 | int col = 1; 284 | 285 | // TODO These cell writing loops should be moved to their own functions 286 | ncpp::Cell c(' '); 287 | nccell cell = NCCELL_TRIVIAL_INITIALIZER; 288 | for (const char& c : content) 289 | { 290 | if (c == '\n') 291 | { 292 | row++; 293 | col = 1; 294 | } 295 | else 296 | { 297 | nccell_load(plane.to_ncplane(), &cell, &c); 298 | plane.putc(row, col, c); 299 | nccell_release(plane.to_ncplane(), &cell); 300 | col++; 301 | } 302 | } 303 | 304 | uint64_t channel = plane.get_channels(); 305 | // ncchannels_set_bg_alpha(&channel, NCALPHA_TRANSPARENT); 306 | plane.perimeter_rounded(0, channel, 0); 307 | 308 | col = (plane.get_dim_x() - title.size()) / 2; 309 | for (const char& c : title) 310 | { 311 | nccell_load(plane.to_ncplane(), &cell, &c); 312 | plane.putc(0, col, c); 313 | nccell_release(plane.to_ncplane(), &cell); 314 | col++; 315 | } 316 | } 317 | 318 | inline void writeStringVectorToTitledPlane(ncpp::Plane& plane, const std::string& title, 319 | const std::vector& content, const Options::color_scheme &color_scheme) 320 | { 321 | const auto bg = std::get<2>(color_scheme); 322 | 323 | plane.erase(); 324 | plane.move_top(); 325 | 326 | const size_t row_count = content.size(); 327 | 328 | const auto longest_entry = std::max_element(content.begin(), content.end(), 329 | [](const auto& a, const auto& b) { return a.size() < b.size(); }); 330 | 331 | title.size() > longest_entry->size() ? ui_helpers::sizePlaneToString(plane, title, color_scheme) : 332 | ui_helpers::sizePlaneToString(plane, *longest_entry, color_scheme); 333 | 334 | const uint col_count = title.size() > longest_entry->size() ? title.size() : longest_entry->size(); 335 | // Add one to longest col to account for border 336 | plane.resize(row_count + 2, col_count + 2); 337 | 338 | // Fill plane, ensures we don't have a transparent background 339 | ncpp::Cell c(' '); 340 | c.set_fg_rgb8(bg.r, bg.g, bg.b); 341 | c.set_bg_rgb8(bg.r, bg.g, bg.b); 342 | plane.polyfill(row_count, col_count, c); 343 | 344 | int row = 1; 345 | int col = 1; 346 | 347 | // TODO These cell writing loops should be moved to their own functions 348 | nccell cell = NCCELL_TRIVIAL_INITIALIZER; 349 | for (const std::string& line : content) 350 | { 351 | for (const char& c : line) 352 | { 353 | nccell_load(plane.to_ncplane(), &cell, &c); 354 | plane.putc(row, col, c); 355 | nccell_release(plane.to_ncplane(), &cell); 356 | col++; 357 | } 358 | row++; 359 | col = 1; 360 | } 361 | 362 | uint64_t channel = plane.get_channels(); 363 | // ncchannels_set_bg_alpha(&channel, NCALPHA_TRANSPARENT); 364 | plane.perimeter_rounded(0, channel, 0); 365 | 366 | col = (plane.get_dim_x() - title.size()) / 2; 367 | for (const char& c : title) 368 | { 369 | nccell_load(plane.to_ncplane(), &cell, &c); 370 | plane.putc(0, col, c); 371 | nccell_release(plane.to_ncplane(), &cell); 372 | col++; 373 | } 374 | } 375 | 376 | // Identical to previous, but renders a cursor at index 377 | inline void writeStringToTitledPlane(ncpp::Plane& plane, const std::string& title, const std::string& content, 378 | const int& cursor_index, const Options::color_scheme &color_scheme) 379 | { 380 | plane.erase(); 381 | plane.move_top(); 382 | 383 | // Add some white space to the front and end of the title string. 384 | title.size() > content.size() ? ui_helpers::sizePlaneToString(plane, title, 0, 2, color_scheme) : 385 | ui_helpers::sizePlaneToString(plane, content, 0, 2, color_scheme); 386 | 387 | int row = 1; 388 | int col = 1; 389 | int string_index = 0; 390 | 391 | // TODO These cell writing loops should be moved to their own functions 392 | nccell cell = NCCELL_TRIVIAL_INITIALIZER; 393 | for (const char& c : content) 394 | { 395 | if (c == '\n') 396 | { 397 | row++; 398 | col = 1; 399 | } 400 | else 401 | { 402 | nccell_load(plane.to_ncplane(), &cell, &c); 403 | plane.putc(row, col, c); 404 | nccell_release(plane.to_ncplane(), &cell); 405 | col++; 406 | } 407 | if (string_index == cursor_index) 408 | { 409 | nccell_load(plane.to_ncplane(), &cell, &c); 410 | plane.putc(row, col, "┃"); 411 | nccell_release(plane.to_ncplane(), &cell); 412 | } 413 | string_index++; 414 | } 415 | 416 | uint64_t channel = plane.get_channels(); 417 | plane.perimeter_rounded(0, channel, 0); 418 | 419 | col = (plane.get_dim_x() - title.size()) / 2; 420 | for (const char& c : title) 421 | { 422 | nccell_load(plane.to_ncplane(), &cell, &c); 423 | plane.putc(0, col, c); 424 | nccell_release(plane.to_ncplane(), &cell); 425 | col++; 426 | } 427 | } 428 | 429 | inline void writeMapToTitledPlane(ncpp::Plane& plane, const std::string& title, 430 | std::map>& content, const Options::color_scheme &color_scheme) 431 | { 432 | plane.erase(); 433 | plane.move_top(); 434 | 435 | // Remove first line in string, which when created from the msg_tree, simply contains the name 436 | // of the root node 437 | for (auto& entry : content) 438 | { 439 | for (std::string& content : entry.second) 440 | { 441 | content.erase(0, content.find("\n") + 1); 442 | } 443 | } 444 | 445 | ui_helpers::sizePlaneToMap(plane, title, content, color_scheme); 446 | 447 | const int bar_length = plane.get_dim_x(); 448 | const auto fill_char = "─"; 449 | 450 | int row = 1; 451 | int col = 1; 452 | 453 | // TODO These cell writing loops should be moved to their own functions 454 | for (auto& entry : content) 455 | { 456 | for (int i = 0; i < bar_length; i++) 457 | { 458 | plane.putstr(row, i, fill_char); 459 | } 460 | col = bar_length / 2 - entry.first.size() / 2; 461 | for (const char& c : entry.first) 462 | { 463 | plane.putc(row, col, c); 464 | col++; 465 | } 466 | col = 1; 467 | row++; 468 | for (std::string& content : entry.second) 469 | { 470 | for (const char& c : content) 471 | { 472 | if (c == '\n') 473 | { 474 | row++; 475 | col = 1; 476 | } 477 | else 478 | { 479 | plane.putc(row, col, c); 480 | col++; 481 | } 482 | } 483 | row++; 484 | col = 1; 485 | } 486 | } 487 | 488 | uint64_t channel = plane.get_channels(); 489 | plane.perimeter_rounded(0, channel, 0); 490 | 491 | // Write planes title 492 | col = (plane.get_dim_x() - title.size()) / 2; 493 | for (const char& c : title) 494 | { 495 | plane.putc(0, col, c); 496 | col++; 497 | } 498 | } 499 | 500 | // Almost identical to writeDetailedTree, but draws a cursor, as well as a different background color for editable 501 | // fields 502 | inline void writeEditingTreeToTitledPlane(ncpp::Plane& plane, const std::string& title, const msg_tree::MsgTree& tree, const Options::color_scheme &color_scheme) 503 | { 504 | const auto bg = std::get<2>(color_scheme); 505 | const auto fg = std::get<1>(color_scheme); 506 | 507 | plane.erase(); 508 | plane.move_top(); 509 | 510 | ui_helpers::sizePlaneToTree(plane, title, tree, color_scheme); 511 | 512 | size_t row = 1; 513 | int col = 1; 514 | 515 | uint64_t channel = plane.get_channels(); 516 | plane.perimeter_rounded(0, channel, 0); 517 | 518 | std::function 520 | drawTreeToPlane; 521 | drawTreeToPlane = [&](const msg_tree::MsgTreeNode& node, ncpp::Plane& plane, size_t& row, const bool& is_root, 522 | uint depth, const std::wstring& prefix) { 523 | // Skip root 524 | if (!is_root) 525 | { 526 | size_t col = 1; 527 | const msg_tree::msg_contents t = node.getValue(); 528 | 529 | std::wstring_convert> converter; 530 | std::wstring node_line; 531 | if (node.isEditable()) 532 | { 533 | node_line += L'[' + converter.from_bytes(t.data_type_) + L"] " + converter.from_bytes(t.entry_name_) + L": " + 534 | converter.from_bytes(t.entry_data_); 535 | } 536 | else 537 | { 538 | node_line += converter.from_bytes(t.entry_name_); 539 | } 540 | node_line.insert(0, prefix); 541 | // Adjust prefix based on node status 542 | if (prefix[prefix.length() - 1] == L'│') 543 | { 544 | node_line[prefix.length() - 1] = L'├'; 545 | } 546 | else if (prefix[prefix.length() - 1] == L' ') 547 | { 548 | node_line[prefix.length() - 1] = L'└'; 549 | } 550 | 551 | for (const auto& c : node_line) 552 | { 553 | plane.putwch(row, col, c); 554 | col++; 555 | } 556 | if (node.getEditingStatus()) 557 | { 558 | plane.putc(row, col, "┃"); 559 | } 560 | if (node.isEditable()) 561 | { 562 | uint64_t highlight = 0; 563 | ncchannels_set_fg_rgb8(&highlight, fg.r, fg.g, fg.b); 564 | ncchannels_set_bg_rgb8(&highlight, bg.r, bg.g, bg.b); 565 | ncchannels_set_bg_alpha(&highlight, ncpp::Cell::AlphaOpaque); 566 | plane.stain(row, col - t.entry_data_.size(), 1, t.entry_data_.size() + 1, highlight, highlight, highlight, 567 | highlight); 568 | } 569 | row++; 570 | } 571 | 572 | for (const auto& child : node.getChildren()) 573 | { 574 | if (&child == &node.getChildren().back() && child.isLeaf()) 575 | { 576 | drawTreeToPlane(child, plane, row, false, depth + 2, prefix + L" └"); 577 | } 578 | else if (&child == &node.getChildren().back()) 579 | { 580 | drawTreeToPlane(child, plane, row, false, depth + 2, prefix + L" "); 581 | } 582 | else if (child.isLeaf()) 583 | { 584 | drawTreeToPlane(child, plane, row, false, depth + 2, prefix + L" ├"); 585 | } 586 | else 587 | { 588 | drawTreeToPlane(child, plane, row, false, depth + 2, prefix + L" │"); 589 | } 590 | } 591 | return; 592 | }; 593 | 594 | uint depth = 0; 595 | drawTreeToPlane(*tree.getRoot(), plane, row, true, depth, L""); 596 | 597 | // Write planes title 598 | col = (plane.get_dim_x() - title.size()) / 2; 599 | for (const char& c : title) 600 | { 601 | plane.putc(0, col, c); 602 | col++; 603 | } 604 | } 605 | 606 | inline void writeSelectionTreeToTitledPlane(ncpp::Plane& plane, const std::string& title, const msg_tree::MsgTree& tree, 607 | const uint& selected_index, const Options::color_scheme &color_scheme) 608 | { 609 | const auto bg = std::get<2>(color_scheme); 610 | const auto fg = std::get<1>(color_scheme); 611 | const auto hl = std::get<3>(color_scheme); 612 | 613 | plane.erase(); 614 | plane.move_top(); 615 | 616 | ui_helpers::sizePlaneToTree(plane, title, tree, color_scheme, false); 617 | 618 | size_t row = 1; 619 | int col = 1; 620 | 621 | uint64_t channel = plane.get_channels(); 622 | plane.perimeter_rounded(0, channel, 0); 623 | 624 | std::function 626 | drawTreeToPlane; 627 | drawTreeToPlane = [&](const msg_tree::MsgTreeNode& node, ncpp::Plane& plane, size_t& row, bool highlight, uint depth, 628 | const std::wstring& prefix) { 629 | std::wstring_convert> converter; 630 | if (row == selected_index) 631 | { 632 | highlight = true; 633 | } 634 | 635 | size_t col = 1; 636 | const msg_tree::msg_contents t = node.getValue(); 637 | std::wstring node_line; 638 | if (node.isLeaf()) 639 | { 640 | node_line += L'[' + converter.from_bytes(t.data_type_) + L"]: " + converter.from_bytes(t.entry_name_); 641 | } 642 | else 643 | { 644 | node_line += converter.from_bytes(t.entry_name_); 645 | } 646 | node_line.insert(0, prefix); 647 | // Adjust prefix based on node status 648 | if (prefix[prefix.length() - 1] == L'│') 649 | { 650 | node_line[prefix.length() - 1] = L'├'; 651 | } 652 | else if (prefix[prefix.length() - 1] == L' ') 653 | { 654 | node_line[prefix.length() - 1] = L'└'; 655 | } 656 | 657 | // (Unfortuanately) Need to ensure we re-set the highlight channel each draw 658 | uint64_t highlight_channel = 0; 659 | ; 660 | if (highlight) 661 | { 662 | ncchannels_set_fg_rgb8(&highlight_channel, fg.r, fg.g, fg.b); 663 | ncchannels_set_bg_rgb8(&highlight_channel, hl.r, hl.g, hl.b); 664 | } 665 | else 666 | { 667 | ncchannels_set_fg_rgb8(&highlight_channel, fg.r, fg.g, fg.b); 668 | ncchannels_set_bg_rgb8(&highlight_channel, bg.r, bg.g, bg.b); 669 | } 670 | ncchannels_set_bg_alpha(&highlight_channel, ncpp::Cell::AlphaOpaque); 671 | 672 | for (const wchar_t& c : node_line) 673 | { 674 | plane.putwch(row, col, c); 675 | col++; 676 | } 677 | plane.stain(row, col - node_line.size(), 1, node_line.size() + 1, highlight_channel, highlight_channel, 678 | highlight_channel, highlight_channel); 679 | row++; 680 | 681 | for (const auto& child : node.getChildren()) 682 | { 683 | if (&child == &node.getChildren().back() && child.isLeaf()) 684 | { 685 | drawTreeToPlane(child, plane, row, highlight, depth + 2, prefix + L" └"); 686 | } 687 | else if (&child == &node.getChildren().back()) 688 | { 689 | drawTreeToPlane(child, plane, row, highlight, depth + 2, prefix + L" "); 690 | } 691 | else if (child.isLeaf()) 692 | { 693 | drawTreeToPlane(child, plane, row, highlight, depth + 2, prefix + L" ├"); 694 | } 695 | else 696 | { 697 | drawTreeToPlane(child, plane, row, highlight, depth + 2, prefix + L" │"); 698 | } 699 | } 700 | return; 701 | }; 702 | 703 | uint depth = 0; 704 | drawTreeToPlane(*tree.getRoot(), plane, row, selected_index == 0, depth, L""); 705 | 706 | // Write planes title 707 | col = (plane.get_dim_x() - title.size()) / 2; 708 | for (const char& c : title) 709 | { 710 | plane.putc(0, col, c); 711 | col++; 712 | } 713 | } 714 | 715 | inline void writeDetailedTreeToTitledPlane(ncpp::Plane& plane, const std::string& title, const msg_tree::MsgTree& tree, const Options::color_scheme &color_scheme) 716 | { 717 | plane.erase(); 718 | plane.move_top(); 719 | 720 | ui_helpers::sizePlaneToTree(plane, title, tree, color_scheme); 721 | 722 | size_t row = 1; 723 | int col = 1; 724 | 725 | std::function 727 | drawTreeToPlane; 728 | drawTreeToPlane = [&](const msg_tree::MsgTreeNode& node, ncpp::Plane& plane, size_t& row, const std::wstring& prefix, 729 | const bool& is_root) { 730 | std::wstring_convert> converter; 731 | 732 | if (!is_root) 733 | { 734 | size_t col = 1; 735 | const msg_tree::msg_contents t = node.getValue(); 736 | std::wstring node_line; 737 | if (node.isLeaf()) 738 | { 739 | node_line += L'[' + converter.from_bytes(t.data_type_) + L"]: " + converter.from_bytes(t.entry_name_); 740 | } 741 | else 742 | { 743 | node_line += converter.from_bytes(t.entry_name_); 744 | } 745 | if (!t.entry_data_.empty()) 746 | { 747 | node_line.append(L": " + converter.from_bytes(t.entry_data_)); 748 | } 749 | node_line.insert(0, prefix); 750 | // Adjust prefix based on node status 751 | if (prefix[prefix.length() - 1] == L'│') 752 | { 753 | node_line[prefix.length() - 1] = L'├'; 754 | } 755 | else if (prefix[prefix.length() - 1] == L' ') 756 | { 757 | node_line[prefix.length() - 1] = L'└'; 758 | } 759 | 760 | for (const wchar_t& c : node_line) 761 | { 762 | plane.putwch(row, col, c); 763 | col++; 764 | } 765 | row++; 766 | } 767 | for (const auto& child : node.getChildren()) 768 | { 769 | if (&child == &node.getChildren().back() && child.isLeaf()) 770 | { 771 | drawTreeToPlane(child, plane, row, prefix + L" └", false); 772 | } 773 | else if (&child == &node.getChildren().back()) 774 | { 775 | drawTreeToPlane(child, plane, row, prefix + L" ", false); 776 | } 777 | else if (child.isLeaf()) 778 | { 779 | drawTreeToPlane(child, plane, row, prefix + L" ├", false); 780 | } 781 | else 782 | { 783 | drawTreeToPlane(child, plane, row, prefix + L" │", false); 784 | } 785 | } 786 | return; 787 | }; 788 | 789 | drawTreeToPlane(*tree.getRoot(), plane, row, L"", true); 790 | 791 | uint64_t channel = plane.get_channels(); 792 | plane.perimeter_rounded(0, channel, 0); 793 | 794 | // Write planes title 795 | col = (plane.get_dim_x() - title.size()) / 2; 796 | for (const char& c : title) 797 | { 798 | plane.putc(0, col, c); 799 | col++; 800 | } 801 | } 802 | 803 | // Draws a bar of plane's BG color at top of plane, spanning entire width, with content string centered 804 | inline void drawHelperBar(ncpp::Plane* plane, const std::string content) 805 | { 806 | const int bar_length = plane->get_dim_x(); 807 | nccell cell = NCCELL_TRIVIAL_INITIALIZER; 808 | const auto fill_char = "─"; 809 | 810 | for (int i = 0; i < bar_length; i++) 811 | { 812 | plane->putstr(0, i, fill_char); 813 | } 814 | 815 | int col = bar_length / 2 - content.size() / 2; 816 | for (const auto& c : content) 817 | { 818 | nccell_load(plane->to_ncplane(), &cell, &c); 819 | plane->putc(0, col, c); 820 | nccell_release(plane->to_ncplane(), &cell); 821 | col++; 822 | } 823 | } 824 | 825 | inline void drawVertLine(ncpp::Plane* plane, const uint64_t& p1, const uint64_t& p2, const uint64_t& horz_location, 826 | const char* symbol) 827 | { 828 | const int direction = p1 < p2 ? 1 : -1; 829 | for (uint64_t index = p1; index != p2; index += direction) 830 | { 831 | plane->putc(index, horz_location, symbol); 832 | } 833 | plane->putc(p2, horz_location, symbol); 834 | } 835 | 836 | inline bool downInput(const ncinput& input) 837 | { 838 | return (input.id == NCKEY_TAB || input.id == NCKEY_DOWN); 839 | } 840 | 841 | inline bool upInput(const ncinput& input) 842 | { 843 | return ((input.id == NCKEY_TAB && input.shift) || input.id == NCKEY_UP); 844 | } 845 | 846 | inline bool mouseClick(const ncinput& input) 847 | { 848 | return (input.id == NCKEY_BUTTON1); 849 | } 850 | 851 | // Case for confirmation selections 852 | inline bool selectInput(const ncinput& input) 853 | { 854 | // Kitty terminal will send both a press, and release input, 855 | // we create a wrapper here to prevent duplicate inputs 856 | return (input.id == NCKEY_ENTER); 857 | } 858 | 859 | inline bool isPress(const ncinput& input) 860 | { 861 | // Kitty terminal will send both a press, and release input, 862 | // Add a lil' diddy here to prevent keys from being sent twice 863 | return (input.evtype != NCTYPE_RELEASE); 864 | } 865 | 866 | } // namespace ui_helpers 867 | 868 | #endif // UI_HELPERS_H_ 869 | -------------------------------------------------------------------------------- /src/introspection_functions.cpp: -------------------------------------------------------------------------------- 1 | #include "ornis/introspection_functions.hpp" 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include "ornis/helper_functions.hpp" 10 | 11 | namespace introspection 12 | { 13 | std::string getTypeSupportLibraryPath(const std::string& package_name, const std::string& typesupport_identifier) 14 | { 15 | const char* filename_prefix; 16 | const char* filename_extension; 17 | const char* dynamic_library_folder; 18 | #ifdef _WIN32 19 | filename_prefix = ""; 20 | filename_extension = ".dll"; 21 | dynamic_library_folder = "/bin/"; 22 | #elif __APPLE__ 23 | filename_prefix = "lib"; 24 | filename_extension = ".dylib"; 25 | dynamic_library_folder = "/lib/"; 26 | #else 27 | filename_prefix = "lib"; 28 | filename_extension = ".so"; 29 | dynamic_library_folder = "/lib/"; 30 | #endif 31 | 32 | std::string package_prefix; 33 | try 34 | { 35 | package_prefix = ament_index_cpp::get_package_prefix(package_name); 36 | } 37 | catch (ament_index_cpp::PackageNotFoundError& e) 38 | { 39 | throw std::runtime_error(e.what()); 40 | } 41 | 42 | auto library_path = package_prefix + dynamic_library_folder + filename_prefix + package_name + "__" + 43 | typesupport_identifier + filename_extension; 44 | return library_path; 45 | } 46 | 47 | const std::tuple extractTypeIdentifier(const std::string& full_type) 48 | { 49 | char type_separator = '/'; 50 | const auto sep_position_back = full_type.find_last_of(type_separator); 51 | const auto sep_position_front = full_type.find_first_of(type_separator); 52 | 53 | // FIXME: Could really just return false or someting instead of crashing the whole goddamn program here 54 | if (sep_position_back == std::string::npos || sep_position_back == 0 || sep_position_back == full_type.length() - 1) 55 | { 56 | throw std::runtime_error("service/message type is not of the form package/type and cannot be processed"); 57 | } 58 | 59 | std::string package_name = full_type.substr(0, sep_position_front); 60 | std::string middle_module = ""; 61 | if (sep_position_back - sep_position_front > 0) 62 | { 63 | middle_module = full_type.substr(sep_position_front + 1, sep_position_back - sep_position_front - 1); 64 | } 65 | std::string type_name = full_type.substr(sep_position_back + 1); 66 | 67 | return std::make_tuple(package_name, middle_module, type_name); 68 | } 69 | 70 | const std::pair extractTypeAndPackage(const std::string& full_type) 71 | { 72 | std::string package_name; 73 | std::string type_name; 74 | 75 | std::tie(package_name, std::ignore, type_name) = extractTypeIdentifier(full_type); 76 | 77 | return { package_name, type_name }; 78 | } 79 | 80 | const rosidl_service_type_support_t* getServiceTypeSupport(const std::string& type, 81 | const std::string& typesupport_identifier) 82 | { 83 | std::string package_name; 84 | std::string middle_module; 85 | std::string type_name; 86 | std::tie(package_name, middle_module, type_name) = extractTypeIdentifier(type); 87 | 88 | std::string dynamic_loading_error = 89 | "Something went wrong loading the typesupport library " 90 | "for service type " + 91 | package_name + "/" + type_name + "."; 92 | 93 | auto library_path = getTypeSupportLibraryPath(package_name, typesupport_identifier); 94 | 95 | try 96 | { 97 | void* typesupport_library = dlopen(library_path.c_str(), RTLD_LAZY); 98 | 99 | const auto symbol_name = typesupport_identifier + "__get_service_type_support_handle__" + package_name + "__" + 100 | (middle_module.empty() ? "srv" : middle_module) + "__" + type_name; 101 | 102 | if (typesupport_library == nullptr) 103 | { 104 | throw std::runtime_error(dynamic_loading_error + " Symbol not found."); 105 | } 106 | 107 | typedef const rosidl_service_type_support_t* (*get_service_ts_func)(); 108 | 109 | get_service_ts_func introspection_type_support_handle_func = 110 | reinterpret_cast(dlsym(typesupport_library, symbol_name.c_str())); 111 | 112 | const rosidl_service_type_support_t* introspection_ts = introspection_type_support_handle_func(); 113 | 114 | return introspection_ts; 115 | } 116 | catch (...) 117 | { 118 | throw std::runtime_error(dynamic_loading_error + " Library could not be found."); 119 | } 120 | } 121 | 122 | const rosidl_message_type_support_t* getMessageTypeSupport(const std::string& type, 123 | const std::string& typesupport_identifier) 124 | { 125 | std::string package_name; 126 | std::string middle_module; 127 | std::string type_name; 128 | std::tie(package_name, middle_module, type_name) = extractTypeIdentifier(type); 129 | 130 | std::string dynamic_loading_error = 131 | "Something went wrong loading the typesupport library " 132 | "for message type " + 133 | package_name + "/" + type_name + "."; 134 | 135 | auto library_path = getTypeSupportLibraryPath(package_name, typesupport_identifier); 136 | 137 | try 138 | { 139 | void* typesupport_library = dlopen(library_path.c_str(), RTLD_LAZY); 140 | 141 | const auto symbol_name = typesupport_identifier + "__get_message_type_support_handle__" + package_name + "__" + 142 | (middle_module.empty() ? "msg" : middle_module) + "__" + type_name; 143 | 144 | if (typesupport_library == nullptr) 145 | { 146 | throw std::runtime_error(dynamic_loading_error + " Symbol not found."); 147 | } 148 | 149 | typedef const rosidl_message_type_support_t* (*get_message_ts_func)(); 150 | 151 | get_message_ts_func introspection_type_support_handle_func = 152 | reinterpret_cast(dlsym(typesupport_library, symbol_name.c_str())); 153 | 154 | const rosidl_message_type_support_t* introspection_ts = introspection_type_support_handle_func(); 155 | 156 | return introspection_ts; 157 | } 158 | catch (...) 159 | { 160 | throw std::runtime_error(dynamic_loading_error + " Library could not be found."); 161 | } 162 | } 163 | 164 | void messageTypeToString(const rosidl_typesupport_introspection_cpp::MessageMember& member_info, 165 | std::string& message_type) 166 | { 167 | switch (member_info.type_id_) 168 | { 169 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT: 170 | message_type = "Float32"; 171 | break; 172 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_DOUBLE: 173 | message_type = "Double"; 174 | break; 175 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_LONG_DOUBLE: 176 | message_type = "Long Double"; 177 | break; 178 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_CHAR: 179 | message_type = "Char"; 180 | break; 181 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_WCHAR: 182 | message_type = "WChar"; 183 | break; 184 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_BOOLEAN: 185 | message_type = "Bool"; 186 | break; 187 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_OCTET: 188 | message_type = "Octect"; 189 | break; 190 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT8: 191 | message_type = "Uint8"; 192 | break; 193 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT8: 194 | message_type = "Int8"; 195 | break; 196 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT16: 197 | message_type = "UInt16"; 198 | break; 199 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT16: 200 | message_type = "Int16"; 201 | break; 202 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT32: 203 | message_type = "UInt32"; 204 | break; 205 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT32: 206 | message_type = "Int32"; 207 | break; 208 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT64: 209 | message_type = "Uint64"; 210 | break; 211 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT64: 212 | message_type = "Int64"; 213 | break; 214 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING: 215 | message_type = "String"; 216 | break; 217 | // TODO Implement Nested. Ignored for now 218 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE: 219 | message_type = "Msg"; 220 | break; 221 | default: 222 | message_type = "?Unknown?"; 223 | // Recieved unkwn message type, fail silently and attempt to parse. 224 | // std::cerr << "Recieved unknown message type!!!: " << std::to_string(member_info.type_id_) 225 | // << "\n"; 226 | break; 227 | } 228 | } 229 | 230 | // Convert an individual member's value from binary to string 231 | // TODO: Make bool, include failure cases 232 | void messageDataToString(const rosidl_typesupport_introspection_cpp::MessageMember& member_info, 233 | const uint8_t* member_data, std::string& message_data) 234 | { 235 | switch (member_info.type_id_) 236 | { 237 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT: 238 | message_data = std::to_string(*reinterpret_cast(member_data)); 239 | break; 240 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_DOUBLE: 241 | message_data = std::to_string(*reinterpret_cast(member_data)); 242 | break; 243 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_LONG_DOUBLE: 244 | message_data = std::to_string(*reinterpret_cast(member_data)); 245 | break; 246 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_CHAR: 247 | message_data = std::to_string(*reinterpret_cast(member_data)); 248 | break; 249 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_WCHAR: 250 | message_data = std::to_string(*reinterpret_cast(member_data)); 251 | break; 252 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_BOOLEAN: 253 | message_data = std::to_string(*reinterpret_cast(member_data)); 254 | break; 255 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_OCTET: 256 | message_data = std::to_string(*reinterpret_cast(member_data)); 257 | break; 258 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT8: 259 | message_data = std::to_string(*reinterpret_cast(member_data)); 260 | break; 261 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT8: 262 | message_data = std::to_string(*reinterpret_cast(member_data)); 263 | break; 264 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT16: 265 | message_data = std::to_string(*reinterpret_cast(member_data)); 266 | break; 267 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT16: 268 | message_data = std::to_string(*reinterpret_cast(member_data)); 269 | break; 270 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT32: 271 | message_data = std::to_string(*reinterpret_cast(member_data)); 272 | break; 273 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT32: 274 | message_data = std::to_string(*reinterpret_cast(member_data)); 275 | break; 276 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT64: 277 | message_data = std::to_string(*reinterpret_cast(member_data)); 278 | break; 279 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT64: 280 | message_data = std::to_string(*reinterpret_cast(member_data)); 281 | break; 282 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING: 283 | message_data = *reinterpret_cast(member_data); 284 | break; 285 | // TODO Implement Nested. Ignored for now 286 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE: 287 | // For nested types, don't copy the data out of the buffer directly. Recursively read the 288 | // nested type into the YAML. 289 | // RosMessage_Cpp nested_member; 290 | // nested_member.type_info = reinterpret_cast(member_info.members_->data); 291 | // nested_member.data = const_cast(member_data); 292 | // message_data = message_to_yaml(nested_member); 293 | break; 294 | default: 295 | // Recieved unkwn message type, fail silently and attempt to parse. 296 | std::cerr << "Recieved unknown message type!!!: " << std::to_string(member_info.type_id_) << "\n"; 297 | break; 298 | } 299 | } 300 | 301 | void messageDataToDouble(const rosidl_typesupport_introspection_cpp::MessageMember& member_info, 302 | const uint8_t* member_data, double& message_data) 303 | { 304 | switch (member_info.type_id_) 305 | { 306 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT: 307 | message_data = double(*reinterpret_cast(member_data)); 308 | break; 309 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_DOUBLE: 310 | message_data = (*reinterpret_cast(member_data)); 311 | break; 312 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_LONG_DOUBLE: 313 | message_data = (*reinterpret_cast(member_data)); 314 | break; 315 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_CHAR: 316 | message_data = double(*reinterpret_cast(member_data)); 317 | break; 318 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_WCHAR: 319 | message_data = double(*reinterpret_cast(member_data)); 320 | break; 321 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_BOOLEAN: 322 | message_data = double(*reinterpret_cast(member_data)); 323 | break; 324 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_OCTET: 325 | message_data = double(*reinterpret_cast(member_data)); 326 | break; 327 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT8: 328 | message_data = double(*reinterpret_cast(member_data)); 329 | break; 330 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT8: 331 | message_data = double(*reinterpret_cast(member_data)); 332 | break; 333 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT16: 334 | message_data = double(*reinterpret_cast(member_data)); 335 | break; 336 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT16: 337 | message_data = double(*reinterpret_cast(member_data)); 338 | break; 339 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT32: 340 | message_data = double(*reinterpret_cast(member_data)); 341 | break; 342 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT32: 343 | message_data = double(*reinterpret_cast(member_data)); 344 | break; 345 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT64: 346 | message_data = double(*reinterpret_cast(member_data)); 347 | break; 348 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT64: 349 | message_data = double(*reinterpret_cast(member_data)); 350 | break; 351 | default: 352 | // Recieved unkwn message type, fail silently and attempt to parse. 353 | break; 354 | } 355 | } 356 | 357 | void readMessageAsTreeString(std::vector& output, uint8_t* message_data, 358 | const rosidl_typesupport_introspection_cpp::MessageMembers* members, int indent) 359 | { 360 | const std::string indent_str(indent, ' '); 361 | for (size_t i = 0; i < members->member_count_; i++) 362 | { 363 | const rosidl_typesupport_introspection_cpp::MessageMember& member = members->members_[i]; 364 | uint8_t* member_data = &message_data[member.offset_]; 365 | 366 | // Perform a check for if we're dealing with a ros message type, and recurse if we are 367 | if (member.type_id_ == rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE) 368 | { 369 | const auto sub_members = 370 | static_cast(member.members_->data); 371 | readMessageAsTreeString(output, member_data, sub_members, indent + 2); 372 | } 373 | else 374 | { 375 | std::string t_string, msg_string; 376 | introspection::messageDataToString(member, member_data, t_string); 377 | if (indent != 0) 378 | { 379 | msg_string.append("\n"); 380 | } 381 | msg_string.append(indent_str + t_string + '\n'); 382 | output.push_back(msg_string); 383 | } 384 | } 385 | } 386 | 387 | std::string readMessageAsString(uint8_t* message_data, 388 | const rosidl_typesupport_introspection_cpp::MessageMembers* members) 389 | { 390 | std::string members_string; 391 | for (size_t i = 0; i < members->member_count_; i++) 392 | { 393 | std::string member_string; 394 | const rosidl_typesupport_introspection_cpp::MessageMember& member = members->members_[i]; 395 | uint8_t* member_data = &message_data[member.offset_]; 396 | 397 | // Perform a check for if we're dealing with a ros message type, and recurse if we are 398 | if (member.type_id_ == rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE) 399 | { 400 | const auto sub_members = 401 | static_cast(member.members_->data); 402 | member_string += readMessageAsString(member_data, sub_members); 403 | } 404 | else 405 | { 406 | introspection::messageDataToString(member, member_data, member_string); 407 | } 408 | members_string += member_string; 409 | } 410 | return members_string; 411 | } 412 | 413 | double readMessageAsDouble(uint8_t* message_data, const rosidl_typesupport_introspection_cpp::MessageMembers* members) 414 | { 415 | if (members->member_count_ > 1) 416 | { 417 | std::cerr << "Attempted to read a member with more than one submembers!"; 418 | return 0.0; 419 | } 420 | const rosidl_typesupport_introspection_cpp::MessageMember& member = members->members_[0]; 421 | uint8_t* member_data = &message_data[member.offset_]; 422 | double member_double; 423 | introspection::messageDataToDouble(member, member_data, member_double); 424 | return member_double; 425 | } 426 | 427 | void writeDataToMessage(uint8_t* message_data, const rosidl_typesupport_introspection_cpp::MessageMembers* members, 428 | std::vector& data) 429 | { 430 | // Iterate through message members, when reach member with no submembers, insert data, repeat. 431 | for (size_t i = 0; i < members->member_count_; i++) 432 | { 433 | const rosidl_typesupport_introspection_cpp::MessageMember& member = members->members_[i]; 434 | uint8_t* member_data = &message_data[member.offset_]; 435 | // Perform a check for if we're dealing with a ros message type, and recurse if we are 436 | if (member.type_id_ == rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE) 437 | { 438 | const auto sub_members = 439 | static_cast(member.members_->data); 440 | writeDataToMessage(member_data, sub_members, data); 441 | } 442 | else 443 | { 444 | stringToMessageData(message_data, member, data.front()); 445 | data.erase(data.begin()); 446 | } 447 | } 448 | } 449 | 450 | // Populates a mesage with data from string. Looks for data between : and \n. Used for the responses from the service 451 | // monitor 452 | bool populateMessage(uint8_t* message_data, const rosidl_typesupport_introspection_cpp::MessageMembers* members, 453 | const std::string& data) 454 | { 455 | std::vector data_strings; 456 | helper_functions::getDataFromRequestString(data_strings, data); 457 | 458 | writeDataToMessage(message_data, members, data_strings); 459 | 460 | return true; 461 | } 462 | 463 | const std::string sanitiseNumericData(const std::string &data) 464 | { 465 | // Ensure String is not empty, 466 | // And if it isn't, ensure it's a valid number 467 | const auto is_numeric = data.find_first_not_of("-.0123456789"); 468 | if (data.empty() || !is_numeric ) 469 | { 470 | return "0"; 471 | } 472 | else 473 | { 474 | return data; 475 | } 476 | } 477 | 478 | void stringToMessageData(uint8_t* message_data, const rosidl_typesupport_introspection_cpp::MessageMember& member_info, 479 | std::string& data) 480 | { 481 | switch (member_info.type_id_) 482 | { 483 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT: 484 | data = sanitiseNumericData(data); 485 | *reinterpret_cast(message_data + member_info.offset_) = std::stof(data); 486 | break; 487 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_DOUBLE: 488 | data = sanitiseNumericData(data); 489 | *reinterpret_cast(message_data + member_info.offset_) = std::stod(data); 490 | break; 491 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_LONG_DOUBLE: 492 | data = sanitiseNumericData(data); 493 | *reinterpret_cast(message_data + member_info.offset_) = std::stold(data); 494 | break; 495 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_CHAR: 496 | *reinterpret_cast(message_data + member_info.offset_) = data.at(0); 497 | break; 498 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_WCHAR: 499 | *reinterpret_cast(message_data + member_info.offset_) = data.at(0); 500 | break; 501 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_BOOLEAN: 502 | data = sanitiseNumericData(data); 503 | *reinterpret_cast(message_data + member_info.offset_) = 504 | (strcasecmp(data.c_str(), "true") == 0 || atoi(data.c_str()) != 0); 505 | break; 506 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_OCTET: 507 | // FIXME: Write this 508 | break; 509 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT8: 510 | data = sanitiseNumericData(data); 511 | *reinterpret_cast(message_data + member_info.offset_) = static_cast(std::stoul(data)); 512 | break; 513 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT8: 514 | data = sanitiseNumericData(data); 515 | *reinterpret_cast(message_data + member_info.offset_) = static_cast(std::stoi(data)); 516 | break; 517 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT16: 518 | data = sanitiseNumericData(data); 519 | *reinterpret_cast(message_data + member_info.offset_) = static_cast(std::stoul(data)); 520 | break; 521 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT16: 522 | data = sanitiseNumericData(data); 523 | *reinterpret_cast(message_data + member_info.offset_) = static_cast(std::stoi(data)); 524 | break; 525 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT32: 526 | data = sanitiseNumericData(data); 527 | *reinterpret_cast(message_data + member_info.offset_) = static_cast(std::stoul(data)); 528 | break; 529 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT32: 530 | data = sanitiseNumericData(data); 531 | *reinterpret_cast(message_data + member_info.offset_) = stoi(data); 532 | break; 533 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT64: 534 | data = sanitiseNumericData(data); 535 | *reinterpret_cast(message_data + member_info.offset_) = stoull(data); 536 | break; 537 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT64: 538 | data = sanitiseNumericData(data); 539 | *reinterpret_cast(message_data + member_info.offset_) = static_cast(std::stoll(data)); 540 | break; 541 | case rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING: 542 | *reinterpret_cast(message_data + member_info.offset_) = data; 543 | break; 544 | default: 545 | // Recieved unkwn message type, fail silently and attempt to parse. 546 | std::cerr << "Recieved unknown message type!!!: " << std::to_string(member_info.type_id_) << "\n"; 547 | break; 548 | } 549 | } 550 | 551 | // Returns the path to an entry. First n-1 elements are the addresses of the members, nth is the offset in the member 552 | // data 553 | std::vector getEntryOffset(std::vector entry_path, const std::string& member_type_id, 554 | const rosidl_typesupport_introspection_cpp::MessageMembers* message_members) 555 | { 556 | std::vector offset_vector; 557 | // If the length of entry_path > 1, we require further recursion 558 | if (entry_path.size() > 1) 559 | { 560 | for (size_t i = 0; i < message_members->member_count_; i++) 561 | { 562 | const rosidl_typesupport_introspection_cpp::MessageMember& member = message_members->members_[i]; 563 | // If the member has the same name as the desired entry, add to offset vector and recurse 564 | if (member.name_ == entry_path[0]) 565 | { 566 | entry_path.erase(entry_path.begin()); 567 | offset_vector.push_back(i); 568 | const auto sub_members = 569 | static_cast(member.members_->data); 570 | 571 | auto tmp_offset_vector(getEntryOffset(entry_path, member_type_id, sub_members)); 572 | if (!tmp_offset_vector.empty()) 573 | { 574 | offset_vector.insert(offset_vector.end(), tmp_offset_vector.begin(), tmp_offset_vector.end()); 575 | return offset_vector; 576 | } 577 | } 578 | } 579 | } 580 | // Else, we're searching for matching final member 581 | else 582 | { 583 | for (size_t i = 0; i < message_members->member_count_; i++) 584 | { 585 | const rosidl_typesupport_introspection_cpp::MessageMember& member = message_members->members_[i]; 586 | // If the member has the same name as the desired entry, add to offset vector and recurse 587 | if (member.name_ == entry_path[0]) 588 | { 589 | std::string member_type; 590 | introspection::messageTypeToString(member, member_type); 591 | // Check the type id 592 | if (member_type == member_type_id) 593 | { 594 | entry_path.erase(entry_path.begin()); 595 | offset_vector.push_back(i); 596 | return offset_vector; 597 | } 598 | } 599 | } 600 | } 601 | 602 | // return a blank vector if it cannot be found; 603 | std::vector empty_vec; 604 | return empty_vec; 605 | } 606 | 607 | void getMessageMember(const std::vector& offsets, 608 | const rosidl_typesupport_introspection_cpp::MessageMembers* message_members, 609 | rosidl_typesupport_introspection_cpp::MessageMember& found_member) 610 | { 611 | if (offsets.size() == 1) 612 | { 613 | found_member = message_members->members_[offsets[0]]; 614 | return; 615 | } 616 | 617 | auto get_member_at_offset = [&](auto&& get_member_at_offset, uint32_t& idx, const std::vector& offset_vec, 618 | const rosidl_typesupport_introspection_cpp::MessageMember& message_member) { 619 | if (idx == offset_vec.size()) 620 | { 621 | return message_member; 622 | } 623 | auto sub_members = 624 | static_cast(message_member.members_->data); 625 | const rosidl_typesupport_introspection_cpp::MessageMember& next_iter = sub_members->members_[offset_vec[idx]]; 626 | idx++; 627 | return get_member_at_offset(get_member_at_offset, idx, offset_vec, next_iter); 628 | }; 629 | 630 | uint32_t idx = 1; 631 | found_member = get_member_at_offset(get_member_at_offset, idx, offsets, message_members->members_[offsets[0]]); 632 | } 633 | 634 | void getMessageMember(const std::vector& offsets, 635 | const rosidl_typesupport_introspection_cpp::MessageMembers* message_members, uint8_t* data, 636 | rosidl_typesupport_introspection_cpp::MessageMember& found_member, uint8_t** found_data) 637 | { 638 | if (offsets.size() == 1) 639 | { 640 | found_member = message_members->members_[offsets[0]]; 641 | *found_data = &data[found_member.offset_]; 642 | return; 643 | } 644 | 645 | auto get_member_at_offset = [&](auto&& get_member_at_offset, uint32_t& idx, const std::vector& offset_vec, 646 | const rosidl_typesupport_introspection_cpp::MessageMember& message_member, 647 | uint8_t* msg_data) { 648 | if (idx == offset_vec.size()) 649 | { 650 | return std::make_pair(message_member, msg_data); 651 | } 652 | auto sub_members = 653 | static_cast(message_member.members_->data); 654 | const rosidl_typesupport_introspection_cpp::MessageMember& next_iter = sub_members->members_[offset_vec[idx]]; 655 | uint8_t* next_iter_data = &msg_data[next_iter.offset_]; 656 | idx++; 657 | return get_member_at_offset(get_member_at_offset, idx, offset_vec, next_iter, next_iter_data); 658 | }; 659 | 660 | uint32_t idx = 1; 661 | auto member_data_pair = 662 | get_member_at_offset(get_member_at_offset, idx, offsets, message_members->members_[offsets[0]], 663 | &data[message_members->members_[offsets[0]].offset_]); 664 | found_member = member_data_pair.first; 665 | *found_data = member_data_pair.second; 666 | } 667 | 668 | bool parsableAsNumeric(const rosidl_typesupport_introspection_cpp::MessageMember& msg_info) 669 | { 670 | // Message range check. Relies on the introspection field types values 671 | return ((msg_info.type_id_ >= 1 && msg_info.type_id_ <= 3) || (msg_info.type_id_ >= 8 && msg_info.type_id_ <= 15)); 672 | } 673 | 674 | } // namespace introspection 675 | --------------------------------------------------------------------------------