diff --git a/src/ErrorHandling.h b/src/ErrorHandling.h
index c4f63be..303996d 100644
--- a/src/ErrorHandling.h
+++ b/src/ErrorHandling.h
@@ -176,6 +176,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 cd02552..1e2c1ee 100644
--- a/src/PositionMonitor.c
+++ b/src/PositionMonitor.c
@@ -10,13 +10,13 @@
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()
+void Ros_PositionMonitor_Initialize(TF_Static_Data* tf_static_data, rcl_publisher_t* publisher_transform_static, tf2_msgs__msg__TFMessage* msg_transform_static)
{
MOTOROS2_MEM_TRACE_START(pos_mon_init);
@@ -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(publisher_transform_static, msg_transform_static);
+
+ Ros_PositionMonitor_CalculateStaticTransforms(tf_static_data);
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,37 +192,69 @@ 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_Cleanup()
+void Ros_PositionMonitor_CalculateStaticTransforms(TF_Static_Data *tf_static_data)
+{
+ 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, &tf_static_data->frameTool0ToFlange);
+ mpInvFrame(&tf_static_data->frameTool0ToFlange, &tf_static_data->frameFlangeToTool0);
+ mpFrameToZYXeuler(&tf_static_data->frameFlangeToTool0, &tf_static_data->coordFlangeToTool0);
+}
+
+bool Ros_PositionMonitor_Send_TF_Static(TF_Static_Data* tf_static_data, rcl_publisher_t* publisher_transform_static, tf2_msgs__msg__TFMessage* msg_transform_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(&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(publisher_transform_static, msg_transform_static, messages.data, motoRos_PositionMonitor_totalRobots);
+ geometry_msgs__msg__TransformStamped__Sequence__fini(&messages);
+ return ret;
+}
+
+void Ros_PositionMonitor_Cleanup(rcl_publisher_t* publisher_transform_static, tf2_msgs__msg__TFMessage* msg_transform_static)
{
MOTOROS2_MEM_TRACE_START(pos_mon_fini);
@@ -247,10 +286,12 @@ 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(publisher_transform_static, msg_transform_static);
+
MOTOROS2_MEM_TRACE_REPORT(pos_mon_fini);
}
-void Ros_PositionMonitor_CalculateTransforms(int groupIndex, long* pulsePos_moto, long* pulsePos_moto_track, INT64 timestamp)
+void Ros_PositionMonitor_CalculateTransforms(int groupIndex, long* pulsePos_moto, long* pulsePos_moto_track, TF_Static_Data *tf_static_data, INT64 timestamp)
{
double track_pos_meters[MAX_PULSE_AXES];
BITSTRING figure;
@@ -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,33 +406,22 @@ 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, &tf_static_data->frameTool0ToFlange, &frameBaseToFlange);
mpFrameToZYXeuler(&frameBaseToFlange, &coordBaseToFlange);
- mpMulFrame(&frameFlangeToTool0, &frameTool0ToTcp, &frameFlangeToTcp);
+ mpMulFrame(&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);
}
-void Ros_PositionMonitor_UpdateLocation()
+void Ros_PositionMonitor_UpdateLocation(TF_Static_Data *tf_static_data)
{
long pulsePos_moto[MAX_CONTROLLABLE_GROUPS][MAX_PULSE_AXES];
long pulsePos_moto_track[MAX_CONTROLLABLE_GROUPS][MAX_PULSE_AXES];
@@ -451,7 +480,7 @@ void Ros_PositionMonitor_UpdateLocation()
// cartesian
if (g_nodeConfigSettings.publish_tf)
- Ros_PositionMonitor_CalculateTransforms(groupIndex, pulsePos_moto[groupIndex], pulsePos_moto_track[groupIndex], theTime);
+ Ros_PositionMonitor_CalculateTransforms(groupIndex, pulsePos_moto[groupIndex], pulsePos_moto_track[groupIndex], tf_static_data, theTime);
//----------------------------
//VELOCITY
diff --git a/src/PositionMonitor.h b/src/PositionMonitor.h
index 3e389be..7fd93f0 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,16 @@ typedef struct
} PositionMonitor_Messages;
extern PositionMonitor_Messages g_messages_PositionMonitor;
-extern void Ros_PositionMonitor_Initialize();
-extern void Ros_PositionMonitor_Cleanup();
-
-extern void Ros_PositionMonitor_UpdateLocation();
+typedef struct
+{
+ MP_FRAME frameTool0ToFlange, frameFlangeToTool0;
+ MP_COORD coordFlangeToTool0;
+} TF_Static_Data;
+
+extern void Ros_PositionMonitor_Initialize(TF_Static_Data* tf_static_data, rcl_publisher_t* publisher_transform_static, tf2_msgs__msg__TFMessage* msg_transform_static);
+extern void Ros_PositionMonitor_Cleanup(rcl_publisher_t* publisher_transform_static, tf2_msgs__msg__TFMessage* msg_transform_static);
+extern void Ros_PositionMonitor_UpdateLocation(TF_Static_Data *);
+extern bool Ros_PositionMonitor_Send_TF_Static(TF_Static_Data*, rcl_publisher_t* publisher_transform_static, tf2_msgs__msg__TFMessage* msg_transform_static);
+void Ros_PositionMonitor_CalculateStaticTransforms(TF_Static_Data*);
#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..67b8bc9
--- /dev/null
+++ b/src/StaticTransformBroadcaster.c
@@ -0,0 +1,113 @@
+// 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"
+
+void Ros_StaticTransformBroadcaster_Init(rcl_publisher_t *publisher_transform_static, tf2_msgs__msg__TFMessage* msg_transform_static)
+{
+ 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(
+ publisher_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
+ ret = tf2_msgs__msg__TFMessage__init(msg_transform_static);
+
+ motoRosAssert(geometry_msgs__msg__TransformStamped__Sequence__init(&msg_transform_static->transforms, STATIC_TRANSFORM_BROADCASTER_MAX_TF_COUNT),
+ SUBCODE_FAIL_ALLOCATE_STATIC_TRANSFORM);
+ msg_transform_static->transforms.size = 0;
+}
+
+bool Ros_StaticTransformBroadcaster_Send(rcl_publisher_t *publisher_transform_static, tf2_msgs__msg__TFMessage* msg_transform_static, geometry_msgs__msg__TransformStamped *msg, int count)
+{
+ rcl_ret_t ret;
+ geometry_msgs__msg__TransformStamped__Sequence* transforms = &msg_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(publisher_transform_static, msg_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_publisher_t* publisher_transform_static, tf2_msgs__msg__TFMessage* msg_transform_static)
+{
+ rcl_ret_t ret;
+ Ros_Debug_BroadcastMsg("Cleanup TF STATIC publisher");
+ ret = rcl_publisher_fini(publisher_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__fini(msg_transform_static);
+}
+
diff --git a/src/StaticTransformBroadcaster.h b/src/StaticTransformBroadcaster.h
new file mode 100644
index 0000000..8394559
--- /dev/null
+++ b/src/StaticTransformBroadcaster.h
@@ -0,0 +1,17 @@
+// 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
+
+#define STATIC_TRANSFORM_BROADCASTER_MAX_TF_COUNT 16
+
+extern bool Ros_StaticTransformBroadcaster_Send(rcl_publisher_t *publisher_transform_static, tf2_msgs__msg__TFMessage *msg_transform_static, geometry_msgs__msg__TransformStamped *msg, int count);
+extern void Ros_StaticTransformBroadcaster_Init(rcl_publisher_t *publisher_transform_static, tf2_msgs__msg__TFMessage *msg_transform_static);
+extern void Ros_StaticTransformBroadcaster_Cleanup(rcl_publisher_t *publisher_transform_static, tf2_msgs__msg__TFMessage *msg_transform_static);
+
+#endif // MOTOROS2_STATIC_TRANSFORM_BROADCASTER_H
diff --git a/src/main.c b/src/main.c
index 4c574c6..280f325 100644
--- a/src/main.c
+++ b/src/main.c
@@ -51,6 +51,10 @@ void Ros_ReportVersionInfoToController()
void RosInitTask()
{
+ TF_Static_Data tf_static_data;
+ rcl_publisher_t publisher_transform_static;
+ tf2_msgs__msg__TFMessage msg_transform_static;
+
g_Ros_Controller.tidIncMoveThread = INVALID_TASK;
Ros_ConfigFile_SetAllDefaultValues();
@@ -123,7 +127,7 @@ void RosInitTask()
Ros_InformChecker_ValidateJob();
- Ros_PositionMonitor_Initialize();
+ Ros_PositionMonitor_Initialize(&tf_static_data, &publisher_transform_static, &msg_transform_static);
Ros_ActionServer_FJT_Initialize(); //initialize action server - FollowJointTrajectory
Ros_ServiceQueueTrajPoint_Initialize();
@@ -146,9 +150,10 @@ void RosInitTask()
Ros_Debug_BroadcastMsg("Initialization complete.");
+ Ros_PositionMonitor_Send_TF_Static(&tf_static_data, &publisher_transform_static, &msg_transform_static);
+
//==================================
ULONG tickBefore = 0;
-
while(g_Ros_Communication_AgentIsConnected)
{
//figure out how long to sleep to achieve the user-configured rate
@@ -182,7 +187,7 @@ void RosInitTask()
}
//Update robot's feedback position and publish the topics
- Ros_PositionMonitor_UpdateLocation();
+ Ros_PositionMonitor_UpdateLocation(&tf_static_data);
}
//==================================
@@ -213,7 +218,7 @@ void RosInitTask()
Ros_ServiceQueueTrajPoint_Cleanup();
Ros_ActionServer_FJT_Cleanup();
- Ros_PositionMonitor_Cleanup();
+ Ros_PositionMonitor_Cleanup(&publisher_transform_static, &msg_transform_static);
Ros_Controller_Cleanup();
Ros_Communication_Cleanup();
Ros_mpGetRobotCalibrationData_Cleanup();