Unable to Publish 2 topics and subscribe to 1 at the same time.

[TzionCastillo1]

Issue template

• Hardware description: Arduino Portenta H7
• RTOS: Micro_ros_arduino for Portenta H7
• Version or commit hash: foxy on Ubuntu 20.04

Steps to reproduce the issue

#include <mpu9250.h>
#include <Servo.h>

#include <micro_ros_arduino.h>

#include <MadgwickAHRS.h>
#include <stdio.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>

//included message types
#include <sensor_msgs/msg/imu.h>
#include <sensor_msgs/msg/magnetic_field.h>
#include <sensor_msgs/msg/nav_sat_status.h>
#include <sensor_msgs/msg/nav_sat_fix.h>
#include <std_msgs/msg/float64.h>
#include <std_msgs/msg/int32.h>
#include <geometry_msgs/msg/twist.h>

#include <Adafruit_GPS.h>
sensor_msgs__msg__Imu IMU;
geometry_msgs__msg__Twist twstmsg;
sensor_msgs__msg__NavSatFix  navsatfix_message;

rcl_publisher_t IMUpublisher;
rcl_publisher_t GPSpublisher;
rcl_subscription_t subscriber;

//std_msgs__msg__Int32 msg;
rclc_executor_t executor;
rclc_support_t support;
rclc_support_t subsupport;
rcl_allocator_t allocator;
rcl_node_t pubnode;
//rcl_node_t subnode;
rcl_timer_t timer;

#define LED_PIN LED_BUILTIN

//Setup LED for Calibration Message
#define CAL_LED LED_BUILTIN
#define ESC_PWM 6
#define SRVO_PWM 7
#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}} //error_loop();
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}

#define MXBIAS 16.298387
#define MYBIAS 4.740027
#define MZBIAS 6.269465
#define AXBIAS -0.179
#define AYBIAS 0.137924
#define AZBIAS .433839

#define GPSSerial Serial1

Mpu9250 mpu(&Wire, 0x69);
Adafruit_GPS GPS(&GPSSerial);
Madgwick filter;
Servo ESC;
Servo SRVO;

float q[4];
float gOff[3];
struct angular_velocity{
  float x;
  float y;
  float z;
}g;

struct linear_acceleration{
  float x;
  float y;
  float z;
}a;

struct magnetometer{
  float x;
  float y;
  float z;
}m;

void error_loop(){
  while(1){
    digitalWrite(LED_PIN, !digitalRead(LED_PIN));
    delay(100);
  }
}

//twist message cb
void subscription_callback(const void *msgin) {
  const geometry_msgs__msg__Twist * twstmsg = (const geometry_msgs__msg__Twist *)msgin;
  // if velocity in x direction is 0 turn off LED, if 1 turn on LED
  digitalWrite(LED_PIN, (twstmsg->linear.x == 0) ? LOW : HIGH);
  int ESCpwmVal = map((twstmsg->linear.x)*100, -100, 100, 1100, 1900);
  int SRVOpwmVal = map((twstmsg->angular.z)*-100, -300, 300, 1170, 1830);
  ESC.writeMicroseconds(ESCpwmVal);
  SRVO.writeMicroseconds(SRVOpwmVal);
}

void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{  
  RCLC_UNUSED(last_call_time);
  if (timer != NULL) {
    IMU.orientation_covariance[0] = -1;
    IMU.orientation.w = (double) q[0];
    IMU.orientation.x = (double) q[1];
    IMU.orientation.y = (double) q[2];
    IMU.orientation.z = (double) q[3];
    IMU.angular_velocity.x = g.x;
    IMU.angular_velocity.y = g.y;
    IMU.angular_velocity.z = g.z;
    IMU.linear_acceleration.x = a.x; 
    IMU.linear_acceleration.y = a.y;
    IMU.linear_acceleration.z = a.z;
    navsatfix_message.longitude = (double) GPS.longitude;
    navsatfix_message.latitude = (double) GPS.latitude;
    RCSOFTCHECK(rcl_publish(&IMUpublisher, &IMU, NULL));
    RCCSOFTCHECK(rcl_publish(&GPSpublisher, &navsatfix_message, NULL));
  }
}

void blink_led(){
  for(int i=0;i<10;i++){
    digitalWrite(CAL_LED, !digitalRead(CAL_LED));
    //tone(BUZZER, 700);
    delay(250);
    //noTone(BUZZER);
    delay(250);
  }
}

void setup() {
  //set_microros_transports();
  Wire.begin();
  while (!mpu.Begin()){
  }
  set_microros_transports();

  //set up attached sensors & actuators

  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, HIGH);
  ESC.attach(ESC_PWM, 1100, 1900);
  ESC.writeMicroseconds(1500);
  SRVO.attach(SRVO_PWM, 500, 2500);
  SRVO.writeMicroseconds(1500);
  gyroOffset();
  delay(5000);

  allocator = rcl_get_default_allocator();

  //create init_options
  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
  //RCCHECK(rclc_support_init(&subsupport, 0, NULL, &allocator));
  // create nodes
  RCCHECK(rclc_node_init_default(&pubnode, "micro_ros_arduino_publisher_node", "", &support));
  //RCCHECK(rclc_node_init_default(&subnode, "micro_ros_arduino_subscriber_node", "", &subsupport));

  // create publisher
  RCCHECK(rclc_publisher_init_default(
    &IMUpublisher,
    &pubnode,
    ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Imu),
    "Imu"));

  RCCHECK(rclc_publisher_init_default(
    &GPSpublisher,
    &pubnode,
    ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, NavSatFix),
    "gps"));

  // create subscriber
  RCCHECK(rclc_subscription_init_default(
    &subscriber,
    &pubnode,
    ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist),
    "twist"));

  // create timer,
  const unsigned int timer_timeout = 5000;
  RCCHECK(rclc_timer_init_default(
    &timer,
    &support,
    RCL_MS_TO_NS(timer_timeout),
    timer_callback));

  // create executor
  RCCHECK(rclc_executor_init(&executor, &support.context, 2, &allocator));

  rclc_executor_add_subscription(&executor, &subscriber, &twstmsg, &subscription_callback, ON_NEW_DATA);
  RCCHECK(rclc_executor_add_timer(&executor, &timer));

  //msg.data = 0;
    //assigning dynamic memory to the frame_id char sequence
  navsatfix_message.header.frame_id.capacity = 100;
  navsatfix_message.header.frame_id.data = 0; //(char*) malloc(navsatfix_message->position_covariance * sizeof(char));
  navsatfix_message.header.frame_id.size = 0;

  //Assigning value to the frame_id char sequence
  //strcpy(navsatfix_message->header.frame_id.data, "Adafruit Ultimate GPS");
  navsatfix_message.header.frame_id.data = "Adafruit Ultimate GPS";
  navsatfix_message.header.frame_id.size = strlen(navsatfix_message.header.frame_id.data);

  navsatfix_message.header.stamp.sec = 10;
  navsatfix_message.header.stamp.nanosec = 20;

  navsatfix_message.altitude = 0;

  GPS.begin(9600);  
  GPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);
  GPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);  
  //delay(3000);

void gyroOffset(){
  //Temporary variables for calculating gyro offset
  float gx,gy,gz = 0;
  int iter = 200;
  for(int j = 0; j<iter; j++){
    if(mpu.Read()){
      gx += mpu.gyro_x_radps() * 180/3.1415;
      gy += mpu.gyro_y_radps() * 180/3.1415;
      gz += mpu.gyro_z_radps() * 180/3.1415;
      delay(50);
    }
  }
  gOff[0] = gx/iter;
  gOff[1] = gy/iter;
  gOff[2] = gz/iter;
}

void loop() {
  //temporary variables for offset Magnetic Field 
  int mx, my, mz, ax, ay, az;
  //delay(10);
  if (mpu.Read()) {
    //quat.x = mpu.getQuaternionX();
    ax = mpu.accel_x_mps2() - AXBIAS;
    ay = mpu.accel_y_mps2() - AYBIAS;
    az = mpu.accel_z_mps2() - AZBIAS;
    a.x = 1.108858*ax + -0.029409*ay + -0.011316*az;
    a.y = -0.029409*ax + 1.085038*ay + 0.006645*az;
    a.z = -0.011316*ax + 0.006645*ay + 1.030198*az;
    g.x = (mpu.gyro_x_radps() * 180/3.1415) - gOff[0];
    g.y = (mpu.gyro_y_radps() * 180/3.1415) - gOff[1];
    g.z = (mpu.gyro_z_radps() * 180/3.1415) - gOff[2];
    mx = mpu.mag_x_ut() - MXBIAS;
    my = mpu.mag_y_ut() - MYBIAS;
    mz = mpu.mag_z_ut() - MZBIAS;
    m.x = 1.151590*mx + -0.042531*my + 0.007655*mz;
    m.y = -0.042531*mx + 1.082125*my + -0.006388*mz;
    m.z = 0.0007655*mx + -0.006388*my + 1.068449*mz;
  }

  for (int i = 0; i<100; i++){
    //filter.updateIMU(g.x,g.y,g.z,a.x,a.y,a.z);
    filter.update(g.x,g.y,g.z,a.x,a.y,a.z,m.x,m.y,m.z); 
  }
  filter.getQuaternion(q);
  RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));  
  char c = GPS.read();
  if (GPS.newNMEAreceived()){
    if (!GPS.parse(GPS.lastNMEA()))
      return;
  }

}

Expected behavior

Both "/gps" and "/Imu" publish, and motor and servo move in response to "/twist" topic.

Actual behavior

All of the topics, subscribers, and publishers are created with the agent. "/gps" and "/Imu" topics are published to twice, as well as the motor & servo respond to the "/twist" topic messages for a few seconds. Everything becomes unresponsive after this.

Additional information

All of the subscriptions and publishers work as expected when isolated, as well as any combination of two of the three, but when I have all three run together I run into this issue.

Any help would be much appreciated.

EDIT

If I comment out the lines to initialize communication with the GPS:

  //GPS.begin(9600);  
  //GPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);
  //GPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);

all topics publish, but the "/gps" topic of course publishes a blank message. The GPS is on a UART port separate from the one connected to the USB on the Portenta, so I'm not sure if this issue is UART related. Also I am able to initialize the GPS and publish readings from it just fine when it is in isolation, or when it is paired with one other publisher or subscriber. Really lost with what could be wrong here.

[TzionCastillo1]

Any help would be appreciated!

[Acuadros95]

Hi @TzionCastillo1,

Sorry for the delay, did you manage to solve this? A first step could be to debug the failure point on the code, this could give us some insight on the problem.

[TzionCastillo1]

I have not yet. It seems to be failing during the execution of the subscription_callback function. I added in a buzzer to help debug, and the buzzer stops producing the tone before the noTone() function tells it to do so. My first instinct after this is that the board is crashing, but the Portenta H7 I am using has a built in function to flash the onboard red LED, and this is not happening.

updated subscription_callback function for debugging

//twist message cb
void subscription_callback(const void *msgin) {
  tone(BUZZER, 5000);
  const geometry_msgs__msg__Twist * twstmsg = (const geometry_msgs__msg__Twist *)msgin;
  // if velocity in x direction is 0 turn off LED, if 1 turn on LED
  //digitalWrite(LED_PIN, (twstmsg->linear.x == 0) ? LOW : HIGH);
  int ESCpwmVal = map((twstmsg->linear.x)*100, -100, 100, 1100, 1900);
  int SRVOpwmVal = map((twstmsg->angular.z)*-100, -300, 300, 1170, 1830);
  ESC.writeMicroseconds(ESCpwmVal);
  SRVO.writeMicroseconds(SRVOpwmVal);
  delay(1000);
  noTone(BUZZER);
}

EDIT:

Looking deeper into this, the amount of time this function is executed for is very strongly correlated with the update rate of the GPS. If I set the gps to send updates every second, the code executes for ~2 seconds. If i set the update rate to once every 5 seconds, the code executes for ~10 seconds, if I set the update rate to once every 10 seconds, the code executes for ~30s. I'm guessing this has something to to with the interrupts coming from the GPS, but I'm not sure where I'd start to fix that.

[Acuadros95]

You can execute the agent in debug mode using the flag -v6, this way we can see if the board is still trying to communicate with the agent.

You can also take a look inside the GPS library to see what its doing, but I am afraid we cannot replicate this issue.

[pablogs9]

Any update on this?

[TzionCastillo1]

Nothing useful. I looked through the Adafruit GPS library and I did not see anything that would interfere with the microros library. I ended up connecting another Arduino to the Portenta's UART port, and had it just send a char, but the same issue persists. I also tired this on the other Serial ports using the Portenta Breakout Board. Outside of this, I'm not really sure where I'd begin to look to fix this bug. I understand this can't be replicated, but if there are recommendations I'd be more than happy to try them out.

[Acuadros95]

Closing as we cannot replicate this, please reopen if needed or on new information on this bug.