opencr tf publisher - no messages received on topic

[mdxtinkernick]

Issue template

• Hardware description: opener 1.0
• RTOS: micro_ros_arduino
• Installation type: micro_ros_setup
• Version or commit hash: humble

Steps to reproduce the issue

upload example code micro-res_tf_publisher.ino to opencr board, connect it to ubuntu virtual machine with micro-res-agent running

Expected behavior

micro-ros-agent recognise board, sets up publisher and tf messages are published on it

Actual behavior

micro-ros-agent recognise board, sets up publisher, but no message are published. The error light on the board is not flashing.

Additional information

publisher, subscriber and service examples work as expected. Have tried the imu demo code from the opencr hardware examples (that just prints ascii to the serial port) and data is printed out, so the imu is working. It is the original board with the MPU9250 imu on it if that makes any difference

[pablogs9]

Which is the output of the micro-ROS Agent using the flag -v6 in the case the communication is failing?

[mdxtinkernick]

here is example with -v6

[1708007808.595466] info     | TermiosAgentLinux.cpp | init                     | Serial port not found. | device: /dev/ttyACM0, error 2, waiting for connection...
[1708007808.772748] info     | TermiosAgentLinux.cpp | init                     | running...             | fd: 3
[1708007810.372695] info     | Root.cpp           | create_client            | create                 | client_key: 0x3BA8E7D4, session_id: 0x81
[1708007810.372820] info     | SessionManager.hpp | establish_session        | session established    | client_key: 0x3BA8E7D4, address: 0
[1708007810.373251] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x3BA8E7D4, len: 19, data: 
0000: 81 00 00 00 04 01 0B 00 00 00 58 52 43 45 01 00 01 0F 00
[1708007810.375171] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x3BA8E7D4, len: 56, data: 
0000: 81 80 00 00 01 07 2E 00 00 0A 00 01 01 03 00 00 1F 00 00 00 00 01 01 20 17 00 00 00 6D 69 63 72
0020: 6F 5F 72 6F 73 5F 61 72 64 75 69 6E 6F 5F 6E 6F 64 65 00 00 00 00 00 00
[1708007810.390030] info     | ProxyClient.cpp    | create_participant       | participant created    | client_key: 0x3BA8E7D4, participant_id: 0x000(1)
[1708007810.390345] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x3BA8E7D4, len: 14, data: 
0000: 81 80 00 00 05 01 06 00 00 0A 00 01 00 00
[1708007810.390494] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x3BA8E7D4, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 01 00 00 00 80
[1708007810.391891] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x3BA8E7D4, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 01 00 00 00 80
[1708007810.391947] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x3BA8E7D4, len: 72, data: 
0000: 81 80 01 00 01 07 3E 00 00 0B 00 02 02 03 00 00 30 00 00 00 06 00 00 00 72 74 2F 74 66 00 00 01
0020: 20 00 00 00 74 66 32 5F 6D 73 67 73 3A 3A 6D 73 67 3A 3A 64 64 73 5F 3A 3A 54 46 4D 65 73 73 61
0040: 67 65 5F 00 00 01 00 00
[1708007810.392126] info     | ProxyClient.cpp    | create_topic             | topic created          | client_key: 0x3BA8E7D4, topic_id: 0x000(2), participant_id: 0x000(1)
[1708007810.392389] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x3BA8E7D4, len: 14, data: 
0000: 81 80 01 00 05 01 06 00 00 0B 00 02 00 00
[1708007810.392615] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x3BA8E7D4, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 02 00 00 00 80
[1708007810.393585] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x3BA8E7D4, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 02 00 00 00 80
[1708007810.393635] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x3BA8E7D4, 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
[1708007810.393814] info     | ProxyClient.cpp    | create_publisher         | publisher created      | client_key: 0x3BA8E7D4, publisher_id: 0x000(3), participant_id: 0x000(1)
[1708007810.394105] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x3BA8E7D4, len: 14, data: 
0000: 81 80 02 00 05 01 06 00 00 0C 00 03 00 00
[1708007810.394245] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x3BA8E7D4, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 03 00 00 00 80
[1708007810.394479] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x3BA8E7D4, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 03 00 00 00 80
[1708007810.395559] debug    | SerialAgentLinux.cpp | recv_message             | [==>> SER <<==]        | client_key: 0x3BA8E7D4, 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 20 0A 00 00 00
0020: 00 00 00 03
[1708007810.396066] info     | ProxyClient.cpp    | create_datawriter        | datawriter created     | client_key: 0x3BA8E7D4, datawriter_id: 0x000(5), publisher_id: 0x000(3)
[1708007810.396276] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x3BA8E7D4, len: 14, data: 
0000: 81 80 03 00 05 01 06 00 00 0D 00 05 00 00
[1708007810.396438] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x3BA8E7D4, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 04 00 00 00 80
[1708007810.695186] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x3BA8E7D4, len: 13, data: 
0000: 81 00 00 00 0B 01 05 00 03 00 03 00 80
[1708007810.895827] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x3BA8E7D4, len: 13, data: 
0000: 81 00 00 00 0B 01 05 00 03 00 03 00 80
[1708007811.095965] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x3BA8E7D4, len: 13, data: 
0000: 81 00 00 00 0B 01 05 00 03 00 03 00 80
[1708007811.296387] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x3BA8E7D4, len: 13, data: 
0000: 81 00 00 00 0B 01 05 00 03 00 03 00 80
[1708007811.496766] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x3BA8E7D4, len: 13, data: 
0000: 81 00 00 00 0B 01 05 00 03 00 03 00 80
[1708007811.697718] debug    | SerialAgentLinux.cpp | send_message             | [** <<SER>> **]        | client_key: 0x3BA8E7D4, len: 13, data: 
0000: 81 00 00 00 0B 01 05 00 03 00 03 00 80

[pablogs9]

Does it works if you publish fake numbers instead of reading from the IMU?

[mdxtinkernick]

I tried putting fake values but no observable difference – set them quaternion to 0,0,0,1 Could try doing the same but comment out all the imu code From: Pablo Garrido ***@***.***> Date: Thursday, 15 February 2024 at 14:41 To: micro-ROS/micro_ros_arduino ***@***.***> Cc: Nick Weldin ***@***.***>, Author ***@***.***> Subject: Re: [micro-ROS/micro_ros_arduino] opencr tf publisher - no messages received on topic (Issue #1678) Does it works if you publish fake numbers instead of reading from the IMU? — Reply to this email directly, view it on GitHub<#1678 (comment)>, or unsubscribe<https://github.com/notifications/unsubscribe-auth/AAUVCSZR52YGDPVKAQ6HIADYTYNCHAVCNFSM6AAAAABDKEE4KWVHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMYTSNBWGIZTGOBQGQ>. You are receiving this because you authored the thread.Message ID: ***@***.***>

[pablogs9]

Yes please, let's discard the IMU problems by commenting the code out

[mdxtinkernick]

no_imu.txt
micro_ros_tf_publisher_no_imu.txt

Here ia

the Arduino code with imu commented out (changed the extension to .txt to enable upload
and the terminal produced from that

[pablogs9]

could you determine where is the board freezing?

[mdxtinkernick]

so it executes line

// create executor
RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));

but does not return form the function call in the if statement after that

if(!micro_ros_utilities_create_message_memory(
ROSIDL_GET_MSG_TYPE_SUPPORT(tf2_msgs, msg, TFMessage),
&tf_message,
(micro_ros_utilities_memory_conf_t) {})
)
{
error_loop();
}

it doesn't get to the error_loop call, or the line after the if statement

[pablogs9]

So what if you try to remove the call to micro_ros_utilities_create_message_memory and initializes the message memory manually as explained here: https://docs.vulcanexus.org/en/latest/rst/tutorials/micro/memory_management/memory_management.html#message-memory

[mdxtinkernick]

I think I understand what that is doing - but don't see what a like for like replacement would be - I will look at the examples in the micro-ros examples types handing example alongside that description page and see if I can work it out

[mdxtinkernick]

The page you linked to seems to make sense, but I don't understand how this relates to what has been done in the example code - why is there are is a sequence of transforms - a tf message only contains 1 transform, and only the 1st element in the micro-ros code is assigned numerical values anyway, the other one seems to be just being used to provide the second frame, but from the ros tf transform stamped message definition I would expect to have a header, a string for child frame and a transform, but the code is referring to a message that has two transforms, which each have a header inside, which then appears to have some of the structure like the ros message.

I looked in the library to see if there were files that defined things like a .msg definition in ros, but couldn't find anything that simple to refer to, so a bit stuck now as to how this works in practice

[pablogs9]

Look for the .h file that defines the type you are using. And recursively checking each member you will find which ones you need to initialize, as explained in the tutorial I attached. Tomorrow I will check this in more detail.

[mdxtinkernick]

Thanks for your help. That's what I was looking at - I hadn't looked closely enough at the definition in the tf_message file - I see where the transforms element comes from now. My confusion now is why the tf_message is being used rather than just transform_stamped - is it a micro-ros specific reason ? Creating a message with two transform_stamped objects seems it would increase the memory footprint, and not sure why its needed. All the tf2 tutorial examples for publishing a transform just use transform stamped.

[mdxtinkernick]

So I'm seeing if a simple TransformStamped version will work - the board now returns successfully from the line

if (!micro_ros_utilities_create_message_memory(
ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, TransformStamped),
&transform_stamped,
(micro_ros_utilities_memory_conf_t){})) {
error_loop();
}

but doesn't get past the next line - tried each of these separately and they both failed

transform_stamped->header.frame_id =
micro_ros_string_utilities_set(transform_stamped->header.frame_id, "/panda_link0");

transform_stamped->child_frame_id =
micro_ros_string_utilities_set(transform_stamped->child_frame_id, "/inertial_unit");

so will look today at replacing those with manual assignment from the example you linked to

[mdxtinkernick]

using

strcpy(transform_stamped->header.frame_id.data, "/inertial_unit");
transform_stamped->header.frame_id.size = strlen(transform_stamped->header.frame_id.data);

compiles, but board does not get past it

[pablogs9]

have you assigned memory to transform_stamped->header.frame_id.data with a malloc?

[mdxtinkernick]

no - I thought the micro_ros_utilities_create_message_memory that was now succeeding was doing that

will try it now

[mdxtinkernick]

added malloc and child_frame_id - the micro_ros_string_utilities_set function then runs on board as well as manually setting data and size.

code now seems to be running to the end of loop, topic is showing up as geometry_msgs/msg/TransformStamped, but still no messages arriving in ros2 topic echo

attached current code on board and terminal -v6 - the terminal has stopped printing out data at the bottom of the document, it is not continuously printing recv_message
terminal.txt
current.txt

[pablogs9]

Your micro-ROS client is still not sending data to the Agent

[mdxtinkernick]

not sure what to do now

I moving away from the example as written - but it did not work for me - is this something that needs fixing, or have others got it to work as it is.

is using the more complex message with two transform objects necessary or does add additional complexity for an example when trying to get things working and understanding what is going on in the code

[mdxtinkernick]

so I found some example code here https://answers.ros.org/question/416189/cannot-send-transform-stamped-sequence-using-micro-ros-arduino-library-on-esp32/

that works on the board once its adjusted to not use wifi - I will work over the weekend to get it working with with the imu, time and publishing a TransformStamped in a way that is closer to the way the current example is structured. Would you be interested in that as a fix or alternative example ?

[pablogs9]

Please create a PR with a fix and we will evaluate if it worth to split it in different example in a further step

[mdxtinkernick]

so went down a bit of a rabbit hole - seems the troubleshooting that I had done at the start was misleading, and so went down the wrong path, none of the changes to allocation of memory produced messages, so on a whim tried switching the message so it wasn't a pointer, replaced -> with . in assignments and passed it to the publisher with & - it worked.

I thought I had tried that right at the beginning but can't have done it right. Will sort out a pr for just that change, one for then refactoring the message type to just a transform_stamped with all the frames set, and add the code for syncing time so that the tf displays more reliably in rviz

[pablogs9]

Ok, so can we close?

[mdxtinkernick]

closing pr to follow