rclc icon indicating copy to clipboard operation
rclc copied to clipboard

Published 20 hours ago • verified publisher icon ros2

Lifecycle service callback isuue

Open SourabhPrasad opened this issue 2 years ago • 17 comments

  • Hardware description: Teensy 3.6
  • Firmware: Arduino
  • Installation type: micro_ros_platformio
  • Version or commit hash: humble

Hi, I am trying to create a lifecycle node using micro_ros_platformio.

Code (main.cpp)

#include <stdio.h>
#include <unistd.h>
#include <ArduinoJson.h>
#include <SPI.h>
#include <SD.h>
#include <TeensyStep.h>

#include <micro_ros_platformio.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <rclc_parameter/rclc_parameter.h>
#include <rcl/error_handling.h>

#include "rclc_lifecycle/rclc_lifecycle.h"
#include <lifecycle_msgs/msg/transition_description.h>
#include <lifecycle_msgs/msg/transition_event.h>
#include <lifecycle_msgs/srv/change_state.h>
#include <lifecycle_msgs/srv/get_state.h>
#include <lifecycle_msgs/srv/get_available_states.h>
#include <lifecycle_msgs/srv/get_available_transitions.h>

#include <rcl_interfaces/msg/log.h>

//define motor pins
#define STEP    2
#define DIR     3
#define ENABLE  4
#define ALARM   5
#define BRAKE   6

#define RCCHECK(fn)     { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){errorLoop();}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}

void errorLoop()
{
  while (1) {
    digitalWrite(13, !digitalRead(13));
    delay(100);
  }
}

rcl_node_t node;
rclc_lifecycle_node_t lifecycleNode;
rclc_support_t support;
rcl_allocator_t allocator;
rclc_executor_t executor;
rclc_parameter_server_t parameterServer;

static const char * jointID          = "j0001";

//create motor control objects
Stepper motor(STEP, DIR);
StepControl stepMotor;
RotateControl rotateMotor;

rcl_ret_t onJointConfigure()
{
  for(int i=0; i<4; i++)
  {
    digitalWrite(13, !digitalRead(13));
    delay(500);
  }
  return RCL_RET_OK;
}

rcl_ret_t onJointActivate()
{
  digitalWrite(13, HIGH);
  return RCL_RET_OK;
}

rcl_ret_t onJointDeactivate()
{
  digitalWrite(13, LOW);
  return RCL_RET_OK;
}

rcl_ret_t onJointCleanup()
{
  for(int i=0; i<6; i++)
  {
    digitalWrite(13, !digitalRead(13));
    delay(500);
  }
  return RCL_RET_OK;
}

void setup() {
  Serial.begin(115200);
  pinMode(13, OUTPUT);

  set_microros_serial_transports(Serial);
  delay(2000);
  allocator = rcl_get_default_allocator();
  rclc_support_init(&support, 0, NULL, &allocator);

  //Initialise node and parameter server
  RCCHECK(rclc_node_init_default(&node, jointID, "", &support));
  //Make the node to lifecycle node
  rcl_lifecycle_state_machine_t stateMachine_ = rcl_lifecycle_get_zero_initialized_state_machine();
  RCCHECK(rclc_make_node_a_lifecycle_node(
    &lifecycleNode,
    &node,
    &stateMachine_,
    &allocator,
    true
  ));

  //initialise executor
  RCCHECK(rclc_executor_init(&executor, &support.context, 6, &allocator));
  rclc_lifecycle_service_context_t context;
  context.lifecycle_node = &lifecycleNode;
  RCCHECK(rclc_lifecycle_init_get_state_server(&context, &executor));
  RCCHECK(rclc_lifecycle_init_get_available_states_server(&context, &executor));
  RCCHECK(rclc_lifecycle_init_change_state_server(&context, &executor));

  RCCHECK(rclc_lifecycle_register_on_configure(&lifecycleNode, &onJointConfigure));
  RCCHECK(rclc_lifecycle_register_on_activate(&lifecycleNode, &onJointActivate));
  RCCHECK(rclc_lifecycle_register_on_deactivate(&lifecycleNode, &onJointDeactivate));
  RCCHECK(rclc_lifecycle_register_on_cleanup(&lifecycleNode, &onJointCleanup));
}

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

colcon_lowmem.meta

{
    "names": {
        "rmw_microxrcedds": {
            "cmake-args": [
                "-DRMW_UXRCE_MAX_NODES=1",
                "-DRMW_UXRCE_MAX_PUBLISHERS=5",
                "-DRMW_UXRCE_MAX_SUBSCRIPTIONS=5",
                "-DRMW_UXRCE_MAX_SERVICES=6",
                "-DRMW_UXRCE_MAX_CLIENTS=1",
                "-DRMW_UXRCE_MAX_HISTORY=1",
                "-DRMW_UXRCE_TRANSPORT=custom"
            ]
        }
    }
}

The callback for the lifecycle services does not get called.

Output

└─➞ ros2 service call /j0001/get_state lifecycle_msgs/srv/GetState {}
requester: making request: lifecycle_msgs.srv.GetState_Request()

Expected behavior

Response is given is by the server

Actual behavior

No response is given by the server

Any help is appreciated Thank you!

SourabhPrasad avatar Jun 10 '22 09:06 SourabhPrasad

@Cryoschrome does a normal subscriber callback work on your platform? CC @pablogs9 do you have this hardware available ro reproduce this test?

JanStaschulat avatar Jun 14 '22 13:06 JanStaschulat

Thanks for the reply @JanStaschulat! Yes, normal subscriber and service callbacks works.

SourabhPrasad avatar Jun 14 '22 14:06 SourabhPrasad

Some points:

  1. rclc_lifecycle_service_context_t context; is in the setup() stack and probably overwritten when loop() stack starts. Is this context important in the lifecycle workflow?
  2. Could you share the log of the micro-ROS Agent with the flag -v6?

Edit: also rcl_lifecycle_state_machine_t stateMachine_ lifespan is constrained to setup() call. Are those two variables supposed to be global in this code?

pablogs9 avatar Jun 14 '22 14:06 pablogs9

Compared to the lifecycle example the context, stateMachine_ and other lifecycle related variables must be in the scope when you call spin_some. This is the case in this example, because there is only one main function.

On your setup, you use two functions. Try using global variables, i.e. for stateMachine_ , so they are in scope in both - the setup() and loop() function.

JanStaschulat avatar Jun 14 '22 14:06 JanStaschulat

Thanks, @JanStaschulat. Please @Cryoschrome could you try and report? If it still does not work, I will try to replicate it here.

pablogs9 avatar Jun 14 '22 14:06 pablogs9

Thanks @pablogs9 and @JanStaschulat! Will makes the changes as per the points you mentioned and report back ASAP!

SourabhPrasad avatar Jun 14 '22 14:06 SourabhPrasad

So, I have made both context and stateMachine_ global, not the code goes into the errorLoop() when executing rclc_make_node_a_lifecucle_node(). I have provided the updated code and agent log below.

Updated Code

#include <stdio.h>
#include <unistd.h>
#include <ArduinoJson.h>
#include <SPI.h>
#include <SD.h>
#include <TeensyStep.h>

#include <micro_ros_platformio.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <rclc_parameter/rclc_parameter.h>
#include <rcl/error_handling.h>

#include "rclc_lifecycle/rclc_lifecycle.h"
#include <lifecycle_msgs/msg/transition_description.h>
#include <lifecycle_msgs/msg/transition_event.h>
#include <lifecycle_msgs/srv/change_state.h>
#include <lifecycle_msgs/srv/get_state.h>
#include <lifecycle_msgs/srv/get_available_states.h>
#include <lifecycle_msgs/srv/get_available_transitions.h>

#include <rcl_interfaces/msg/log.h>

//define motor pins
#define STEP    2
#define DIR     3
#define ENABLE  4
#define ALARM   5
#define BRAKE   6

#define RCCHECK(fn)     { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){errorLoop();}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}

void errorLoop()
{
  while (1) {
    digitalWrite(13, !digitalRead(13));
    delay(100);
  }
}

rcl_node_t node;
rclc_lifecycle_node_t lifecycleNode;
rclc_lifecycle_service_context_t context;
rcl_lifecycle_state_machine_t stateMachine_;
rclc_support_t support;
rcl_allocator_t allocator;
rclc_executor_t executor;
rclc_parameter_server_t parameterServer;

static const char * jointID = "j0001";

//create motor control objects
Stepper motor(STEP, DIR);
StepControl stepMotor;
RotateControl rotateMotor;

rcl_ret_t onJointConfigure()
{
  for(int i=0; i<4; i++)
  {
    digitalWrite(13, !digitalRead(13));
    delay(500);
  }
  return RCL_RET_OK;
}

rcl_ret_t onJointActivate()
{
  digitalWrite(13, HIGH);
  return RCL_RET_OK;
}

rcl_ret_t onJointDeactivate()
{
  digitalWrite(13, LOW);
  return RCL_RET_OK;
}

rcl_ret_t onJointCleanup()
{
  for(int i=0; i<6; i++)
  {
    digitalWrite(13, !digitalRead(13));
    delay(500);
  }
  return RCL_RET_OK;
}

void setup() {
  Serial.begin(115200);
  pinMode(13, OUTPUT);

  set_microros_serial_transports(Serial);
  delay(2000);
  allocator = rcl_get_default_allocator();
  rclc_support_init(&support, 0, NULL, &allocator);

  //Initialise node and parameter server
  RCCHECK(rclc_node_init_default(&node, jointID, "", &support));
  stateMachine_ = rcl_lifecycle_get_zero_initialized_state_machine();
  //Make the node to lifecycle node
  RCCHECK(rclc_make_node_a_lifecycle_node(
    &lifecycleNode,
    &node,
    &stateMachine_,
    &allocator,
    true
  ));

  //initialise executor
  RCCHECK(rclc_executor_init(&executor, &support.context, 6, &allocator));
  context.lifecycle_node = &lifecycleNode;
  RCCHECK(rclc_lifecycle_init_get_state_server(&context, &executor));
  RCCHECK(rclc_lifecycle_init_get_available_states_server(&context, &executor));
  RCCHECK(rclc_lifecycle_init_change_state_server(&context, &executor));

  RCCHECK(rclc_lifecycle_register_on_configure(&lifecycleNode, &onJointConfigure));
  RCCHECK(rclc_lifecycle_register_on_activate(&lifecycleNode, &onJointActivate));
  RCCHECK(rclc_lifecycle_register_on_deactivate(&lifecycleNode, &onJointDeactivate));
  RCCHECK(rclc_lifecycle_register_on_cleanup(&lifecycleNode, &onJointCleanup));
}

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

Agent log

docker run -it --rm -v /dev:/dev --privileged --net=host microros/micro-ros-agent:humble serial --dev /dev/ttyACM0 -v6
[1655276895.062033] info     | TermiosAgentLinux.cpp | init                     | running...             | fd: 3
[1655276895.062305] info     | Root.cpp           | set_verbose_level        | logger setup           | verbose_level: 6
[1655276896.102016] info     | TermiosAgentLinux.cpp | fini                     | server stopped         | fd: 3
[1655276896.112123] info     | TermiosAgentLinux.cpp | init                     | Serial port not found. | device: /dev/ttyACM0, error 2, waiting for connection...
[1655276897.114265] info     | TermiosAgentLinux.cpp | init                     | Serial port not found. | device: /dev/ttyACM0, error 2, waiting for connection...
[1655276898.040509] info     | TermiosAgentLinux.cpp | init                     | running...             | fd: 3
[1655276900.059096] info     | Root.cpp           | create_client            | create                 | client_key: 0x6460B8CA, session_id: 0x81
[1655276900.059171] info     | SessionManager.hpp | establish_session        | session established    | client_key: 0x6460B8CA, address: 0
[1655276900.059320] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x6460B8CA, len: 19, data: 
0000: 81 00 00 00 04 01 0B 00 00 00 58 52 43 45 01 00 01 0F 00
[1655276900.064604] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x6460B8CA, len: 36, data: 
0000: 81 80 00 00 01 07 1C 00 00 0A 00 01 01 03 00 00 0E 00 00 00 00 01 FA 3D 06 00 00 00 6A 30 30 30
0020: 31 00 00 00
[1655276900.075719] info     | ProxyClient.cpp    | create_participant       | participant created    | client_key: 0x6460B8CA, participant_id: 0x000(1)
[1655276900.075879] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x6460B8CA, len: 14, data: 
0000: 81 80 00 00 05 01 06 00 00 0A 00 01 00 00
[1655276900.075906] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x6460B8CA, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 01 00 00 00 80
[1655276900.076255] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x6460B8CA, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 01 00 00 00 80
[1655276900.080541] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x6460B8CA, len: 104, data: 
0000: 81 80 01 00 01 07 5E 00 00 0B 00 02 02 03 00 00 50 00 00 00 1A 00 00 00 72 74 2F 6A 30 30 30 31
0020: 2F 74 72 61 6E 73 69 74 69 6F 6E 5F 65 76 65 6E 74 00 00 01 2C 00 00 00 6C 69 66 65 63 79 63 6C
0040: 65 5F 6D 73 67 73 3A 3A 6D 73 67 3A 3A 64 64 73 5F 3A 3A 54 72 61 6E 73 69 74 69 6F 6E 45 76 65
0060: 6E 74 5F 00 00 01 00 00
[1655276900.080733] info     | ProxyClient.cpp    | create_topic             | topic created          | client_key: 0x6460B8CA, topic_id: 0x000(2), participant_id: 0x000(1)
[1655276900.080793] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x6460B8CA, len: 14, data: 
0000: 81 80 01 00 05 01 06 00 00 0B 00 02 00 00
[1655276900.080827] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x6460B8CA, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 02 00 00 00 80
[1655276900.085576] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x6460B8CA, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 02 00 00 00 80
[1655276900.085613] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x6460B8CA, 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
[1655276900.085731] info     | ProxyClient.cpp    | create_publisher         | publisher created      | client_key: 0x6460B8CA, publisher_id: 0x000(3), participant_id: 0x000(1)
[1655276900.085803] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x6460B8CA, len: 14, data: 
0000: 81 80 02 00 05 01 06 00 00 0C 00 03 00 00
[1655276900.085834] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x6460B8CA, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 03 00 00 00 80
[1655276900.090543] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x6460B8CA, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 03 00 00 00 80
[1655276900.090579] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x6460B8CA, 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 1F 03 00 01 00 0A 00 00 00
0020: 00 00 00 03
[1655276900.091347] info     | ProxyClient.cpp    | create_datawriter        | datawriter created     | client_key: 0x6460B8CA, datawriter_id: 0x000(5), publisher_id: 0x000(3)
[1655276900.091425] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x6460B8CA, len: 14, data: 
0000: 81 80 03 00 05 01 06 00 00 0D 00 05 00 00
[1655276900.091455] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x6460B8CA, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 04 00 00 00 80
[1655276900.091874] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x6460B8CA, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 04 00 00 00 80
[1655276900.096544] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x6460B8CA, len: 224, data: 
0000: 81 80 04 00 01 07 D5 00 00 0E 00 08 08 03 00 00 C7 00 00 00 14 00 00 00 2F 6A 30 30 30 31 2F 63
0020: 68 61 6E 67 65 5F 73 74 61 74 65 00 30 00 00 00 6C 69 66 65 63 79 63 6C 65 5F 6D 73 67 73 3A 3A
0040: 73 72 76 3A 3A 64 64 73 5F 3A 3A 43 68 61 6E 67 65 53 74 61 74 65 5F 52 65 71 75 65 73 74 5F 00
0060: 31 00 00 00 6C 69 66 65 63 79 63 6C 65 5F 6D 73 67 73 3A 3A 73 72 76 3A 3A 64 64 73 5F 3A 3A 43
0080: 68 61 6E 67 65 53 74 61 74 65 5F 52 65 73 70 6F 6E 73 65 5F 00 01 00 00 1D 00 00 00 72 71 2F 6A
00A0: 30 30 30 31 2F 63 68 61 6E 67 65 5F 73 74 61 74 65 52 65 71 75 65 73 74 00 01 FF 1F 1B 00 00 00
00C0: 72 72 2F 6A 30 30 30 31 2F 63 68 61 6E 67 65 5F 73 74 61 74 65 52 65 70 6C 79 00 00 01 00 00 00
[1655276900.097697] info     | ProxyClient.cpp    | create_replier           | replier created        | client_key: 0x6460B8CA, requester_id: 0x000(7), participant_id: 0x000(1)
[1655276900.097776] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x6460B8CA, len: 14, data: 
0000: 81 80 04 00 05 01 06 00 00 0E 00 08 00 00
[1655276900.097797] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x6460B8CA, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 05 00 00 00 80
[1655276900.102526] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x6460B8CA, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 05 00 00 00 80
[1655276900.102557] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x6460B8CA, len: 44, data: 
0000: 81 80 05 00 08 01 10 00 00 0F 00 08 80 00 00 01 FF FF 00 00 00 00 00 00 08 01 10 00 00 10 00 08
0020: 00 00 00 01 00 00 00 00 00 00 00 00
[1655276900.102882] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x6460B8CA, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 06 00 00 00 80
[1655276901.102554] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x6460B8CA, len: 12, data: 
0000: 81 80 06 00 03 01 04 00 00 11 00 08
[1655276901.103110] debug    | ProxyClient.cpp    | delete_object_unlock     | object deleted         | client_key: 0x6460B8CA, object_id: 0x0000
[1655276901.103181] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x6460B8CA, len: 14, data: 
0000: 81 80 05 00 05 01 06 00 00 11 00 08 00 00
[1655276901.103198] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x6460B8CA, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 07 00 00 00 80
[1655276901.107535] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x6460B8CA, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 06 00 00 00 80
[1655276901.107566] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x6460B8CA, len: 12, data: 
0000: 81 80 07 00 03 01 04 00 00 12 00 02
[1655276901.107614] debug    | ProxyClient.cpp    | delete_object_unlock     | object deleted         | client_key: 0x6460B8CA, object_id: 0x0000
[1655276901.107679] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x6460B8CA, len: 14, data: 
0000: 81 80 06 00 05 01 06 00 00 12 00 02 00 00
[1655276901.107695] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x6460B8CA, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 08 00 00 00 80
[1655276901.112716] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x6460B8CA, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 07 00 00 00 80
[1655276901.112764] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x6460B8CA, len: 20, data: 
0000: 81 80 08 00 03 01 04 00 00 13 00 05 03 01 04 00 00 14 00 03
[1655276901.113062] debug    | ProxyClient.cpp    | delete_object_unlock     | object deleted         | client_key: 0x6460B8CA, object_id: 0x0000
[1655276901.113114] debug    | ProxyClient.cpp    | delete_object_unlock     | object deleted         | client_key: 0x6460B8CA, object_id: 0x0000
[1655276901.113147] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x6460B8CA, len: 14, data: 
0000: 81 80 07 00 05 01 06 00 00 13 00 05 00 00
[1655276901.113168] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x6460B8CA, len: 14, data: 
0000: 81 80 08 00 05 01 06 00 00 14 00 03 00 00
[1655276901.113182] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x6460B8CA, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 09 00 00 00 80
[1655276901.117559] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x6460B8CA, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 08 00 00 00 80
[1655276901.117590] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x6460B8CA, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 09 00 00 00 80

SourabhPrasad avatar Jun 15 '22 07:06 SourabhPrasad

Your micro-ROS client is only creating a lifecycle_msgs/TransitionEvent publisher and a lifecycle_msgs/ChangeState service and then is failing.

Maybe this is related to the colcon meta configuration issue with platform.io: https://github.com/micro-ROS/micro_ros_platformio/issues/33

In order to check this, could you find rmw_microxrcedds_c/config.h in your include folder and check if #define RMW_UXRCE_MAX_SERVICES 6 is correctly set?

pablogs9 avatar Jun 15 '22 07:06 pablogs9

I checked the rmw_microxrcedds_c/config.h and no #define RMW_UXRCE_MAX_SERVICES is not set correctly, its is set as 1. Would updating it to the correct value in rmw_microxrcedds_c/config.h fix the issue?

SourabhPrasad avatar Jun 15 '22 08:06 SourabhPrasad

Ok, the colcon metas are misbehaving.

You need to create a user meta (instead of modifying the provided colcon_lowmem.meta) as explained here: https://github.com/micro-ROS/micro_ros_platformio#other-configuration

In order to make this "user meta" work, please use this branch: https://github.com/micro-ROS/micro_ros_platformio/pull/34

Just note that this branch is not working in platformio 6.0.2, so make sure that you have an older version.

Also make sure that you start in a clean environment, with no micro-ROS built.

Sorry for this workaround, but we are just working in this fix right now. If you could provide feedback it would be nice.

pablogs9 avatar Jun 15 '22 08:06 pablogs9

Any update on this?

pablogs9 avatar Jun 20 '22 05:06 pablogs9

Sorry, didn't get the time to work on this. I implemented the fix you mentioned in the previous reply and confirmed that the rmw_microxrcedds_c/config.h is being created correctly. I ran the my initial program currently I am getting a reponse from the /node/get_state and /node/get_available_state services. But still no response is given by /node/change_state service. I tried increase the number of services in user.meta to 15 and still no luck.

Agent log

[1655988890.036268] info     | TermiosAgentLinux.cpp | init                     | running...             | fd: 3
[1655988890.036624] info     | Root.cpp           | set_verbose_level        | logger setup           | verbose_level: 6
[1655988890.766793] info     | TermiosAgentLinux.cpp | fini                     | server stopped         | fd: 3
[1655988890.776950] info     | TermiosAgentLinux.cpp | init                     | Serial port not found. | device: /dev/ttyACM0, error 2, waiting for connection...
[1655988891.778083] info     | TermiosAgentLinux.cpp | init                     | Serial port not found. | device: /dev/ttyACM0, error 2, waiting for connection...
[1655988892.782521] info     | TermiosAgentLinux.cpp | init                     | Serial port not found. | device: /dev/ttyACM0, error 2, waiting for connection...
[1655988893.224660] info     | TermiosAgentLinux.cpp | init                     | running...             | fd: 3
[1655988895.243311] info     | Root.cpp           | create_client            | create                 | client_key: 0x1D91D971, session_id: 0x81
[1655988895.243388] info     | SessionManager.hpp | establish_session        | session established    | client_key: 0x1D91D971, address: 0
[1655988895.243525] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x1D91D971, len: 19, data: 
0000: 81 00 00 00 04 01 0B 00 00 00 58 52 43 45 01 00 01 0F 00
[1655988895.248891] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x1D91D971, len: 36, data: 
0000: 81 80 00 00 01 07 1C 00 00 0A 00 01 01 03 00 00 0E 00 00 00 00 01 7A 3D 06 00 00 00 6A 30 30 30
0020: 31 00 00 00
[1655988895.257751] info     | ProxyClient.cpp    | create_participant       | participant created    | client_key: 0x1D91D971, participant_id: 0x000(1)
[1655988895.257839] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x1D91D971, len: 14, data: 
0000: 81 80 00 00 05 01 06 00 00 0A 00 01 00 00
[1655988895.257858] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x1D91D971, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 01 00 00 00 80
[1655988895.258230] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x1D91D971, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 01 00 00 00 80
[1655988895.262637] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x1D91D971, len: 104, data: 
0000: 81 80 01 00 01 07 5E 00 00 0B 00 02 02 03 00 00 50 00 00 00 1A 00 00 00 72 74 2F 6A 30 30 30 31
0020: 2F 74 72 61 6E 73 69 74 69 6F 6E 5F 65 76 65 6E 74 00 00 01 2C 00 00 00 6C 69 66 65 63 79 63 6C
0040: 65 5F 6D 73 67 73 3A 3A 6D 73 67 3A 3A 64 64 73 5F 3A 3A 54 72 61 6E 73 69 74 69 6F 6E 45 76 65
0060: 6E 74 5F 00 00 01 00 00
[1655988895.262722] info     | ProxyClient.cpp    | create_topic             | topic created          | client_key: 0x1D91D971, topic_id: 0x000(2), participant_id: 0x000(1)
[1655988895.262756] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x1D91D971, len: 14, data: 
0000: 81 80 01 00 05 01 06 00 00 0B 00 02 00 00
[1655988895.262770] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x1D91D971, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 02 00 00 00 80
[1655988895.267686] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x1D91D971, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 02 00 00 00 80
[1655988895.267717] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x1D91D971, 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
[1655988895.267853] info     | ProxyClient.cpp    | create_publisher         | publisher created      | client_key: 0x1D91D971, publisher_id: 0x000(3), participant_id: 0x000(1)
[1655988895.267940] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x1D91D971, len: 14, data: 
0000: 81 80 02 00 05 01 06 00 00 0C 00 03 00 00
[1655988895.267961] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x1D91D971, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 03 00 00 00 80
[1655988895.272907] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x1D91D971, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 03 00 00 00 80
[1655988895.272939] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x1D91D971, 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 00 03 00 01 00 0A 00 00 00
0020: 00 00 00 03
[1655988895.273586] info     | ProxyClient.cpp    | create_datawriter        | datawriter created     | client_key: 0x1D91D971, datawriter_id: 0x000(5), publisher_id: 0x000(3)
[1655988895.273632] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x1D91D971, len: 14, data: 
0000: 81 80 03 00 05 01 06 00 00 0D 00 05 00 00
[1655988895.273647] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x1D91D971, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 04 00 00 00 80
[1655988895.274086] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x1D91D971, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 04 00 00 00 80
[1655988895.278743] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x1D91D971, len: 224, data: 
0000: 81 80 04 00 01 07 D5 00 00 0E 00 08 08 03 00 00 C7 00 00 00 14 00 00 00 2F 6A 30 30 30 31 2F 63
0020: 68 61 6E 67 65 5F 73 74 61 74 65 00 30 00 00 00 6C 69 66 65 63 79 63 6C 65 5F 6D 73 67 73 3A 3A
0040: 73 72 76 3A 3A 64 64 73 5F 3A 3A 43 68 61 6E 67 65 53 74 61 74 65 5F 52 65 71 75 65 73 74 5F 00
0060: 31 00 00 00 6C 69 66 65 63 79 63 6C 65 5F 6D 73 67 73 3A 3A 73 72 76 3A 3A 64 64 73 5F 3A 3A 43
0080: 68 61 6E 67 65 53 74 61 74 65 5F 52 65 73 70 6F 6E 73 65 5F 00 01 00 00 1D 00 00 00 72 71 2F 6A
00A0: 30 30 30 31 2F 63 68 61 6E 67 65 5F 73 74 61 74 65 52 65 71 75 65 73 74 00 01 00 00 1B 00 00 00
00C0: 72 72 2F 6A 30 30 30 31 2F 63 68 61 6E 67 65 5F 73 74 61 74 65 52 65 70 6C 79 00 00 01 00 00 00
[1655988895.279892] info     | ProxyClient.cpp    | create_replier           | replier created        | client_key: 0x1D91D971, requester_id: 0x000(7), participant_id: 0x000(1)
[1655988895.279956] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x1D91D971, len: 14, data: 
0000: 81 80 04 00 05 01 06 00 00 0E 00 08 00 00
[1655988895.279967] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x1D91D971, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 05 00 00 00 80
[1655988895.280350] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x1D91D971, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 05 00 00 00 80
[1655988895.284683] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x1D91D971, len: 232, data: 
0000: 81 80 05 00 08 01 10 00 00 0F 00 08 80 00 00 01 FF FF 00 00 00 00 00 00 01 07 CA 00 00 10 00 18
0020: 08 03 72 61 BC 00 00 00 11 00 00 00 2F 6A 30 30 30 31 2F 67 65 74 5F 73 74 61 74 65 00 FF FF FF
0040: 2D 00 00 00 6C 69 66 65 63 79 63 6C 65 5F 6D 73 67 73 3A 3A 73 72 76 3A 3A 64 64 73 5F 3A 3A 47
0060: 65 74 53 74 61 74 65 5F 52 65 71 75 65 73 74 5F 00 00 00 00 2E 00 00 00 6C 69 66 65 63 79 63 6C
0080: 65 5F 6D 73 67 73 3A 3A 73 72 76 3A 3A 64 64 73 5F 3A 3A 47 65 74 53 74 61 74 65 5F 52 65 73 70
00A0: 6F 6E 73 65 5F 00 01 00 1A 00 00 00 72 71 2F 6A 30 30 30 31 2F 67 65 74 5F 73 74 61 74 65 52 65
00C0: 71 75 65 73 74 00 01 1F 18 00 00 00 72 72 2F 6A 30 30 30 31 2F 67 65 74 5F 73 74 61 74 65 52 65
00E0: 70 6C 79 00 00 01 00 00
[1655988895.285798] info     | ProxyClient.cpp    | create_replier           | replier created        | client_key: 0x1D91D971, requester_id: 0x001(7), participant_id: 0x000(1)
[1655988895.286019] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x1D91D971, len: 14, data: 
0000: 81 80 05 00 05 01 06 00 00 10 00 18 00 00
[1655988895.286030] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x1D91D971, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 06 00 00 00 80
[1655988895.286427] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x1D91D971, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 06 00 00 00 80
[1655988895.290692] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x1D91D971, len: 284, data: 
0000: 81 80 06 00 08 01 10 00 00 11 00 18 80 00 00 01 FF FF 00 00 00 00 00 00 01 07 FD 00 00 12 00 28
0020: 08 03 00 00 EF 00 00 00 1C 00 00 00 2F 6A 30 30 30 31 2F 67 65 74 5F 61 76 61 69 6C 61 62 6C 65
0040: 5F 73 74 61 74 65 73 00 37 00 00 00 6C 69 66 65 63 79 63 6C 65 5F 6D 73 67 73 3A 3A 73 72 76 3A
0060: 3A 64 64 73 5F 3A 3A 47 65 74 41 76 61 69 6C 61 62 6C 65 53 74 61 74 65 73 5F 52 65 71 75 65 73
0080: 74 5F 00 20 38 00 00 00 6C 69 66 65 63 79 63 6C 65 5F 6D 73 67 73 3A 3A 73 72 76 3A 3A 64 64 73
00A0: 5F 3A 3A 47 65 74 41 76 61 69 6C 61 62 6C 65 53 74 61 74 65 73 5F 52 65 73 70 6F 6E 73 65 5F 00
00C0: 01 00 00 00 25 00 00 00 72 71 2F 6A 30 30 30 31 2F 67 65 74 5F 61 76 61 69 6C 61 62 6C 65 5F 73
00E0: 74 61 74 65 73 52 65 71 75 65 73 74 00 01 00 00 23 00 00 00 72 72 2F 6A 30 30 30 31 2F 67 65 74
0100: 5F 61 76 61 69 6C 61 62 6C 65 5F 73 74 61 74 65 73 52 65 70 6C 79 00 00 01 00 00 00
[1655988895.291993] info     | ProxyClient.cpp    | create_replier           | replier created        | client_key: 0x1D91D971, requester_id: 0x002(7), participant_id: 0x000(1)
[1655988895.292062] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x1D91D971, len: 14, data: 
0000: 81 80 06 00 05 01 06 00 00 12 00 28 00 00
[1655988895.292075] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x1D91D971, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 07 00 00 00 80
[1655988895.292500] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x1D91D971, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 07 00 00 00 80
[1655988895.296986] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x1D91D971, len: 308, data: 
0000: 81 80 07 00 08 01 10 00 00 13 00 28 80 00 00 01 FF FF 00 00 00 00 00 00 01 07 16 01 00 14 00 38
0020: 08 03 00 03 08 01 00 00 21 00 00 00 2F 6A 30 30 30 31 2F 67 65 74 5F 61 76 61 69 6C 61 62 6C 65
0040: 5F 74 72 61 6E 73 69 74 69 6F 6E 73 00 FF FF FF 3C 00 00 00 6C 69 66 65 63 79 63 6C 65 5F 6D 73
0060: 67 73 3A 3A 73 72 76 3A 3A 64 64 73 5F 3A 3A 47 65 74 41 76 61 69 6C 61 62 6C 65 54 72 61 6E 73
0080: 69 74 69 6F 6E 73 5F 52 65 71 75 65 73 74 5F 00 3D 00 00 00 6C 69 66 65 63 79 63 6C 65 5F 6D 73
00A0: 67 73 3A 3A 73 72 76 3A 3A 64 64 73 5F 3A 3A 47 65 74 41 76 61 69 6C 61 62 6C 65 54 72 61 6E 73
00C0: 69 74 69 6F 6E 73 5F 52 65 73 70 6F 6E 73 65 5F 00 01 00 00 2A 00 00 00 72 71 2F 6A 30 30 30 31
00E0: 2F 67 65 74 5F 61 76 61 69 6C 61 62 6C 65 5F 74 72 61 6E 73 69 74 69 6F 6E 73 52 65 71 75 65 73
0100: 74 00 01 00 28 00 00 00 72 72 2F 6A 30 30 30 31 2F 67 65 74 5F 61 76 61 69 6C 61 62 6C 65 5F 74
0120: 72 61 6E 73 69 74 69 6F 6E 73 52 65 70 6C 79 00 00 01 00 00
[1655988895.298131] info     | ProxyClient.cpp    | create_replier           | replier created        | client_key: 0x1D91D971, requester_id: 0x003(7), participant_id: 0x000(1)
[1655988895.298197] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x1D91D971, len: 14, data: 
0000: 81 80 07 00 05 01 06 00 00 14 00 38 00 00
[1655988895.298208] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x1D91D971, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 08 00 00 00 80
[1655988895.298754] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x1D91D971, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 08 00 00 00 80
[1655988895.302754] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x1D91D971, len: 292, data: 
0000: 81 80 08 00 08 01 10 00 00 15 00 38 80 00 00 01 FF FF 00 00 00 00 00 00 01 07 05 01 00 16 00 48
0020: 08 03 6E 67 F7 00 00 00 1C 00 00 00 2F 6A 30 30 30 31 2F 67 65 74 5F 74 72 61 6E 73 69 74 69 6F
0040: 6E 5F 67 72 61 70 68 00 3C 00 00 00 6C 69 66 65 63 79 63 6C 65 5F 6D 73 67 73 3A 3A 73 72 76 3A
0060: 3A 64 64 73 5F 3A 3A 47 65 74 41 76 61 69 6C 61 62 6C 65 54 72 61 6E 73 69 74 69 6F 6E 73 5F 52
0080: 65 71 75 65 73 74 5F 00 3D 00 00 00 6C 69 66 65 63 79 63 6C 65 5F 6D 73 67 73 3A 3A 73 72 76 3A
00A0: 3A 64 64 73 5F 3A 3A 47 65 74 41 76 61 69 6C 61 62 6C 65 54 72 61 6E 73 69 74 69 6F 6E 73 5F 52
00C0: 65 73 70 6F 6E 73 65 5F 00 01 FF 1F 25 00 00 00 72 71 2F 6A 30 30 30 31 2F 67 65 74 5F 74 72 61
00E0: 6E 73 69 74 69 6F 6E 5F 67 72 61 70 68 52 65 71 75 65 73 74 00 01 02 20 23 00 00 00 72 72 2F 6A
0100: 30 30 30 31 2F 67 65 74 5F 74 72 61 6E 73 69 74 69 6F 6E 5F 67 72 61 70 68 52 65 70 6C 79 00 00
0120: 01 00 00 00
[1655988895.303947] info     | ProxyClient.cpp    | create_replier           | replier created        | client_key: 0x1D91D971, requester_id: 0x004(7), participant_id: 0x000(1)
[1655988895.304017] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x1D91D971, len: 14, data: 
0000: 81 80 08 00 05 01 06 00 00 16 00 48 00 00
[1655988895.304045] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x1D91D971, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 09 00 00 00 80
[1655988895.308727] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x1D91D971, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 09 00 00 00 80
[1655988895.308766] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x1D91D971, len: 24, data: 
0000: 81 80 09 00 08 01 10 00 00 17 00 48 80 00 00 01 FF FF 00 00 00 00 00 00
[1655988895.308909] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x1D91D971, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 0A 00 00 00 80

SourabhPrasad avatar Jun 23 '22 12:06 SourabhPrasad

Which is the output of ros2 service list?

pablogs9 avatar Jun 23 '22 13:06 pablogs9

ros2 service list gives the following output.

/node/change_state
/node/get_available_states
/node/get_available_transitions
/node/get_state
/node/get_transition_graph

SourabhPrasad avatar Jun 23 '22 13:06 SourabhPrasad

Service is up & running. @JanStaschulat have you tested lifecycle in micro-ROS w/ Micro XRCE-DDS rmw? is the inner memory for types being correctly initialized and so on?

pablogs9 avatar Jun 23 '22 13:06 pablogs9

Just checked, we used publisher, timer, rclc_parameter, and micro_ros_diagnostics on ESP32 with FreeRTOS in the final OFERA demo. But not lifecycle.

JanStaschulat avatar Jun 23 '22 16:06 JanStaschulat

@norro Has lifecycle been tested in micro-ROS with Micro XRCE-DDS rmw?

JanStaschulat avatar Jun 23 '22 16:06 JanStaschulat