search

micro-ROS / micro_ros_setup

Support macros for building micro-ROS-based firmware.
Apache License 2.0
385 stars 138 forks source link

files to edit to achieve custom transport #383

Closed jacopagno closed 3 years ago

jacopagno commented 3 years ago

Hi! I'm trying to use CAN bus for custom transport. I read the tutorial about micro ros custom transport and this thread(#206) but i'm having problems to navigate the directory created from tutorial. For my understanding to obtain a custom transport you need to:

-change agent transport in /src/eProsoma/Micro-XRCEE-DDS-Agent change transport/custom/CustomAgent.cpp modify init/fini/recv_message/send_message.

Am i right or i need to change something else? And i need to create a main.c like the examples in the directories or it does everthing on his own?

Thanks for your help.

pablogs9 commented 3 years ago

This is the old way of modifiying transports, in order to apply a custom transport just:

Regarding the agent side, maybe @Acuadros95 can tell us some more details.

Acuadros95 commented 3 years ago

To implement your custom Agent follow the steps from the provided example: link

You have to implemente the following function prototypes for your transport:

Then, define a custom endpoint to identify each client:

With these definitions, you can generate your custom Agent using the constructor of eprosima::uxr::CustomAgent custom_agent. All of these steps are present on the example main function, feel free to come back if any specific problem arises.

jacopagno commented 3 years ago

Thank you very much for the fast answer, i try to implement all as you suggested. For now i'll close the thread.

jacopagno commented 3 years ago

hi again, i'm having problems with the client. The function rclc_support_init stops the program. I read the thread #281 and #277 but didn't find a solution. I think the problem could be in the colcon.meta:

  "names": {
        "tracetools": {
            "cmake-args": [
                "-DTRACETOOLS_DISABLED=ON",
                "-DTRACETOOLS_STATUS_CHECKING_TOOL=OFF"
            ]
        },
        "rcutils": {
            "cmake-args": [
                "-DENABLE_TESTING=OFF",
                "-DRCUTILS_NO_FILESYSTEM=ON",
                "-DRCUTILS_AVOID_DYNAMIC_ALLOCATION=ON",
                "-DRCUTILS_NO_64_ATOMIC=ON"                
            ]
        },
        "microxrcedds_client": {
            "cmake-args": [
                "-DUCLIENT_PIC=OFF",
                "-DUCLIENT_PROFILE_DISCOVERY=OFF",
                "-DUCLIENT_PROFILE_UDP=OFF",
                "-DUCLIENT_PROFILE_CUSTOM_TRANSPORT=ON",
                "-DUCLIENT_PROFILE_SERIAL=OFF",
                "-DUCLIENT_PROFILE_TCP=OFF"
            ]
        },
        "rmw_microxrcedds": {
            "cmake-args": [
                "-DRMW_UXRCE_XML_BUFFER_LENGTH=400",
                "-DRMW_UXRCE_TRANSPORT=custom",
                "-DRMW_UXRCE_MAX_NODES=1",
                "-DRMW_UXRCE_MAX_PUBLISHERS=1",
                "-DRMW_UXRCE_MAX_SUBSCRIPTIONS=1",
                "-DRMW_UXRCE_MAX_SERVICES=0",
                "-DRMW_UXRCE_MAX_CLIENTS=0",
                "-DRMW_UXRCE_MAX_HISTORY=2"
            ]
        },
        "tracetools": {
            "cmake-args": [
                "-DTRACETOOLS_DISABLED=ON",
                "-DTRACETOOLS_STATUS_CHECKING_TOOL=OFF"
            ]
        }
    }
}

or the way i've build and flash the system. I've done it through STM32cubeIDE following https://github.com/micro-ROS/micro_ros_stm32cubemx_utils. I've also tryied with ros2 run micro_ros_setup configure_firmware.sh int32_publisher --transport serial but dind't work. Thanks for the help.

pablogs9 commented 3 years ago

Please debug and tell us where (in which line) is the system stopping. Since rclc_support_init calls your custom transports, check that there is no problem in those transports.

jacopagno commented 3 years ago

thank you for the answer. I think the problem is in the connection between agent and client. The client after the reset sends 31 bytes but agent after the init tries to read 41 bytes. Is it the right behavior? The frame sends by the client is :

can0  001   [7]  7E 00 00 18 00 80 00
can0  001   [8]  00 00 00 01 10 00 58 52
can0  001   [8]  43 45 01 00 01 0F 58 51
pablogs9 commented 3 years ago

Yes, your custom transports are not behaving correctly. Maybe you want to share them and we can take a look.

jacopagno commented 3 years ago

So for the client i've used the example on STM32F446re repository and set the custom transport with

rmw_uros_set_custom_transport(
        true, // with CAN2.0 8 B payload
        (void *) &hcan1,
        freertos_custom_open,
        freertos_custom_close,
        freertos_custom_write,
        freertos_custom_read);

and for the write i send framed packet of CAN message with the last of variable length(i've made a change from the last message). 

    cycle=len/8;
    rest=len%8;

    for(cycle;cycle>0;cycle--){
        pHeader.DLC = 8;
        ret = HAL_CAN_AddTxMessage( can, &pHeader, ptr , &pTxMailbox );
        if (ret == HAL_OK)  write += 8;
        ptr += 8;
    }
    if (rest!=0){
        pHeader.DLC = rest;
        ret = HAL_CAN_AddTxMessage( can, &pHeader, ptr , &pTxMailbox );
        if (ret == HAL_OK)  write += rest;
    }

        return (ret == HAL_OK && write==len) ? write : 0;
   }
pablogs9 commented 3 years ago

Please share the whole code of the custom transports.

jacopagno commented 3 years ago

I've fixed with adding while(HAL_CAN_IsTxMessagePending(can,pTxMailbox)); after the addTxMesssage. Btw i don't understand where the issue is. The byte sent by client is the same and the requested byte are still 41 but with this line the session works.

pablogs9 commented 3 years ago

Ok, closing

jacopagno commented 3 years ago

hi again, so i've tested a bit and found a problem in deserialization. The log from client is:

[1631689648.254658] info     | Root.cpp           | set_verbose_level        | logger setup           | verbose_level: 6
[1631689648.254728] info     | custom_agent.cpp   | operator()               | This is an example of a custom Micro XRCE-DDS Agent INIT function | fd: 3
[1631689648.254733] info     | CustomAgent.cpp    | init                     | Custom agent status: opened | CAN_BUS agent running
[1631689653.171288] debug    | CustomAgent.cpp    | recv_message             | [==>> CAN_BUS <<==]    | client_key: 0x00000000, len: 24, data: 
0000: 80 00 00 00 00 01 10 00 58 52 43 45 01 00 01 0F 58 51 F4 2D 81 00 FC 01
[1631689653.171590] info     | Root.cpp           | create_client            | create                 | client_key: 0x5851F42D, session_id: 0x81
[1631689653.171678] info     | SessionManager.hpp | establish_session        | session established    | client_key: 0x1481765933, address: ID: 1
[1631689653.171912] debug    | CustomAgent.cpp    | send_message             | [** <<CAN_BUS>> **]    | client_key: 0x5851F42D, len: 19, data: 
0000: 81 00 00 00 04 01 0B 00 00 00 58 52 43 45 01 00 01 0F 00
[1631689655.452818] debug    | CustomAgent.cpp    | recv_message             | [==>> CAN_BUS <<==]    | client_key: 0x5851F42D, len: 24, data: 
0000: 80 00 00 00 00 01 10 00 58 52 43 45 01 00 01 0F 58 51 F4 2D 81 00 FC 01
[1631689655.452981] info     | SessionManager.hpp | establish_session        | session re-established | client_key: 0x1481765933, address: ID: 1
[1631689655.453153] debug    | CustomAgent.cpp    | send_message             | [** <<CAN_BUS>> **]    | client_key: 0x5851F42D, len: 19, data: 
0000: 81 00 00 00 04 01 0B 00 00 00 58 52 43 45 01 00 01 0F 00
[1631689658.460079] error    | InputMessage.cpp   | log_error                | deserialization error  | buffer: 
[1631689658.460159] debug    | CustomAgent.cpp    | recv_message             | [==>> CAN_BUS <<==]    | client_key: 0x5851F42D, len: 0, data: 
[1631689660.461112] error    | InputMessage.cpp   | log_error                | deserialization error  | buffer: 
[1631689660.461183] debug    | CustomAgent.cpp    | recv_message             | [==>> CAN_BUS <<==]    | client_key: 0x5851F42D, len: 0, data:

In this moment the client sends 42 bytes to initialize a node. The client main is the same of the example;

    rv = rmw_uros_set_custom_transport(
        true,
        (void *) &hcan1,
        freertos_serial_open,
        freertos_serial_close,
        freertos_serial_write,
        freertos_serial_read);

    rcl_allocator_t freeRTOS_allocator = rcutils_get_zero_initialized_allocator();

    freeRTOS_allocator.allocate = microros_allocate;
    freeRTOS_allocator.deallocate = microros_deallocate;
    freeRTOS_allocator.reallocate = microros_reallocate;
    freeRTOS_allocator.zero_allocate =  microros_zero_allocate;

    if (!rcutils_set_default_allocator(&freeRTOS_allocator)) {
          printf("Error on default allocators (line %d)\n", __LINE__);
      }

        allocator = rcl_get_default_allocator();
    rclc_support_init(&support, 0, NULL, &allocator);
    rclc_node_init_default(&node, "cubemx_node", "", &support);

The agent tries to read only 41 bytes and i think for this reason make a deserialization error. The agent receive is:

        uint8_t* ptr = buffer;
        uint8_t cycle = buffer_length/8;
        uint8_t rest = buffer_length%8;
        for(uint8_t cycle;cycle>0;cycle--){
             read(poll_fd.fd,&frame,sizeof(struct can_frame));
            memcpy(ptr,&(frame.data),8);
            ptr += 8;
            rv += 8;
        }
        if(rest!=0){
            read(poll_fd.fd,&frame,sizeof(struct can_frame));
            memcpy(ptr,&(frame.data),rest);
            rv += rest; 
       }
       source_endpoint->set_member_value<uint32_t>("ID",frame.ID);
       return rv;
      }

Thank you for the help.

pablogs9 commented 3 years ago

Please share the whole code of the custom transports and your main application.

jacopagno commented 3 years ago

The main application is:

uint8_t rv;
    rv = rmw_uros_set_custom_transport(
        true,
        (void *) &hcan1,
        freertos_serial_open,
        freertos_serial_close,
        freertos_serial_write,
        freertos_serial_read);

    rcl_allocator_t freeRTOS_allocator = rcutils_get_zero_initialized_allocator();

    freeRTOS_allocator.allocate = microros_allocate;
    freeRTOS_allocator.deallocate = microros_deallocate;
    freeRTOS_allocator.reallocate = microros_reallocate;
    freeRTOS_allocator.zero_allocate =  microros_zero_allocate;

    if (!rcutils_set_default_allocator(&freeRTOS_allocator)) {
          printf("Error on default allocators (line %d)\n", __LINE__);
      }

      rcl_publisher_t publisher;
      std_msgs__msg__Int32 msg;
      rclc_support_t support;
      rcl_allocator_t allocator;
      rcl_node_t node;

      allocator = rcl_get_default_allocator();

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

    // create node
    rclc_node_init_default(&node, "cubemx_node", "", &support);

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

      msg.data = 0;
  /* Infinite loop */
  for(;;)
  {
      rcl_ret_t ret = rcl_publish(&publisher, &msg, NULL);
          if (ret != RCL_RET_OK)
          {
            printf("Error publishing (line %d)\n", __LINE__);
          }

          msg.data++;
          osDelay(1000);
  }
  /* USER CODE END 5 */
}

The client custom transport is

#include <uxr/client/transport.h>

#include <rmw_microxrcedds_c/config.h>

#include "main.h"
#include "cmsis_os.h"

#include <unistd.h>
#include <stdio.h>
#include <string.h>
#include <stdbool.h>

#include "stm32f4xx_hal.h"
#include "stm32f4xx_hal_can.h"

#ifdef RMW_UXRCE_TRANSPORT_CUSTOM

// --- micro-ROS Transports ---
#define UART_DMA_BUFFER_SIZE 256

static uint8_t can_buffer[UART_DMA_BUFFER_SIZE];
static size_t can_head = 0, can_tail = 0;
static uint8_t free_level=0;
bool freertos_serial_open(struct uxrCustomTransport * transport){
    CAN_HandleTypeDef * can = (CAN_HandleTypeDef*) transport->args;

    HAL_CAN_Start(can);

    CAN_FilterTypeDef CAN_FilterInitStructure;
    //multi-client error?
    CAN_FilterInitStructure.FilterMode = CAN_FILTERMODE_IDMASK;
    CAN_FilterInitStructure.FilterScale = CAN_FILTERSCALE_32BIT;
    CAN_FilterInitStructure.FilterIdHigh = 0x0000;
    CAN_FilterInitStructure.FilterIdLow = 0x0000;
    CAN_FilterInitStructure.FilterMaskIdHigh = 0x0000;
    CAN_FilterInitStructure.FilterMaskIdLow = 0x0000;
    CAN_FilterInitStructure.FilterFIFOAssignment = CAN_FilterFIFO0;
    CAN_FilterInitStructure.FilterActivation = ENABLE;
    CAN_FilterInitStructure.FilterBank = 16;
    CAN_FilterInitStructure.SlaveStartFilterBank = 0;
    HAL_CAN_ConfigFilter( can, &CAN_FilterInitStructure );
    HAL_CAN_ActivateNotification( transport->args, CAN_IT_RX_FIFO0_MSG_PENDING | CAN_IT_TX_MAILBOX_EMPTY );

    return true;
}

bool freertos_serial_close(struct uxrCustomTransport * transport){
    CAN_HandleTypeDef * can = (CAN_HandleTypeDef*) transport->args;
    HAL_CAN_Stop(can);
    return true;
}

size_t freertos_serial_write(struct uxrCustomTransport* transport, const uint8_t * buf, size_t len, uint8_t * err){
    CAN_HandleTypeDef * can = (CAN_HandleTypeDef*) transport->args;

    HAL_StatusTypeDef ret = HAL_ERROR;
    CAN_TxHeaderTypeDef pHeader;
    size_t write=0;
    uint8_t * ptr = buf;
    uint32_t pTxMailbox;
    uint8_t cycle,rest;
    pHeader.StdId = 0X01;//rendere modulare
    pHeader.ExtId = 0X01;
    pHeader.IDE = CAN_ID_STD;
    pHeader.RTR = CAN_RTR_DATA;
    pHeader.TransmitGlobalTime = 1;

    cycle=len/8;
    rest=len%8;

    for(cycle;cycle>0;cycle--){
        pHeader.DLC = 8;
        free_level ++;
        ret = HAL_CAN_AddTxMessage( can, &pHeader, ptr , &pTxMailbox );
        while(free_level==3);
        //while(HAL_CAN_IsTxMessagePending(can,pTxMailbox));
        if (ret == HAL_OK)  write += 8;
        ptr += 8;
    }
    if (rest!=0){
        pHeader.DLC = rest;
        free_level ++;
        ret = HAL_CAN_AddTxMessage( can, &pHeader, ptr , &pTxMailbox );
        while(free_level==3);
        //while(HAL_CAN_IsTxMessagePending(can,pTxMailbox));
        if (ret == HAL_OK)  write += rest;
    }
    while(free_level!=0);
        return (ret == HAL_OK && write==len) ? write : 0;
   }

size_t freertos_serial_read(struct uxrCustomTransport* transport, uint8_t* buf, size_t len, int timeout, uint8_t* err){

    CAN_HandleTypeDef * can = (CAN_HandleTypeDef*) transport->args;

    size_t wrote = 0;
    while ((can_head != can_tail) && (wrote < len)){
        buf[wrote] = can_buffer[can_head];
        can_head = (can_head + 1) % UART_DMA_BUFFER_SIZE;
        wrote++;
    }
    HAL_Delay(1000);
    return wrote;
}

void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan){

CAN_RxHeaderTypeDef RxHeader;
uint8_t data[8];
uint32_t index;

HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &RxHeader, data);

for(index=0;index<RxHeader.DLC;index++){
can_buffer[can_tail]=data[index];
can_tail ++;
}

}

void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef *hcan){
    if(free_level>0) free_level --;
}

void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef *hcan){
    if(free_level>0) free_level --;
}

void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef *hcan){
    if(free_level>0) free_level --;
}

The agent custom is:

#include <uxr/agent/transport/custom/CustomAgent.hpp>
#include <uxr/agent/transport/endpoint/CustomEndPoint.hpp>

#include <poll.h>
#include <sys/socket.h>
#include <unistd.h>
#include <signal.h>
#include <netinet/in.h>
#include <arpa/inet.h>

#include <linux/can/raw.h>
#include <boost/asio.hpp>
/**
 * This custom XRCE Agent example attempts to show how easy is for the user to define a custom
 * Micro XRCE-DDS Agent behaviour, in terms of transport initialization and closing, and also
 * regarding read and write operations.
 * For this simple case, an UDP socket is opened on port 8888. Additionally, some information
 * messages are being printed to demonstrate the custom behaviour.
 * As the endpoint is already defined, we are using the provided
 * `eprosima::uxr::IPv4EndPoint` by the library.
 * Other transport protocols might need to implement their own endpoint struct.
 */

int main(int argc, char** argv)
{
    eprosima::uxr::Middleware::Kind mw_kind(eprosima::uxr::Middleware::Kind::FASTDDS);
    struct pollfd poll_fd;

    /**
     * @brief Agent's initialization behaviour description.
     */
    eprosima::uxr::CustomAgent::InitFunction init_function = [&]() -> bool
    {
        struct sockaddr_can addr;
        struct can_frame frame;
        struct can_frame rec_frame;
        struct ifreq ifr;

        bool rv = false;
        poll_fd.fd = socket(PF_CAN, SOCK_RAW, CAN_RAW);

        if (-1 != poll_fd.fd)
        {
            struct sockaddr_in address{};

            strcpy(ifr.ifr_name, "can0");
            ioctl(poll_fd.fd, SIOCGIFINDEX, &ifr);

            addr.can_family  = AF_CAN;
            addr.can_ifindex = ifr.ifr_ifindex;

            if (-1 != bind(poll_fd.fd,
                           (struct sockaddr *)&addr,
                           sizeof(addr)))
            {
                poll_fd.events = POLLIN;
                rv = true;

                UXR_AGENT_LOG_INFO(
                    UXR_DECORATE_GREEN(
                        "This is an example of a custom Micro XRCE-DDS Agent INIT function"),
                    "fd: {}",
                    poll_fd.fd);
            }
        }

        return rv;
    };

    /**
     * @brief Agent's destruction actions.
     */
    eprosima::uxr::CustomAgent::FiniFunction fini_function = [&]() -> bool
    {
        if (-1 == poll_fd.fd)
        {
            return true;
        }

        if (0 == close(poll_fd.fd))
        {
            poll_fd.fd = -1;

            UXR_AGENT_LOG_INFO(
                UXR_DECORATE_GREEN(
                    "This is an example of a custom Micro XRCE-DDS Agent FINI function"),
                "fd: {}",
                poll_fd.fd);

            return true;
        }
        else
        {
            return false;
        }
    };

    /**
     * @brief Agent's incoming data functionality.
     */
    eprosima::uxr::CustomAgent::RecvMsgFunction recv_msg_function = [&](
            eprosima::uxr::CustomEndPoint* source_endpoint,
            uint8_t* buffer,
            size_t buffer_length,
            int timeout,
            eprosima::uxr::TransportRc& transport_rc) -> ssize_t
    {
        struct can_frame frame;
        uint8_t cycle,rest;

        uint8_t support[100];
        uint8_t index=0;
        uint16_t rv=0;
        uint8_t i;
        uint16_t ret;

        uint8_t* ptr = buffer;
        cycle = buffer_length/8;
        rest = buffer_length%8;
        for(cycle;cycle>0;cycle--){
            read(poll_fd.fd,&frame,sizeof(struct can_frame));
            memcpy(ptr,&(frame.data),8);
            ptr += 8;
            rv += 8;
        }
        if(rest!=0){
            read(poll_fd.fd,&frame,sizeof(struct can_frame));
            memcpy(ptr,&(frame.data),rest);
            rv += rest; 
            }
        source_endpoint->set_member_value<uint32_t>("ID",frame.can_id);
        return rv;

    };

    /**
     * @brief Agent's outcoming data flow definition.
     */
    eprosima::uxr::CustomAgent::SendMsgFunction send_msg_function = [&](
        const eprosima::uxr::CustomEndPoint* destination_endpoint,
        uint8_t* buffer,
        size_t message_length,
        eprosima::uxr::TransportRc& transport_rc) -> ssize_t
    {

        struct can_frame frame;
        ssize_t byte_sent = 0;
        uint8_t cycle,rest; 
        uint8_t* ptr = buffer;
        int rv;
        //frame.can_id = frame.can_id | (destination_endpoint->get_addr());
        frame.can_id = frame.can_id = destination_endpoint->get_member<uint32_t>("ID");
        frame.can_id = frame.can_id | (0 << 29); //data frame
        frame.can_id = frame.can_id | (0 << 30); //standard
        frame.can_id = frame.can_id | (0 << 31); // 11 bit ID

        cycle=message_length / 8;
        rest=message_length % 8;

    for(cycle;cycle>0;cycle--){
        frame.can_dlc = 8;
        memcpy(&(frame.data),ptr,frame.can_dlc);
        rv = write(poll_fd.fd,&frame,sizeof(struct can_frame));

        if (rv != -1)  byte_sent += 8;
        ptr += 8;
    }
    if (rest!=0){
        frame.can_dlc = rest;
        memcpy(&(frame.data),ptr,frame.can_dlc);
        rv = write(poll_fd.fd,&frame,sizeof(can_frame));
        if (rv != -1)  byte_sent += rest;
    }

        return byte_sent;
    };

    /**
     * Run the main application.
     */
    try
    {
        /**
         * EndPoint definition for this transport. We define an address and a port.
         */
        eprosima::uxr::CustomEndPoint custom_endpoint;
        custom_endpoint.add_member<uint32_t>("ID");
        /**
         * Create a custom agent instance.
         */
        eprosima::uxr::CustomAgent custom_agent(
            "CAN_BUS",
            &custom_endpoint,
            mw_kind,
            true,
            init_function,
            fini_function,
            send_msg_function,
            recv_msg_function);

        /**
         * Set verbosity level
         */
        custom_agent.set_verbose_level(6);

        /**
         * Run agent and wait until receiving an stop signal.
         */
        custom_agent.start();

        int n_signal = 0;
        sigset_t signals;
        sigwait(&signals, &n_signal);

        /**
         * Stop agent, and exit.
         */
        custom_agent.stop();
        return 0;
    }
    catch (const std::exception& e)
    {
        std::cout << e.what() << std::endl;
        return 1;
    }
}
pablogs9 commented 3 years ago

When using framing in a micro-ROS transport the behaviour of the callbacks are:

Check my comments in your log:

[1631689648.254658] info     | Root.cpp           | set_verbose_level        | logger setup           | verbose_level: 6
[1631689648.254728] info     | custom_agent.cpp   | operator()               | This is an example of a custom Micro XRCE-DDS Agent INIT function | fd: 3
[1631689648.254733] info     | CustomAgent.cpp    | init                     | Custom agent status: opened | CAN_BUS agent running
// Here the client writes 24 B to init a XRCE session
[1631689653.171288] debug    | CustomAgent.cpp    | recv_message             | [==>> CAN_BUS <<==]    | client_key: 0x00000000, len: 24, data: 
0000: 80 00 00 00 00 01 10 00 58 52 43 45 01 00 01 0F 58 51 F4 2D 81 00 FC 01
[1631689653.171590] info     | Root.cpp           | create_client            | create                 | client_key: 0x5851F42D, session_id: 0x81
// The agent receives them correctly and creates a XRCE session
[1631689653.171678] info     | SessionManager.hpp | establish_session        | session established    | client_key: 0x1481765933, address: ID: 1
// The agent answer the sesion creation request with 19 B
[1631689653.171912] debug    | CustomAgent.cpp    | send_message             | [** <<CAN_BUS>> **]    | client_key: 0x5851F42D, len: 19, data: 
0000: 81 00 00 00 04 01 0B 00 00 00 58 52 43 45 01 00 01 0F 00
// Ans here the client tries to create a session one more time with 24 B. Has the client received the confirmation from the agent?
[1631689655.452818] debug    | CustomAgent.cpp    | recv_message             | [==>> CAN_BUS <<==]    | client_key: 0x5851F42D, len: 24, data: 
0000: 80 00 00 00 00 01 10 00 58 52 43 45 01 00 01 0F 58 51 F4 2D 81 00 FC 01
// The agent regenerates the session as requested
[1631689655.452981] info     | SessionManager.hpp | establish_session        | session re-established | client_key: 0x1481765933, address: ID: 1
// And send the session confirmation again
[1631689655.453153] debug    | CustomAgent.cpp    | send_message             | [** <<CAN_BUS>> **]    | client_key: 0x5851F42D, len: 19, data: 
0000: 81 00 00 00 04 01 0B 00 00 00 58 52 43 45 01 00 01 0F 00
[1631689658.460079] error    | InputMessage.cpp   | log_error                | deserialization error  | buffer: 
// Here the canbus receiver in the agent has returned a 0 lenght, have you set transport_rc?
[1631689658.460159] debug    | CustomAgent.cpp    | recv_message             | [==>> CAN_BUS <<==]    | client_key: 0x5851F42D, len: 0, data: 
[1631689660.461112] error    | InputMessage.cpp   | log_error                | deserialization error  | buffer: 
[1631689660.461183] debug    | CustomAgent.cpp    | recv_message             | [==>> CAN_BUS <<==]    | client_key: 0x5851F42D, len: 0, data:

So, answering your question, those 40 and 41 that you are mentioning are just:

In the case of the agent, if you read 0 data, you have to set transport_rc. One approach that I would take is to handle an only CAN message each time in the tx/rx callbacks

Maybe @Acuadros95 can add something else.

Acuadros95 commented 3 years ago

I think this is the most relevant part in your case:

* In **read callbacks**: you have `timeout` ms to receive up to `len` Bytes and write them in `buf`. But you can receive less and return the number of Bytes received.

You should use the timeout in both of your read functions, but focus on the Agent side first.

As @pablogs9 explained. the read function should try to read up to len bytes and return the actual data length when the timeout is completed, check this custom read function which takes the timeout into consideration: link

jacopagno commented 3 years ago

Ok i changed all the function and i think i'm almost fine. I have only a issue when create participant. I get the message but get DDS error. The log is

 [1631775217.406482] debug    | CustomAgent.cpp    | recv_message             | [==>> CAN_BUS <<==]    | client_key: 0x5851F42D, len: 44, data: 
0000: 81 80 00 00 01 07 22 00 00 0A 00 01 01 03 00 00 14 00 00 00 00 01 00 00 0C 00 00 00 63 75 62 65
0020: 6D 78 5F 6E 6F 64 65 00 00 00 00 00
[1631775217.406782] debug    | ProxyClient.cpp    | create_participant       | DDS error              | client_key: 0x5851F42D, participant_id: 0x000(1)

Thank you very much for the support.

pablogs9 commented 3 years ago

How are you running the agent? copy here the command.

Also, do you have a recent version of the agent and client libraries?

jacopagno commented 3 years ago 
 eprosima::uxr::CustomAgent::SendMsgFunction send_msg_function = [&](
        const eprosima::uxr::CustomEndPoint* destination_endpoint,
        uint8_t* buffer,
        size_t message_length,
        eprosima::uxr::TransportRc& transport_rc) -> ssize_t
    {

        struct can_frame frame;
        int rv;
        static uint8_t byte_write=0;
        UXR_AGENT_LOG_INFO(UXR_DECORATE_BLUE("Byte"),"to write: {}",message_length);
        frame.can_id = frame.can_id = destination_endpoint->get_member<uint32_t>("ID");
        frame.can_id = frame.can_id | (0 << 29); //data frame
        frame.can_id = frame.can_id | (0 << 30); //standard
        frame.can_id = frame.can_id | (0 << 31); // 11 bit ID
        static uint8_t index=0;
        static uint8_t* ptr = buffer;

        if(index == 0 && ptr != buffer) ptr = buffer;

        if(message_length-index>=8){
            frame.can_dlc = 8;
            memcpy(frame.data,ptr,frame.can_dlc);
            rv = write(poll_fd.fd,&frame,sizeof(struct can_frame));
            byte_write = 8;
            index += 8;
            ptr += 8;

        }

        else{
            frame.can_dlc = message_length-index;
            memcpy(frame.data,ptr,frame.can_dlc);
            rv = write(poll_fd.fd,&frame,sizeof(struct can_frame));
            byte_write = frame.can_dlc;
            index += frame.can_dlc;
            ptr += frame.can_dlc;
        }
        usleep(100);
        if(index == message_length) index = 0;
         UXR_AGENT_LOG_INFO(UXR_DECORATE_BLUE("Byte"),"to write: {}",frame.can_dlc);

        //if(rv!=-1) byte_write = frame.can_dlc;
        //transport_rc = eprosima::uxr::TransportRc::ok;

    return byte_write;
}

I've tried to send only up to byte but if i don't receive all the byte requested give me error that the bytes read are 0.

Acuadros95 commented 3 years ago

The DDS error is related to the interaction between the Agent and ROS2 middleware. Which branch and commit of the agent are you using to build your custom agent?

pablogs9 commented 3 years ago

If you receive 0 bytes you should also set transport_rc = eprosima::uxr::TransportRc::timeout_error

jacopagno commented 3 years ago

https://github.com/micro-ROS/micro-ROS-Agent/tree/foxy i use this.