search

micro-ROS / micro_ros_arduino

micro-ROS library for Arduino
Apache License 2.0
452 stars 117 forks source link

nav_msgs/msg/Odometry data not being published #884

Closed grassjelly closed 2 years ago

grassjelly commented 2 years ago

Steps to reproduce the issue

Upload the code below:

#include <Arduino.h>
#include <micro_ros_arduino.h>
#include <stdio.h>

#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <micro_ros_utilities/type_utilities.h>
#include <micro_ros_utilities/string_utilities.h>
#include <nav_msgs/msg/odometry.h>

#define LED_PIN 13 //used for debugging status
#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){rclErrorLoop();}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}

rcl_publisher_t odom_publisher;
nav_msgs__msg__Odometry odom_msg;

rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
rcl_timer_t control_timer;

unsigned long long time_offset = 0;
bool micro_ros_init_successful = false;

void setup() 
{
    pinMode(LED_PIN, OUTPUT);
    micro_ros_init_successful = false;
    set_microros_transports();
    odom_msg.header.frame_id = micro_ros_string_utilities_set(odom_msg.header.frame_id, "odom");
    odom_msg.child_frame_id = micro_ros_string_utilities_set(odom_msg.child_frame_id, "base_footprint");
}

void loop() 
{
    static unsigned long prev_connect_test_time;
    // check if the agent got disconnected at 10Hz
    if(millis() - prev_connect_test_time >= 100)
    {
        prev_connect_test_time = millis();
        // check if the agent is connected
        if(RMW_RET_OK == rmw_uros_ping_agent(10, 2))
        {
            // reconnect if agent got disconnected or haven't at all
            if (!micro_ros_init_successful) 
            {
                createEntities();
            } 
        } 
        else if(micro_ros_init_successful)
        {
            // clean up micro-ROS components
            destroyEntities();
        }
    }

    if(micro_ros_init_successful)
    {
        rclc_executor_spin_some(&executor, RCL_MS_TO_NS(10));
    }
}

void controlCallback(rcl_timer_t * timer, int64_t last_call_time) 
{
    publishData();
}

void createEntities()
{
    allocator = rcl_get_default_allocator();
    //create init_options
    RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
    // create node
    RCCHECK(rclc_node_init_default(&node, "odometry_test", "", &support));
    // create odometry publisher
    RCCHECK(rclc_publisher_init_best_effort( 
        &odom_publisher, 
        &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(nav_msgs, msg, Odometry),
        "odom/unfiltered"
    ));

    const unsigned int control_timeout = 20;
    RCCHECK(rclc_timer_init_default( 
        &control_timer, 
        &support,
        RCL_MS_TO_NS(control_timeout),
        controlCallback
    ));
    executor = rclc_executor_get_zero_initialized_executor();
    RCCHECK(rclc_executor_init(&executor, &support.context, 1, & allocator));
    RCCHECK(rclc_executor_add_timer(&executor, &control_timer));

    if(!micro_ros_utilities_create_message_memory(
        ROSIDL_GET_MSG_TYPE_SUPPORT(nav_msgs, msg, Odometry),
        &odom_msg,
        (micro_ros_utilities_memory_conf_t) {})
        )
    {
        rclErrorLoop();
    }

    // synchronize time with the agent
    syncTime();
    digitalWrite(LED_PIN, HIGH);
    micro_ros_init_successful = true;
}

void destroyEntities()
{
    digitalWrite(LED_PIN, LOW);

    rcl_publisher_fini(&odom_publisher, &node);
    rcl_node_fini(&node);
    rcl_timer_fini(&control_timer);
    rclc_executor_fini(&executor);
    rclc_support_fini(&support);

    micro_ros_init_successful = false;
}

void publishData()
{
    struct timespec time_stamp = getTime();

    odom_msg.header.stamp.sec = time_stamp.tv_sec;
    odom_msg.header.stamp.nanosec = time_stamp.tv_nsec;
    odom_msg.pose.pose.orientation.w = 1.;

    RCSOFTCHECK(rcl_publish(&odom_publisher, &odom_msg, NULL));
}

void syncTime()
{
    // get the current time from the agent
    unsigned long now = millis();
    RCCHECK(rmw_uros_sync_session(10));
    unsigned long long ros_time_ms = rmw_uros_epoch_millis(); 
    // now we can find the difference between ROS time and uC time
    time_offset = ros_time_ms - now;
}

struct timespec getTime()
{
    struct timespec tp = {0};
    // add time difference between uC time and ROS time to
    // synchronize time with ROS
    unsigned long long now = millis() + time_offset;
    tp.tv_sec = now / 1000;
    tp.tv_nsec = (now % 1000) * 1000000;

    return tp;
}

void rclErrorLoop() 
{
    while(true)
    {
        flashLED(2);
    }
}

void flashLED(int n_times)
{
    for(int i=0; i<n_times; i++)
    {
        digitalWrite(LED_PIN, HIGH);
        delay(150);
        digitalWrite(LED_PIN, LOW);
        delay(150);
    }
    delay(1000);
}

Expected behavior

Receive data on odom/unfiltered topic

Actual behavior

Nothing is received

Additional information

Odometry msg is not published although other messages (IMU) are. The above code is a minimal version of my code from Linorobot project to solely publish odometry data. The same code used to work from previous commits of micro_ros_arduino.

Thank you!

grassjelly commented 2 years ago

I haven't fully investigated yet but could this be related to this? https://github.com/micro-ROS/micro_ros_arduino/issues/48

pablogs9 commented 2 years ago

Try to fill odom_msg.header.frame_id and odom_msg.child_frame_id both are strings and thery are not being initialized.

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

grassjelly commented 2 years ago

hey @pablogs9 sorry missed those from stripping the code. Added the string utility to add the frames as well as the message memory utility but data remains unpublished.

pablogs9 commented 2 years ago

Be careful because you are allocating memory each time micro_ros_utilities_create_message_memory is being called in createEntities.

Can you try a basic approach with something like the below code and share the micro-ROS Agent output using -v6? Make sure that the agent is open before connecting the board.

#include <Arduino.h>
#include <micro_ros_arduino.h>
#include <stdio.h>

#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <micro_ros_utilities/type_utilities.h>
#include <micro_ros_utilities/string_utilities.h>
#include <nav_msgs/msg/odometry.h>

#define LED_PIN 13 //used for debugging status
#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){rclErrorLoop();}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}

rcl_publisher_t odom_publisher;
nav_msgs__msg__Odometry odom_msg;

rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
rcl_timer_t control_timer;

unsigned long long time_offset = 0;
bool micro_ros_init_successful = false;

void setup() 
{
    pinMode(LED_PIN, OUTPUT);
    micro_ros_init_successful = false;
    set_microros_transports();

    micro_ros_utilities_create_message_memory(
        ROSIDL_GET_MSG_TYPE_SUPPORT(nav_msgs, msg, Odometry),
        &odom_msg,
        (micro_ros_utilities_memory_conf_t) {});

    odom_msg.header.frame_id = micro_ros_string_utilities_set(odom_msg.header.frame_id, "odom");
    odom_msg.child_frame_id = micro_ros_string_utilities_set(odom_msg.child_frame_id, "base_footprint");

    createEntities();
}

void loop() 
{
    struct timespec time_stamp = getTime();

    odom_msg.header.stamp.sec = time_stamp.tv_sec;
    odom_msg.header.stamp.nanosec = time_stamp.tv_nsec;
    odom_msg.pose.pose.orientation.w = 1.;

    RCSOFTCHECK(rcl_publish(&odom_publisher, &odom_msg, NULL));
}

....
grassjelly commented 2 years ago

I think it's crashing..here's the output:


[1648134162.581297] info     | TermiosAgentLinux.cpp | init                     | Serial port not found. | device: /ttyACM0, error 2, waiting for connection...
[1648134163.587304] info     | TermiosAgentLinux.cpp | init                     | Serial port not found. | device: /ttyACM0, error 2, waiting for connection...
[1648134164.592930] info     | TermiosAgentLinux.cpp | init                     | Serial port not found. | device: /ttyACM0, error 2, waiting for connection...
[1648134165.598368] info     | TermiosAgentLinux.cpp | init                     | Serial port not found. | device: /ttyACM0, error 2, waiting for connection...
[1648134166.604641] info     | TermiosAgentLinux.cpp | init                     | Serial port not found. | device: /ttyACM0, error 2, waiting for connection...

It says Serial Port not found but I can see the board when running: ls /dev/ttyACM*

pablogs9 commented 2 years ago

Try with the device mounted in /dev/serial/by-id/[SOMETHING RELATED TO YOUR TEENSY] I think this is safer that tty.

pablogs9 commented 2 years ago 
[1648134162.581297] info     | TermiosAgentLinux.cpp | init                     | Serial port not found. | device: /ttyACM0, error 2, waiting for connection...

This means that the agent is not finding the port or it does not have permission for using it.

grassjelly commented 2 years ago

Sorry, there was a typo. Here's the output:


1648135181.802430] info     | TermiosAgentLinux.cpp | init                     | running...             | fd: 3
[1648135181.802796] info     | Root.cpp           | set_verbose_level        | logger setup           | verbose_level: 6
[1648135181.817659] info     | Root.cpp           | create_client            | create                 | client_key: 0x4EBFC5B2, session_id: 0x81
[1648135181.817781] info     | SessionManager.hpp | establish_session        | session established    | client_key: 0x4EBFC5B2, address: 0
[1648135181.817990] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x4EBFC5B2, len: 19, data: 
0000: 81 00 00 00 04 01 0B 00 00 00 58 52 43 45 01 00 01 0F 00
[1648135181.818123] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x4EBFC5B2, len: 44, data: 
0000: 81 80 00 00 01 07 24 00 00 0A 00 01 01 03 00 00 16 00 00 00 00 01 CC 17 0E 00 00 00 6F 64 6F 6D
0020: 65 74 72 79 5F 74 65 73 74 00 00 00
[1648135181.837958] info     | ProxyClient.cpp    | create_participant       | participant created    | client_key: 0x4EBFC5B2, participant_id: 0x000(1)
[1648135181.838167] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x4EBFC5B2, len: 14, data: 
0000: 81 80 00 00 05 01 06 00 00 0A 00 01 00 00
[1648135181.838208] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x4EBFC5B2, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 01 00 00 00 80
[1648135181.838351] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x4EBFC5B2, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 01 00 00 00 80
[1648135181.838400] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x4EBFC5B2, len: 88, data: 
0000: 81 80 01 00 01 07 4D 00 00 0B 00 02 02 03 00 00 3F 00 00 00 13 00 00 00 72 74 2F 6F 64 6F 6D 2F
0020: 75 6E 66 69 6C 74 65 72 65 64 00 00 01 FF FF FF 1F 00 00 00 6E 61 76 5F 6D 73 67 73 3A 3A 6D 73
0040: 67 3A 3A 64 64 73 5F 3A 3A 4F 64 6F 6D 65 74 72 79 5F 00 00 01 00 00 00
[1648135181.838559] info     | ProxyClient.cpp    | create_topic             | topic created          | client_key: 0x4EBFC5B2, topic_id: 0x000(2), participant_id: 0x000(1)
[1648135181.838611] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x4EBFC5B2, len: 14, data: 
0000: 81 80 01 00 05 01 06 00 00 0B 00 02 00 00
[1648135181.838641] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x4EBFC5B2, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 02 00 00 00 80
[1648135181.838821] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x4EBFC5B2, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 02 00 00 00 80
[1648135181.838858] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x4EBFC5B2, len: 24, data: 
0000: 81 80 02 00 01 07 10 00 00 0C 00 03 03 03 00 00 02 00 00 00 00 00 00 01
[1648135181.839014] info     | ProxyClient.cpp    | create_publisher         | publisher created      | client_key: 0x4EBFC5B2, publisher_id: 0x000(3), participant_id: 0x000(1)
[1648135181.839102] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x4EBFC5B2, len: 14, data: 
0000: 81 80 02 00 05 01 06 00 00 0C 00 03 00 00
[1648135181.839150] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x4EBFC5B2, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 03 00 00 00 80
[1648135181.839241] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x4EBFC5B2, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 03 00 00 00 80
[1648135181.839277] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x4EBFC5B2, len: 36, data: 
0000: 81 80 03 00 01 07 1C 00 00 0D 00 05 05 03 00 00 0E 00 00 00 00 02 01 FF 02 00 01 20 05 00 00 00
0020: 00 00 00 03
[1648135181.840017] info     | ProxyClient.cpp    | create_datawriter        | datawriter created     | client_key: 0x4EBFC5B2, datawriter_id: 0x000(5), publisher_id: 0x000(3)
[1648135181.840101] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x4EBFC5B2, len: 14, data: 
0000: 81 80 03 00 05 01 06 00 00 0D 00 05 00 00
[1648135181.840137] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x4EBFC5B2, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 04 00 00 00 80
[1648135181.840257] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x4EBFC5B2, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 04 00 00 00 80
pablogs9 commented 2 years ago

This output is with the code I shared or with the one you provided?

pablogs9 commented 2 years ago

Update, I have just flashed this code in my Teensy 4.1 and use a USB-Serial transport and Arduino IDE:

#include <micro_ros_arduino.h>

#include <stdio.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <micro_ros_utilities/type_utilities.h>
#include <micro_ros_utilities/string_utilities.h>

#include <nav_msgs/msg/odometry.h>

rcl_publisher_t publisher;
nav_msgs__msg__Odometry odom_msg;

rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
rcl_timer_t timer;

#define LED_PIN 13

#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) {
    RCSOFTCHECK(rcl_publish(&publisher, &odom_msg, NULL));
  }
}

void setup() {
  set_microros_transports();

  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, HIGH);

  delay(2000);

  allocator = rcl_get_default_allocator();

  micro_ros_utilities_create_message_memory(
    ROSIDL_GET_MSG_TYPE_SUPPORT(nav_msgs, msg, Odometry),
    &odom_msg,
    (micro_ros_utilities_memory_conf_t) {});

  odom_msg.header.frame_id = micro_ros_string_utilities_set(odom_msg.header.frame_id, "odom");
  odom_msg.child_frame_id = micro_ros_string_utilities_set(odom_msg.child_frame_id, "base_footprint");

  //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(nav_msgs, msg, Odometry),
    "micro_ros_arduino_node_publisher"));

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

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

I works as expected:

image

Can you try it?

grassjelly commented 2 years ago

Hey Pablo. Thanks for helping, it works now. Earlier you've mentioned to make sure that the agent must be open before connecting the board. Does it always have to be this way? Realized that it only works when the agent is open first.

pablogs9 commented 2 years ago

You need to instantiate the micro-ROS agent before connecting the board because the client will try to connect to the agent in rclc_support_init() for a certain amount of times with a certain timeout. You can check the return value of this function and retry until the agent is reached. Also, you can ping the agent in order to wait until it is available. But in general, you should handle the client-agent connection.

Here you have a good example of how to handle this reconnection: https://github.com/micro-ROS/micro_ros_tivac_launchpad_app/blob/053ff64f635ccf8cef558e1639725abbd3c24db3/src/microros.c#L104-L135

If you don't mind I'm closing. Please reopen if you have further questions.

grassjelly commented 2 years ago

Got it. Thanks again @pablogs9

kjrusscher commented 4 months ago

I had this same problem. For others with the same problem: solution is to use the default initialization function for the odometry publisher. So replacing rclc_publisher_init_best_effort with rclc_publisher_init_default.