Skip to content

Commit

Permalink
Create static transform broadcaster and broadcast flange->tool0 on /t…
Browse files Browse the repository at this point in the history 
…f_static
  • Loading branch information
@jimmy-mcelwain
jimmy-mcelwain committed Oct 22, 2024
1 parent 225ed07 commit 1b636a3
Show file tree
Hide file tree
Showing 10 changed files with 247 additions and 52 deletions.
4 changes: 4 additions & 0 deletions src/ErrorHandling.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
1 change: 1 addition & 0 deletions src/MotoROS.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down
2 changes: 2 additions & 0 deletions src/MotoROS2_AllControllers.vcxproj
View file
Original file line number Diff line number Diff line change
Expand Up @@ -343,6 +343,7 @@
<ClCompile Include="ServiceStopTrajMode.c" />
<ClCompile Include="ServiceStartTrajMode.c" />
<ClCompile Include="ServiceSelectMotionTool.c" />
<ClCompile Include="StaticTransformBroadcaster.c" />
<ClCompile Include="Tests_ActionServer_FJT.c" />
<ClCompile Include="Tests_ControllerStatusIO.c" />
<ClCompile Include="Tests_CtrlGroup.c" />
Expand Down Expand Up @@ -377,6 +378,7 @@
<ClInclude Include="ServiceStopTrajMode.h" />
<ClInclude Include="ServiceStartTrajMode.h" />
<ClInclude Include="ServiceSelectMotionTool.h" />
<ClInclude Include="StaticTransformBroadcaster.h" />
<ClInclude Include="Tests_ActionServer_FJT.h" />
<ClInclude Include="Tests_ControllerStatusIO.h" />
<ClInclude Include="Tests_CtrlGroup.h" />
Expand Down
20 changes: 16 additions & 4 deletions src/MotoROS2_AllControllers.vcxproj.filters
View file
Original file line number Diff line number Diff line change
Expand Up @@ -333,6 +333,12 @@
<ClCompile Include="RosMotoPlusConversionUtils.c">
<Filter>Source Files\Utilities</Filter>
</ClCompile>
<ClCompile Include="Tests_ActionServer_FJT.c">
<Filter>Source Files</Filter>
</ClCompile>
<ClCompile Include="StaticTransformBroadcaster.c">
<Filter>Source Files\Topics and Publishers</Filter>
</ClCompile>
</ItemGroup>
<ItemGroup>
<ClInclude Include="MotoROS.h">
Expand Down Expand Up @@ -425,9 +431,6 @@
<ClInclude Include="RosMotoPlusConversionUtils.h">
<Filter>Header Files\Utilities</Filter>
</ClInclude>
<ClInclude Include="MotoPlusExterns.h">
<Filter>Header Files\Robot Controller</Filter>
</ClInclude>
<ClInclude Include="InformCheckerAndGenerator.h">
<Filter>Header Files\Robot Controller</Filter>
</ClInclude>
Expand All @@ -443,5 +446,14 @@
<ClInclude Include="Ros_mpGetRobotCalibrationData.h">
<Filter>Header Files\Robot Controller</Filter>
</ClInclude>
<ClInclude Include="..\lib\MotoPlusExterns.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="Tests_ActionServer_FJT.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="StaticTransformBroadcaster.h">
<Filter>Header Files\Topics and Publishers</Filter>
</ClInclude>
</ItemGroup>
</Project>
</Project>
107 changes: 68 additions & 39 deletions src/PositionMonitor.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()
{
Expand All @@ -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);
}
Expand Down Expand Up @@ -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];

Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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);
}

Expand All @@ -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);
}

//=======================
Expand Down Expand Up @@ -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);

//============================
Expand All @@ -343,10 +384,9 @@ void Ros_PositionMonitor_CalculateTransforms(int groupIndex, long* pulsePos_moto
// <image url="$(ProjectDir)image_comments\tf_diagram.png" />
//
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);
Expand All @@ -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);
}

Expand Down
23 changes: 15 additions & 8 deletions src/PositionMonitor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand All @@ -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
1 change: 1 addition & 0 deletions src/RosApiNameConstants.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"

Expand Down
Loading

0 comments on commit 1b636a3

Please sign in to comment.