stable

src/microros/microros_clock.hpp

@@ -1,4 +0,0 @@
-#pragma once
-#include <rcl/time.h>
-
-static rcl_clock_t clock;

src/microros/microros_state_machine.cpp

@@ -1,6 +1,5 @@
 #include "microros_state_machine.hpp"
 
-#include "microros_clock.hpp"
 #include "led_controller.hpp"
 #include "servo_controller.hpp"
 
@@ -42,10 +41,6 @@ void micro_ros_state_machine() {
                 k_msleep(1000);
                 break;
             }
-            if (rcl_clock_init(RCL_ROS_TIME, &clock, &allocator) != RCL_RET_OK) {
-                printf("Failed to initialize clock\n");
-                return;
-            }
             if (rclc_executor_init(&executor, &support.context, 8, &allocator) != RCL_RET_OK){
                 printk("Can't init executor!\r\n");
                 break;
@@ -74,6 +69,7 @@ void micro_ros_state_machine() {
             }
             micro_ros_state = ROS_AGENT_CONNECTION_CONNECTED;
             printk("micro ros agent reachable!\r\n");
+            rmw_uros_sync_session(100);
 
         case ROS_AGENT_CONNECTION_CONNECTED:
             if (rclc_executor_spin_some(&executor, 100) != RCL_RET_OK) {

src/servo_controller/servo_controller.cpp

@@ -1,18 +1,20 @@
 #include "servo_controller.hpp"
 #include "devices.hpp"
 #include "rcl_util.hpp"
-#include "microros_clock.hpp"
 
 #include <std_msgs/msg/float32.h>
-#include <geometry_msgs/msg/transform_stamped.h>
 #include <zephyr/drivers/counter.h>
 #include <zephyr/kernel.h>
+#include <rmw_microros/rmw_microros.h>
+
 
 const uint32_t min_pulse_ns = 500000;
 const uint32_t max_pulse_ns = 2500000;
 
 rcl_node_t ServoController::node;
-rcl_subscription_t ServoController::servo0_subscription;
+rcl_subscription_t ServoController::servo0_sub_pos;
+rcl_subscription_t ServoController::servo0_sub_maxv;
+rcl_subscription_t ServoController::servo0_sub_maxa;
 rcl_publisher_t ServoController::servo0_publisher;
 float ServoController::current_angle = 90.0f;
 float ServoController::target_angle = 90.0f;
@@ -20,6 +22,8 @@ uint32_t ServoController::last_step = 0;
 float ServoController::current_velocity = 0.0f;
 float ServoController::last_error = 0.0f;
 float ServoController::integral = 0.0f;
+float ServoController::max_velocity = 20.0f;
+float ServoController::max_accel = 35.0f;
 
 struct counter_alarm_cfg alarm_cfg;
 
@@ -29,6 +33,14 @@ void servo0_set_target_callback(const void *msg){
     ServoController::target_angle = (float)((std_msgs__msg__Float32 *)msg)->data;
 }
 
+void servo0_set_maxv_callback(const void *msg){
+    ServoController::max_velocity = (float)((std_msgs__msg__Float32 *)msg)->data;
+}
+
+void servo0_set_maxa_callback(const void *msg){
+    ServoController::max_accel = (float)((std_msgs__msg__Float32 *)msg)->data;
+}
+
 void servo_pid_work_cb(struct k_work *work)
 {
     ServoController::servo_pid();
@@ -54,24 +66,51 @@ rcl_ret_t ServoController::setup(rclc_support_t* support, rclc_executor_t* execu
     RCL_RETURN_ON_ERROR(rclc_publisher_init_default(
         &ServoController::servo0_publisher,                          
         &ServoController::node,                                     
-        ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, TransformStamped),
+        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32),
-        "/tf"                             
+        "/nucleo/servo0/position"                             
     ));
 
     RCL_RETURN_ON_ERROR(rclc_subscription_init_default(
-        &ServoController::servo0_subscription,
+        &ServoController::servo0_sub_pos,
         &ServoController::node,
         ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32),
         "/nucleo/servo0/target"));
 
+    RCL_RETURN_ON_ERROR(rclc_subscription_init_default(
+        &ServoController::servo0_sub_maxv,
+        &ServoController::node,
+        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32),
+        "/nucleo/servo0/max_v"));
+
+    RCL_RETURN_ON_ERROR(rclc_subscription_init_default(
+        &ServoController::servo0_sub_maxa,
+        &ServoController::node,
+        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32),
+        "/nucleo/servo0/max_a"));
+
     RCL_RETURN_ON_ERROR(rclc_executor_add_subscription(
         executor,
-        &ServoController::servo0_subscription,
+        &ServoController::servo0_sub_pos,
         &servo_target_msg,
         servo0_set_target_callback,
         ON_NEW_DATA
     ));
 
+    RCL_RETURN_ON_ERROR(rclc_executor_add_subscription(
+        executor,
+        &ServoController::servo0_sub_maxv,
+        &servo_target_msg,
+        servo0_set_maxv_callback,
+        ON_NEW_DATA
+    ));
+
+    RCL_RETURN_ON_ERROR(rclc_executor_add_subscription(
+        executor,
+        &ServoController::servo0_sub_maxa,
+        &servo_target_msg,
+        servo0_set_maxa_callback,
+        ON_NEW_DATA
+    ));
 
     alarm_cfg = {
         .callback = counter_cb,
@@ -123,40 +162,18 @@ void ServoController::servo_pid() {
 }
 
 rcl_ret_t ServoController::publish() {
-    geometry_msgs__msg__TransformStamped tf;  
+    std_msgs__msg__Float32 msg;
+    std_msgs__msg__Float32__init(&msg);
 
-    int64_t now_ns = 0;
+    msg.data = ServoController::current_angle;
-    rcl_clock_get_now(&clock, &now_ns);
-    tf.header.stamp.sec     = now_ns / 1000000000ULL;
-    tf.header.stamp.nanosec = now_ns % 1000000000ULL;
 
-    // Fill header
+    return rcl_publish(&ServoController::servo0_publisher, &msg, NULL);
-    char frame_id_buf[32] = "livox_base";
-    tf.header.frame_id.data = frame_id_buf;
-    tf.header.frame_id.size = strlen(frame_id_buf);
-    tf.header.frame_id.capacity = sizeof(frame_id_buf);
-
-    char child_id_buf[32] = "frame_default";
-    tf.child_frame_id.data = child_id_buf;
-    tf.child_frame_id.size = strlen(child_id_buf);
-    tf.child_frame_id.capacity = sizeof(child_id_buf);
-
-    // Rotation around Z
-    float yaw = (current_angle - 90.0f) * 3.141592653589793f / 180.0f;
-    tf.transform.translation.x = 0.0;
-    tf.transform.translation.y = 0.0;
-    tf.transform.translation.z = 0.0;
-
-    tf.transform.rotation.x = 0.0f;
-    tf.transform.rotation.y = 0.0f;
-    tf.transform.rotation.z = sinf(yaw / 2.0f);
-    tf.transform.rotation.w = cosf(yaw / 2.0f);
-
-    return rcl_publish(&ServoController::servo0_publisher, &tf, NULL);
 }
 
 void ServoController::kill() {
     rcl_publisher_fini(&ServoController::servo0_publisher, &ServoController::node);
-    rcl_subscription_fini(&ServoController::servo0_subscription, &ServoController::node);
+    rcl_subscription_fini(&ServoController::servo0_sub_pos, &ServoController::node);
+    rcl_subscription_fini(&ServoController::servo0_sub_maxv, &ServoController::node);
+    rcl_subscription_fini(&ServoController::servo0_sub_maxa, &ServoController::node);
     rcl_node_fini(&ServoController::node);
 }

src/servo_controller/servo_controller.hpp

@@ -16,10 +16,16 @@ public:
     static float current_angle;
     static float target_angle;
 
+    // Limits
+    static float max_velocity;
+    static float max_accel;
+
 private:
     // ros
     static rcl_node_t node;
-    static rcl_subscription_t servo0_subscription;
+    static rcl_subscription_t servo0_sub_pos;
+    static rcl_subscription_t servo0_sub_maxv;
+    static rcl_subscription_t servo0_sub_maxa;
     static rcl_publisher_t servo0_publisher;
 
     // pid states
@@ -33,8 +39,4 @@ private:
     static constexpr float kp = 2.0f;
     static constexpr float ki = 0.0f;
     static constexpr float kd = 0.05f;
-
-    // Limits
-    static constexpr float max_velocity = 50.0f;  // deg/s
-    static constexpr float max_accel = 100.0f;     // deg/s^2
 };