ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

C++ set publisher type at runtime

asked 2018-10-12 12:24:50 -0500

AndyZe gravatar image

How much of a pain would it be to set the type of a Publisher at run time, based on a parameter?

I'm looking at a publisher that could take one of 2 types:

n.advertise<trajectory_msgs::JointTrajectory>

or

n.advertise<std_msgs::Float64MultiArray>
edit retag flag offensive close merge delete

Comments

1

I'm not sure this is possible with C++. The template args need to be available at compile time, so something needs to be specified there.

I think I know the context in which you're asking this question. Perhaps creating two publisher member variables but only advertise(..)ing one of them ..

gvdhoorn gravatar image gvdhoorn  ( 2018-10-12 15:53:38 -0500 )edit

.. based on whether the driver takes a properly typed trajectory or 6 floats could be a work-around.

Generic subscribers can be created (shapeshifter). Generic publishers perhaps (also with shapeshifter).

gvdhoorn gravatar image gvdhoorn  ( 2018-10-12 15:54:41 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2018-10-12 18:39:03 -0500

ahendrix gravatar image

It might be possible to use the ShapeShifter class for this, but that will only handle the interaction with the ros::Publisher object; it won't handle the type conversion from whatever internal type you have to the correct message type.

Since you only have to handle two cases, I'd suggest making a virtual base class and a class for each type that you want to publish (obviously, types and exact code flow may differ from what you're trying to do):

class TrajectoryPublisherBase {
  public:
    virtual void advertise(ros::NodeHandle& nh) = 0;
    virtual void publish(const MyJointTrajectoryType& joint_trajectory) = 0;
};

class JointTrajectoryPublisher {
  private:
    ros::Publisher pub_;
  public:
    virtual void advertise(ros::NodeHandle& nh, std::string topic) {
       pub_ = nh.advertise<trajectory_msgs::JointTrajectory>(topic);
    }
    virtual void publish(const MyJointTrajectoryType& joint_trajectory) {
        trajectory_msgs::JointTrajectory out;
        // TODO some code to convert your joint trajectory type to a trajectory_msgs::JointTrajectory
        pub_.publish(out);
    }
};

class MultiArrayPublisher {
  private:
    ros::Publisher pub_;
  public:
    virtual void advertise(ros::NodeHandle& nh, std::string topic) {
       pub_ = nh.advertise<tstd_msgs::Float64MultiArray>(topic);
    }
    virtual void publish(const MyJointTrajectoryType& joint_trajectory) {
        std_msgs::Float64MultiArray out;
        // TODO some code to convert your joint trajectory type to a std_msgs::Float64MultiArray
        pub_.publish(out);
    }
};

main() {
  std::shared_ptr<TrajectoryPublisherBase> pub;

  if(joint_trajectory) {
    pub.reset(new JointTrajectoryPublisher());
  } else {
    pub.reset(new MultiArrayPublisher());
  }

  MyJointTrajectoryType& joint_trajectory
  pub->publish(joint_trajectory);
}
edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2018-10-12 12:24:50 -0500

Seen: 454 times

Last updated: Oct 12 '18