bridge_ros_marble.cpp
Go to the documentation of this file.
00001 
00034 // Author: Isaac Saito
00035 #include <ros/exception.h>
00036 #include <sensor_msgs/NavSatFix.h>
00037 #include <sensor_msgs/NavSatStatus.h>
00038 
00039 #include <rqt_marble/bridge_ros_marble.h>
00040 
00041 using namespace rqt_marble;
00042 using namespace sensor_msgs;
00043 
00044 BridgeRosMarble::BridgeRosMarble()
00045 {
00046   //ros::init(0, NULL, "BridgeLibMarble");
00047   this->do_navigation = true;
00048   this->_publisher = this->_node.advertise<RouteGps>("route_gps", 1000);
00049 }
00050 
00051 void BridgeRosMarble::setDoNavigation(bool doNav)
00052 {
00053   this->do_navigation = doNav;
00054 }
00055 
00056 RouteGps BridgeRosMarble::publishRouteInGps(Marble::Route route)
00057 {
00058   RouteGps route_gps;
00059   if (!this->do_navigation || route.size() == 0)
00060   {
00061     ros::Exception("Marble::Route passed does not contain route.");
00062   }
00063 
00064   int size_route = route.size();
00065   ROS_DEBUG("size of route %i", size_route);
00066   for (int i = 0; i < size_route; i++)
00067   {
00068     Marble::GeoDataLineString route_segment_line_str = route.at(i).path();
00069     for (int j = 0; j < route_segment_line_str.size(); j++)
00070     {
00071       Marble::GeoDataCoordinates coord = route_segment_line_str.at(j);
00072       // create GPS msg for ROS
00073       NavSatFix gps_msg = NavSatFix();
00074       gps_msg.latitude = coord.latitude();
00075       gps_msg.longitude = coord.longitude();
00076       gps_msg.status.status = NavSatStatus::STATUS_FIX;
00077       gps_msg.status.service = NavSatStatus::SERVICE_GPS;
00078 
00079       ROS_INFO("#%dth seg; coord#%dlongi=%f lat=%f", i, j, gps_msg.latitude,
00080                gps_msg.longitude);
00081 
00082       route_gps.routes.push_back(gps_msg);
00083     }
00084   }
00085   this->_publisher.publish(route_gps);
00086   return route_gps;
00087 }


rqt_marble
Author(s): Isaac Saito , Tobias Baer , Jan Aidel
autogenerated on Fri Apr 10 2015 02:55:01