zero cmd vel node

urc_platform/CMakeLists.txt

@@ -19,6 +19,7 @@ include_directories(
 # Library creation
 add_library(${PROJECT_NAME} SHARED
   src/joystick_driver.cpp
+  src/zero_cmd_vel_node.cpp
 )
 
 set(dependencies
@@ -41,6 +42,12 @@ rclcpp_components_register_node(
   EXECUTABLE ${PROJECT_NAME}_JoystickDriver
 )
 
+rclcpp_components_register_node(
+  ${PROJECT_NAME}
+  PLUGIN "zero_cmd_vel_node::ZeroCmdVelNode"
+  EXECUTABLE ${PROJECT_NAME}_ZeroCmdVelNode
+)
+
 # Install launch files.
 install(
   DIRECTORY
@@ -76,4 +83,4 @@ ament_export_include_directories(include)
 ament_export_libraries(${PROJECT_NAME})
 ament_export_dependencies(${dependencies})
 
-ament_package()
+ament_package()

urc_platform/include/zero_cmd_vel_node.hpp

@@ -0,0 +1,22 @@
+#ifndef JOYSTICK_DRIVER_H
+#define JOYSTICK_DRIVER_H
+
+#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <rclcpp/publisher.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_components/register_node_macro.hpp>
+
+namespace zero_cmd_vel_node {
+class ZeroCmdVelNode : public rclcpp::Node {
+public:
+  explicit ZeroCmdVelNode(const rclcpp::NodeOptions &options);
+
+private:
+  void publish_zero_cmd_vel();
+  std::shared_ptr<rclcpp::Publisher<geometry_msgs::msg::TwistStamped>>
+      drivetrain_cmd_publisher;
+  rclcpp::TimerBase::SharedPtr timer_;
+};
+} // namespace zero_cmd_vel_node
+
+#endif

urc_platform/src/zero_cmd_vel_node.cpp

@@ -0,0 +1,26 @@
+#include "zero_cmd_vel_node.hpp"
+
+namespace zero_cmd_vel_node {
+
+ZeroCmdVelNode::ZeroCmdVelNode(const rclcpp::NodeOptions &options)
+    : rclcpp::Node("zero_cmd_vel_node", options) {
+  declare_parameter("zero_cmd_vel_topic", "/zero_cmd_vel");
+  drivetrain_cmd_publisher = create_publisher<geometry_msgs::msg::TwistStamped>(
+      get_parameter("zero_cmd_vel_topic").as_string(),
+      rclcpp::SystemDefaultsQoS());
+
+  timer_ = this->create_wall_timer(
+      std::chrono::milliseconds(100),
+      std::bind(&ZeroCmdVelNode::publish_zero_cmd_vel, this));
+}
+
+void ZeroCmdVelNode::publish_zero_cmd_vel() {
+  geometry_msgs::msg::TwistStamped zero_cmd_vel;
+  zero_cmd_vel.twist.linear.x = 0.0;
+  zero_cmd_vel.twist.angular.z = 0.0;
+  drivetrain_cmd_publisher->publish(zero_cmd_vel);
+
+} // namespace zero_cmd_vel_node
+} // namespace zero_cmd_vel_node
+
+RCLCPP_COMPONENTS_REGISTER_NODE(zero_cmd_vel_node::ZeroCmdVelNode);