AP_DDS: Service to check if vehicle is armable

[tizianofiorenzani]

Issue

Closes #28286

What Changed

• Create a new service /ap/prearm_check of type [std_srvs/srv/Trigger] https://docs.ros.org/en/noetic/api/std_srvs/html/srv/Trigger.html)

Response:

• success (true if armable)
• message: a string message (Vehicle is armable or Vehicle is NOT armable).

Test

Run the SITL and then use the DDS test to verify that vehicle is armable:

colcon build --packages-up-to ardupilot_dds_tests 
source install/setup.bash
ros2 run ardupilot_dds_tests pre_arm_check

Or call the service manually:

ros2 service list

/ap/arm_motors
/ap/mode_switch
/ap/prearm_check

ros2 service call /ap/prearm_check std_srvs/srv/Trigger 
requester: making request: std_srvs.srv.Trigger_Request()

response:
std_srvs.srv.Trigger_Response(success=True, message='Vehicle is Armable')

[tizianofiorenzani]

@Ryanf55 I have trouble getting the services working with the pytest fixture. I have added a runtime test that can be run with

ros2 run ardupilot_dds_tests pre_arm_check

[peterbarker]

Shouldn't this be strcpy?

I note there are other uses ofstrcpy in the DDS directory - but that's no reason to introduce more.

[Ryanf55]

Sorry, I'm not following. Did you mean for use of the safer strncpy?

warning - I went down the strcpy/strncpy rabbit hole

In AP's libraries/**/*.cpp I see

• 29 uses of strcpy
• 142 uses of strncpy

And, in AP_DDS, the usage seems constrained entirely to our frame_id:

ryan@B650-970:~/Dev/ros2_ws/src/ardupilot/libraries/AP_DDS$ grep -Rn strcpy
AP_DDS_Client.cpp:151:    strcpy(msg.header.frame_id, WGS_84_FRAME_ID);
AP_DDS_Client.cpp:225:        strcpy(msg.transforms[i].header.frame_id, BASE_LINK_FRAME_ID);
AP_DDS_Client.cpp:226:        strcpy(msg.transforms[i].child_frame_id, gps_frame_id);
AP_DDS_Client.cpp:321:    strcpy(msg.header.frame_id, BASE_LINK_FRAME_ID);
AP_DDS_Client.cpp:370:    strcpy(msg.header.frame_id, BASE_LINK_FRAME_ID);
AP_DDS_Client.cpp:412:    strcpy(msg.header.frame_id, BASE_LINK_FRAME_ID);
AP_DDS_Client.cpp:451:    strcpy(msg.header.frame_id, BASE_LINK_NED_FRAME_ID);
AP_DDS_Client.cpp:494:    strcpy(msg.header.frame_id, BASE_LINK_FRAME_ID);

^ Looks like these can be moved to strncpy.

FYI, the destination array in build/sitl/libraries/AP_DDS/generated/std_msgs/msg/Header.h is this:

char frame_id[255];

This article is pretty interesting (with morning coffee) against strncpy: https://software.codidact.com/posts/281518

AND, if I try copying a massive string in (greater than the destination), the compiler prevents it with stringop-overflow.

[1019/1423] Compiling libraries/AP_DDS/AP_DDS_Client.cpp
In file included from /usr/include/string.h:535,
                 from ../../libraries/AP_Common/missing/string.h:1,
                 from ../../libraries/AP_HAL/AP_HAL_Namespace.h:3,
                 from ../../libraries/AP_HAL/Semaphores.h:3,
                 from ../../libraries/AP_HAL_SITL/Semaphores.h:6,
                 from ../../libraries/AP_HAL/board/sitl.h:61,
                 from ../../libraries/AP_HAL/AP_HAL_Boards.h:132,
                 from ../../libraries/AP_DDS/AP_DDS_Client.cpp:1:
In function ‘char* strcpy(char*, const char*)’,
    inlined from ‘static void AP_DDS_Client::populate_static_transforms(tf2_msgs_msg_TFMessage&)’ at ../../libraries/AP_DDS/AP_DDS_Client.cpp:225:15:
/usr/include/x86_64-linux-gnu/bits/string_fortified.h:79:33: error: ‘void* __builtin___memcpy_chk(void*, const void*, long unsigned int, long unsigned int)’ writing 301 bytes into a region of size 255 overflows the destination [-Werror=stringop-overflow=]
   79 |   return __builtin___strcpy_chk (__dest, __src, __glibc_objsize (__dest));
      |          ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
compilation terminated due to -Wfatal-errors.
cc1plus: all warnings being treated as errors

If I supply a 255 character string (null being the 256th) with strcpy, the compiler recognizes it as too big, and fails to compile.

Switching to strncpy fails to compile too:

In function ‘char* strncpy(char*, const char*, size_t)’,
    inlined from ‘static void AP_DDS_Client::populate_static_transforms(tf2_msgs_msg_TFMessage&)’ at ../../libraries/AP_DDS/AP_DDS_Client.cpp:295:16:
/usr/include/x86_64-linux-gnu/bits/string_fortified.h:95:34: error: ‘char* __builtin_strncpy(char*, const char*, long unsigned int)’ output truncated before terminating nul copying 255 bytes from a string of the same length [-Werror=stringop-truncation]
   95 |   return __builtin___strncpy_chk (__dest, __src, __len,
      |          ~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~~
   96 |                                   __glibc_objsize (__dest));
      |                                   ~~~~~~~~~~~~~~~~~~~~~~~~~
compilation terminated due to -Wfatal-errors.
cc1plus: all warnings being treated as errors

Yes, I'm happy to change, but it's useful to understand why we prefer strncpy over strcpy in this use case when the compiler is already enforcing the safety of this operation.