bridge_ros_marble.cpp

Go to the documentation of this file.

// Author: Isaac Saito
#include <ros/exception.h>
#include <sensor_msgs/NavSatFix.h>
#include <sensor_msgs/NavSatStatus.h>

#include <rqt_marble/bridge_ros_marble.h>

using namespace rqt_marble;
using namespace sensor_msgs;

BridgeRosMarble::BridgeRosMarble()
{
  //ros::init(0, NULL, "BridgeLibMarble");
  this->do_navigation = true;
  this->_publisher = this->_node.advertise<RouteGps>("route_gps", 1000);
}

void BridgeRosMarble::setDoNavigation(bool doNav)
{
  this->do_navigation = doNav;
}

RouteGps BridgeRosMarble::publishRouteInGps(Marble::Route route)
{
  RouteGps route_gps;
  if (!this->do_navigation || route.size() == 0)
  {
    ros::Exception("Marble::Route passed does not contain route.");
  }

  int size_route = route.size();
  ROS_DEBUG("size of route %i", size_route);
  for (int i = 0; i < size_route; i++)
  {
    Marble::GeoDataLineString route_segment_line_str = route.at(i).path();
    for (int j = 0; j < route_segment_line_str.size(); j++)
    {
      Marble::GeoDataCoordinates coord = route_segment_line_str.at(j);
      // create GPS msg for ROS
      NavSatFix gps_msg = NavSatFix();
      gps_msg.latitude = coord.latitude();
      gps_msg.longitude = coord.longitude();
      gps_msg.status.status = NavSatStatus::STATUS_FIX;
      gps_msg.status.service = NavSatStatus::SERVICE_GPS;

      ROS_INFO("#%dth seg; coord#%dlongi=%f lat=%f", i, j, gps_msg.latitude,
               gps_msg.longitude);

      route_gps.routes.push_back(gps_msg);
    }
  }
  this->_publisher.publish(route_gps);
  return route_gps;
}