view main/robots/odr/TrackWaypointsTask.cpp @ 259:7124845bf907 main

Compute distance in meters rather than km.
author Bob Cook <bob@bobcookdev.com>
date Sat, 26 Sep 2015 12:39:42 -0700
parents c4009187a522
children
line wrap: on
line source

// ----------------------------------------------------------------------------------------
//
//  robots/odr/TrackWaypointsTask.cpp
//    
//  Bob Cook Development, Robotics Library
//  http://www.bobcookdev.com/rl/
//
//  Subsumption task to navigate to a list of successive GPS locations (waypoints).
//
//  Copyright (c) 2013-2015 Bob Cook
//
//  Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
//
//  The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
//
//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//
//  The algorithm to compute heading (bearing) and distance between GPS coordinates was
//  found at this website: http://www.movable-type.co.uk/scripts/latlong.html
//
// ----------------------------------------------------------------------------------------

#include "TrackWaypointsTask.h"

#include <algorithm>
#include <cmath>

#include <Poco/Logger.h>
#include <Poco/NumberFormatter.h>

#include "Scoreboard.h"

// ----------------------------------------------------------------------------------------

static const Poco::Timestamp::TimeDiff sRunTimeUntilWaypointCheck
                                                      = Poco::Timestamp::resolution() * 5;

// ----------------------------------------------------------------------------------------

TrackWaypointsTask::TrackWaypointsTask( const std::string& loggerName )
    : TaskObject( loggerName ),
      m_waypoints(),
      m_nextUpdateTime()
{
}

// ----------------------------------------------------------------------------------------

TrackWaypointsTask::~TrackWaypointsTask()
{
}

// ----------------------------------------------------------------------------------------

void TrackWaypointsTask::update()
{
    if ( m_nextUpdateTime.isElapsed( sRunTimeUntilWaypointCheck ) )
    {
        m_nextUpdateTime.update();

        refreshWaypointTrack();

        // decide if we have reached the waypoint
    }
}

// ----------------------------------------------------------------------------------------

bool TrackWaypointsTask::wantsControl()
{
    return false; // nothing to do here
}

// ----------------------------------------------------------------------------------------

void TrackWaypointsTask::takeControl()
{
    // nothing to do here
}

// ----------------------------------------------------------------------------------------

static double deg_to_rad( double degrees )
{
    return degrees * M_PI / 180.0;
}

// ----------------------------------------------------------------------------------------

static double rad_to_deg( double radians )
{
    return radians * 180.0 / M_PI;
}

// ----------------------------------------------------------------------------------------

void TrackWaypointsTask::refreshWaypointTrack()
{
    // compute the desired heading to the waypoint from the current location

    double currLat = Scoreboard::gpsLatitude();
    double currLon = Scoreboard::gpsLongitude();

    double deltaLonRads  = ::deg_to_rad( m_waypointLongitude - currLon );
    double currLatRads   = ::deg_to_rad( currLat );
    double targetLatRads = ::deg_to_rad( m_waypointLatitude );

    double y = ::sin( deltaLonRads ) * ::cos( targetLatRads );
    double x = ::cos( currLatRads ) * ::sin( targetLatRads ) -
               ::sin( currLatRads ) * ::cos( targetLatRads ) * ::cos( deltaLonRads );

    m_headingToWaypoint = ::rad_to_deg( ::atan2( y, x ) );

    // compute the distance to the desired waypoint

    static const double R = 6371.0; // km

    double deltaLatRads = ::deg_to_rad( m_waypointLatitude - currLat );

    double a = ::sin( deltaLatRads / 2.0 ) * ::sin( deltaLatRads / 2.0 ) +
               ::sin( deltaLonRads / 2.0 ) * ::sin( deltaLonRads / 2.0 ) *
               ::cos( currLatRads ) * ::cos( targetLatRads );

    double c = 2.0 * ::atan2( ::sqrt( a ), ::sqrt( 1.0 - a ) );

    m_distanceToWaypoint = R * c * 1000; // meters
}

// ----------------------------------------------------------------------------------------