changeset 124:9b41739be9c2 main

Updates to the simulator for changes to the CAN messages for the front-facing sonar package.
author Bob Cook <bob@bobcookdev.com>
date Thu, 15 Dec 2011 06:42:45 -0800
parents 25d2ba77fe8b
children ddbad6e9b017
files main/robots/odr-sim/Receiver.cpp main/robots/odr-sim/Receiver.h main/robots/odr-sim/SimDisplay.cpp main/robots/odr-sim/SimDisplay.h main/robots/odr-sim/SonarFrontStatus.cpp main/robots/odr-sim/SonarFrontStatus.h main/robots/odr-sim/jamfile
diffstat 7 files changed, 271 insertions(+), 2 deletions(-) [+]
line wrap: on
line diff
--- a/main/robots/odr-sim/Receiver.cpp	Wed Dec 14 11:46:20 2011 -0800
+++ b/main/robots/odr-sim/Receiver.cpp	Thu Dec 15 06:42:45 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	Wed Dec 14 11:46:20 2011 -0800
+++ b/main/robots/odr-sim/Receiver.h	Thu Dec 15 06:42:45 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	Wed Dec 14 11:46:20 2011 -0800
+++ b/main/robots/odr-sim/SimDisplay.cpp	Thu Dec 15 06:42:45 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();
 }
 
@@ -550,3 +598,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	Wed Dec 14 11:46:20 2011 -0800
+++ b/main/robots/odr-sim/SimDisplay.h	Thu Dec 15 06:42:45 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	Thu Dec 15 06:42:45 2011 -0800
@@ -0,0 +1,103 @@
+// ----------------------------------------------------------------------------------------
+//
+//  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;
+
+// ----------------------------------------------------------------------------------------
+
+//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 = can_build_message_id( can_node_odr_sonar_front,
+                                                   can_node_broadcast,
+                                                   can_dataid_heartbeat );
+
+            uint32_t data = can_heartbeat_data;
+
+            CANMessage::QueueToSend(
+                new CANMessage( msgid,
+                                reinterpret_cast< uint8_t* >( &data ),
+                                sizeof( data ) ) );
+        }
+        catch ( ... )
+        {
+            // nothing to do, but don't stop the thread
+        }
+
+        if ( m_quitEvent.tryWait( 1000 ) ) // 1000 ms == 1s
+        {
+            return;
+        }
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr-sim/SonarFrontStatus.h	Thu Dec 15 06:42:45 2011 -0800
@@ -0,0 +1,65 @@
+// ----------------------------------------------------------------------------------------
+//
+//  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 void setSonarFrontLeft( uint16_t value );
+
+    public:
+        SonarFrontStatus();
+        virtual ~SonarFrontStatus();
+        virtual void run();
+        void timeToQuit();
+
+    private:
+        static void sendSonarFrontHeartbeat();
+
+    private:
+        static Poco::RWLock sm_rwLock;
+
+    private:
+        Poco::Event m_quitEvent;
+};
+
+// ----------------------------------------------------------------------------------------
+#endif // #ifndef BCDRL_ROBOTS_ODRSIM_SONARFRONTSTATUS_H
+// ----------------------------------------------------------------------------------------
+
--- a/main/robots/odr-sim/jamfile	Wed Dec 14 11:46:20 2011 -0800
+++ b/main/robots/odr-sim/jamfile	Thu Dec 15 06:42:45 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