Ask Your Question
0

Can one node publish only a single static transform?

asked 2019-04-12 01:55:07 -0500

updated 2019-04-14 01:57:49 -0500

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");
        }
    }
edit retag flag offensive close merge delete

Comments

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.

janindu gravatar imagejanindu ( 2019-04-12 03:26:03 -0500 )edit

I just added it. I don't know if you will be notified, so commenting.

Harsh2308 gravatar imageHarsh2308 ( 2019-04-12 03:36:17 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2019-04-14 00:02:50 -0500

janindu gravatar image

updated 2019-04-14 01:21:23 -0500

The problem is with the ros::spin() call at the end of the file. Until ros::spin() is called, the subscriber callbacks will not be executed. You can read up on it here.

I suggest you change the callbacks similar to this.

oid 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; // I'm not sure what this does. Not declared anywhere in your code snippet
            static_broadcaster[i].sendTransform(static_transformStamped[i]);
        }
         ROS_INFO("camera fiducial detected");
    }
}

And then change your main to

#include <ros/ros.h>
#include<fiducial_msgs/FiducialTransformArray.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
static tf2_ros::StaticTransformBroadcaster static_broadcaster[3];
geometry_msgs::TransformStamped static_transformStamped[3];
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);


          ros::spin();
          return 0;
    }

Furthermore, you don't need an array of static_broadcaster. One broadcaster can send multiple static transformation messages.

EDIT I have tested this code. It works.

#include <ros/ros.h>
#include<fiducial_msgs/FiducialTransformArray.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>

void transform_callback1(const fiducial_msgs::FiducialTransformArray);
void transform_callback2(const fiducial_msgs::FiducialTransformArray);
void transform_callback3(const fiducial_msgs::FiducialTransformArray);

static int a = 0;
static int b = 0;
static int c = 0;

geometry_msgs::TransformStamped static_transformStamped[3];

int main(int argc, char **argv)
{
  ros::init(argc, argv, "calibrate");
  ros::NodeHandle n;
  tf2_ros::StaticTransformBroadcaster static_broadcaster;
  ros::Subscriber sub1 = n.subscribe("/camera1/fiducial_transforms", 10,transform_callback1); ///fill a     static_transform_stamped message
  ros::Subscriber sub2 = n.subscribe("/camera2/fiducial_transforms", 10,transform_callback2);
  ros::Subscriber sub3 = n.subscribe("/camera3/fiducial_transforms", 10,transform_callback3);

  while( a == 0 || b == 0 || c == 0) {
      ros::spinOnce();
  }

  for (int i=0; i<3; i++) {
    static_broadcaster.sendTransform(static_transformStamped[i]);
  }

  ROS_INFO(" All TFs published");

  return 0;
}

void transform_callback1(const fiducial_msgs::FiducialTransformArray msg)
    {
       if(msg.transforms.size()>0 && a == 0)
        {
            a = 1;

            static_transformStamped[0].header.stamp = ros::Time::now();
            static_transformStamped[0].child_frame_id = "aruco1";
            static_transformStamped[0].header.frame_id = "/camera_color_frame";
            static_transformStamped[0].transform = msg.transforms[0].transform;

            ROS_INFO("camera1 fiducial detected");
        }
    }

void transform_callback2(const fiducial_msgs::FiducialTransformArray msg)
    {
       if(msg.transforms.size()>0 && b == 0)
        {
            b = 1;

            static_transformStamped[1].header.stamp = ros::Time::now();
            static_transformStamped[1].child_frame_id = "aruco2";
            static_transformStamped[1].header.frame_id = "/camera_color_frame";
            static_transformStamped[1].transform = msg.transforms[0].transform;

            ROS_INFO("camera2 fiducial detected");
        }
    }

void transform_callback3(const fiducial_msgs::FiducialTransformArray msg)
    {
       if(msg.transforms.size()>0 && c == 0)
        {
            c = 1;

            static_transformStamped[2].header.stamp = ros::Time::now();
            static_transformStamped[2].child_frame_id = "aruco3";
            static_transformStamped[2].header.frame_id = "/camera_color_frame";
            static_transformStamped[2].transform = msg.transforms[0].transform;

            ROS_INFO("camera3 fiducial detected");
        }
    }
edit flag offensive delete link more

Comments

Thanks for the reply. I changed to multiple broadcasters assuming single broadcaster can only send one static transform. I don't understand why will I want to send static transforms on every iteration in the spin loop. The purpose of static transform is to register that transform once and for all on the tf tree. If you look at the demo static_broadcaster code, you will see that the node publishes the transform once only.

Sending transforms on every callback fails the concept of static transform. Correct me if I am wrong. Thanks.

( Also what I actually want to do is: [ 1. I have 3 cameras , all of them see a fiducial[aruco marker] and I get 3 cameratoaruco transforms. 2. I compute transformation b/w camera1&camera2 and camera1&camera3. 3. I send these as static transforms. ] )

Harsh2308 gravatar imageHarsh2308 ( 2019-04-14 00:47:34 -0500 )edit

You are correct in that you only need to broadcast a static transformation once. If you look at the callback, the variable j makes sure that the transform is broadcasted only once.

janindu gravatar imagejanindu ( 2019-04-14 00:53:35 -0500 )edit

Yes you are right. So what is the difference doing it before and now? In your solution, again the line static_transformStamped[i].transform = msg.transforms[0].transform; is executed only once (for all i s).

Harsh2308 gravatar imageHarsh2308 ( 2019-04-14 00:58:16 -0500 )edit

The ros::spin() needs to be called within your program for callbacks to be executed. In your code, the static transformations are broadcasted beforeros::spin() is called. So the transform stamped message array is not set with your transformation data. That only happens once the subscriber callbacks are executed. You can read up on ros::spin() and how it works here.

janindu gravatar imagejanindu ( 2019-04-14 01:01:37 -0500 )edit

Ah, my bad. I actually posted a truncated code as it seemed super redundant. Anyway, as you can see now I was ensuring few callbacks are made before I move to sending static transform. I have edited the code where the problem was occurring.

Harsh2308 gravatar imageHarsh2308 ( 2019-04-14 01:11:43 -0500 )edit

I have edited the answer with a fully working solution. Github repo

janindu gravatar imagejanindu ( 2019-04-14 01:24:12 -0500 )edit

And please take a couple of minutes to reformat your question. When you edited, the code formatting was lost and now it's hard to read.

janindu gravatar imagejanindu ( 2019-04-14 01:31:19 -0500 )edit

Hi. When node dies tf_static(if called later when transforms were published) shows no output. I added ros::spin() at the end that ensures node doesn't die and that way tf_static shows the three transforms. Your solution works, Thanks. I still don't find how is your code different and what is wrong with my code above in the question? Marking this as correct.

Harsh2308 gravatar imageHarsh2308 ( 2019-04-14 02:17:57 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2019-04-12 01:55:07 -0500

Seen: 91 times

Last updated: Apr 14