diff --git a/src/ErrorHandling.h b/src/ErrorHandling.h
index 91d8078..bdf7a34 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 19dc651..bd8a7a9 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 b5fad92..785d018 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 6b58586..3ca16b6 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 8e566f0..a9180f8 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 3e389be..8fbb984 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 4523d31..b338a98 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 0000000..e5c3dd6
--- /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(&msg[i], &transforms->data[j]);
+ 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(&msg[i], &transforms->data[transforms->size]);
+ 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 0000000..efab71b
--- /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 0b73b20..a5290e9 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();