Rietveld Code Review Tool
Help | Bug tracker | Discussion group | Source code | Sign in
(204)

Unified Diff: src/mobility-service-interface/model/google-maps-decoder.cc

Issue 265730044: Code review for the mobility-service-interface
Patch Set: Created 8 years, 6 months ago
Use n/p to move between diff chunks; N/P to move between comments. Please Sign in to add in-line comments.
Jump to:
View side-by-side diff with in-line comments
Download patch
Index: src/mobility-service-interface/model/google-maps-decoder.cc
===================================================================
new file mode 100644
--- /dev/null
+++ b/src/mobility-service-interface/model/google-maps-decoder.cc
@@ -0,0 +1,277 @@
+
+/* -*- Mode:C++; c-file-style:"gnu"; indent-tabs-mode:nil; -*- */
+/*
+ * Copyright (c) 2014 CISTER - Research Center in Real-Time & Embedded Computing Systems
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation;
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Author: Tiago Cerqueira <1090678@isep.ipp.pt> <tiago.miguel43@gmail.com>
+ * Author: Michele Albano <mialb@isep.ipp.pt> <michele.albano@gmail.com@gmail.com>
+ */
+
+#include "google-maps-decoder.h"
+NS_LOG_COMPONENT_DEFINE ("GoogleMapsDecoder");
+namespace ns3 {
+
+GoogleMapsDecoder::GoogleMapsDecoder ()
+{
+ NS_LOG_FUNCTION (this);
+ m_earth = GeographicLib::Geocentric (GeographicLib::Constants::WGS84_a (), GeographicLib::Constants::WGS84_f ());
+ m_proj = GeographicLib::LocalCartesian (41.1621429, -8.6218531, 0, m_earth);
+}
+
+GoogleMapsDecoder::GoogleMapsDecoder (double lat, double lng, double altitude)
+{
+ NS_LOG_FUNCTION (this);
+ m_earth = GeographicLib::Geocentric (GeographicLib::Constants::WGS84_a (), GeographicLib::Constants::WGS84_f ());
+ m_proj = GeographicLib::LocalCartesian (lat, lng, altitude, m_earth);
+}
+GoogleMapsDecoder::GoogleMapsDecoder (const GoogleMapsDecoder& orig)
+{
+}
+
+GoogleMapsDecoder::~GoogleMapsDecoder ()
+{
+ NS_LOG_FUNCTION (this);
+}
+
+int
+GoogleMapsDecoder::ConvertToGeoCoordinates (std::string polyline, std::list<Ptr<Point> > &pointList)
+{
+ NS_LOG_FUNCTION (this);
+ int index = 0;
+ int currentLat = 0;
+ int currentLng = 0;
+ int next5bits;
+ int sum;
+ int shifter;
+ int polylineSize = polyline.length ();
+ try
+ {
+ while (index < polylineSize)
+ {
+ //calculate next latitude
+ sum = 0;
+ shifter = 0;
+ do
+ {
+ next5bits = (int) polyline.at (index++) - 63;
+ sum |= (next5bits & 31) << shifter;
+ shifter += 5;
+ }
+ while (next5bits >= 32 && index < polylineSize);
+ if (index >= polylineSize)
+ {
+ break;
+ }
+ currentLat += (sum & 1) == 1 ? ~(sum >> 1) : (sum >> 1);
+
+ //calculate next longitude
+ sum = 0;
+ shifter = 0;
+ do
+ {
+ next5bits = int(polyline.at (index++) - 63);
+ sum |= (next5bits & 31) << shifter;
+ shifter += 5;
+ }
+ while (next5bits >= 32 && index < polylineSize);
+ if (index >= polylineSize && next5bits >= 32)
+ {
+ break;
+ }
+
+ currentLng += (sum & 1) == 1 ? ~(sum >> 1) : (sum >> 1);
+ //Point *p = new Point(currentLat/100000.0, currentLng/100000.0);
+ Ptr<Point> p = CreateObject <Point> (currentLat / 100000.0, currentLng / 100000.0);
+ //p->WriteToStdOut ();
+ pointList.push_back (p);
+ }
+ }
+ catch (int ex)
+ {
+ return ex;
+ }
+ return 1;
+}
+
+int
+GoogleMapsDecoder::ConvertToCartesian (LegList& legList)
+{
+ NS_LOG_FUNCTION (this);
+ double totalDistanceInStep = 0;
+ double x = 0, y = 0, z = 0, h = 0;
+ StepList stepList;
+ Ptr<Point> pTemp;
+ Ptr<Leg> leg;
+ std::list<Ptr<Point> > pointList, tempPointList;
+ std::list<Ptr<Point> >::iterator tempPointIterator;
+ for (LegList::iterator i = legList.begin (); i != legList.end (); i++)
+ {
+ leg = (*i);
+ leg->GetStepList (stepList);
+ for (StepList::iterator j = stepList.begin (); j != stepList.end (); j++)
+ {
+ pointList = (*j)->GetPointList ();
+ ConvertToGeoCoordinates ((*j)->GetPolyline (), pointList);
+ (*j)->SetPointList (pointList);
+ if((*j)->GetTravelTime ()==0)
+ {
+ pTemp = new Point(*pointList.back ());
+ pointList.clear ();
+ pointList.push_front (pTemp);
+ (*j)->SetPointList(pointList);
+ }
+ for (std::list <Ptr<Point> >::iterator k = pointList.begin (); k != pointList.end (); k++)
+ {
+ pTemp = (*k);
+ m_proj.Forward ((pTemp->GetLatitude ()), (pTemp->GetLongitude ()), h, x, y, z);
+ pTemp->SetXCoordinate (x);
+ pTemp->SetYCoordinate (y);
+ pTemp->SetZCoordinate (z);
+ }
+ totalDistanceInStep = leg->CalculateTotalDistanceInStep (*(*j));
+ FillInWaypointTime (pointList, totalDistanceInStep, stepList.begin (),j, legList.begin (),i,(*j)->GetTravelTime () );
+ }
+ }
+ return 0;
+}
+
+int
+GoogleMapsDecoder::SetWaypointTime (double startAt, double totalDistanceInStep, Ptr<Point> p1, Ptr<Point> p2, double travelTime)
+{
+ NS_LOG_FUNCTION (this);
+ double timeElapsed = 0;
+ double partialDistance = 0;
+ double waypointTime = 0;
+ if (startAt >= 0)
+ {
+ partialDistance = CalculatePartialDistance (p1->GetXCoordinate (), p2->GetXCoordinate (), p1->GetYCoordinate (), p2->GetYCoordinate ());
+ timeElapsed = (partialDistance * travelTime) / totalDistanceInStep;
+ waypointTime = (startAt + timeElapsed);
+ p2->SetWaypointTime (waypointTime);
+ NS_LOG_DEBUG("The time of the current waypoint is: " << waypointTime << " seconds.");
+ }
+ else
+ {
+ waypointTime = 0;
+ p1->SetWaypointTime (waypointTime);
+ NS_LOG_DEBUG("This is the first point in the list, and its time was set to zero seconds");
+
+ }
+ return waypointTime;
+}
+
+double
+GoogleMapsDecoder::CalculatePartialDistance (double x1, double x2, double y1, double y2)
+{
+ NS_LOG_FUNCTION (this);
+ double distance = sqrt (((pow ((x2 - x1), 2.0)) + (pow ((y2 - y1), 2.0))));
+ return distance;
+}
+
+void
+GoogleMapsDecoder::FillInWaypointTime (std::list<Ptr<Point> > &pointList, double totalDistanceInStep, std::list<Ptr<Step> >::iterator stepListBegin, std::list<Ptr<Step> >::iterator currentStepPosition, std::list<Ptr<Leg> >::iterator legListBegin, std::list<Ptr<Leg> >::iterator currentLegPosition, double travelTime)
+{
+ NS_LOG_FUNCTION (this);
+ std::list<Ptr<Step> >::iterator tempStepListIterator;
+ std::list<Ptr<Leg> >::iterator tempLegIterator;
+ std::list<Ptr<Step> > tempStepList;
+ bool firstExecution = true;
+ Ptr<Point> lastPoint,pTemp;
+ std::list<Ptr<Leg> > legList;
+ std::list<Ptr<Point> > tmpPointList;
+ std::list<Ptr<Point> >::iterator lastPointIterator;
+ double waypointTime = 0;
+ for (std::list <Ptr<Point> >::iterator k = pointList.begin (); k != pointList.end (); k++)
+ {
+ pTemp = (*k);
+ lastPointIterator = k;
+
+ //First point gets set to 0 seconds
+ if (firstExecution && currentLegPosition == legListBegin && currentStepPosition == stepListBegin)
+ {
+ SetWaypointTime (-1, totalDistanceInStep, pTemp, NULL, (*(*currentStepPosition)).GetTravelTime ());
+ firstExecution = false;
+ }
+ else
+ {
+ //If it's not the first point in the list, we can check the last time by iterating the list back
+ if (k != pointList.begin ())
+ {
+ lastPointIterator--;
+ lastPoint = *lastPointIterator;
+ waypointTime = SetWaypointTime ((lastPoint)->GetWaypointTime (), totalDistanceInStep, (lastPoint), pTemp, travelTime);
+ if(waypointTime - (lastPoint->GetWaypointTime()) < 0)
+ {
+ NS_LOG_DEBUG(this << ": The following consecutive waypoints's time difference is less than zero!");
+ NS_LOG_DEBUG("Previous waypoint:" << std::endl << *lastPoint << std::endl << "Current waypoint: " << *(*k));
+ }
+ waypointTime = 0;
+ }
+ else
+ {
+ //If it's the first point in the list, we need to check if it's at the start of its steplist
+ if (currentStepPosition != stepListBegin)
+ {
+ //If it's not, we can iterate back a step and check the time of the last point of the previous step.
+ tempStepListIterator = currentStepPosition;
+ //Iterate back a step
+ tempStepListIterator--;
+ //Get last point in that step
+ tmpPointList = (*tempStepListIterator)->GetPointList ();
+ unsigned int size = tmpPointList.size();
+ if(size==1)
+ {
+ lastPoint = tmpPointList.front ();
+ }
+ else
+ {
+ lastPoint = (tmpPointList.back ());
+ }
+ waypointTime = SetWaypointTime ((lastPoint)->GetWaypointTime (), totalDistanceInStep, (lastPoint), pTemp, travelTime);
+ if(waypointTime - (lastPoint->GetWaypointTime()) < 0)
+ {
+ NS_LOG_DEBUG(this << ": The following consecutive waypoints's time difference is less than zero!");
+ NS_LOG_DEBUG("Previous waypoint:" << std::endl << *lastPoint << std::endl << "Current waypoint: " << *(*k));
+ }
+ waypointTime = 0;
+ }
+ else
+ {
+ //If we are at the start of the steplist we need to iterate back a leg and check its last step and the last point in that step
+ if (currentLegPosition != legListBegin)
+ {
+ tempLegIterator = currentLegPosition;
+ //Iterate back a leg
+ tempLegIterator--;
+ //Get the steplist in that leg
+ (*tempLegIterator)->GetStepList (tempStepList);
+ //Get the last point of the last step of the previous leg.
+ lastPoint = ((*tempStepList.back ()).GetPointList ()).back ();
+ waypointTime = SetWaypointTime ((lastPoint)->GetWaypointTime (), totalDistanceInStep, (lastPoint), pTemp, travelTime);
+ if(waypointTime - (lastPoint->GetWaypointTime()) < 0)
+ {
+ NS_LOG_DEBUG(this << ": The following consecutive waypoints's time difference is less than zero!");
+ NS_LOG_DEBUG("Previous waypoint:" << std::endl << *lastPoint << std::endl << "Current waypoint: " << *(*k));
+ }
+ waypointTime = 0;
+ }
+ }
+ }
+ }
+ }
+}
+} //namespace ns3
+

Powered by Google App Engine
RSS Feeds Recent Issues | This issue
This is Rietveld f62528b