Add demux (#106)

* Copy, rename and make buildable Signed-off-by: Rufus Wong <[email protected]> * Swap input and output topic Signed-off-by: Rufus Wong <[email protected]> * Reset publisher if output topic changes Signed-off-by: Rufus Wong <[email protected]> * Add tests Signed-off-by: Rufus Wong <[email protected]> * Fix test failures Signed-off-by: Rufus Wong <[email protected]> * Fix header guard Signed-off-by: Rufus Wong <[email protected]> * Make linter happy Signed-off-by: Rufus Wong <[email protected]> * Add README entry Signed-off-by: Rufus Wong <[email protected]> * Remove lazy parameter Signed-off-by: Rufus Wong <[email protected]> * Rename default input topic to ~/input Signed-off-by: Rufus Wong <[email protected]> * Fix typo Signed-off-by: Rufus Wong <[email protected]> * Remove usage of ament_cmake_auto Signed-off-by: Rufus Wong <[email protected]> * Add missing dependency on ament_cmake Signed-off-by: Rufus Wong <[email protected]> * Fix uncrustify warnings Signed-off-by: Rufus Wong <[email protected]> * Add initial_topic_ variable to avoid confusion This addresses comment #106 (comment) Signed-off-by: Rufus Wong <[email protected]> * Fix outtopic spelling This addresses comment #106 (comment) Signed-off-by: Rufus Wong <[email protected]> * Add error message on incorrect number of arguments Addresses #106 (comment) Signed-off-by: Rufus Wong <[email protected]> * Formatting Signed-off-by: Rufus Wong <[email protected]> --------- Signed-off-by: Rufus Wong <[email protected]>
ros-tooling · Jul 9, 2024 · ad48bc0 · ad48bc0
1 parent a54c290
commit ad48bc0
Show file tree

Hide file tree

Showing 17 changed files with 604 additions and 45 deletions.
diff --git a/README.md b/README.md
@@ -14,6 +14,7 @@ The tools in this package are provided as composable ROS 2 component nodes, so t
 - [Throttle](#throttle): Republishes data with bandwidth or rate limit.
 - [Drop](#drop): Republishes by dropping X out of every Y incoming messages.
 - [Mux](#mux): Multiplexes incoming topics to an output.
+- [Demux](#demux): Demultiplexes an incoming topic to one of multiple outputs
 - [Delay](#delay): Delays and republishes incoming data.
 
 ### Relay
@@ -184,10 +185,10 @@ and to add and delete input topics. At startup, the first input topic on the com
 #### Usage
 
 ```shell
-ros2 run topic_tools mux <outopic> <intopic1> [intopic2...]
+ros2 run topic_tools mux <outtopic> <intopic1> [intopic2...]
 ```
 
-Subscribe to <intopic1>...N and publish currently selected topic to outopic. mux will start with <intopic1> selected.
+Subscribe to <intopic1>...N and publish currently selected topic to outtopic. mux will start with <intopic1> selected.
 - `outtopic`: Outgoing topic to publish on
 - `intopicN`: Incoming topic to subscribe to
 
@@ -208,6 +209,37 @@ ros2 run topic_tools mux sel_cmdvel auto_cmdvel joystick_cmdvel
 - `initial_topic` (str, default="")
     - Input topic to select on startup. If `__none`, start with no input topic. If unset, default to first topic in arguments
 
+### Demux
+
+demux is a ROS2 node that subscribes to an incoming topic and republishes incoming data to one of many topic,
+i.e., it's a demultiplexer that switches an input towards 1 of N outputs. Services are offered to switch among output topics,
+and to add and delete output topics. At startup, the first output topic on the command line is selected.
+
+#### Usage
+
+```shell
+ros2 run topic_tools demux <intopic> <outtopic1> [outtopic2...]
+```
+
+Subscribe to <intopic1> and publish currently to selected outtopic among <outtopic1>...N. demux will start with <outtopic1> selected.
+- `intopic`: Incoming topic to subscribe to
+- `outtopicN`: Outgoing topic to publish on
+
+E.g. demux one command stream (cmdvel) between two command streams (turtle1_cmdvel and turtle2_cmdvel):
+
+```shell
+ros2 run topic_tools demux cmdvel turtle1_cmdvel turtle2_cmdvel
+```
+
+#### Parameters
+
+- `input_topic` (string, default=`~/input`)
+    - the same as if provided as a command line argument
+- `output_topics` (string array)
+    - the same as if provided as a command line argument
+- `initial_topic` (str, default="")
+    - Output topic to select on startup. If `__none`, start with no output topic. If unset, default to first topic in arguments
+
 ### Delay
 
 Delay is a ROS 2 node that can subscribe to a topic and republish incoming data to another topic, delaying the message by a fixed duration.

diff --git a/topic_tools/CMakeLists.txt b/topic_tools/CMakeLists.txt
@@ -1,6 +1,11 @@
 cmake_minimum_required(VERSION 3.8)
 project(topic_tools)
 
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(topic_tools_interfaces REQUIRED)
+
 if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 17)
 endif()
@@ -9,90 +14,103 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-# find dependencies
-find_package(ament_cmake_auto REQUIRED)
-ament_auto_find_build_dependencies()
-
-ament_python_install_package(${PROJECT_NAME}
-  SETUP_CFG
-    ${PROJECT_NAME}/setup.cfg
-  SCRIPTS_DESTINATION
-    lib/${PROJECT_NAME}
-)
+include_directories(include)
 
-ament_auto_add_library(relay_node SHARED
+add_library(relay_node SHARED
   src/relay_node.cpp
   src/tool_base_node.cpp
 )
+ament_target_dependencies(relay_node rclcpp rclcpp_components)
 target_compile_definitions(relay_node PRIVATE "TOPIC_TOOLS_BUILDING_LIBRARY")
 
 rclcpp_components_register_nodes(relay_node "topic_tools::RelayNode")
 
-ament_auto_add_executable(relay
+add_executable(relay
   src/relay.cpp
 )
 
 target_link_libraries(relay
   relay_node
 )
 
-ament_auto_add_library(throttle_node SHARED
+add_library(throttle_node SHARED
   src/throttle_node.cpp
   src/tool_base_node.cpp
 )
+ament_target_dependencies(throttle_node rclcpp rclcpp_components)
 target_compile_definitions(throttle_node PRIVATE "TOPIC_TOOLS_BUILDING_LIBRARY")
 
 rclcpp_components_register_nodes(throttle_node "topic_tools::ThrottleNode")
 
-ament_auto_add_executable(throttle
+add_executable(throttle
   src/throttle.cpp
 )
 
 target_link_libraries(throttle
   throttle_node
 )
 
-ament_auto_add_library(drop_node SHARED
+add_library(drop_node SHARED
   src/drop_node.cpp
   src/tool_base_node.cpp
 )
+ament_target_dependencies(drop_node rclcpp rclcpp_components)
 target_compile_definitions(drop_node PRIVATE "TOPIC_TOOLS_BUILDING_LIBRARY")
 
 rclcpp_components_register_nodes(drop_node "topic_tools::DropNode")
 
-ament_auto_add_executable(drop
+add_executable(drop
   src/drop.cpp
 )
 
 target_link_libraries(drop
   drop_node
 )
 
-ament_auto_add_library(mux_node SHARED
+add_library(mux_node SHARED
   src/mux_node.cpp
   src/tool_base_node.cpp
 )
+ament_target_dependencies(mux_node rclcpp rclcpp_components topic_tools_interfaces)
 target_compile_definitions(mux_node PRIVATE "TOPIC_TOOLS_BUILDING_LIBRARY")
 
 rclcpp_components_register_nodes(mux_node "topic_tools::MuxNode")
 
-ament_auto_add_executable(mux
+add_executable(mux
   src/mux.cpp
 )
 
 target_link_libraries(mux
   mux_node
 )
 
-ament_auto_add_library(delay_node SHARED
+add_library(demux_node SHARED
+  src/demux_node.cpp
+  src/tool_base_node.cpp
+)
+ament_target_dependencies(demux_node rclcpp rclcpp_components topic_tools_interfaces)
+target_compile_definitions(demux_node PRIVATE "TOPIC_TOOLS_BUILDING_LIBRARY")
+
+rclcpp_components_register_nodes(demux_node "topic_tools::DemuxNode")
+
+add_executable(demux
+  src/demux.cpp
+)
+
+target_link_libraries(demux
+  demux_node
+)
+
+add_library(delay_node SHARED
   src/delay_node.cpp
   src/tool_base_node.cpp
 )
+ament_target_dependencies(delay_node rclcpp rclcpp_components)
 target_compile_definitions(delay_node PRIVATE "TOPIC_TOOLS_BUILDING_LIBRARY")
 
 rclcpp_components_register_nodes(delay_node "topic_tools::DelayNode")
 
-ament_auto_add_executable(delay
+add_executable(delay
   src/delay.cpp
 )
 
@@ -151,9 +169,26 @@ if(BUILD_TESTING)
     rclcpp::rclcpp
     ${std_msgs_TARGETS}
   )
+
+  ament_add_gtest(test_demux test/test_demux.cpp)
+  target_include_directories(test_demux PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+  )
+
+  target_link_libraries(test_demux
+    demux_node
+    rclcpp::rclcpp
+    ${std_msgs_TARGETS}
+  )
 endif()
 
-ament_auto_package(
-  INSTALL_TO_SHARE
-    launch
+install(TARGETS relay_node relay throttle_node throttle drop_node drop mux_node mux demux_node demux delay_node delay
+  DESTINATION lib/${PROJECT_NAME}
 )
+
+install(DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()
diff --git a/topic_tools/include/topic_tools/demux_node.hpp b/topic_tools/include/topic_tools/demux_node.hpp
@@ -0,0 +1,65 @@
+// Copyright 2024 Rufus Wong
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef TOPIC_TOOLS__DEMUX_NODE_HPP_
+#define TOPIC_TOOLS__DEMUX_NODE_HPP_
+
+#include <memory>
+#include <optional>  // NOLINT : https://github.com/ament/ament_lint/pull/324
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "rclcpp/rclcpp.hpp"
+#include "topic_tools/tool_base_node.hpp"
+#include "topic_tools_interfaces/srv/demux_add.hpp"
+#include "topic_tools_interfaces/srv/demux_delete.hpp"
+#include "topic_tools_interfaces/srv/demux_list.hpp"
+#include "topic_tools_interfaces/srv/demux_select.hpp"
+
+namespace topic_tools
+{
+static const char NONE_TOPIC[] = "__none";
+
+class DemuxNode final : public ToolBaseNode
+{
+public:
+  TOPIC_TOOLS_PUBLIC
+  explicit DemuxNode(const rclcpp::NodeOptions & options);
+
+private:
+  void process_message(std::shared_ptr<rclcpp::SerializedMessage> msg) override;
+  void make_subscribe_unsubscribe_decisions() override;
+  void on_demux_add(
+    const std::shared_ptr<topic_tools_interfaces::srv::DemuxAdd::Request> request,
+    std::shared_ptr<topic_tools_interfaces::srv::DemuxAdd::Response> response);
+  void on_demux_delete(
+    const std::shared_ptr<topic_tools_interfaces::srv::DemuxDelete::Request> request,
+    std::shared_ptr<topic_tools_interfaces::srv::DemuxDelete::Response> response);
+  void on_demux_list(
+    const std::shared_ptr<topic_tools_interfaces::srv::DemuxList::Request> request,
+    std::shared_ptr<topic_tools_interfaces::srv::DemuxList::Response> response);
+  void on_demux_select(
+    const std::shared_ptr<topic_tools_interfaces::srv::DemuxSelect::Request> request,
+    std::shared_ptr<topic_tools_interfaces::srv::DemuxSelect::Response> response);
+
+  std::vector<std::string> output_topics_;
+  rclcpp::Service<topic_tools_interfaces::srv::DemuxAdd>::SharedPtr demux_add_srv_;
+  rclcpp::Service<topic_tools_interfaces::srv::DemuxDelete>::SharedPtr demux_delete_srv_;
+  rclcpp::Service<topic_tools_interfaces::srv::DemuxList>::SharedPtr demux_list_srv_;
+  rclcpp::Service<topic_tools_interfaces::srv::DemuxSelect>::SharedPtr demux_select_srv_;
+};
+}  // namespace topic_tools
+
+#endif  // TOPIC_TOOLS__DEMUX_NODE_HPP_
diff --git a/topic_tools/package.xml b/topic_tools/package.xml
@@ -11,7 +11,7 @@
   <maintainer email="[email protected]">ROS Tooling Working Group</maintainer>
   <license>Apache License 2.0</license>
 
-  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>ament_cmake</buildtool_depend>
   <buildtool_depend>ament_cmake_python</buildtool_depend>
   <buildtool_depend>rosidl_default_generators</buildtool_depend>
 

diff --git a/topic_tools/src/demux.cpp b/topic_tools/src/demux.cpp
@@ -0,0 +1,45 @@
+// Copyright 2024 Rufus Wong
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "rclcpp/rclcpp.hpp"
+#include "topic_tools/demux_node.hpp"
+
+int main(int argc, char * argv[])
+{
+  auto args = rclcpp::init_and_remove_ros_arguments(argc, argv);
+  auto options = rclcpp::NodeOptions{};
+
+  if (args.size() < 3) {
+    RCLCPP_ERROR(
+        rclcpp::get_logger("demux"),
+        "Incorect number of arguments. "
+        "Usage: "
+        "ros2 run topic_tools demux <intopic> <outtopic1> [outtopic2...]");
+    return 1;
+  }
+
+  options.append_parameter_override("input_topic", args.at(1));
+  options.append_parameter_override(
+      "output_topics", std::vector<std::string>{args.begin() + 2, args.end()});
+
+  auto node = std::make_shared<topic_tools::DemuxNode>(options);
+
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+  return 0;
+}