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);
}
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 .... 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).