From 42ef189aab0699afc121e7594944fb68b303a04b Mon Sep 17 00:00:00 2001
From: Alexander Entinger <consulting@lxrobotics.com>
Date: Mon, 29 Jan 2024 07:11:23 +0100
Subject: [PATCH] Fix all those bugs in the implementation.

---
 include/viper/Node.h |  4 ++--
 src/Node.cpp         | 14 ++++----------
 2 files changed, 6 insertions(+), 12 deletions(-)

diff --git a/include/viper/Node.h b/include/viper/Node.h
index fa9c1c2..45012f8 100644
--- a/include/viper/Node.h
+++ b/include/viper/Node.h
@@ -82,8 +82,8 @@ class Node : public rclcpp::Node
 
 /************** code************/
  
-static uint16_t constexpr setpoint_velocity_ID = 113;
- cyphal::Publisher<zubax::primitive::real16::Vector31> setpoint_velocity_ID;
+  static uint16_t constexpr SETPOINT_VELOCITY_ID = 113;
+  cyphal::Publisher<zubax::primitive::real16::Vector31_1_0> _setpoint_velocity_pub;
 
   static std::chrono::milliseconds constexpr CTRL_LOOP_RATE{10};
   rclcpp::TimerBase::SharedPtr _ctrl_loop_timer;
diff --git a/src/Node.cpp b/src/Node.cpp
index 66b32ae..e980e20 100644
--- a/src/Node.cpp
+++ b/src/Node.cpp
@@ -81,10 +81,9 @@ Node::Node()
 
   _ctrl_loop_timer = create_wall_timer(CTRL_LOOP_RATE, [this]() { this->ctrl_loop(); });
 
-  _cyphal_demo_pub = _node_hdl.create_publisher<uavcan::primitive::real16::Integer8_1_0>(CYPHAL_DEMO_PORT_ID, 1*1000*1000UL);
-
- setpoint_velocity_ID= _node_hdl.create_subscription<zubax::primitive::real16::Vector31>(setpoint_velocity_ID,1*1000*1000UL)
+  _cyphal_demo_pub = _node_hdl.create_publisher<uavcan::primitive::scalar::Integer8_1_0>(CYPHAL_DEMO_PORT_ID, 1*1000*1000UL);
 
+  _setpoint_velocity_pub = _node_hdl.create_publisher<zubax::primitive::real16::Vector31_1_0>(SETPOINT_VELOCITY_ID, 1*1000*1000UL);
 
   RCLCPP_INFO(get_logger(), "%s init complete.", get_name());
 }
@@ -225,13 +224,8 @@ void Node::ctrl_loop()
   _cyphal_demo_pub->publish(demo_msg);
   demo_cnt++;
 
-setpoint_velocity_ID = _node_hdl.create_publisher<zubax::primitive::real16::Vector31>(setpoint_velocity_ID, 1*1000*1000UL)
-float motor_values[4] = {100.0, 100.0, 100.0, 100.0};
-zubax::primitive::real16::Vector31 const motor_msg{motor_values};
-setpoint_velocity_ID->publish(motor_msg);
-
-
-
+  zubax::primitive::real16::Vector31_1_0 const motor_msg{100.0, 100.0, 100.0, 100.0};
+  _setpoint_velocity_pub->publish(motor_msg);
 }
 
 /**************************************************************************************