micro_ros_arduino The session has been established, but the nodes are not functioning correctly.

-Im using a raspberry pi pico rp2040, a raspberry pi 4 4gb and a computer to access via ssh to the raspberry pi

RTOS: FREERTOS
Installation type: I use a docker to acces micro ros: https://hub.docker.com/r/microros/micro-ros-agent (Humble) Also install the micro-xrce-dds-agent following the steps right here: https://snapcraft.io/micro-xrce-dds-agent
Im using micro ros to connect with ROS2 Humble.

Steps to reproduce the issue

Install ROS2 humble using the debian packages. Follow the instructions that is shown in this pages: https://hub.docker.com/r/microros/micro-ros-agent (humble) https://snapcraft.io/micro-xrce-dds-agent

Expected behavior

Actual behavior

The expected behavior was obtained by the computer that has the "micro-ros-agent." When you plug in the Raspberry Pi Pico, everything works fine. However, in the case where we are using a Raspberry Pi 4, we can't install the "micro-ros-agent" because the terminal says that it only works on AMD64 systems, not on ARM64. Therefore, we searched for information and found an alternative called "micro-xrce-dds-agent."

With this latter agent, the connection is initialized, but the nodes don't work imagen_2023-10-29_030953707 imagen_2023-10-29_031008002

Feel free if you want to answer me in spanish, I'm from Colombia. Your work is amazing.

Oct 29 '23 03:10 Chuspi29

This is the code we are using:

#include <iostream>

#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <std_msgs/msg/int32.h>
#include <rmw_microros/rmw_microros.h>
#include "FreeRTOS.h"
#include "task.h"
#include "pico/stdlib.h"
#include "hardware/pwm.h"
#include "hardware/adc.h"
#include "hardware/gpio.h"
extern "C" {
    #include "pico_uart_transports.h" // Archivo de encabezado correspondiente
}
#include <std_msgs/msg/float32.h>
#include <geometry_msgs/msg/vector3.h>

volatile long encoder_count = 0;
volatil
e long encoder_count2 = 0;
//Definicion global pin Led deafultc
#define LED_PIN PICO_DEFAULT_LED_PIN

//Defino pi para posterior uso
#define PI 3.14159265358979323846

float w_motor1=0;
float w_motor2=0;
float U1=0;
float U2=0;    
float e[2]={0,0};
int led_value=0;

float Wi[2] = {13,13};

//---------------------------------------------------------------------             Clase Motor                ----------------------------------------------------------------------------------------
class Motor {
    private : //atributos
    int pin_num_pwm, pin_num_in1, pin_num_in2,sensorA_pin, sensorB_pin, freq=1000 , FC1 , FC2;
    //parametros
    int ppr;
    int decoder_number;
    int gear_ratio;

    absolute_time_t prevT = get_absolute_time();
    double posPrev;
    int slice_num;
    int CH;
    
    public://metodos
    

        //Metodo constructor
        Motor(int _pin_num_pwm, int _pin_num_in1, int _pin_num_in2, int _sensorA_pin, int _sensorB_pin, int _freq){

        this-> pin_num_pwm =    _pin_num_pwm;
        this-> pin_num_in1 =    _pin_num_in1;
        this-> pin_num_in2 =    _pin_num_in2;
        this-> sensorA_pin =    _sensorA_pin;
        this-> sensorB_pin =    _sensorB_pin;
        this-> freq        =    _freq;
                
        gpio_init(this->pin_num_pwm);
        gpio_init(this->pin_num_in1);
        gpio_init(this->pin_num_in2);
        gpio_init(this->sensorA_pin);
        gpio_init(this->sensorB_pin);

        
        gpio_set_function(this->pin_num_pwm, GPIO_FUNC_PWM);
        gpio_set_dir(this->pin_num_in1, GPIO_OUT);
        gpio_set_dir(this->pin_num_in2, GPIO_OUT);
        gpio_set_dir(this->sensorA_pin, GPIO_IN);
        gpio_set_dir(this->sensorB_pin, GPIO_IN);
        gpio_pull_down(this->sensorA_pin);  
        gpio_pull_down(this->sensorB_pin);  


      
        this->slice_num = pwm_gpio_to_slice_num(this->pin_num_pwm);
        this->CH = pwm_gpio_to_channel(this->pin_num_pwm);
        pwm_set_clkdiv(slice_num,10.0);
        pwm_set_wrap(slice_num, 65535);
        pwm_set_enabled(slice_num,true);
        
        
        // Encoder and motor parameters
        this-> ppr = 12;
        this-> decoder_number = 2;
        this-> gear_ratio = 30;


        // Speed time variables
        this-> posPrev = 0;
        
        
      
        
    }
    
    
    
    
    // Metodo get para obtener pin Sensor B
    int getsensorB_pin() {
        return this->sensorB_pin;
    }
    int getsensorA_pin() {
        return this->sensorA_pin;
    }
    int getEncoderatio() {
        return (this->ppr * this->gear_ratio* this->decoder_number)/360;
    }

    //Funcion para asignar direccion y ciclo de dureza
    void set_motor(int dir,int speed){
        if (dir==1){
            gpio_put(this->pin_num_in1,0);
            gpio_put(this->pin_num_in2,1);
            
            
        }
        else if (dir==-1){
            gpio_put(this->pin_num_in1,1);
            gpio_put(this->pin_num_in2,0);
        }
        else{
            gpio_put(this->pin_num_in1,0);
            gpio_put(this->pin_num_in2,0);
        }
        
        pwm_set_chan_level(this->slice_num,this->CH, speedToU16(speed));
            
    }
    
    //Metodo conversor de porcentaje 0-100 a 16 bits
    
    int speedToU16(int speed){
        int speed_u16=speed*65535.0/100;
        return speed_u16;
        
    }
    
    //Metodo para obtener la velocidad del motor

    double encoder2speed(int encoder_count_){
        absolute_time_t currT =get_absolute_time();
        
        int64_t deltaT = absolute_time_diff_us(this-> prevT, currT);
        this -> prevT=currT;
        double resta=encoder_count_-this->posPrev;
        double vel = ((resta*1000000)/deltaT);
        this-> posPrev =encoder_count_;
        double vel_rad_raw = ((vel*2*PI)/(180));
        return vel_rad_raw;
        
    }
};


    Motor motor_1(16, 18, 17, 21,22, 1000);
    Motor motor_2(15, 13, 12, 9,8, 1000);

    int sensorB_M1=motor_1.getsensorB_pin(); 
    int  sensorA_M1= motor_1.getsensorA_pin();
    int sensorB_M2=motor_2.getsensorB_pin(); 
    int  sensorA_M2= motor_2.getsensorA_pin();



int estado=0;


//----------------------------------------------------------------------          Definicion funciones Callback Interrupciones           --------------------------------------------------    
static void encoderInterruptHandler(uint gpio, uint32_t events) {
    if(gpio==sensorA_M1){
        if(gpio_get(sensorB_M1)==1){
            encoder_count--;
        }
        else{
            encoder_count++;
        }
    }

    else if (gpio==sensorB_M1){
        if(gpio_get(sensorA_M1)==1){
            encoder_count++;
        }
        else{
            encoder_count--;
        }
    }

    else if (gpio==sensorA_M2){
        if(gpio_get(sensorB_M2)==1){
            encoder_count2--;
        }
        else{
            encoder_count2++;
        }
    }
    else if(gpio==sensorB_M2){
        if(gpio_get(sensorA_M2)==1){
            encoder_count2++;
        }
        else{
            encoder_count2--;
        }

    }

}







int ms_to_ticks(int ms){

    int ticks= (ms*100)/1000;

    return ticks;
}

//----------------------------------------------------------------------------------- Definicion de Publicadores , Mensajes etc ----------------------------------------------------------------------

rcl_publisher_t publisher_1;
geometry_msgs_msg_Vector3 msg_1;


rcl_subscription_t subscriber;
geometry_msgs_msg_Vector3 msg;



rcl_node_t node;
rcl_allocator_t allocator; //Distribuidor de memoria
rclc_support_t support; 
rclc_executor_t executor;  //Distribuidor de Tareas_uROS




//-----------------------------------------------------------------------------------  Tareas de FREERTOS ---------------------------------------------------------------------------------------------------

void stateMachine(void *pvParameters)
{   

//Variables


    //Estados
    int Estado=0; //unconfigured

            float Uprev[2]={0,0};
            float eprev[2]={0,0};
            float eprev2[2]={0,0};
            float inicial_2=0;
            float inicial=0;

            float q0[2]={0.8,0.8};
            float q1[2]={-0.7,-0.7};
            float q2[2]={0,0};




        
           


    while (true) {

        switch (Estado) {
            case 0: //unconfigured


                //Variables
                led_value=0;



             
                

                vTaskDelay(ms_to_ticks(5000));
                Estado=1;
            
            break;


            case 1: //active

                
                 w_motor1=motor_1.encoder2speed(encoder_count);
                 w_motor2=motor_2.encoder2speed(encoder_count2);
                e[0]=Wi[0]-w_motor1;
                e[1]=Wi[1]-w_motor2;




                    U1=Uprev[0]+ q0[0]e[0] + q1[0] eprev[0] + q2[0]*eprev2[0];

                    U2=Uprev[1]+ q0[1]e[1] + q1[1] eprev[1] + q2[1]*eprev2[1];
                

                float U1_abs=std::abs(U1);
                float U2_abs=std::abs(U2);
                if (U1>0){
                    motor_1.set_motor(1, U1_abs);
                        gpio_put(LED_PIN, 1);


                 } else{
                    motor_1.set_motor(-1,U1_abs);
                        gpio_put(LED_PIN, 0);
                }
                if (U2>0){
                    motor_2.set_motor(1, U2_abs);
                } else{
                    motor_2.set_motor(-1, U2_abs);
                }

                Uprev[0]=U1;
                Uprev[1]=U2;
                eprev2[0]=eprev[0];
                eprev2[1]=eprev[1];
                eprev[0]=e[0];
                eprev[1]=e[1];

                vTaskDelay(ms_to_ticks(20));

            break;
        }
}
}


void Print_Task(void *pvParameters) {
    while (1) {


                msg_1.x=w_motor1;

                msg_1.y=w_motor2;

                rcl_publish(&publisher_1, &msg_1, NULL);
                

        

        vTaskDelay(ms_to_ticks(100));
    }
}


void Spin_Task(void *pvParameters) {

    while (true)
    {
        rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));

        vTaskDelay(ms_to_ticks(50));
    }

}

void subscription_callback(const void * msgin)
{

  // Cast received message to used type
const geometry_msgs_msgVector3 *msg_vector3 = (const geometry_msgsmsg_Vector3 *)msgin;

    Wi[0] = msg_vector3->x;
    Wi[1]= msg_vector3->y;


}







//-------------------------------------------------------------------------- Main ---------------------------------------------------------------------------------------------




int main()
{



    gpio_set_irq_enabled_with_callback(sensorA_M1, GPIO_IRQ_EDGE_RISE, 1, &encoderInterruptHandler);
    gpio_set_irq_enabled_with_callback(sensorA_M2, GPIO_IRQ_EDGE_RISE, 1, &encoderInterruptHandler);
    gpio_set_irq_enabled_with_callback(sensorB_M1, GPIO_IRQ_EDGE_RISE, 1, &encoderInterruptHandler);
    gpio_set_irq_enabled_with_callback(sensorB_M2, GPIO_IRQ_EDGE_RISE, 1, &encoderInterruptHandler);

    rmw_uros_set_custom_transport(
        true,
        NULL,
        pico_serial_transport_open,
        pico_serial_transport_close,
        pico_serial_transport_write,
        pico_serial_transport_read
    ); // DEFINIR TIPO DE TRANSPORTE USB

    gpio_init(LED_PIN);
    gpio_set_dir(LED_PIN, GPIO_OUT);


    allocator = rcl_get_default_allocator(); //inicializar el allocator

    // Wait for agent successful ping for 2 minutes.
    const int timeout_ms = 1000; 
    const uint8_t attempts = 120;

    rcl_ret_t ret = rmw_uros_ping_agent(timeout_ms, attempts);



        if (ret != RCL_RET_OK)
    {
        // Unreachable agent, exiting program.
        return ret;
    }  



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

    rclc_node_init_default(&node, "pico_node", "", &support);

    rclc_publisher_init_default(
        &publisher_1,
        &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs,msg,Vector3),
        "/reptil321");

    rcl_ret_t rc = rclc_subscription_init_default(
     &subscriber, 
     &node,
     ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Vector3)
     , "/rex123");




    rclc_executor_init(&executor, &support.context, 2, &allocator);
    gpio_put(LED_PIN, 1);

    rclc_executor_add_subscription(
    &executor, &subscriber, &msg,
    &subscription_callback, ON_NEW_DATA);

    xTaskCreate(Print_Task, "Imprimir", 512, NULL, 1, NULL);
    xTaskCreate(stateMachine, "Maquina de estados", 1024, NULL, 2, NULL);
    xTaskCreate(Spin_Task, "Spin", 512, NULL, 4, NULL);
    vTaskStartScheduler();

    return 0;
}

Oct 29 '23 03:10 Chuspi29

Does the provided example works as expected?

Oct 30 '23 06:10 pablogs9

micro_ros_arduino micro_ros_arduino copied to clipboard

The session has been established, but the nodes are not functioning correctly.

Steps to reproduce the issue

Expected behavior

Actual behavior

micro_ros_arduino
micro_ros_arduino copied to clipboard