Genozen commented 2 years ago

1062

Hardware description: ESP32 Heltec LoRa v2
RTOS: microROS platformIO
Installation type: microROS platformIO
Version or commit hash: Linux 20.04 + Foxy ROS2.

Steps to reproduce the issue

Similar to #1062 Whenever I added msg2.data.data[0] = 0.1; causes Wifi:Association refused temporarily, comeback time 299 mSec. in the terminal

Expected behavior

stores multiple float data in the array

Code:


#include <Arduino.h>
#include <micro_ros_platformio.h>

#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
// #include <micro_ros_utilities/type_utilities.h>
#include <micro_ros_utilities/string_utilities.h>
#include <std_msgs/msg/float32_multi_array.h>

#include <std_msgs/msg/int32.h> //for integer messages
#include <std_msgs/msg/string.h>
#include <std_msgs/msg/int32_multi_array.h>

#include "freertos/FreeRTOS.h"      // freeRTOS Libraries 
#include "freertos/task.h"          // freeRTOS Task Header
#include "esp_system.h"
#include "soc/rtc_wdt.h"   
#include <mySD.h>
#include <SparkFun_u-blox_GNSS_Arduino_Library.h>

// #if !defined(MICRO_ROS_TRANSPORT_ARDUINO_SERIAL)
// #error This example is only avaliable for Arduino framework with serial transport.
// #endif

//publisher initialization
rcl_publisher_t publisher;
rcl_publisher_t publisher2;
std_msgs__msg__Int32 msg;
std_msgs__msg__Float32MultiArray msg_array_float;

// std_msgs__msg__Int32MultiArray msg_array;
// std_msgs__msg__String msg_array_string;

// std_msgs__msg__Int32 msg_gps;
// std_msgs__msg__String msg_gps;

//rclc initialization...
rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
rcl_timer_t timer;

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

// ---------------- ---------------- ---------------- ---------------- ----------------
// ----------------------------------- FreeRTOS ------------------------------------
// ---------------- ---------------- ---------------- ---------------- ----------------
TaskHandle_t Task1;
TaskHandle_t Task2;
TaskHandle_t Task3;
TaskHandle_t Task4;

//Task counters
int GPScounter = 0;
int IMUcounter = 0;
int LORAcounter = 0;

//Mutex and Semaphores
static SemaphoreHandle_t GPSMutex;
static SemaphoreHandle_t IMUMutex;
static SemaphoreHandle_t LORAMutex;

String GPSlist = "emptyGPSlist";
String IMUlist = "emptyIMUlist";
String message = "emptyMessage";

// Task Rates
static const int RATE_GPS = 5000; //500; //2000;  // ms
static const int RATE_IMU = 1000; //100; //1000;  // ms
static const int RATE_LORA = 3000; //500; //1000;

// ---------------- ---------------- ---------------- ---------------- ----------------
// ----------------------------------- GPS ------------------------------------
// ---------------- ---------------- ---------------- ---------------- ----------------
#define PIN_SDA 4
#define PIN_SCL 15
SFE_UBLOX_GNSS myGNSS;  //M8Q GNSS

//GPS vars
float latitude = 0.0;
float longitude = 0.0;
float altitude = 0.0;
float GPSspeed = 0.0;
float GPSheading = 0.0;

// ---------------- ---------------- ---------------- ---------------- ----------------
// ----------------------------------- SDcard ------------------------------------
// ---------------- ---------------- ---------------- ---------------- ----------------
//SD Card initialization
#define SD_MOSI 18
#define SD_MISO 19
#define SD_CLK 5
const int SDchipSelect = 23; //For SDcard CS pin at 32
File myFile;

#define Select LOW
#define DeSelect HIGH

#define H_MOSI 12 //13
#define H_MISO 38 //12
#define H_CLK 13
#define H_CS 23
SPIClass SPI2(HSPI);

// Error handle loop
void error_loop() {
  while(1) {
    delay(100);
  }
}

//This is the function where the message data is set and the msg are actually published. RCLCPP_INFO macro ensures every published msg is printed to the console
void timer_callback(rcl_timer_t * timer, int64_t last_call_time) {
  RCLC_UNUSED(last_call_time);
  if (timer != NULL) {
    RCSOFTCHECK(rcl_publish(&publisher, &msg_array_float, NULL));
    msg_array_float.data.data[0] = 0.1;
    // RCSOFTCHECK(rcl_publish(&publisher2, &msg_array_float, NULL));
    // msg.data++;
  }
}

void SetupSD(){
    //Initialize SD Card
  Serial.print("Initializing SD card...");
  // see if the card is present and can be initialized:
  Serial.println("SD.begin....");
  if (!SD.begin(H_CS, H_MOSI , H_MISO, H_CLK)) {
    Serial.println("Card failed, or not present");
    // don't do anything more:
    while (1);
  }
  Serial.println("card initialized.");

  delay(100);
  // open the file. note that only one file can be open at a time,
  // so you have to close this one before opening another.
  myFile = SD.open("/test.txt", FILE_WRITE);
  delay(100);

  // if the file opened okay, write to it:
  if (myFile) {
    Serial.print("Writing to test.txt...");
    myFile.println("testing 1, 2, 3.");
    // close the file:
    myFile.close();
    Serial.println("done.");
  } else {
    // if the file didn't open, print an error:
    Serial.println("error opening test.txt");
  }
  delay(100);
  // re-open the file for reading:
//   myFile = SD.open("/test.txt");
//   if (myFile) {
//     Serial.println("/test.txt:");

//     // read from the file until there's nothing else in it:
//     while (myFile.available()) {
//       Serial.write(myFile.read());
//     }
//     // close the file:
//     myFile.close();
//   } else {
//     // if the file didn't open, print an error:
//     Serial.println("error opening test.txt");
//   }
}

void update_latitude_and_longitude(void *pvParameter) {
  // setup
  if (!myGNSS.begin()) {
    Serial.println(F("u-blox GNSS not detected at default I2C address. Please check wiring. Freezing."));
    myGNSS.setI2COutput(COM_TYPE_UBX); //Set the I2C port to output UBX only (turn off NMEA noise)
    myGNSS.saveConfigSelective(VAL_CFG_SUBSEC_IOPORT); //Save (only) the communications port settings to flash and BBR
    while(1);
  }
  delay(500);
  // loop
  for (;;) {
    xSemaphoreTake(GPSMutex, portMAX_DELAY);
//    GPSlist = String(GPScounter);
    GPScounter++;
//    xSemaphoreGive(GPSMutex);
    //    latitude  = myGNSS.getLatitude()/10000000.;
    //    longitude = myGNSS.getLongitude()/10000000.;
    //    altitude  = myGNSS.getAltitude()/1000.;
    // latitude  = myGNSS.getLatitude();
    // longitude = myGNSS.getLongitude();
    // altitude  = myGNSS.getAltitude();
    // GPSspeed = myGNSS.getGroundSpeed();
    // GPSheading = myGNSS.getHeading();

    //    Serial.print(latitude);
    //    Serial.print(longitude);
    // Serial.println(String(myGNSS.getLatitude())+ "," + String(myGNSS.getLongitude()));
    //    Serial.println("GPS Heap:" + xPortGetFreeHeapSize());

//    GPSlist = String(myGNSS.getLatitude()) + "," + String(myGNSS.getLongitude());
    GPSlist = String(GPScounter) + "," + String(myGNSS.getLatitude()) + "," + String(myGNSS.getLongitude());
    // msg.data = myGNSS.getLatitude();
    // msg_gps.data = 'c';
    // msg_array_float.data.data[0] = 0.1;
    Serial.println(GPSlist);
//    heap_caps_check_integrity_all(); //heap memory allocation check....
//    heap_caps_check_integrity_addr();
    rtc_wdt_feed(); //feed watchdog
    xSemaphoreGive(GPSMutex);
    vTaskDelay(RATE_GPS / portTICK_PERIOD_MS);
  }
}

void setup() {
  // ----------- Configure serial transport
  Serial.begin(115200);
  // set_microros_serial_transports(Serial);

  //***** For Router WiFi....
  // IPAddress agent_ip(---------);
  // size_t agent_port = 8888;
  // char ssid[] = "-----------";
  // char psk[]= "------";

  set_microros_wifi_transports(ssid, psk, agent_ip, agent_port);

  SPI2.begin(H_CLK, H_MISO, H_MOSI, H_CS);
  // cli();
  delay(500);
  SetupSD();
  delay(100);

  Wire.begin(4, 15);
  // Wire.begin();
  delay(100);
  Wire.setClock(400000); //fast mode
  delay(100);

  delay(2000);
  Serial.println("Initialized...");

  //create Mutex/Semaphores
  GPSMutex = xSemaphoreCreateMutex();
  IMUMutex = xSemaphoreCreateMutex();
  LORAMutex = xSemaphoreCreateMutex();
  xTaskCreatePinnedToCore(update_latitude_and_longitude, "update_latitude_and_longitude", 8912, NULL, 1, &Task1, 1);
  delay(100); //important delay

  //=========================ROS2 stuff setup..............===================
  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_platformio_node", "", &support));

  // create publisher
  RCCHECK(rclc_publisher_init_best_effort(
    &publisher,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
    "micro_ros_platformio_node_publisher"));

  // // create publisher2
  // RCCHECK(rclc_publisher_init_default(
  //   &publisher2,
  //   &node,
  //   ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
  //   "micro_ros_platformio_node_publisher_lat"));

  // micro_ros_utilities_create_message_memory(
  //   ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
  //   &msg_gps,
  //   (micro_ros_utilities_memory_conf_t) {}
  // );
  // micro_ros_utilities_create_message_memory(
  //   ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, String),
  //   &msg_array_string,
  //   (micro_ros_utilities_memory_conf_t) {}
  // );

  // create timer,
  const unsigned int timer_timeout = 1; //this is the speed on which it is published, set to 1 for extra speed
  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));

  msg.data = 0;
  // msg_string.data = "na";
  // msg_gps.data = 0;
}

void loop() {
  // delay(100);
  // msg_array_string.data = micro_ros_string_utilities_set(msg_array_string.data, "Stationary");
  RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
}

Genozen commented 2 years ago

I should mention the board resets:

[1658775353.731866] info     | ProxyClient.cpp    | create_datawriter        | datawriter created     | client_key: 0x02A35511, datawriter_id: 0x000(5), publisher_id: 0x000(3)
[1658775353.731987] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x02A35511, len: 14, data: 
0000: 81 80 03 00 05 01 06 00 00 0D 00 05 00 00
[1658775353.732062] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x02A35511, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 04 00 00 00 80
[1658775353.753863] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x02A35511, len: 13, data: 
0000: 81 00 00 00 0B 01 05 00 03 00 03 00 80
[1658775353.765599] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x02A35511, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 04 00 00 00 80
[1658775353.774613] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x02A35511, len: 16, data: 
0000: 81 01 00 00 07 01 08 00 00 0E 00 05 00 00 00 00
[1658775353.774955] debug    | DataWriter.cpp     | write                    | [** <<DDS>> **]        | client_key: 0x00000000, len: 4, data: 
0000: 00 00 00 00

Full Serial Terminal Message:

Initialized...
Guru Meditation Error: Core  1 panic'ed (LoadProhibited). Exception was unhandled.

Core  1 register dump:
PC      : 0x400d2b62  PS      : 0x00060e30  A0      : 0x80139cf8  A1      : 0x3ffb2730  
A2      : 0x3f800000  A3      : 0x00000000  A4      : 0x00150ab8  A5      : 0x00000000  
A6      : 0x00000000  A7      : 0x00000000  A8      : 0x00000000  A9      : 0x3ffb2710  
A10     : 0x00000000  A11     : 0x00000001  A12     : 0x00000000  A13     : 0x00ffffff  
A14     : 0x01000000  A15     : 0x00000000  SAR     : 0x00000017  EXCCAUSE: 0x0000001c  
EXCVADDR: 0x00000000  LBEG    : 0x40089648  LEND    : 0x4008965e  LCOUNT  : 0xffffffff  

Backtrace:0x400d2b5f:0x3ffb27300x40139cf5:0x3ffb2750 0x40137d4f:0x3ffb2790 0x401381f9:0x3ffb27b0 0x401382ad:0x3ffb27e0 0x400d2b8c:0x3ffb2800 0x400dd959:0x3ffb2820 

ELF file SHA256: 0000000000000000

Rebooting...
ets Jul 29 2019 12:21:46

rst:0xc (SW_CPU_RESET),boot:0x17 (SPI_FAST_FLASH_BOOT)
configsip: 0, SPIWP:0xee
clk_drv:0x00,q_drv:0x00,d_drv:0x00,cs0_drv:0x00,hd_drv:0x00,wp_drv:0x00
mode:DIO, clock div:2
load:0x3fff0030,len:1184
load:0x40078000,len:12784
load:0x40080400,len:3032
entry 0x400805e4
E (613) wifi:Association refused temporarily, comeback time 299 mSec
E (918) wifi:Association refused temporarily, comeback time 299 mSec
E (1222) wifi:Association refused temporarily, comeback time 299 mSec

pablogs9 commented 2 years ago

Is msg2.data.data[0] = 0.1; really related to Wifi:Association refused temporarily, comeback time 299 mSec.? Could you verify that removing this line everything works?

Some comments:

There is no msg2 variable in your code.
msg_array_float is not being initialized, so msg_array_float.data.data[0] = 0.1; might be a segmentation fault. Please read this tutorial: https://micro.ros.org/docs/tutorials/advanced/handling_type_memory/

Genozen commented 2 years ago

Thank you. The issue has been solved: Error is: Guru Meditation Error: Core 1 panic'ed (LoadProhibited). Exception was unhandled. Solution is to initialized the fields:

 msg2.data.capacity = 10;
  msg2.data.data = (float*)infoBuffer;
  msg2.data.size = 10;
  infoBuffer = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};

Then update the infoBuffer

See example from: https://github.com/husarion/heat-follower-robot/blob/main/mcu/src/main.cpp

micro-ROS / micro_ros_arduino

Float32MultiArray causes Wifi:Association refused temporarily, comeback time 299 mSec. #1107

1062

Steps to reproduce the issue

Expected behavior