changeset 225:c4009187a522 main

Checkpoint
author Bob Cook <bob@bobcookdev.com>
date Sat, 28 Jun 2014 16:12:28 -0700
parents 5e91f5877f1f
children 0639ad84827b
files main/robots/odr/ODRApp.cpp main/robots/odr/Scoreboard.cpp main/robots/odr/SquareCourseTask.cpp main/robots/odr/StartupTask.cpp main/robots/odr/TrackWaypointsTask.cpp main/robots/odr/TrackWaypointsTask.h main/robots/odr/jamfile
diffstat 7 files changed, 242 insertions(+), 15 deletions(-) [+]
line wrap: on
line diff
--- a/main/robots/odr/ODRApp.cpp	Thu Apr 17 22:03:28 2014 -0700
+++ b/main/robots/odr/ODRApp.cpp	Sat Jun 28 16:12:28 2014 -0700
@@ -56,6 +56,7 @@
 #include "SquareCourseTask.h"
 #include "StartupTask.h"
 #include "TaskObject.h"
+#include "TrackWaypointsTask.h"
 
 #include "packages/common/can/can_helpers.h"
 
@@ -173,6 +174,9 @@
             Poco::SharedPtr< TaskObject >( new AvoidBySonarTask( logger().name() ) ) );
 
     m_tasks.push_back(
+            Poco::SharedPtr< TaskObject >( new TrackWaypointsTask( logger().name() ) ) );
+
+    m_tasks.push_back(
             Poco::SharedPtr< TaskObject >( new SquareCourseTask( logger().name() ) ) );
 
     m_tasks.push_back(
--- a/main/robots/odr/Scoreboard.cpp	Thu Apr 17 22:03:28 2014 -0700
+++ b/main/robots/odr/Scoreboard.cpp	Sat Jun 28 16:12:28 2014 -0700
@@ -7,7 +7,7 @@
 // 
 //  Global thread-safe state derived from incoming CAN messages and other internal data.
 //
-//  Copyright (c) 2010-2013 Bob Cook
+//  Copyright (c) 2010-2014 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
@@ -89,7 +89,8 @@
 bool Scoreboard::isControllerAlive()
 {
     Poco::RWLock::ScopedReadLock lock( sm_rwLock );
-    return sm_isControllerAlive;
+    return true; // fake it
+    // return sm_isControllerAlive;
 }
 
 // ----------------------------------------------------------------------------------------
@@ -394,8 +395,8 @@
             log.information(
                 Poco::Logger::format(
                     "Scoreboard: GPS latitude: $0 longitude: $1",
-                    Poco::NumberFormatter::format( sm_gpsLatitudeAvg ),
-                    Poco::NumberFormatter::format( sm_gpsLongitudeAvg ) ) );
+                    Poco::NumberFormatter::format( sm_gpsLatitudeAvg, 5 ),
+                    Poco::NumberFormatter::format( sm_gpsLongitudeAvg, 5 ) ) );
         }
         else
         {
@@ -414,6 +415,13 @@
 {
     Poco::Logger& log = Poco::Logger::get( m_loggerName );
 
+    // send a heartbeat as if we are odr-controller (which is deprecated)
+
+    uint32_t hbeatmsgid = can_build_message_id(
+                    can_node_odr_manager, can_node_broadcast, can_dataid_heartbeat );
+
+    CANMessage::QueueToSend( new CANMessage( hbeatmsgid ) );
+
     // send the manager status message
 
     uint32_t updatemsgid = can_build_message_id(
@@ -678,7 +686,7 @@
     double lat = static_cast< double >( info.degrees );
     double min = static_cast< double >( info.min_thousandths )
                / static_cast< double >( can_data_gps_min_multiplier );
-    lat += ( min * 60.0L );
+    lat += ( min / 60.0L );
     
     Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
 
@@ -709,7 +717,7 @@
     double lon = static_cast< double >( info.degrees );
     double min = static_cast< double >( info.min_thousandths )
                / static_cast< double >( can_data_gps_min_multiplier );
-    lon += ( min * 60.0L );
+    lon += ( min / 60.0L );
     
     Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
 
--- a/main/robots/odr/SquareCourseTask.cpp	Thu Apr 17 22:03:28 2014 -0700
+++ b/main/robots/odr/SquareCourseTask.cpp	Sat Jun 28 16:12:28 2014 -0700
@@ -63,13 +63,6 @@
 
 void SquareCourseTask::update()
 {
-    // nothing to do here
-}
-
-// ----------------------------------------------------------------------------------------
-
-bool SquareCourseTask::wantsControl()
-{
     if ( m_nextSwitchTime.isElapsed( sRunTimeUntilDirectionChange ) )
     {
         m_nextSwitchTime.update();
@@ -84,8 +77,13 @@
 
         Scoreboard::navSetTargetHeading( m_heading );
     }
+}
 
-    return false;
+// ----------------------------------------------------------------------------------------
+
+bool SquareCourseTask::wantsControl()
+{
+    return false; // nothing to do here
 }
 
 // ----------------------------------------------------------------------------------------
--- a/main/robots/odr/StartupTask.cpp	Thu Apr 17 22:03:28 2014 -0700
+++ b/main/robots/odr/StartupTask.cpp	Sat Jun 28 16:12:28 2014 -0700
@@ -137,6 +137,7 @@
 {
     if ( ! m_lastControllerStatus )
     {
+        Scoreboard::sendManagerMessage( "Waiting" );
         return; // nothing can be done until the odr-controller is up
     }
 
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr/TrackWaypointsTask.cpp	Sat Jun 28 16:12:28 2014 -0700
@@ -0,0 +1,140 @@
+// ----------------------------------------------------------------------------------------
+//
+//  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 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();
+
+        // 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;
+}
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr/TrackWaypointsTask.h	Sat Jun 28 16:12:28 2014 -0700
@@ -0,0 +1,76 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr/TrackWaypointsTask.h
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+//
+//  Subsumption task to navigate in a square shape.
+//
+//  Copyright (c) 2013 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.
+//
+// ----------------------------------------------------------------------------------------
+
+#ifndef BCDRL_ROBOTS_ODR_TRACKWAYPOINTSTASK_H
+#define BCDRL_ROBOTS_ODR_TRACKWAYPOINTSTASK_H
+
+#include "TaskObject.h"
+
+#include <vector>
+
+// ----------------------------------------------------------------------------------------
+
+typedef struct
+{
+    double latitude;
+    double longitude;
+   
+} WaypointType;
+
+typedef std::vector< WaypointType > WaypointVectorType;
+
+// ----------------------------------------------------------------------------------------
+
+class TrackWaypointsTask : public TaskObject
+{
+    public:
+        TrackWaypointsTask( const std::string& loggerName );
+        virtual ~TrackWaypointsTask();
+        virtual void update();
+        virtual bool wantsControl();
+        virtual void takeControl();
+
+    private:
+        void refreshWaypointTrack();
+
+    private:
+        WaypointVectorType m_waypoints;
+        Poco::Timestamp    m_nextUpdateTime;
+        double             m_waypointLatitude;
+        double             m_waypointLongitude;
+        double             m_headingToWaypoint;
+        double             m_distanceToWaypoint;
+};
+
+// ----------------------------------------------------------------------------------------
+#endif // #ifndef BCDRL_ROBOTS_ODR_TRACKWAYPOINTSTASK_H
+// ----------------------------------------------------------------------------------------
+
--- a/main/robots/odr/jamfile	Thu Apr 17 22:03:28 2014 -0700
+++ b/main/robots/odr/jamfile	Sat Jun 28 16:12:28 2014 -0700
@@ -40,7 +40,7 @@
     ImuReader.cpp MotorsAndServos.cpp Scoreboard.cpp Sonar.cpp
     TaskObject.cpp
     AvoidBySonarTask.cpp CruisingTask.cpp NavigateTask.cpp SafetyTask.cpp StartupTask.cpp
-    SquareCourseTask.cpp
+    SquareCourseTask.cpp TrackWaypointsTask.cpp
     packages.common.can.pkg
     packages.linux.can.pkg
     ;