custom twist mux implementation

urc_bringup/launch/bringup_simulation.launch.py

@@ -121,12 +121,9 @@ def generate_launch_description():
     )
 
     twist_mux_node = Node(
-        package="twist_mux",
-        executable="twist_mux",
+        package="urc_platform",
+        executable="urc_platform_TwistMux",
         name="twist_mux",
-        output="screen",
-        parameters=[twist_mux_config],
-        remappings=[("/cmd_vel_out", "/rover_drivetrain_controller/cmd_vel")],
     )
 
     # ekf_launch = IncludeLaunchDescription(

urc_platform/CMakeLists.txt

@@ -19,6 +19,7 @@ include_directories(
 # Library creation
 add_library(${PROJECT_NAME} SHARED
   src/joystick_driver.cpp
+  src/twist_mux.cpp
 )
 
 set(dependencies
@@ -41,6 +42,12 @@ rclcpp_components_register_node(
   EXECUTABLE ${PROJECT_NAME}_JoystickDriver
 )
 
+rclcpp_components_register_node(
+  ${PROJECT_NAME}
+  PLUGIN "twist_mux::TwistMux"
+  EXECUTABLE ${PROJECT_NAME}_TwistMux
+)
+
 # Install launch files.
 install(
   DIRECTORY

urc_platform/include/twist_mux.hpp

@@ -0,0 +1,36 @@
+#ifndef TWIST_MUX
+#define TWIST_MUX
+
+#include "geometry_msgs/msg/twist_stamped.hpp"
+#include "std_msgs/msg/bool.hpp"
+#include "std_msgs/msg/string.hpp"
+#include <rclcpp/publisher.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_components/register_node_macro.hpp>
+
+namespace twist_mux
+{
+class TwistMux : public rclcpp::Node
+{
+public:
+  explicit TwistMux(const rclcpp::NodeOptions & options);
+
+private:
+  rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr
+    autonomous_subscriber;
+  rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr
+    teleop_subscriber;
+
+  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr enabled_subscriber;
+
+  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr mode_subscriber;
+
+  std::shared_ptr<rclcpp::Publisher<geometry_msgs::msg::TwistStamped>>
+  cmd_publisher;
+
+  bool enabled;
+  std::string mode;
+};
+} // namespace twist_mux
+
+#endif

urc_platform/src/twist_mux.cpp

@@ -0,0 +1,57 @@
+#include "twist_mux.hpp"
+#include <rclcpp/logging.hpp>
+
+namespace twist_mux
+{
+TwistMux::TwistMux(const rclcpp::NodeOptions & options)
+: Node("twist_mux", options)
+{
+  declare_parameter("autonomous_cmd_vel_topic", "/cmd_vel_autonomous");
+  declare_parameter("teleop_cmd_vel_topic", "/cmd_vel_teleop");
+  declare_parameter("enabled_topic", "/cmd_vel_enabled");
+  declare_parameter("mode_topic", "/cmd_vel_mode");
+  declare_parameter("cmd_out_topic", "/rover_drivetrain_controller/cmd_vel");
+
+  enabled = true;
+  mode = "autonomous";
+
+  cmd_publisher = create_publisher<geometry_msgs::msg::TwistStamped>(
+    get_parameter("cmd_out_topic").as_string(), rclcpp::SystemDefaultsQoS());
+
+  autonomous_subscriber = create_subscription<geometry_msgs::msg::TwistStamped>(
+    get_parameter("autonomous_cmd_vel_topic").as_string(),
+    rclcpp::SystemDefaultsQoS(),
+    [this](const geometry_msgs::msg::TwistStamped::SharedPtr msg) {
+      if (enabled && mode == "autonomous") {
+        cmd_publisher->publish(*msg);
+      }
+    });
+
+  teleop_subscriber = create_subscription<geometry_msgs::msg::TwistStamped>(
+    get_parameter("teleop_cmd_vel_topic").as_string(),
+    rclcpp::SystemDefaultsQoS(),
+    [this](const geometry_msgs::msg::TwistStamped::SharedPtr msg) {
+      if (enabled && mode == "teleop") {
+        cmd_publisher->publish(*msg);
+      }
+    });
+
+  enabled_subscriber = create_subscription<std_msgs::msg::Bool>(
+    get_parameter("enabled_topic").as_string(), rclcpp::SystemDefaultsQoS(),
+    [this](const std_msgs::msg::Bool::SharedPtr msg) {
+      enabled = msg->data;
+      if (!enabled) {
+        geometry_msgs::msg::TwistStamped msg;
+        msg.header.stamp = now();
+        cmd_publisher->publish(msg);
+      }
+      // RCLCPP_WARN(get_logger(), "Enabled: %d", enabled);
+    });
+
+  mode_subscriber = create_subscription<std_msgs::msg::String>(
+    get_parameter("mode_topic").as_string(), rclcpp::SystemDefaultsQoS(),
+    [this](const std_msgs::msg::String::SharedPtr msg) {mode = msg->data;});
+}
+} // namespace twist_mux
+
+RCLCPP_COMPONENTS_REGISTER_NODE(twist_mux::TwistMux)