From 1b636a3ae3a2fee8919a5a7591c35efe2ae3bfe2 Mon Sep 17 00:00:00 2001 From: jimmy-mcelwain Date: Fri, 4 Oct 2024 10:05:23 -0400 Subject: [PATCH] Create static transform broadcaster and broadcast flange->tool0 on /tf_static --- src/ErrorHandling.h | 4 + src/MotoROS.h | 1 + src/MotoROS2_AllControllers.vcxproj | 2 + src/MotoROS2_AllControllers.vcxproj.filters | 20 +++- src/PositionMonitor.c | 107 +++++++++++------- src/PositionMonitor.h | 23 ++-- src/RosApiNameConstants.h | 1 + src/StaticTransformBroadcaster.c | 115 ++++++++++++++++++++ src/StaticTransformBroadcaster.h | 20 ++++ src/main.c | 6 +- 10 files changed, 247 insertions(+), 52 deletions(-) create mode 100644 src/StaticTransformBroadcaster.c create mode 100644 src/StaticTransformBroadcaster.h diff --git a/src/ErrorHandling.h b/src/ErrorHandling.h index 91d80783..bdf7a34b 100644 --- a/src/ErrorHandling.h +++ b/src/ErrorHandling.h @@ -175,6 +175,10 @@ typedef enum SUBCODE_FAIL_INVALID_BASE_TRACK_MOTION_TYPE, SUBCODE_DEBUG_INIT_FAIL_MP_NICDATA, + SUBCODE_FAIL_CREATE_PUBLISHER_STATIC_TRANSFORM, + SUBCODE_FAIL_ALLOCATE_STATIC_TRANSFORM, + SUBCODE_FAIL_ALLOCATE_FLANGE_TOOL0, + SUBCODE_FAIL_STATIC_TRANSFORM_MEM_LIMIT, } ALARM_ASSERTION_FAIL_SUBCODE; //8011 typedef enum diff --git a/src/MotoROS.h b/src/MotoROS.h index 19dc651d..bd8a7a94 100644 --- a/src/MotoROS.h +++ b/src/MotoROS.h @@ -88,6 +88,7 @@ #include "CtrlGroup.h" #include "ControllerStatusIO.h" #include "PositionMonitor.h" +#include "StaticTransformBroadcaster.h" #include "ServiceQueueTrajPoint.h" #include "ServiceReadWriteIO.h" #include "ServiceResetError.h" diff --git a/src/MotoROS2_AllControllers.vcxproj b/src/MotoROS2_AllControllers.vcxproj index b5fad929..785d0186 100644 --- a/src/MotoROS2_AllControllers.vcxproj +++ b/src/MotoROS2_AllControllers.vcxproj @@ -343,6 +343,7 @@ + @@ -377,6 +378,7 @@ + diff --git a/src/MotoROS2_AllControllers.vcxproj.filters b/src/MotoROS2_AllControllers.vcxproj.filters index 6b585860..3ca16b61 100644 --- a/src/MotoROS2_AllControllers.vcxproj.filters +++ b/src/MotoROS2_AllControllers.vcxproj.filters @@ -333,6 +333,12 @@ Source Files\Utilities + + Source Files + + + Source Files\Topics and Publishers + @@ -425,9 +431,6 @@ Header Files\Utilities - - Header Files\Robot Controller - Header Files\Robot Controller @@ -443,5 +446,14 @@ Header Files\Robot Controller + + Header Files + + + Header Files + + + Header Files\Topics and Publishers + - + \ No newline at end of file diff --git a/src/PositionMonitor.c b/src/PositionMonitor.c index 8e566f07..a9180f8a 100644 --- a/src/PositionMonitor.c +++ b/src/PositionMonitor.c @@ -10,11 +10,11 @@ PositionMonitor_Publishers g_publishers_PositionMonitor; PositionMonitor_Messages g_messages_PositionMonitor; - static void Ros_PositionMonitor_Initialize_GlobalJointStatePublisher(rmw_qos_profile_t const* const qos_profile); static void Ros_PositionMonitor_Initialize_PerGroupJointStatePublisher(rmw_qos_profile_t const* const qos_profile, CtrlGroup* const ctrlGroup, int grpIndex); -static void Ros_PositionMonitor_Initialize_TfPublisher(rmw_qos_profile_t const* const qos_profile, int totalRobots); +static void Ros_PositionMonitor_Initialize_TfPublisher(rmw_qos_profile_t const* const qos_profile); +static int motoRos_PositionMonitor_totalRobots = 0; void Ros_PositionMonitor_Initialize() { @@ -29,20 +29,27 @@ void Ros_PositionMonitor_Initialize() //================================== //create a JointState publisher for each group - int totalRobots = 0; + motoRos_PositionMonitor_totalRobots = 0; for (int grpIndex = 0; grpIndex < g_Ros_Controller.numGroup; grpIndex++) { CtrlGroup* ctrlGroup = g_Ros_Controller.ctrlGroups[grpIndex]; Ros_PositionMonitor_Initialize_PerGroupJointStatePublisher(qos_profile_js, ctrlGroup, grpIndex); if (Ros_CtrlGroup_IsRobot(ctrlGroup)) - totalRobots += 1; + motoRos_PositionMonitor_totalRobots += 1; } //================================== //Create publisher for cartesian transform //Rviz2 expects the QoS to be RELIABLE, but user could have configured something else const rmw_qos_profile_t* qos_profile_tf = Ros_ConfigFile_To_Rmw_Qos_Profile(g_nodeConfigSettings.qos_tf); - Ros_PositionMonitor_Initialize_TfPublisher(qos_profile_tf, totalRobots); + Ros_PositionMonitor_Initialize_TfPublisher(qos_profile_tf); + + //================================== + //Create publisher for static cartesian transform (e.g. tool0 to flange) + + Ros_StaticTransformBroadcaster_Init(); + + Ros_PositionMonitor_CalculateStaticTransforms(); MOTOROS2_MEM_TRACE_REPORT(pos_mon_init); } @@ -153,7 +160,7 @@ static void Ros_PositionMonitor_Initialize_PerGroupJointStatePublisher(rmw_qos_p rosidl_runtime_c__float64__Sequence__init(&ctrlGroup->msgJointState->effort, ctrlGroup->numAxes); } -static void Ros_PositionMonitor_Initialize_TfPublisher(rmw_qos_profile_t const* const qos_profile, int totalRobots) +static void Ros_PositionMonitor_Initialize_TfPublisher(rmw_qos_profile_t const* const qos_profile) { char formatBuffer[MAX_TF_FRAME_NAME_LENGTH]; @@ -185,36 +192,68 @@ static void Ros_PositionMonitor_Initialize_TfPublisher(rmw_qos_profile_t const* //create message for cartesian transform g_messages_PositionMonitor.transform = tf2_msgs__msg__TFMessage__create(); - motoRosAssert(geometry_msgs__msg__TransformStamped__Sequence__init(&g_messages_PositionMonitor.transform->transforms, totalRobots * NUMBER_TRANSFORM_LINKS_PER_ROBOT), + motoRosAssert(geometry_msgs__msg__TransformStamped__Sequence__init(&g_messages_PositionMonitor.transform->transforms, motoRos_PositionMonitor_totalRobots * PUBLISHED_NUMBER_TRANSFORM_LINKS_PER_ROBOT), SUBCODE_FAIL_ALLOCATE_TRANSFORM); bzero(formatBuffer, MAX_TF_FRAME_NAME_LENGTH); int robotIterator = 0; const char* frame_prefix = g_nodeConfigSettings.tf_frame_prefix; - for (int i = 0; i < (totalRobots * NUMBER_TRANSFORM_LINKS_PER_ROBOT); i += NUMBER_TRANSFORM_LINKS_PER_ROBOT) + for (int i = 0; i < (motoRos_PositionMonitor_totalRobots * PUBLISHED_NUMBER_TRANSFORM_LINKS_PER_ROBOT); i += PUBLISHED_NUMBER_TRANSFORM_LINKS_PER_ROBOT) { snprintf(formatBuffer, MAX_TF_FRAME_NAME_LENGTH, "%sworld", frame_prefix); - rosidl_runtime_c__String__assign(&g_messages_PositionMonitor.transform->transforms.data[i + tfLink_WorldToBase].header.frame_id, formatBuffer); + rosidl_runtime_c__String__assign(&g_messages_PositionMonitor.transform->transforms.data[i + publishIndex_tfLink_WorldToBase].header.frame_id, formatBuffer); snprintf(formatBuffer, MAX_TF_FRAME_NAME_LENGTH, "%sr%d/base", frame_prefix, robotIterator + 1); - rosidl_runtime_c__String__assign(&g_messages_PositionMonitor.transform->transforms.data[i + tfLink_WorldToBase].child_frame_id, formatBuffer); - rosidl_runtime_c__String__assign(&g_messages_PositionMonitor.transform->transforms.data[i + tfLink_BaseToFlange].header.frame_id, formatBuffer); + rosidl_runtime_c__String__assign(&g_messages_PositionMonitor.transform->transforms.data[i + publishIndex_tfLink_WorldToBase].child_frame_id, formatBuffer); + rosidl_runtime_c__String__assign(&g_messages_PositionMonitor.transform->transforms.data[i + publishIndex_tfLink_BaseToFlange].header.frame_id, formatBuffer); snprintf(formatBuffer, MAX_TF_FRAME_NAME_LENGTH, "%sr%d/flange", frame_prefix, robotIterator + 1); - rosidl_runtime_c__String__assign(&g_messages_PositionMonitor.transform->transforms.data[i + tfLink_BaseToFlange].child_frame_id, formatBuffer); - rosidl_runtime_c__String__assign(&g_messages_PositionMonitor.transform->transforms.data[i + tfLink_FlangeToTool0].header.frame_id, formatBuffer); - rosidl_runtime_c__String__assign(&g_messages_PositionMonitor.transform->transforms.data[i + tfLink_FlangeToTcp].header.frame_id, formatBuffer); - - snprintf(formatBuffer, MAX_TF_FRAME_NAME_LENGTH, "%sr%d/tool0", frame_prefix, robotIterator + 1); - rosidl_runtime_c__String__assign(&g_messages_PositionMonitor.transform->transforms.data[i + tfLink_FlangeToTool0].child_frame_id, formatBuffer); + rosidl_runtime_c__String__assign(&g_messages_PositionMonitor.transform->transforms.data[i + publishIndex_tfLink_BaseToFlange].child_frame_id, formatBuffer); + rosidl_runtime_c__String__assign(&g_messages_PositionMonitor.transform->transforms.data[i + publishIndex_tfLink_FlangeToTcp].header.frame_id, formatBuffer); snprintf(formatBuffer, MAX_TF_FRAME_NAME_LENGTH, "%sr%d/tcp_%d", frame_prefix, robotIterator + 1, g_Ros_Controller.ctrlGroups[robotIterator]->tool); - rosidl_runtime_c__String__assign(&g_messages_PositionMonitor.transform->transforms.data[i + tfLink_FlangeToTcp].child_frame_id, formatBuffer); + rosidl_runtime_c__String__assign(&g_messages_PositionMonitor.transform->transforms.data[i + publishIndex_tfLink_FlangeToTcp].child_frame_id, formatBuffer); robotIterator += 1; } } +void Ros_PositionMonitor_CalculateStaticTransforms() +{ + MP_XYZ vectorOrg, vectorX, vectorY; //for mpMakeFrame + vectorOrg.x = 0; vectorOrg.y = 0; vectorOrg.z = 0; + vectorX.x = 0; vectorX.y = 0; vectorX.z = 1; + vectorY.x = 0; vectorY.y = -1; vectorY.z = 0; + mpMakeFrame(&vectorOrg, &vectorX, &vectorY, &g_TF_Static_Data.frameTool0ToFlange); + mpInvFrame(&g_TF_Static_Data.frameTool0ToFlange, &g_TF_Static_Data.frameFlangeToTool0); + mpFrameToZYXeuler(&g_TF_Static_Data.frameFlangeToTool0, &g_TF_Static_Data.coordFlangeToTool0); +} + +bool Ros_PositionMonitor_Send_TF_Static() +{ + //Timestamp + INT64 theTime = rmw_uros_epoch_nanos(); + geometry_msgs__msg__TransformStamped__Sequence messages; + motoRosAssert(geometry_msgs__msg__TransformStamped__Sequence__init(&messages, motoRos_PositionMonitor_totalRobots), + SUBCODE_FAIL_ALLOCATE_FLANGE_TOOL0); + char formatBuffer[MAX_TF_FRAME_NAME_LENGTH]; + bzero(formatBuffer, MAX_TF_FRAME_NAME_LENGTH); + const char* frame_prefix = g_nodeConfigSettings.tf_frame_prefix; + for (int i = 0; i < motoRos_PositionMonitor_totalRobots; i++) + { + snprintf(formatBuffer, MAX_TF_FRAME_NAME_LENGTH, "%sr%d/flange", frame_prefix, i + 1); + rosidl_runtime_c__String__assign(&messages.data[i].header.frame_id, formatBuffer); + snprintf(formatBuffer, MAX_TF_FRAME_NAME_LENGTH, "%sr%d/tool0", frame_prefix, i + 1); + rosidl_runtime_c__String__assign(&messages.data[i].child_frame_id, formatBuffer); + Ros_MpCoord_To_GeomMsgsTransform(&g_TF_Static_Data.coordFlangeToTool0, &messages.data[i].transform); + Ros_Nanos_To_Time_Msg(theTime, &messages.data[i].header.stamp); + } + messages.size = motoRos_PositionMonitor_totalRobots; + bool ret = Ros_StaticTransformBroadcaster_Send(messages.data, motoRos_PositionMonitor_totalRobots); + geometry_msgs__msg__TransformStamped__Sequence__fini(&messages); + return ret; +} + void Ros_PositionMonitor_Cleanup() { MOTOROS2_MEM_TRACE_START(pos_mon_fini); @@ -247,6 +286,8 @@ void Ros_PositionMonitor_Cleanup() Ros_Debug_BroadcastMsg("Failed cleaning up TF publisher: %d", ret); tf2_msgs__msg__TFMessage__destroy(g_messages_PositionMonitor.transform); + Ros_StaticTransformBroadcaster_Cleanup(); + MOTOROS2_MEM_TRACE_REPORT(pos_mon_fini); } @@ -266,9 +307,9 @@ void Ros_PositionMonitor_CalculateTransforms(int groupIndex, long* pulsePos_moto //this CtrlGroup's pose can be converted, so update ROS transforms //first update stamp of this group's transforms - for (int i = 0; i < NUMBER_TRANSFORM_LINKS_PER_ROBOT; i += 1) + for (int i = 0; i < PUBLISHED_NUMBER_TRANSFORM_LINKS_PER_ROBOT; i += 1) { - Ros_Nanos_To_Time_Msg(timestamp, &g_messages_PositionMonitor.transform->transforms.data[(groupIndex * NUMBER_TRANSFORM_LINKS_PER_ROBOT) + i].header.stamp); + Ros_Nanos_To_Time_Msg(timestamp, &g_messages_PositionMonitor.transform->transforms.data[(groupIndex * PUBLISHED_NUMBER_TRANSFORM_LINKS_PER_ROBOT) + i].header.stamp); } //======================= @@ -325,7 +366,7 @@ void Ros_PositionMonitor_CalculateTransforms(int groupIndex, long* pulsePos_moto mpFrameToZYXeuler(&frameWorldToRobot, &coordWorldToBase); } - geometry_msgs__msg__Transform* transform = &g_messages_PositionMonitor.transform->transforms.data[(groupIndex * NUMBER_TRANSFORM_LINKS_PER_ROBOT) + tfLink_WorldToBase].transform; + geometry_msgs__msg__Transform* transform = &g_messages_PositionMonitor.transform->transforms.data[(groupIndex * PUBLISHED_NUMBER_TRANSFORM_LINKS_PER_ROBOT) + publishIndex_tfLink_WorldToBase].transform; Ros_MpCoord_To_GeomMsgsTransform(&coordWorldToBase, transform); //============================ @@ -343,10 +384,9 @@ void Ros_PositionMonitor_CalculateTransforms(int groupIndex, long* pulsePos_moto // // MP_FRAME frameBaseToTcp, frameBaseToTool0, frameBaseToFlange, frameTool0ToTcp, frameTcpToTool0; - MP_FRAME frameTool0ToFlange, frameFlangeToTool0, frameFlangeToTcp; + MP_FRAME frameFlangeToTcp; MP_TOOL_RSP_DATA retToolData; - MP_COORD coordToolData, coordBaseToFlange, coordFlangeToTool0; - MP_XYZ vectorOrg, vectorX, vectorY; //for mpMakeFrame + MP_COORD coordToolData, coordBaseToFlange; long anglePos_moto[MAX_PULSE_AXES]; mpConvPulseToAngle(groupIndex, pulsePos_moto, anglePos_moto); @@ -366,29 +406,18 @@ void Ros_PositionMonitor_CalculateTransforms(int groupIndex, long* pulsePos_moto mpMulFrame(&frameBaseToTcp, &frameTcpToTool0, &frameBaseToTool0); - //Make rotational frames - vectorOrg.x = 0; vectorOrg.y = 0; vectorOrg.z = 0; - vectorX.x = 0; vectorX.y = 0; vectorX.z = 1; - vectorY.x = 0; vectorY.y = -1; vectorY.z = 0; - mpMakeFrame(&vectorOrg, &vectorX, &vectorY, &frameTool0ToFlange); - mpInvFrame(&frameTool0ToFlange, &frameFlangeToTool0); - mpFrameToZYXeuler(&frameFlangeToTool0, &coordFlangeToTool0); - - mpMulFrame(&frameBaseToTool0, &frameTool0ToFlange, &frameBaseToFlange); + mpMulFrame(&frameBaseToTool0, &g_TF_Static_Data.frameTool0ToFlange, &frameBaseToFlange); mpFrameToZYXeuler(&frameBaseToFlange, &coordBaseToFlange); - mpMulFrame(&frameFlangeToTool0, &frameTool0ToTcp, &frameFlangeToTcp); + mpMulFrame(&g_TF_Static_Data.frameFlangeToTool0, &frameTool0ToTcp, &frameFlangeToTcp); mpFrameToZYXeuler(&frameFlangeToTcp, &coordToolData); //======================= - transform = &g_messages_PositionMonitor.transform->transforms.data[(groupIndex * NUMBER_TRANSFORM_LINKS_PER_ROBOT) + tfLink_BaseToFlange].transform; + transform = &g_messages_PositionMonitor.transform->transforms.data[(groupIndex * PUBLISHED_NUMBER_TRANSFORM_LINKS_PER_ROBOT) + publishIndex_tfLink_BaseToFlange].transform; Ros_MpCoord_To_GeomMsgsTransform(&coordBaseToFlange, transform); - transform = &g_messages_PositionMonitor.transform->transforms.data[(groupIndex * NUMBER_TRANSFORM_LINKS_PER_ROBOT) + tfLink_FlangeToTool0].transform; - Ros_MpCoord_To_GeomMsgsTransform(&coordFlangeToTool0, transform); - - transform = &g_messages_PositionMonitor.transform->transforms.data[(groupIndex * NUMBER_TRANSFORM_LINKS_PER_ROBOT) + tfLink_FlangeToTcp].transform; + transform = &g_messages_PositionMonitor.transform->transforms.data[(groupIndex * PUBLISHED_NUMBER_TRANSFORM_LINKS_PER_ROBOT) + publishIndex_tfLink_FlangeToTcp].transform; Ros_MpCoord_To_GeomMsgsTransform(&coordToolData, transform); } diff --git a/src/PositionMonitor.h b/src/PositionMonitor.h index 3e389be1..8fbb984e 100644 --- a/src/PositionMonitor.h +++ b/src/PositionMonitor.h @@ -10,13 +10,12 @@ typedef enum { - tfLink_WorldToBase = 0, - tfLink_BaseToFlange, - tfLink_FlangeToTool0, - tfLink_FlangeToTcp, - - NUMBER_TRANSFORM_LINKS_PER_ROBOT -} TransformLinkIndex; + publishIndex_tfLink_WorldToBase = 0, + publishIndex_tfLink_BaseToFlange, + //publishIndex_tfLink_FlangeToTool0, + publishIndex_tfLink_FlangeToTcp, + PUBLISHED_NUMBER_TRANSFORM_LINKS_PER_ROBOT +} PublishedTransformLinkIndex; typedef struct { @@ -32,9 +31,17 @@ typedef struct } PositionMonitor_Messages; extern PositionMonitor_Messages g_messages_PositionMonitor; +typedef struct +{ + MP_FRAME frameTool0ToFlange, frameFlangeToTool0; + MP_COORD coordFlangeToTool0; +} TF_Static_Data; +TF_Static_Data g_TF_Static_Data; + extern void Ros_PositionMonitor_Initialize(); extern void Ros_PositionMonitor_Cleanup(); - extern void Ros_PositionMonitor_UpdateLocation(); +extern bool Ros_PositionMonitor_Send_TF_Static(); +void Ros_PositionMonitor_CalculateStaticTransforms(); #endif // MOTOROS2_POSITION_MONITOR_H diff --git a/src/RosApiNameConstants.h b/src/RosApiNameConstants.h index 4523d31a..b338a988 100644 --- a/src/RosApiNameConstants.h +++ b/src/RosApiNameConstants.h @@ -13,6 +13,7 @@ // Topic, service and action server names //============================================ #define TOPIC_NAME_TF "tf" +#define TOPIC_NAME_TF_STATIC "tf_static" #define TOPIC_NAME_ROBOT_STATUS "robot_status" #define TOPIC_NAME_JOINT_STATES "joint_states" diff --git a/src/StaticTransformBroadcaster.c b/src/StaticTransformBroadcaster.c new file mode 100644 index 00000000..b94b66ba --- /dev/null +++ b/src/StaticTransformBroadcaster.c @@ -0,0 +1,115 @@ +// StaticTransformBroadcaster.c + +// SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. +// SPDX-FileCopyrightText: 2022-2023, Delft University of Technology +// +// SPDX-License-Identifier: Apache-2.0 + +#include "MotoROS.h" +rcl_publisher_t g_publishers_transform_static; +tf2_msgs__msg__TFMessage* g_messages_transform_static; + +void Ros_StaticTransformBroadcaster_Init() +{ + char formatBuffer[MAX_TF_FRAME_NAME_LENGTH]; + + // default TF topic name + bzero(formatBuffer, MAX_TF_FRAME_NAME_LENGTH); + snprintf(formatBuffer, MAX_TF_FRAME_NAME_LENGTH, "%s", TOPIC_NAME_TF_STATIC); + + //check whether we should make the topic name absolute (so it can't/won't + //be namespaced any further) + if (g_nodeConfigSettings.namespace_tf == FALSE) + { + Ros_Debug_BroadcastMsg("StaticTransformBroadcaster: TF_STATIC topic absolute"); + snprintf(formatBuffer, MAX_TF_FRAME_NAME_LENGTH, "/%s", TOPIC_NAME_TF_STATIC); + } + + Ros_Debug_BroadcastMsg("StaticTransformBroadcaster: publishing TF_STATIC to '%s'", formatBuffer); + + const rmw_qos_profile_t qos_profile_transform_static = + { + RMW_QOS_POLICY_HISTORY_KEEP_LAST, + 1, + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false + }; + + //------------- + //create TF publisher (static) + rcl_ret_t ret = rclc_publisher_init( + &g_publishers_transform_static, + &g_microRosNodeInfo.node, + ROSIDL_GET_MSG_TYPE_SUPPORT(tf2_msgs, msg, TFMessage), + formatBuffer, + &qos_profile_transform_static); + + motoRosAssert(ret == RCL_RET_OK, SUBCODE_FAIL_CREATE_PUBLISHER_STATIC_TRANSFORM); + + //-------------- + //create message for static cartesian transform + g_messages_transform_static = tf2_msgs__msg__TFMessage__create(); + + motoRosAssert(geometry_msgs__msg__TransformStamped__Sequence__init(&g_messages_transform_static->transforms, STATIC_TRANSFORM_BROADCASTER_MAX_TF_COUNT), + SUBCODE_FAIL_ALLOCATE_STATIC_TRANSFORM); + g_messages_transform_static->transforms.size = 0; +} + +bool Ros_StaticTransformBroadcaster_Send(geometry_msgs__msg__TransformStamped *msg, int count) +{ + rcl_ret_t ret; + geometry_msgs__msg__TransformStamped__Sequence* transforms = &g_messages_transform_static->transforms; + for (int i = 0; i < count; i++) + { + Ros_Debug_BroadcastMsg("Attempting to publish static transform, %s->%s...", msg->header.frame_id.data, msg->child_frame_id.data); + bool match_found = false; + for (int j = 0; j < transforms->size; j++) + { + if (rosidl_runtime_c__String__are_equal(&msg[i].child_frame_id, &transforms->data[j].child_frame_id)) + { + match_found = true; + Ros_Debug_BroadcastMsg("Transform already published. Updating data"); + geometry_msgs__msg__TransformStamped__copy(&transforms->data[j], msg); + break; + } + } + if (!match_found) + { + if (transforms->size < transforms->capacity) + { + Ros_Debug_BroadcastMsg("No match found. Adding transform to TF message."); + geometry_msgs__msg__TransformStamped__copy(&transforms->data[transforms->size], msg); + transforms->size++; + } + else + { + motoRosAssert_withMsg(FALSE, SUBCODE_FAIL_STATIC_TRANSFORM_MEM_LIMIT, "Memory limit reached"); + } + } + } + ret = rcl_publish(&g_publishers_transform_static, g_messages_transform_static, NULL); + if (ret == RCL_RET_OK) + { + Ros_Debug_BroadcastMsg("Updated message successfully published"); + return true; + } + // publishing can fail, but we choose to ignore those errors in this implementation + Ros_Debug_BroadcastMsg("Updated message failed to publish..."); + return false; +} + +void Ros_StaticTransformBroadcaster_Cleanup() +{ + rcl_ret_t ret; + Ros_Debug_BroadcastMsg("Cleanup TF STATIC publisher"); + ret = rcl_publisher_fini(&g_publishers_transform_static, &g_microRosNodeInfo.node); + if (ret != RCL_RET_OK) + Ros_Debug_BroadcastMsg("Failed cleaning up TF STATIC publisher: %d", ret); + tf2_msgs__msg__TFMessage__destroy(g_messages_transform_static); +} + diff --git a/src/StaticTransformBroadcaster.h b/src/StaticTransformBroadcaster.h new file mode 100644 index 00000000..efab71b6 --- /dev/null +++ b/src/StaticTransformBroadcaster.h @@ -0,0 +1,20 @@ +// StaticTransformBroadcaster.h + +// SPDX-FileCopyrightText: 2022-2023, Yaskawa America, Inc. +// SPDX-FileCopyrightText: 2022-2023, Delft University of Technology +// +// SPDX-License-Identifier: Apache-2.0 + +#ifndef MOTOROS2_STATIC_TRANSFORM_BROADCASTER_H +#define MOTOROS2_STATIC_TRANSFORM_BROADCASTER_H + +extern rcl_publisher_t g_publishers_transform_static; +extern tf2_msgs__msg__TFMessage* g_messages_transform_static; + +#define STATIC_TRANSFORM_BROADCASTER_MAX_TF_COUNT 16 + +extern bool Ros_StaticTransformBroadcaster_Send(geometry_msgs__msg__TransformStamped *msg, int count); +extern void Ros_StaticTransformBroadcaster_Init(); +extern void Ros_StaticTransformBroadcaster_Cleanup(); + +#endif // MOTOROS2_STATIC_TRANSFORM_BROADCASTER_H diff --git a/src/main.c b/src/main.c index 0b73b202..a5290e9f 100644 --- a/src/main.c +++ b/src/main.c @@ -148,9 +148,13 @@ void RosInitTask() //================================== ULONG tickBefore = 0; - + bool static_has_sent = false; while(g_Ros_Communication_AgentIsConnected) { + if(!static_has_sent) + { + static_has_sent = Ros_PositionMonitor_Send_TF_Static(); + } //figure out how long to sleep to achieve the user-configured rate ULONG tickNow = tickGet();