Can one node publish only a single static transform?
My code is like this where I am publishing 3 static transforms from three static publishers-
#include <ros/ros.h>
#include<fiducial_msgs/FiducialTransformArray.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
bool arr[3]={0,0,0};
geometry_msgs::TransformStamped static_transformStamped[3];
bool all_true(bool *arr)
{
for(int i=0:i<3;i++)
if(arr[i]==0)return false;
return true;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "calibrate");
ros::NodeHandle n;
tf2_ros::StaticTransformBroadcaster static_broadcaster[3];
ros::Subscriber sub = n.subscribe("/camera/fiducial_transforms", 10,transform_callback1);
//fill a static_transform_stamped message
ros::Subscriber sub2 = n.subscribe("/camera1/fiducial_transforms", 10,transform_callback2);
ros::Subscriber sub3 = n.subscribe("/camera2/fiducial_transforms", 10,transform_callback3);
//few callbacks made
while(!all_true(arr))
{
ros::spinOnce();
usleep(1000000);
}
for(int i=0;i<3;i++)
static_broadcaster[i].sendTransform(static_transformStamped[i]);
ros::spin();
return 0;
}
But on doing
rostopic echo /tf_static
I get only one(the last one) static transform. So what is happening here? Do I need to create a new node for publishing a different static transform?
EDIT: Adding the callbacks definition-
void transform_callback1(const fiducial_msgs::FiducialTransformArray msg)
{
static int i=0,j=0;
if(msg.transforms.size()>0)
{
if(j==0)
{
j+=1;
static_transformStamped[i].header.stamp = msg.header.stamp;//ros::Time::now();
static_transformStamped[i].child_frame_id = "aruco";
static_transformStamped[i].header.frame_id = msg.header.frame_id;//"/camera_color_frame";
static_transformStamped[i].transform = msg.transforms[0].transform;
arr[i]=1;
}
ROS_INFO("camera fiducial detected");
}
}
void transform_callback2(const fiducial_msgs::FiducialTransformArray msg)
{
static int i=1,j=0;
if(msg.transforms.size()>0)
{
if(j==0)
{
j+=1;
static_transformStamped[i].header.stamp = msg.header.stamp;//ros::Time::now();
static_transformStamped[i].child_frame_id = "aruco1";
static_transformStamped[i].header.frame_id = msg.header.frame_id;//"/camera_color_frame";
static_transformStamped[i].transform = msg.transforms[0].transform;
arr[i]=1;
}
ROS_INFO("camera 1 fiducial detected");
}
}
void transform_callback3(const fiducial_msgs::FiducialTransformArray msg)
{
static int i=2,j=0;
if(msg.transforms.size()>0)
{
if(j==0)
{
j+=1;
static_transformStamped[i].header.stamp = msg.header.stamp;//ros::Time::now();
static_transformStamped[i].child_frame_id = "aruco2";
static_transformStamped[i].header.frame_id = msg.header.frame_id;//"/camera_color_frame";
static_transformStamped[i].transform = msg.transforms[0].transform;
arr[i]=1;
}
ROS_INFO("camera 2 fiducial detected");
}
}
Can you edit to add the construction of static transform stamped message please? I sense that the problem is with the header of the transforms.
I just added it. I don't know if you will be notified, so commenting.