protobuf2ros_publisher.cc

Go to the documentation of this file.

/* 
* Roboception GmbH 
* Munich, Germany 
* www.roboception.com 
* 
* Copyright (c) 2017 Roboception GmbH 
* All rights reserved 
* 
* Author: Christian Emmerich 
*/


#include "protobuf2ros_publisher.h"

#include "protobuf2ros_conversions.h"

using namespace std;

namespace rc
{

Protobuf2RosPublisher::Protobuf2RosPublisher(ros::NodeHandle &nh,
                                             const string &topic,
                                             const string &pbMsgType,
                                             const string &frame_id_prefix)
        : _tfPrefix(frame_id_prefix)
{
  if (pbMsgType == "Imu")
  {
    pub = nh.advertise<sensor_msgs::Imu>(topic, 1000);
  } else if (pbMsgType == "Frame")
  {
    pub = nh.advertise<geometry_msgs::PoseStamped>(topic, 1000);
  } else
  {
    stringstream msg;
    msg << "Protobuf message type '" << pbMsgType << "' not supported!";
    throw runtime_error(msg.str());
  }
}


bool Protobuf2RosPublisher::used()
{
  return pub.getNumSubscribers() > 0;
}


void Protobuf2RosPublisher::publish(shared_ptr<::google::protobuf::Message> pbMsg)
{
  string msg_name = pbMsg->GetDescriptor()->name();

  if (msg_name == "Imu")
  {
    // dynamically cast to Imu type, check validity, and transform to ros

    shared_ptr <roboception::msgs::Imu> protoImu =
            dynamic_pointer_cast<roboception::msgs::Imu>(pbMsg);
    if (!protoImu)
    {
      throw runtime_error("Given protobuf message was not of type Imu!");
    }

    auto rosImu = toRosImu(protoImu);
    rosImu->header.frame_id = _tfPrefix + rosImu->header.frame_id;
    pub.publish(rosImu);
  }
  else if (msg_name == "Frame")
  {
    // dynamically cast to Frame type, check validity, and transform to ros

    shared_ptr <roboception::msgs::Frame> protoFrame =
            dynamic_pointer_cast<roboception::msgs::Frame>(pbMsg);
    if (!protoFrame)
    {
      throw runtime_error("Given protobuf message was not of type Frame!");
    }

    auto rosPose = toRosPoseStamped(protoFrame);
    rosPose->header.frame_id = _tfPrefix + rosPose->header.frame_id;
    pub.publish(rosPose);
  }
  else
  {
    stringstream msg;
    msg << "Protobuf message type '" << msg_name << "' not supported!";
    throw runtime_error(msg.str());
  }
}

}