changeset 133:98172ca268ae main

Pull latest changes.
author Bob Cook <bob@bobcookdev.com>
date Tue, 27 Dec 2011 12:38:44 -0800
parents 0ea0246d4a70 (current diff) 97148838cfce (diff)
children 4da44007d8cb caee904383e5
files
diffstat 25 files changed, 1192 insertions(+), 116 deletions(-) [+]
line wrap: on
line diff
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/.hgignore	Tue Dec 27 12:38:44 2011 -0800
@@ -0,0 +1,2 @@
+/gen/
+/bin/
--- a/main/packages/linux/can/CANMessage.cpp	Tue Dec 27 12:38:18 2011 -0800
+++ b/main/packages/linux/can/CANMessage.cpp	Tue Dec 27 12:38:44 2011 -0800
@@ -44,6 +44,13 @@
 
 // ----------------------------------------------------------------------------------------
 
+bool CANMessage::IsSendQueueEmpty()
+{
+    return sm_MsgsToSend.empty();
+}
+
+// ----------------------------------------------------------------------------------------
+
 void CANMessage::QueueToSend( CANMessage* msg )
 {
     sm_MsgsToSend.enqueueNotification( msg );
@@ -53,7 +60,7 @@
 
 CANMessage* CANMessage::WaitDequeueToSend( long timeoutMilliseconds )
 {
-    return reinterpret_cast<CANMessage*>(
+    return reinterpret_cast< CANMessage* >(
             sm_MsgsToSend.waitDequeueNotification( timeoutMilliseconds ) );
 }
 
@@ -68,7 +75,7 @@
 
 CANMessage* CANMessage::WaitDequeueReceived( long timeoutMilliseconds )
 {
-    return reinterpret_cast<CANMessage*>(
+    return reinterpret_cast< CANMessage* >(
             sm_MsgsReceived.waitDequeueNotification( timeoutMilliseconds ) );
 }
 
@@ -125,6 +132,15 @@
 
 // ----------------------------------------------------------------------------------------
 
+CANMessage::CANMessage( uint32_t message_id )
+    : Poco::Notification(),
+      m_id( message_id ),
+      m_length( 0 )
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
 CANMessage::CANMessage( uint32_t message_id, const uint8_t* data, uint8_t length )
     : Poco::Notification(),
       m_id( message_id ),
--- a/main/packages/linux/can/CANMessage.h	Tue Dec 27 12:38:18 2011 -0800
+++ b/main/packages/linux/can/CANMessage.h	Tue Dec 27 12:38:44 2011 -0800
@@ -7,7 +7,7 @@
 //    
 //  CAN message object that can be queued or dequeued from multiple threads.
 //
-//  Copyright (c) 2010 Bob Cook
+//  Copyright (c) 2011 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
@@ -44,6 +44,7 @@
 class CANMessage : public Poco::Notification
 {
     public:
+        static bool IsSendQueueEmpty();
         static void QueueToSend( CANMessage* msg );
         static CANMessage* WaitDequeueToSend( long timeoutMilliseconds );
         static void QueueReceived( CANMessage* msg );
@@ -51,6 +52,7 @@
         static std::string asText( const CANMessage* msg );
 
     public:
+        CANMessage( uint32_t message_id );
         CANMessage( uint32_t message_id, const uint8_t* data, uint8_t length );
         CANMessage* clone() const;
         uint32_t msgIdentifier() const;
--- a/main/packages/linux/can/CANMsgProcessor.cpp	Tue Dec 27 12:38:18 2011 -0800
+++ b/main/packages/linux/can/CANMsgProcessor.cpp	Tue Dec 27 12:38:44 2011 -0800
@@ -45,6 +45,18 @@
 
 // ----------------------------------------------------------------------------------------
 
+CANMsgProcessor::CANMsgProcessor( const std::string& canInterface )
+    : Poco::Runnable(),
+      m_canInterfaceName( canInterface ),
+      m_loggerName(),
+      m_quitEvent( false /* not auto-reset */ ),
+      m_logIncoming( false ),
+      m_logOutgoing( false )
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
 CANMsgProcessor::CANMsgProcessor
 (
     const std::string& canInterface,
@@ -53,9 +65,9 @@
     : Poco::Runnable(),
       m_canInterfaceName( canInterface ),
       m_loggerName( logger ),
+      m_quitEvent( false /* not auto-reset */ ),
       m_logIncoming( false ),
-      m_logOutgoing( false ),
-      m_quitNow( false )
+      m_logOutgoing( false )
 {
 }
 
@@ -83,7 +95,7 @@
 
 void CANMsgProcessor::timeToQuit()
 {
-    m_quitNow = true;
+    m_quitEvent.set();
 }
 
 // ----------------------------------------------------------------------------------------
@@ -94,9 +106,6 @@
 
     for ( ;; )
     {
-        log.information( std::string( "CANMsgProcessor::run() starting for " )
-                + m_canInterfaceName );
-
         try
         {
             CANSocket sock;
@@ -127,7 +136,7 @@
 
                 Poco::AutoPtr<CANMessage> msg( CANMessage::WaitDequeueToSend( 100 ) ); // 100ms
 
-                if ( msg.get() != 0 && ! m_quitNow )
+                if ( msg.get() != 0 )
                 {
                     if ( sock.poll( writeWaitTime, Poco::Net::Socket::SELECT_WRITE ) )
                     {
@@ -155,9 +164,8 @@
 
                 // time to quit?
 
-                if ( m_quitNow )
+                if ( m_quitEvent.tryWait( 0 ) && CANMessage::IsSendQueueEmpty() )
                 {
-                    log.information(std::string( "CANMsgProcessor::run() stopping" ) );
                     return;
                 }
             }
--- a/main/packages/linux/can/CANMsgProcessor.h	Tue Dec 27 12:38:18 2011 -0800
+++ b/main/packages/linux/can/CANMsgProcessor.h	Tue Dec 27 12:38:44 2011 -0800
@@ -36,6 +36,7 @@
 
 #include <string>
 
+#include <Poco/Event.h>
 #include <Poco/Runnable.h>
 
 // ----------------------------------------------------------------------------------------
@@ -43,6 +44,7 @@
 class CANMsgProcessor : public Poco::Runnable
 {
     public:
+        CANMsgProcessor( const std::string& canInterface );
         CANMsgProcessor( const std::string& canInterface, const std::string& logger );
         virtual ~CANMsgProcessor();
         virtual void run();
@@ -53,9 +55,9 @@
     private:
         std::string m_canInterfaceName;
         std::string m_loggerName;
+        Poco::Event m_quitEvent;
         bool        m_logIncoming;
         bool        m_logOutgoing;
-        bool        m_quitNow;
 };
 
 // ----------------------------------------------------------------------------------------
--- a/main/packages/linux/can/CANSocket.cpp	Tue Dec 27 12:38:18 2011 -0800
+++ b/main/packages/linux/can/CANSocket.cpp	Tue Dec 27 12:38:44 2011 -0800
@@ -109,30 +109,16 @@
 CANMessage* CANSocket::read()
 {
     struct can_frame recvFrame;
-    size_t recvBytes = impl()->receiveBytes(&recvFrame, sizeof(recvFrame));
+    size_t recvBytes = impl()->receiveBytes( &recvFrame, sizeof( recvFrame ) );
 
     if ( recvBytes < sizeof( recvFrame ) )
     {
         throw Poco::Exception( "incomplete CAN frame received" );
     }
 
-    // our mcp2515 code reverses the bit order of sid+eid so reverse it again here
-    // (hopefully that will be fixed at some point)
-
-    uint32_t translatedId;
-
-    if ( recvFrame.can_id & CAN_EFF_FLAG )
-    {
-        recvFrame.can_id &= CAN_EFF_MASK;
-        translatedId  = recvFrame.can_id >> 18;
-        translatedId |= recvFrame.can_id << 11;
-    }
-    else
-    {
-        translatedId = recvFrame.can_id & CAN_SFF_MASK;
-    }
-
-    return new CANMessage( translatedId, recvFrame.data, recvFrame.can_dlc );
+    return new CANMessage( recvFrame.can_id & CAN_EFF_MASK,
+                           recvFrame.data, 
+                           recvFrame.can_dlc );
 }
 
 // ----------------------------------------------------------------------------------------
@@ -140,16 +126,9 @@
 void CANSocket::write( const CANMessage& msg )
 {
     struct can_frame sendFrame;
+    sendFrame.can_id  = msg.msgIdentifier() | CAN_EFF_FLAG;
     sendFrame.can_dlc = msg.msgData( sendFrame.data, sizeof( sendFrame.data ) );
 
-    // reverse sid+eid because that is what our mcp2515 code expects
-    // (hopefully that will be fixed at some point)
-
-    uint32_t translatedId = msg.msgIdentifier() >> 11;
-    translatedId |= ( msg.msgIdentifier() & 0x7ff ) << 18;
-
-    sendFrame.can_id = translatedId | CAN_EFF_FLAG;
-
     size_t sentBytes = impl()->sendBytes( &sendFrame, sizeof( sendFrame ) );
 
     if (sentBytes < sizeof( sendFrame ) )
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/cantool/CanToolApp.cpp	Tue Dec 27 12:38:44 2011 -0800
@@ -0,0 +1,182 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/cantool/CanToolApp.cpp
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+//
+//  Application object implementation for demonstrating the CANSocket class.    
+//
+//  Copyright (c) 2011 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.
+//
+// ----------------------------------------------------------------------------------------
+
+#include <iostream>
+#include <memory>
+#include <stdio.h>
+#include <string>
+#include <vector>
+
+#include <Poco/AutoPtr.h>
+#include <Poco/Thread.h>
+#include <Poco/Util/Application.h>
+#include <Poco/Util/HelpFormatter.h>
+#include <Poco/Util/Option.h>
+#include <Poco/Util/OptionSet.h>
+
+#include "CanToolApp.h"
+#include "Commands.h"
+
+#include "packages/common/can/can_helpers.h"
+
+#include "packages/linux/can/CANMessage.h"
+#include "packages/linux/can/CANMsgProcessor.h"
+
+// ----------------------------------------------------------------------------------------
+
+void CommandListen( const std::vector< std::string >& args )
+{
+    std::cout << "listening:" << std::endl;
+
+    for ( ;; )
+    {
+        CANMessage* msg = CANMessage::WaitDequeueReceived( 250 ); // 250 ms
+
+        if ( msg == 0 )
+        {
+            continue;
+        }
+
+        std::cout << "received: " << CANMessage::asText( msg ) << std::endl;
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
+CanToolApp::CanToolApp()
+    : m_helpRequested( false ),
+      m_canInterfaceName( "can0" ),
+      m_commandDispatchTable()
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
+CanToolApp::~CanToolApp()
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
+void CanToolApp::initialize( Poco::Util::Application& self )
+{
+    loadConfiguration();
+    Poco::Util::Application::initialize( self );
+
+    m_commandDispatchTable[ "listen" ] = CommandListen;
+    m_commandDispatchTable[ "sonarfront" ] = CommandSonarFront;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void CanToolApp::uninitialize()
+{
+    Poco::Util::Application::uninitialize();
+}
+
+// ----------------------------------------------------------------------------------------
+
+void CanToolApp::defineOptions( Poco::Util::OptionSet& options )
+{
+    Poco::Util::Application::defineOptions( options );
+
+    options.addOption(
+        Poco::Util::Option( "help", "h", "display argument help information" )
+            .required( false )
+            .repeatable( false )
+            .callback( Poco::Util::OptionCallback< CanToolApp >(
+                                                    this, &CanToolApp::handleHelp ) ) );
+
+    options.addOption(
+        Poco::Util::Option( "interface", "i", "specify the CAN interface" )
+            .required( false )
+            .repeatable( false )
+            .callback( Poco::Util::OptionCallback< CanToolApp >(
+                                               this, &CanToolApp::setCanInterface ) ) );
+}
+
+// ----------------------------------------------------------------------------------------
+
+void CanToolApp::handleHelp( const std::string& name, const std::string& value )
+{
+    Poco::Util::HelpFormatter helpFormatter( options() );
+    helpFormatter.setCommand( commandName() );
+    helpFormatter.format( std::cout );
+    stopOptionsProcessing();
+    m_helpRequested = true;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void CanToolApp::setCanInterface( const std::string& name, const std::string& value )
+{
+    m_canInterfaceName = value;
+}
+
+// ----------------------------------------------------------------------------------------
+
+int CanToolApp::main( const std::vector< std::string >& args )
+{
+    if ( m_helpRequested )
+    {
+        return Poco::Util::Application::EXIT_OK;
+    }
+
+    loadConfiguration();
+
+    Poco::Thread    canMsgProcessorThread;
+    CANMsgProcessor canMsgProcessor( m_canInterfaceName );
+    canMsgProcessorThread.start( canMsgProcessor );
+
+    if ( ! args.empty() )
+    {
+        if ( m_commandDispatchTable.find( args[ 0 ] ) != m_commandDispatchTable.end() )
+        {
+            ( m_commandDispatchTable[ args[ 0 ] ] )( args );
+        }
+        else
+        {
+            std::cerr << "command \"" << args[ 0 ] << "\" not recognized" << std::endl;
+        }
+    }
+    else
+    {
+        std::cerr << "error: no command given" << std::endl;
+    }
+
+    canMsgProcessor.timeToQuit();
+    canMsgProcessorThread.join();
+
+    return Poco::Util::Application::EXIT_OK;
+}
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/cantool/CanToolApp.h	Tue Dec 27 12:38:44 2011 -0800
@@ -0,0 +1,79 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr/CanToolApp.h
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+//    
+//  Application object for the OutDoor Robot
+//
+//  Copyright (c) 2010 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_CANTOOL_CANTOOLAPP_H
+#define BCDRL_ROBOTS_CANTOOL_CANTOOLAPP_H
+
+#include <Poco/Util/Application.h>
+
+namespace Poco
+{
+    namespace Util
+    {
+        class OptionSet;
+    }
+}
+
+#include <map>
+#include <string>
+#include <vector>
+
+// ----------------------------------------------------------------------------------------
+
+class CanToolApp : public Poco::Util::Application
+{
+    typedef void (*commandFuncType)( const std::vector< std::string >& args );
+    typedef std::map< std::string, commandFuncType > commandMapType;
+
+    public:
+        CanToolApp();
+        virtual ~CanToolApp();
+
+    protected:
+        void initialize( Poco::Util::Application& self );
+        void uninitialize();
+        void defineOptions( Poco::Util::OptionSet& options );
+        void handleHelp( const std::string& name, const std::string& value );
+        int  main( const std::vector< std::string >& args );
+
+    private:
+        void setCanInterface( const std::string& name, const std::string& value );
+
+    private:
+        bool           m_helpRequested;
+        std::string    m_canInterfaceName;
+        commandMapType m_commandDispatchTable;
+};
+
+// ----------------------------------------------------------------------------------------
+#endif // #ifndef BCDRL_ROBOTS_CANTOOL_CANTOOLAPP_H
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/cantool/CmdSonarFront.cpp	Tue Dec 27 12:38:44 2011 -0800
@@ -0,0 +1,82 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/cantool/CmdSonarFront.cpp
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+//
+//  Command "sonarfront" implementation.
+//
+//  Copyright (c) 2011 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.
+//
+// ----------------------------------------------------------------------------------------
+
+#include <iostream>
+
+#include "Commands.h"
+
+#include "packages/common/can/can_helpers.h"
+#include "packages/common/can/can_messages.h"
+#include "packages/common/can/can_nodes.h"
+
+#include "packages/linux/can/CANMessage.h"
+#include "packages/linux/can/CANMsgProcessor.h"
+
+// ----------------------------------------------------------------------------------------
+
+void CommandSonarFront( const std::vector< std::string >& args )
+{
+    if ( args.size() < 2 )
+    {
+        std::cout << "error: sonarfront <on|off>" << std::endl;
+        return;
+    }
+
+    if ( args[ 1 ] != "on" && args[ 1 ] != "off" )
+    {
+    }
+
+    uint32_t msgid;
+
+    if ( args[ 1 ] == "on" )
+    {
+        msgid = can_build_message_id( can_node_odr_manager,
+                                      can_node_odr_sonar_front,
+                                      can_dataid_sonar_front_enable );
+    }
+    else if ( args[ 1 ] == "off" )
+    {
+        msgid = can_build_message_id( can_node_odr_manager,
+                                      can_node_odr_sonar_front,
+                                      can_dataid_sonar_front_disable );
+    }
+    else
+    {
+        std::cout << "error: sonarfront <on|off>" << std::endl;
+        return;
+    }
+
+    CANMessage::QueueToSend( new CANMessage( msgid ) );
+    std::cout << "sent " << args[ 1 ] << std::endl;
+}
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/cantool/Commands.h	Tue Dec 27 12:38:44 2011 -0800
@@ -0,0 +1,42 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/cantool/Commands.h
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+//
+//  Command function prototypes.
+//
+//  Copyright (c) 2011 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.
+//
+// ----------------------------------------------------------------------------------------
+
+#include <string>
+#include <vector>
+
+// ----------------------------------------------------------------------------------------
+
+void CommandListen( const std::vector< std::string >& args );
+
+void CommandSonarFront( const std::vector< std::string >& args );
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/cantool/jamfile	Tue Dec 27 12:38:44 2011 -0800
@@ -0,0 +1,54 @@
+# -----------------------------------------------------------------------------------------
+#
+#   robots/cantool/jamfile
+#
+#   Bob Cook Development, Robotics Library
+#   http://www.bobcookdev.com/rl/
+#    
+#   Copyright (c) 2011 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.
+#
+# -----------------------------------------------------------------------------------------
+
+
+if $(TRACE) { echo "trace /robots/cantool/jamfile" ; }
+
+SubDir TOP robots cantool ;
+
+# -----------------------------------------------------------------------------------------
+
+COMMON_SOURCES = 
+    main.cpp
+    CanToolApp.cpp
+    CmdSonarFront.cpp
+    packages.common.can.pkg
+    packages.linux.can.pkg
+    ;
+
+POCO_SHARED = PocoFoundation.so PocoNet.so PocoUtil.so PocoXML.so ;
+POCO_STATIC = PocoFoundation.a  PocoNet.a  PocoUtil.a  PocoXML.a ;
+
+# -----------------------------------------------------------------------------------------
+
+ubuntu_executable cantool_ubuntu : $(COMMON_SOURCES) $(POCO_SHARED) ;
+overo_executable  cantool        : $(COMMON_SOURCES) $(POCO_STATIC) ;
+
+# -----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/cantool/main.cpp	Tue Dec 27 12:38:44 2011 -0800
@@ -0,0 +1,68 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/cantool/main.cpp
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+//    
+//  Main entry point for the CanTool software.
+//
+//  Copyright (c) 2011 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.
+//
+// ----------------------------------------------------------------------------------------
+
+#include "CanToolApp.h"
+
+#include <iostream>
+#include <stdexcept>
+
+// ----------------------------------------------------------------------------------------
+
+int main( int argc, char** argv )
+{
+    int result = -1;
+
+    try
+    {
+        CanToolApp app;
+        app.init( argc, argv );
+        result = app.run();
+    }
+    catch ( const Poco::Exception& ex )
+    {
+        std::cerr << "Failed to start application, Poco::Exception: "
+            << ex.displayText() << std::endl;
+    }
+    catch ( const std::exception& ex )
+    {
+        std::cerr << "Failed to start application, std::exception: "
+            << ex.what() << std::endl;
+    }
+    catch ( ... )
+    {
+        std::cerr << "Failed to start application, unknown exception" << std::endl;
+    }
+
+    return result;
+}
+
+// ----------------------------------------------------------------------------------------
+
--- a/main/robots/odr-sim/ControllerStatus.cpp	Tue Dec 27 12:38:18 2011 -0800
+++ b/main/robots/odr-sim/ControllerStatus.cpp	Tue Dec 27 12:38:44 2011 -0800
@@ -156,7 +156,7 @@
             // nothing to do, but don't stop the thread
         }
 
-        if ( m_quitEvent.tryWait( 1000 ) ) // 1000 ms == 1s
+        if ( m_quitEvent.tryWait( 2500 ) ) // 2500 ms == 2.5s
         {
             return;
         }
--- a/main/robots/odr-sim/Receiver.cpp	Tue Dec 27 12:38:18 2011 -0800
+++ b/main/robots/odr-sim/Receiver.cpp	Tue Dec 27 12:38:44 2011 -0800
@@ -114,6 +114,16 @@
 
 // ----------------------------------------------------------------------------------------
 
+void Receiver::recvSonarFrontStateChange( bool enabled )
+{
+    if ( m_display )
+    {
+        m_display->updateSonarFrontState( enabled );
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
 void Receiver::run()
 {
     for ( ;; )
@@ -152,6 +162,14 @@
                 case can_dataid_servo_position:
                     recvServoPosition( msg );
                     break;
+
+                case can_dataid_sonar_front_enable:
+                    recvSonarFrontStateChange( true );
+                    break;
+
+                case can_dataid_sonar_front_disable:
+                    recvSonarFrontStateChange( false );
+                    break;
             }
         }
         catch ( ... )
--- a/main/robots/odr-sim/Receiver.h	Tue Dec 27 12:38:18 2011 -0800
+++ b/main/robots/odr-sim/Receiver.h	Tue Dec 27 12:38:44 2011 -0800
@@ -63,6 +63,7 @@
         void recvMgrUpdate( CANMessage* msg );
         void recvServoPosition( CANMessage* msg );
         void recvMotorSpeed( CANMessage* msg );
+        void recvSonarFrontStateChange( bool enabled );
 };
 
 // ----------------------------------------------------------------------------------------
--- a/main/robots/odr-sim/SimDisplay.cpp	Tue Dec 27 12:38:18 2011 -0800
+++ b/main/robots/odr-sim/SimDisplay.cpp	Tue Dec 27 12:38:44 2011 -0800
@@ -47,6 +47,7 @@
 #include <Poco/Thread.h>
 
 #include "ControllerStatus.h"
+#include "SonarFrontStatus.h"
 
 // ----------------------------------------------------------------------------------------
 
@@ -128,6 +129,32 @@
 
 // ----------------------------------------------------------------------------------------
 
+static void SonarFrontKeepAliveCB( Fl_Widget* widget, void* value )
+{
+    static SonarFrontStatus s_sfsRunnable;
+    static Poco::Thread     s_sfsThread;
+
+    Fl_Light_Button* button = dynamic_cast< Fl_Light_Button* >( widget );
+    if ( button == 0 )
+    {
+        return; // opps, not what we thought?!
+    }
+
+    if ( button->value() == 0 )
+    {
+        // off
+        s_sfsRunnable.timeToQuit();
+        s_sfsThread.join();
+    }
+    else
+    {
+        // on
+        s_sfsThread.start( s_sfsRunnable );
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
 SimDisplay::SimDisplay()
     : m_window( 0 ),
       m_dialMotorSpeedFront( 0 ),
@@ -140,11 +167,13 @@
       m_textServoPosRear( 0 ),
       m_boxMgrHeartbeat( 0 ),
       m_textManagerMsg( 0 ),
+      m_boxSonarFrontState( 0 ),
       m_buttonSendCtlUpdate( 0 ),
       m_buttonEstop( 0 ),
       m_buttonMotorCtl( 0 ),
       m_buttonA( 0 ),
       m_buttonB( 0 ),
+      m_buttonSonarFrontKA( 0 ),
       m_timerMainHbTimeout()
 {
 }
@@ -312,9 +341,18 @@
         m_textManagerMsg->label( "" );
     }
 
-    m_timerMainHbTimeout.setPeriodicInterval( 5000 );
+    m_timerMainHbTimeout.setPeriodicInterval( 6000 );
     Poco::TimerCallback< SimDisplay > cb( *this, &SimDisplay::timeoutMgrHeartbeat );
     m_timerMainHbTimeout.start( cb );
+
+    m_boxSonarFrontState = new Fl_Box( 10, 305, 165, 20 );
+    if ( m_boxSonarFrontState )
+    {
+        m_boxSonarFrontState->box( FL_THIN_DOWN_BOX );
+        m_boxSonarFrontState->align( FL_ALIGN_CENTER );
+        m_boxSonarFrontState->label( "Front Sonar Off" );
+        m_boxSonarFrontState->color( FL_BACKGROUND_COLOR );
+    }
 }
 
 // ----------------------------------------------------------------------------------------
@@ -365,6 +403,16 @@
         m_buttonB->callback( &PressButtonTwoCB );
     }
 
+    m_buttonSonarFrontKA = new Fl_Light_Button( 250, 140, 165, 20 );
+    if ( m_buttonSonarFrontKA )
+    {
+        m_buttonSonarFrontKA->label( "Sonar Front Alive" );
+        m_buttonSonarFrontKA->selection_color( FL_GREEN );
+        m_buttonSonarFrontKA->callback( &SonarFrontKeepAliveCB );
+        m_buttonSonarFrontKA->set();
+        m_buttonSonarFrontKA->do_callback();
+    }
+
     m_window->end();
 }
 
@@ -480,14 +528,7 @@
    
     if ( m_boxMgrHeartbeat )
     {
-        if ( m_boxMgrHeartbeat->color() == FL_GREEN )
-        {
-            m_boxMgrHeartbeat->color( FL_WHITE );
-        }
-        else
-        {
-            m_boxMgrHeartbeat->color( FL_GREEN );
-        }
+        m_boxMgrHeartbeat->color( FL_GREEN );
         m_boxMgrHeartbeat->redraw();
     }
 
@@ -550,3 +591,30 @@
 }
 
 // ----------------------------------------------------------------------------------------
+
+void SimDisplay::updateSonarFrontState( bool enabled )
+{
+    Fl::lock();
+
+    if ( m_boxSonarFrontState )
+    {
+        if ( enabled )
+        {
+            m_boxSonarFrontState->label( "Front Sonar On" );
+            m_boxSonarFrontState->color( FL_GREEN );
+        }
+        else
+        {
+            m_boxSonarFrontState->label( "Front Sonar Off" );
+            m_boxSonarFrontState->color( FL_BACKGROUND_COLOR );
+        }
+
+        m_boxSonarFrontState->redraw();
+    }
+
+    Fl::flush();
+
+    Fl::unlock();
+}
+
+// ----------------------------------------------------------------------------------------
--- a/main/robots/odr-sim/SimDisplay.h	Tue Dec 27 12:38:18 2011 -0800
+++ b/main/robots/odr-sim/SimDisplay.h	Tue Dec 27 12:38:44 2011 -0800
@@ -56,6 +56,7 @@
         void updateManagerMsg( const char* msg, int length );
         void doMgrHeartbeat();
         void resetMgrHeartbeat();
+        void updateSonarFrontState( bool enabled );
 
     private:
         Fl_Double_Window* m_window;
@@ -69,11 +70,13 @@
         Fl_Box*           m_textServoPosRear;
         Fl_Box*           m_boxMgrHeartbeat;
         Fl_Box*           m_textManagerMsg;
+        Fl_Box*           m_boxSonarFrontState;
         Fl_Light_Button*  m_buttonSendCtlUpdate;
         Fl_Light_Button*  m_buttonEstop;
         Fl_Light_Button*  m_buttonMotorCtl;
         Fl_Toggle_Button* m_buttonA;
         Fl_Toggle_Button* m_buttonB;
+        Fl_Light_Button*  m_buttonSonarFrontKA;
         Poco::Timer       m_timerMainHbTimeout;
 
     private:
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr-sim/SonarFrontStatus.cpp	Tue Dec 27 12:38:44 2011 -0800
@@ -0,0 +1,135 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr-sim/SonarFrontStatus.cpp
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  Thread that periodically sends the front sonar pod "heartbeat" message.
+//
+//  Copyright (c) 2011 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.
+//
+// ----------------------------------------------------------------------------------------
+
+#include "SonarFrontStatus.h"
+
+#include "packages/common/can/can_helpers.h"
+#include "packages/common/can/can_messages.h"
+#include "packages/common/can/can_nodes.h"
+
+#include "packages/linux/can/CANMessage.h"
+
+// ----------------------------------------------------------------------------------------
+
+Poco::RWLock SonarFrontStatus::sm_rwLock;
+bool         SonarFrontStatus::sm_isEnabled = false;
+
+// ----------------------------------------------------------------------------------------
+
+bool SonarFrontStatus::isSonarFrontEnabled()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_isEnabled;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void SonarFrontStatus::setSonarFrontEnabled()
+{
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+    sm_isEnabled = true;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void SonarFrontStatus::setSonarFrontDisabled()
+{
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+    sm_isEnabled = false;
+}
+
+// ----------------------------------------------------------------------------------------
+
+//void SonarFrontStatus::setSonarFrontLeft( uint16_t value )
+//{
+//}
+
+// ----------------------------------------------------------------------------------------
+
+SonarFrontStatus::SonarFrontStatus()
+    : Poco::Runnable(),
+      m_quitEvent( true /* auto-reset */ )
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
+SonarFrontStatus::~SonarFrontStatus()
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
+void SonarFrontStatus::timeToQuit()
+{
+    m_quitEvent.set();
+}
+
+// ----------------------------------------------------------------------------------------
+
+void SonarFrontStatus::run()
+{
+    for ( ;; )
+    {
+        try
+        {
+            uint32_t msgid;
+
+            if ( SonarFrontStatus::isSonarFrontEnabled() )
+            {
+                msgid = can_build_message_id( can_node_odr_sonar_front,
+                                              can_node_broadcast,
+                                              can_dataid_sonar_front_state_enabled );
+            }
+            else
+            {
+                msgid = can_build_message_id( can_node_odr_sonar_front,
+                                              can_node_broadcast,
+                                              can_dataid_sonar_front_state_disabled );
+            }
+
+            CANMessage::QueueToSend( new CANMessage( msgid ) );
+
+        }
+        catch ( ... )
+        {
+            // nothing to do, but don't stop the thread
+        }
+
+        if ( m_quitEvent.tryWait( 2500 ) ) // 2500 ms == 2.5s
+        {
+            return;
+        }
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr-sim/SonarFrontStatus.h	Tue Dec 27 12:38:44 2011 -0800
@@ -0,0 +1,69 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr-sim/SonarFrontStatus.h
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  Thread that periodically sends the front sonar pod "heartbeat" message.
+//
+//  Copyright (c) 2011 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_ODRSIM_SONARFRONTSTATUS_H
+#define BCDRL_ROBOTS_ODRSIM_SONARFRONTSTATUS_H
+
+#include <Poco/Event.h>
+#include <Poco/Runnable.h>
+#include <Poco/RWLock.h>
+
+// ----------------------------------------------------------------------------------------
+
+class SonarFrontStatus : public Poco::Runnable
+{
+    public:
+        static bool isSonarFrontEnabled();
+        static void setSonarFrontEnabled();
+        static void setSonarFrontDisabled();
+        //static void setSonarFrontLeft( uint16_t value );
+
+    public:
+        SonarFrontStatus();
+        virtual ~SonarFrontStatus();
+        virtual void run();
+        void timeToQuit();
+
+    private:
+        static void sendSonarFrontHeartbeat();
+
+    private:
+        static Poco::RWLock sm_rwLock;
+        static bool         sm_isEnabled;
+
+    private:
+        Poco::Event m_quitEvent;
+};
+
+// ----------------------------------------------------------------------------------------
+#endif // #ifndef BCDRL_ROBOTS_ODRSIM_SONARFRONTSTATUS_H
+// ----------------------------------------------------------------------------------------
+
--- a/main/robots/odr-sim/jamfile	Tue Dec 27 12:38:18 2011 -0800
+++ b/main/robots/odr-sim/jamfile	Tue Dec 27 12:38:44 2011 -0800
@@ -35,7 +35,11 @@
 
 ubuntu_executable odr-sim
     : main.cpp
-      ODRSimApp.cpp ControllerStatus.cpp Receiver.cpp SimDisplay.cpp
+      ODRSimApp.cpp
+      ControllerStatus.cpp 
+      Receiver.cpp 
+      SimDisplay.cpp 
+      SonarFrontStatus.cpp
       packages.common.can.pkg
       packages.linux.can.pkg
       PocoFoundation.so PocoNet.so PocoUtil.so PocoXML.so
--- a/main/robots/odr/MotorsAndServos.cpp	Tue Dec 27 12:38:18 2011 -0800
+++ b/main/robots/odr/MotorsAndServos.cpp	Tue Dec 27 12:38:44 2011 -0800
@@ -63,7 +63,7 @@
 
     uint32_t servomsgid
         = can_build_message_id(
-                can_node_odr_main, can_node_odr_controller, can_dataid_servo_position );
+                can_node_odr_manager, can_node_odr_controller, can_dataid_servo_position );
 
     CANMessage::QueueToSend(
             new CANMessage( servomsgid,
@@ -81,7 +81,7 @@
 
     uint32_t motormsgid
         = can_build_message_id(
-                can_node_odr_main, can_node_odr_controller, can_dataid_motor_speed );
+                can_node_odr_manager, can_node_odr_controller, can_dataid_motor_speed );
 
     CANMessage::QueueToSend(
             new CANMessage( motormsgid,
--- a/main/robots/odr/ODRApp.cpp	Tue Dec 27 12:38:18 2011 -0800
+++ b/main/robots/odr/ODRApp.cpp	Tue Dec 27 12:38:44 2011 -0800
@@ -57,7 +57,8 @@
 // ----------------------------------------------------------------------------------------
 
 ODRApp::ODRApp()
-    : m_helpRequested( false )
+    : m_helpRequested( false ),
+      m_runloopActive( false ) 
 {
 }
 
@@ -120,18 +121,6 @@
     loadConfiguration();
 
     logger().information( "------------------------------------------------------" );
-    logger().information( "ODR startup" );
-
-    if ( config().getBool( "application.runAsDaemon", false ) )
-    {
-        logger().information( "i am a daemon!" );
-    }
-
-    if ( isInteractive() )
-    {
-        logger().information( "is interactive!" );
-    }
-
     logger().information( "ODRApp::main() started" );
 
     std::string canInterfaceName = config().getString( "can.interfaceName", "can0" );
@@ -147,35 +136,34 @@
     Scoreboard   scoreboard( logger().name() );
     scoreboardThread.start( scoreboard );
 
-    MotorsAndServos().motorSpeeds( 32, 32 );
-    MotorsAndServos().servoPositions( 12, -12 );
-    Poco::Thread::sleep( 20000 );
-    MotorsAndServos().motorSpeeds( 0, 0 );
-    MotorsAndServos().servoPositions( 0, 0 );
+    m_runloopActive = true;
 
-    MotorsAndServos().servoPositions( 0, 12 );
-    Poco::Thread::sleep( 5000 );
-    MotorsAndServos().motorSpeeds( 0, 0 );
-    MotorsAndServos().servoPositions( 0, 0 );
-
-#if 0
-    for ( uint8_t i = 0; i < 10; ++i )
+    while ( m_runloopActive )
     {
-        Poco::Thread::sleep( 5000 );
-        MotorsAndServos().servoPositions( 10, -10 );
-        Poco::Thread::sleep( 5000 );
-        MotorsAndServos().servoPositions( -8, 0 );
+        try
+        {
+            runloop();
+        }
+        catch ( const Poco::Exception& ex )
+        {
+            logger().error(
+                    Poco::Logger::format(
+                        "Failed in runloop(), Poco::Exception: $0", ex.displayText() ) );
+        }
+        catch ( const std::exception& ex )
+        {
+            logger().error(
+                    Poco::Logger::format(
+                        "Failed in runloop(), std::exception: $0", ex.what() ) );
+        }
+        catch ( ... )
+        {
+            logger().error( "Failed in runloop(), unknown exception" );
+        }
     }
-    MotorsAndServos().motorSpeeds( 0, 0 );
-    MotorsAndServos().servoPositions( 0, 0 );
-#endif
 
-//    uint8_t msgdata[8] = { 0x01, 0x02, 0x03, 0x04 };
-//    CANMessage::QueueToSend( new CANMessage( 0x333, msgdata, 4 ) );
-    //MotorsAndServos().servoPositions( 10, -10 );
-
-    //Poco::Thread::sleep( 20000 );
-    Poco::Thread::sleep( 5000 );
+    Scoreboard::sendManagerMessage( "Goodbye" );
+    Poco::Thread::sleep( 2000 );
 
     scoreboard.timeToQuit();
     scoreboardThread.join();
@@ -190,3 +178,61 @@
 
 // ----------------------------------------------------------------------------------------
 
+void ODRApp::runloop()
+{
+    for ( ;; )
+    {
+        logger().information( "waiting for the ODR controller" );
+        while ( ! Scoreboard::isControllerAlive() )
+        {
+            Poco::Thread::sleep( 250 );
+        }
+        logger().information( "the ODR controller is alive" );
+
+        Scoreboard::sendManagerMessage( "Press A" );
+
+        bool didTellLogAboutSonar = false;
+        while ( ! Scoreboard::isButtonOneDown() )
+        {
+            Poco::Thread::sleep( 250 );
+
+            if ( Scoreboard::isButtonTwoDown() )
+            {
+                m_runloopActive = false;
+                return;
+            }
+
+            if ( Scoreboard::isSonarFrontAlive() && ! didTellLogAboutSonar )
+            {
+                logger().information( "the front sonar is alive" );
+                didTellLogAboutSonar = true;
+            }
+        }
+        while ( Scoreboard::isButtonOneDown() )
+        {
+            Poco::Thread::sleep( 250 );
+        }
+
+        Scoreboard::sendManagerMessage( "Running" );
+        
+        Scoreboard::setSonarFrontEnabled();
+
+        if ( Scoreboard::isEStopActive() )
+        {
+            logger().information( "estop is active" );
+        }
+
+        MotorsAndServos().motorSpeeds( 32, 32 );
+        MotorsAndServos().servoPositions( 12, -12 );
+        Poco::Thread::sleep( 20000 );
+        MotorsAndServos().motorSpeeds( 0, 0 );
+        MotorsAndServos().servoPositions( 0, 0 );
+
+        Scoreboard::sendManagerMessage( "Done" );
+        Scoreboard::setSonarFrontDisabled();
+        Poco::Thread::sleep( 5000 );
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
--- a/main/robots/odr/ODRApp.h	Tue Dec 27 12:38:18 2011 -0800
+++ b/main/robots/odr/ODRApp.h	Tue Dec 27 12:38:44 2011 -0800
@@ -61,7 +61,11 @@
         int  main(const std::vector<std::string>& args);
 
     private:
+        void runloop();
+
+    private:
         bool m_helpRequested;
+        bool m_runloopActive;
 };
 
 // ----------------------------------------------------------------------------------------
--- a/main/robots/odr/Scoreboard.cpp	Tue Dec 27 12:38:18 2011 -0800
+++ b/main/robots/odr/Scoreboard.cpp	Tue Dec 27 12:38:44 2011 -0800
@@ -47,6 +47,14 @@
 
 Poco::RWLock Scoreboard::sm_rwLock;
 bool         Scoreboard::sm_isControllerAlive;
+bool         Scoreboard::sm_isEStopActive;
+bool         Scoreboard::sm_isButtonOneDown;
+bool         Scoreboard::sm_isButtonTwoDown;
+bool         Scoreboard::sm_isSonarFrontAlive;
+bool         Scoreboard::sm_isSonarFrontEnabled;
+uint16_t     Scoreboard::sm_sonarFrontLeft;
+uint16_t     Scoreboard::sm_sonarFrontCenter;
+uint16_t     Scoreboard::sm_sonarFrontRight;
 
 // ----------------------------------------------------------------------------------------
 
@@ -58,21 +66,133 @@
 
 // ----------------------------------------------------------------------------------------
 
+bool Scoreboard::isEStopActive()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_isEStopActive;
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool Scoreboard::isButtonOneDown()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_isButtonOneDown;
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool Scoreboard::isButtonTwoDown()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_isButtonTwoDown;
+}
+                 
+// ----------------------------------------------------------------------------------------
+
+void Scoreboard::sendManagerMessage( const char* msg )
+{
+    uint32_t updatemsgid = can_build_message_id(
+                    can_node_odr_manager, can_node_broadcast, can_dataid_odrmgr_update );
+
+    can_data_odrmgr_update update;
+
+    uint8_t len = 0;
+    for ( ; len < sizeof( update.msg ); ++len )
+    {
+        if ( *msg == '\0' ) break;
+        update.msg[len] = *msg++;
+    }
+
+    CANMessage::QueueToSend(
+            new CANMessage( updatemsgid, reinterpret_cast< uint8_t* >( &update ), len ) );
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool Scoreboard::isSonarFrontAlive()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_isSonarFrontAlive;
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool Scoreboard::isSonarFrontEnabled()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_isSonarFrontEnabled;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void Scoreboard::setSonarFrontEnabled()
+{
+    uint32_t msgid = can_build_message_id( can_node_odr_manager,
+                                           can_node_odr_sonar_front,
+                                           can_dataid_sonar_front_enable );
+
+    CANMessage::QueueToSend( new CANMessage( msgid ) );
+}
+
+// ----------------------------------------------------------------------------------------
+
+void Scoreboard::setSonarFrontDisabled()
+{
+    uint32_t msgid = can_build_message_id( can_node_odr_manager,
+                                           can_node_odr_sonar_front,
+                                           can_dataid_sonar_front_disable );
+
+    CANMessage::QueueToSend( new CANMessage( msgid ) );
+}
+
+// ----------------------------------------------------------------------------------------
+
+uint16_t Scoreboard::sonarFrontLeft()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_sonarFrontLeft;
+}
+
+// ----------------------------------------------------------------------------------------
+
+uint16_t Scoreboard::sonarFrontCenter()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_sonarFrontCenter;
+}
+
+// ----------------------------------------------------------------------------------------
+
+uint16_t Scoreboard::sonarFrontRight()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_sonarFrontRight;
+}
+
+// ----------------------------------------------------------------------------------------
+
 Scoreboard::Scoreboard( const std::string& loggerName )
     : Poco::Runnable(),
       m_loggerName( loggerName ),
       m_quitEvent( false /* not auto-reset */ ),
-      m_timerSendMainHeartbeat(),
-      m_timerControllerTimeout()
+      m_timerSendManagerUpdate(),
+      m_timerControllerUpdate(),
+      m_timerSonarFrontUpdate()
 {
-    m_timerSendMainHeartbeat.setPeriodicInterval( 1000 );
-    m_timerSendMainHeartbeat.start(
-            Poco::TimerCallback< Scoreboard >( *this, &Scoreboard::sendMainHeartbeat ) );
+    m_timerSendManagerUpdate.setPeriodicInterval( 5000 );
+    m_timerSendManagerUpdate.start(
+            Poco::TimerCallback< Scoreboard >( *this, &Scoreboard::sendManagerUpdate ) );
 
-    m_timerControllerTimeout.setPeriodicInterval( 5000 );
-    m_timerControllerTimeout.start(
+    m_timerControllerUpdate.setPeriodicInterval( 5000 );
+    m_timerControllerUpdate.start(
             Poco::TimerCallback< Scoreboard >(
-                    *this, &Scoreboard::timeoutControllerHeartbeat ) );
+                    *this, &Scoreboard::timeoutControllerUpdate ) );
+
+    m_timerSonarFrontUpdate.setPeriodicInterval( 5000 );
+    m_timerSonarFrontUpdate.start(
+            Poco::TimerCallback< Scoreboard >(
+                    *this, &Scoreboard::timeoutSonarFrontUpdate ) );
 }
 
 // ----------------------------------------------------------------------------------------
@@ -90,21 +210,18 @@
 
 // ----------------------------------------------------------------------------------------
 
-void Scoreboard::sendMainHeartbeat( Poco::Timer& timer )
+void Scoreboard::sendManagerUpdate( Poco::Timer& timer )
 {
-    uint32_t data = can_heartbeat_data;
-
-    uint32_t heartbeatmsgid = can_build_message_id(
-                    can_node_odr_main, can_node_broadcast, can_dataid_heartbeat );
+    uint32_t updatemsgid = can_build_message_id(
+                    can_node_odr_manager, can_node_broadcast, can_dataid_odrmgr_update );
 
     CANMessage::QueueToSend(
-            new CANMessage( heartbeatmsgid,
-                    reinterpret_cast< uint8_t* >( &data ), sizeof( data ) ) );
+            new CANMessage( updatemsgid, 0, 0 ) );
 }
 
 // ----------------------------------------------------------------------------------------
 
-void Scoreboard::timeoutControllerHeartbeat( Poco::Timer& timer )
+void Scoreboard::timeoutControllerUpdate( Poco::Timer& timer )
 {
     Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
     sm_isControllerAlive = false;
@@ -112,6 +229,14 @@
 
 // ----------------------------------------------------------------------------------------
 
+void Scoreboard::timeoutSonarFrontUpdate( Poco::Timer& timer )
+{
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+    sm_isSonarFrontAlive = false;
+}
+
+// ----------------------------------------------------------------------------------------
+
 void Scoreboard::recvHeartbeat( uint8_t srcNode )
 {
     Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
@@ -119,14 +244,54 @@
     switch ( srcNode )
     {
         case can_node_odr_controller:
-            m_timerControllerTimeout.restart();
-            sm_isControllerAlive = true;
+            break;
+
+        case can_node_odr_sonar_front:
+            sm_isSonarFrontAlive = true;
             break;
     }
 }
 
 // ----------------------------------------------------------------------------------------
 
+void Scoreboard::recvControllerUpdate( CANMessage* msg )
+{
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+
+    m_timerControllerUpdate.restart();
+    sm_isControllerAlive = true;
+
+    can_data_odrctl_update info;
+    msg->msgData( reinterpret_cast< uint8_t* >( &info ), sizeof( info ) );
+
+    sm_isEStopActive = info.estop;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void Scoreboard::recvButtonUpdate( CANMessage* msg )
+{
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+
+    can_data_odrctl_button info;
+    msg->msgData( reinterpret_cast< uint8_t* >( &info ), sizeof( info ) );
+
+    sm_isButtonOneDown = info.button_1;
+    sm_isButtonTwoDown = info.button_2;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void Scoreboard::recvSonarFrontStatus( bool enabled )
+{
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+
+    sm_isSonarFrontAlive = true;
+    sm_isSonarFrontEnabled = enabled;
+}
+
+// ----------------------------------------------------------------------------------------
+
 void Scoreboard::run()
 {
     Poco::Logger& log = Poco::Logger::get( m_loggerName );
@@ -164,18 +329,36 @@
                     case can_dataid_heartbeat:
                         recvHeartbeat( srcNode );
                         break;
+
+                    case can_dataid_odrctl_update:
+                        recvControllerUpdate( msg );
+                        break;
+
+                    case can_dataid_odrctl_button:
+                        recvButtonUpdate( msg );
+                        break;
+
+                    case can_dataid_sonar_front_state_enabled:
+                        recvSonarFrontStatus( true /* enabled */ );
+                        break;
+
+                    case can_dataid_sonar_front_state_disabled:
+                        recvSonarFrontStatus( false /* not enabled */ );
+                        break;
                 }
             }
         }
         catch ( const Poco::Exception& ex )
         {
-            log.error( std::string( "Scoreboard::run() caught Poco::Exception: " )
-                    + ex.displayText() );
+            log.error(
+                Poco::Logger::format(
+                    "Scoreboard::run() caught Poco::Exception: $0", ex.displayText() ) );
         }
         catch ( const std::exception& ex )
         {
-            log.error( std::string( "Scoreboard::run() caught std::exception: " )
-                    + ex.what() );
+            log.error(
+                Poco::Logger::format(
+                    "Scoreboard::run() caught std::exception: $0", ex.what() ) );
         }
         catch ( ... )
         {
--- a/main/robots/odr/Scoreboard.h	Tue Dec 27 12:38:18 2011 -0800
+++ b/main/robots/odr/Scoreboard.h	Tue Dec 27 12:38:44 2011 -0800
@@ -39,12 +39,28 @@
 #include <Poco/RWLock.h>
 #include <Poco/Timer.h>
 
+class CANMessage;
+
 // ----------------------------------------------------------------------------------------
 
 class Scoreboard : public Poco::Runnable
 {
     public:
+        // odr-controller
         static bool isControllerAlive();
+        static bool isEStopActive();
+        static bool isButtonOneDown();
+        static bool isButtonTwoDown();
+        static void sendManagerMessage( const char* msg );
+
+        // odr-sonar-front
+        static bool     isSonarFrontAlive();
+        static bool     isSonarFrontEnabled();
+        static void     setSonarFrontEnabled();
+        static void     setSonarFrontDisabled();
+        static uint16_t sonarFrontLeft();
+        static uint16_t sonarFrontCenter();
+        static uint16_t sonarFrontRight();
 
     public:
         Scoreboard( const std::string& loggerName );
@@ -55,17 +71,30 @@
     private:
         static Poco::RWLock sm_rwLock;
         static bool         sm_isControllerAlive;
+        static bool         sm_isEStopActive;
+        static bool         sm_isButtonOneDown;
+        static bool         sm_isButtonTwoDown;
+        static bool         sm_isSonarFrontAlive;
+        static bool         sm_isSonarFrontEnabled;
+        static uint16_t     sm_sonarFrontLeft;
+        static uint16_t     sm_sonarFrontCenter;
+        static uint16_t     sm_sonarFrontRight;
 
     private:
         std::string m_loggerName;
         Poco::Event m_quitEvent;
-        Poco::Timer m_timerSendMainHeartbeat;
-        Poco::Timer m_timerControllerTimeout;
+        Poco::Timer m_timerSendManagerUpdate;
+        Poco::Timer m_timerControllerUpdate;
+        Poco::Timer m_timerSonarFrontUpdate;
 
     private:
-        void sendMainHeartbeat( Poco::Timer& timer );
-        void timeoutControllerHeartbeat( Poco::Timer& timer );
+        void sendManagerUpdate( Poco::Timer& timer );
+        void timeoutControllerUpdate( Poco::Timer& timer );
+        void timeoutSonarFrontUpdate( Poco::Timer& timer );
         void recvHeartbeat( uint8_t srcNode );
+        void recvControllerUpdate( CANMessage* msg );
+        void recvButtonUpdate( CANMessage* msg );
+        void recvSonarFrontStatus( bool enabled );
 };
 
 // ----------------------------------------------------------------------------------------