Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

Using global variables isn't good practice and is probably the source of the problems you're having now.

I'd suggest going about this in a different way. A better option is to make a class with the publisher and subscriber as class members. The main advantage is that you can publish the "transformed" message directly from the subscriber callback. This will also be much easier to expand and troubleshoot as you add functionality to your node.

Here's a minimal example, and here's a tutorial showing how to do this:

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>

class PoseTransformer
{
public:
    PoseTransformer(ros::NodeHandle &nh)
    {
        pose_sub_ = nh.subscribe<geometry_msgs::PoseStamped>("vrpn_client_node/quadpeter/pose", 1, &PoseTransformer::pose_cb, this);
        pose_pub_ = nh.advertise<geometry_msgs::PoseStamped>("mavros/vision_pose/pose", 1);
    }

private:
    void pose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg)
    {
        geometry_msgs::PoseStamped pose_transformed = *msg;
        // do transforms, etc. here
        pose_pub_.publish(pose_transformed);
    }

    ros::Subscriber pose_sub_;
    ros::Publisher pose_pub_;
};


int main(int argc, char** argv) {
  ros::init(argc, argv, "vicon_node");
  ros::NodeHandle nh;
  PoseTransformer pose_transformer(nh);
  ros::spin();
  return 0;
}

Things may need to change a bit depending on the specific needs of your application, e.g. if you need to publish at a different rate than your subscriber.