micro-ROS / micro_ros_arduino

micro-ROS library for Arduino
Apache License 2.0
437 stars 113 forks source link

JointState subscription callback not invoked, error_loop triggered instead #1841

Closed pky404 closed 1 week ago

pky404 commented 1 week ago

I'm using the below code to subscribe to JointState messages in micro-ROS with Arduino (Portenta H7). The subscription callback is never invoked, and instead, the error_loop is triggered. The subscriber is initialized to the topic /joint_commands, and a simple message is being published using: ros2 topic pub /joint_commands sensor_msgs/msg/JointState "{header: {stamp: {sec: 0, nanosec: 0}, frame_id: ''}, name: ['amr0/base_joint'], position: [0.0], velocity: [0.0], effort: [0.0]}"


#include <micro_ros_arduino.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <Arduino_PortentaMachineControl.h>
#include <sensor_msgs/msg/joint_state.h>

rcl_subscription_t subscriber;
sensor_msgs__msg__JointState joint_state_msg;
rclc_executor_t executor;
rcl_allocator_t allocator;
rclc_support_t support;
rcl_node_t node;

#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}

void error_loop() {
    Serial.println("Error!!!");
    delay(100);
}

void subscription_callback(const void *msgin) {
    const sensor_msgs__msg__JointState *msg = (const sensor_msgs__msg__JointState *)msgin;
    Serial.println("JointState message received");
    if (msg->position.size > 0) {
        Serial.print("Joint 0 Position: ");
        Serial.println(msg->position.data[0]);
    }
}

void setup() {
  Serial.begin(115200);
  set_microros_transports();

  delay(2000);

  allocator = rcl_get_default_allocator();

  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));

  RCCHECK(rclc_node_init_default(&node, "micro_ros_arduino_node", "", &support));

  RCCHECK(rclc_subscription_init_default(
    &subscriber,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, JointState),
    "joint_commands"));

  RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
  RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &joint_state_msg, &subscription_callback, ON_NEW_DATA));
}

void loop() {
  delay(100);
  RCCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
}

Despite the proper initialization, the callback is not being triggered. It would be great to know if there are any known issues or suggestions for debugging this.

hippo5329 commented 1 week ago

You will need to handle joint_states message memory allocation.

https://micro.ros.org/docs/tutorials/advanced/handling_type_memory/