Device entering error loop when attempting to publish

[TzionCastillo1]

When attempting to publish a NavSatFix message on an Arduino Portenta H7, the arduino immediatley enters the error loop, and nothing is output on the /sensor_msgs/NavSatFix topic, or the /gps topic, but both topics are present. The micro-ros-agent is running on ros2 Foxy, on Ubuntu 20.04, and is outputting as expected:

Publishing was working with the "micro-ros-publisher" example, so I'm not sure where I went wrong and any help would be appreciated.

Here is my code:

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

#include <sensor_msgs/msg/nav_sat_fix.h>
#include <std_msgs/msg/int32.h>
#include <sensor_msgs/msg/nav_sat_status.h>

rcl_publisher_t publisher;
rcl_publisher_t gpspublisher;
std_msgs__msg__Int32 msg;
rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
rcl_timer_t timer;

sensor_msgs__msg__NavSatFix * navsatfix_message;


#include <Adafruit_GPS.h>

#define GPSSerial Serial1

Adafruit_GPS GPS(&GPSSerial);

#define GPSECHO false
#define LED_PIN LED_BUILTIN

#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)){}}



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

void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{  
  RCLC_UNUSED(last_call_time);
  if (timer != NULL) {
    /*Serial.println("Fix:");
    Serial.print(GPS.fix);
    Serial.println("Lat:");
    Serial.print(GPS.latitude, 4);
    Serial.println(GPS.lat);
    */
    navsatfix_message->longitude = (float) GPS.longitude;
    navsatfix_message->latitude = (float) GPS.latitude;
    msg.data = int(GPS.latitude);
    
    RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
    //msg.data++;
    RCSOFTCHECK(rcl_publish(&gpspublisher, navsatfix_message, NULL));
  }
}

void setup() {
  set_microros_transports();
  
  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, HIGH);  
  //Serial.begin(115200);
  //Serial.println("GPS Publisher test");
  // 9600 NMEA is the default baud rate for Adafruit MTK GPS's- some use 4800

  delay(2000);

  allocator = rcl_get_default_allocator();

  //create init_options
  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));

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

  // create publisher
  RCCHECK(rclc_publisher_init_default(
    &publisher,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
    "gps"));

  RCCHECK(rclc_publisher_init_default(
    &gpspublisher,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, NavSatFix),
    "sensor_msgs/NavSatFix"));

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

  // create executor
  RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
  RCCHECK(rclc_executor_add_timer(&executor, &timer));

  
  //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;

  //Assigning statuc memory to the status && service int memories
  /*navsatfix_message->status.status.capacity = 10;
  navsatfix_message->status.status.data = 0;
  navsatfix_message->status.status.size = 1;

  navsatfix_message->status.service.capacity = 1;
  navsatfix_message->status.service.data = 1;
  navsatfix_message->status.service.size = 1;
  */
  
  msg.data = 0;
  GPS.begin(9600);
  // uncomment this line to turn on RMC (recommended minimum) and GGA (fix data) including altitude
  GPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);
  // uncomment this line to turn on only the "minimum recommended" data
  //GPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
  // For parsing data, we don't suggest using anything but either RMC only or RMC+GGA since
  // the parser doesn't care about other sentences at this time
  // Set the update rate
  GPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ); // 1 Hz update rate
  // For the parsing code to work nicely and have time to sort thru the data, and
  // print it out we don't suggest using anything higher than 1 Hz

  // Request updates on antenna status, comment out to keep quiet
  GPS.sendCommand(PGCMD_ANTENNA);

}

void loop() {
  //delay(100);
  RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
  char c = GPS.read();
  if (GPS.newNMEAreceived()){
    if (!GPS.parse(GPS.lastNMEA()))
      return;
  }
  
}

[pablogs9]

Your navsatfix_message is a not initialized pointer. It is probably causing a mem fault.

[TzionCastillo1]

That was correct, Thanks!