changeset 211:ddd95e6ede8a main

Pull from upstream.
author Bob Cook <bob@bobcookdev.com>
date Thu, 17 Apr 2014 21:56:30 -0700
parents 0e1a8cacc09d (current diff) 15d96880b4ba (diff)
children d1725a29353f
files
diffstat 6 files changed, 392 insertions(+), 2 deletions(-) [+]
line wrap: on
line diff
--- a/main/robots/cantool/CanToolApp.cpp	Thu Apr 17 21:56:02 2014 -0700
+++ b/main/robots/cantool/CanToolApp.cpp	Thu Apr 17 21:56:30 2014 -0700
@@ -70,6 +70,7 @@
     Poco::Util::Application::initialize( self );
 
     m_commandDispatchTable[ "estop" ] = CommandEStop;
+    m_commandDispatchTable[ "gps" ] = CommandGps;
     m_commandDispatchTable[ "listen" ] = CommandListen;
     m_commandDispatchTable[ "speed" ] = CommandSpeed;
     m_commandDispatchTable[ "steer" ] = CommandSteer;
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/cantool/CmdGps.cpp	Thu Apr 17 21:56:30 2014 -0700
@@ -0,0 +1,230 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/cantool/CmdGps.cpp
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+//
+//  Command "gps" implementation.
+//
+//  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.
+//
+// ----------------------------------------------------------------------------------------
+
+#include <iostream>
+
+#include <Poco/Exception.h>
+#include <Poco/NumberFormatter.h>
+#include <Poco/NumberParser.h>
+
+#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"
+
+// ----------------------------------------------------------------------------------------
+
+static void PrintHelp()
+{
+    std::cerr << "error: gps lat[itude]  <degrees> <minutes>" << std::endl;
+    std::cerr << "error: gps lon[gitude] <degrees> <minutes>" << std::endl;
+    std::cerr << "error: gps fix <satellites>" << std::endl;
+    std::cerr << "       (enter negatives as: \"gps lat -- -48 17.038\")" << std::endl;
+}
+
+// ----------------------------------------------------------------------------------------
+
+static void DoCommandLat( const std::string& degreesStr, const std::string& minutesStr )
+{
+    uint32_t msgid = can_build_message_id( can_node_sensor_gps,
+                                           can_node_broadcast,
+                                           can_dataid_latitude );
+
+    can_data_gps_data msg;
+
+    try
+    {
+        int    deg = Poco::NumberParser::parse( degreesStr );
+        double min = Poco::NumberParser::parseFloat( minutesStr );
+
+        if ( deg < -180 || deg > 180 )
+        {
+            throw Poco::SyntaxException( "value out of range", 1 );
+        }
+
+        if ( min < 0 || min > 60.0 )
+        {
+            throw Poco::SyntaxException( "value out of range", 1 );
+        }
+
+        msg.degrees         = static_cast< int16_t >( deg );
+        msg.min_thousandths = static_cast< uint32_t >( min * can_data_gps_min_multiplier );
+    }
+    catch ( const Poco::SyntaxException& ex )
+    {
+        PrintHelp();
+        return;
+    }
+
+    CANMessage::QueueToSend(
+        new CANMessage( msgid, reinterpret_cast< uint8_t* >( &msg ), sizeof( msg ) ) );
+
+    std::cout << "sent gps latitude "
+        << Poco::NumberFormatter::format( msg.degrees )
+        << "," 
+        << Poco::NumberFormatter::format( msg.min_thousandths ) 
+        << std::endl;
+}
+
+// ----------------------------------------------------------------------------------------
+
+static void DoCommandLon( const std::string& degreesStr, const std::string& minutesStr )
+{
+    uint32_t msgid = can_build_message_id( can_node_sensor_gps,
+                                           can_node_broadcast,
+                                           can_dataid_longitude );
+
+    can_data_gps_data msg;
+
+    try
+    {
+        int    deg = Poco::NumberParser::parse( degreesStr );
+        double min = Poco::NumberParser::parseFloat( minutesStr );
+
+        if ( deg < -180 || deg > 180 )
+        {
+            throw Poco::SyntaxException( "value out of range", 1 );
+        }
+
+        if ( min < 0 || min > 60.0 )
+        {
+            throw Poco::SyntaxException( "value out of range", 1 );
+        }
+
+        msg.degrees         = static_cast< int16_t >( deg );
+        msg.min_thousandths = static_cast< uint32_t >( min * can_data_gps_min_multiplier );
+    }
+    catch ( const Poco::SyntaxException& ex )
+    {
+        PrintHelp();
+        return;
+    }
+
+    CANMessage::QueueToSend(
+        new CANMessage( msgid, reinterpret_cast< uint8_t* >( &msg ), sizeof( msg ) ) );
+
+    std::cout << "sent gps longitude "
+        << Poco::NumberFormatter::format( msg.degrees )
+        << "," 
+        << Poco::NumberFormatter::format( msg.min_thousandths ) 
+        << std::endl;
+}
+
+// ----------------------------------------------------------------------------------------
+
+static void DoCommandFix( const std::string& satellitesStr )
+{
+    uint32_t msgid = can_build_message_id( can_node_sensor_gps,
+                                           can_node_broadcast,
+                                           can_dataid_gps_fix );
+
+    can_data_gps_fix msg;
+
+    try
+    {
+        int satCount = Poco::NumberParser::parse( satellitesStr );
+
+        if ( satCount < 0 || satCount > 99 )
+        {
+            throw Poco::SyntaxException( "value out of range", 1 );
+        }
+
+        msg.satellites = static_cast< uint8_t >( satCount );
+    }
+    catch ( const Poco::SyntaxException& ex )
+    {
+        PrintHelp();
+        return;
+    }
+
+    CANMessage::QueueToSend(
+        new CANMessage( msgid, reinterpret_cast< uint8_t* >( &msg ), sizeof( msg ) ) );
+
+    std::cout << "sent gps fix "
+        << Poco::NumberFormatter::format( msg.satellites )
+        << std::endl;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void CommandGps( const std::vector< std::string >& args )
+{
+    if ( args.size() < 3 )
+    {
+        PrintHelp();
+        return;
+    }
+
+    if ( args[ 1 ] == "lat" || args[ 1 ] == "latitude" )
+    {
+        if ( args.size() != 4 )
+        {
+            PrintHelp();
+        }
+        else
+        {
+            DoCommandLat( args[ 2 ], args[ 3 ] );
+        }
+    }
+    else if ( args[ 1 ] == "lon" || args[ 1 ] == "longitude" )
+    {
+        if ( args.size() != 4 )
+        {
+            PrintHelp();
+        }
+        else
+        {
+            DoCommandLon( args[ 2 ], args[ 3 ] );
+        }
+    }
+    else if ( args[ 1 ] == "fix" )
+    {
+        if ( args.size() != 3 )
+        {
+            PrintHelp();
+        }
+        else
+        {
+            DoCommandFix( args[ 2 ] );
+        }
+    }
+    else
+    {
+        PrintHelp();
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
--- a/main/robots/cantool/CmdListen.cpp	Thu Apr 17 21:56:02 2014 -0700
+++ b/main/robots/cantool/CmdListen.cpp	Thu Apr 17 21:56:30 2014 -0700
@@ -72,6 +72,9 @@
         case can_node_odr_sonar_front:
             return "odr-sonar-front";
 
+        case can_node_sensor_gps:
+            return "sensor-gps";
+
         default:
             return "(unknown node)";
     }
@@ -79,6 +82,47 @@
 
 // ----------------------------------------------------------------------------------------
 
+static void MsgGpsLatitude( const CANMessage* msg )
+{
+    can_data_gps_data info;
+    msg->msgData( reinterpret_cast< uint8_t* >( &info ), sizeof( info ) );
+
+    int    deg = info.degrees;
+    double min = static_cast< double >( info.min_thousandths ) / can_data_gps_min_multiplier;
+
+    std::cout << "gps latitude:  " << Poco::NumberFormatter::format( deg )
+        << " " << Poco::NumberFormatter::format( min )
+        << std::endl;
+}
+
+// ----------------------------------------------------------------------------------------
+
+static void MsgGpsLongitude( const CANMessage* msg )
+{
+    can_data_gps_data info;
+    msg->msgData( reinterpret_cast< uint8_t* >( &info ), sizeof( info ) );
+
+    int    deg = info.degrees;
+    double min = static_cast< double >( info.min_thousandths ) / can_data_gps_min_multiplier;
+
+    std::cout << "gps longitude: " << Poco::NumberFormatter::format( deg )
+        << " " << Poco::NumberFormatter::format( min )
+        << std::endl;
+}
+
+// ----------------------------------------------------------------------------------------
+
+static void MsgGpsFix( const CANMessage* msg )
+{
+    can_data_gps_fix info;
+    msg->msgData( reinterpret_cast< uint8_t* >( &info ), sizeof( info ) );
+
+    std::cout << "gps fix (" << Poco::NumberFormatter::format( info.satellites )
+        << " satellites)" << std::endl;
+}
+
+// ----------------------------------------------------------------------------------------
+
 static void MsgSonarFrontUpdate( const CANMessage* msg )
 {
     can_data_sonar_front info;
@@ -137,6 +181,18 @@
                 std::cout << "heartbeat from " << NodeName( srcNode ) << std::endl;
                 break;
 
+            case can_dataid_latitude:
+                MsgGpsLatitude( msg );
+                break;
+
+            case can_dataid_longitude:
+                MsgGpsLongitude( msg );
+                break;
+
+            case can_dataid_gps_fix:
+                MsgGpsFix( msg );
+                break;
+
             case can_dataid_sonar_front:
                 MsgSonarFrontUpdate( msg );
                 break;
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/cantool/CmdSpeed.cpp	Thu Apr 17 21:56:30 2014 -0700
@@ -0,0 +1,101 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/cantool/CmdSpeed.cpp
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+//
+//  Command "speed" implementation.
+//
+//  Copyright (c) 2011-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.
+//
+// ----------------------------------------------------------------------------------------
+
+#include <iostream>
+
+#include <Poco/Exception.h>
+#include <Poco/NumberFormatter.h>
+#include <Poco/NumberParser.h>
+
+#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"
+
+// ----------------------------------------------------------------------------------------
+
+static void PrintHelp()
+{
+    std::cerr << "error: speed <front> <rear>" << std::endl;
+    std::cerr << "       where \"speed\" is specified as [-50,50]" << std::endl;
+    std::cerr << "       (enter negative values as: \"speed -- -10 -10\"" << std::endl;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void CommandSpeed( const std::vector< std::string >& args )
+{
+    if ( args.size() < 3 )
+    {
+        PrintHelp();
+        return;
+    }
+
+    uint32_t msgid = can_build_message_id( can_node_odr_manager,
+                                           can_node_odr_motion,
+                                           can_dataid_wheel_speed );
+    can_data_wheel_speed msg;
+
+    try
+    {
+        int front = Poco::NumberParser::parse( args[ 1 ] );
+        int rear  = Poco::NumberParser::parse( args[ 2 ] );
+
+        if ( front < -50 || front > 50 || rear < -50 || rear > 50 )
+        {
+            throw Poco::SyntaxException( "value out of range", 1 );
+        }
+
+        msg.rpm_front = front;
+        msg.rpm_rear  = rear;
+    }
+    catch ( const Poco::SyntaxException& ex )
+    {
+        PrintHelp();
+        return;
+    }
+
+    CANMessage::QueueToSend(
+        new CANMessage( msgid, reinterpret_cast< uint8_t* >( &msg ), sizeof( msg ) ) );
+
+    std::cout << "sent "
+        << Poco::NumberFormatter::format( msg.rpm_front )
+        << "," 
+        << Poco::NumberFormatter::format( msg.rpm_rear ) 
+        << std::endl;
+}
+
+// ----------------------------------------------------------------------------------------
+
--- a/main/robots/cantool/Commands.h	Thu Apr 17 21:56:02 2014 -0700
+++ b/main/robots/cantool/Commands.h	Thu Apr 17 21:56:30 2014 -0700
@@ -43,5 +43,7 @@
 
 void CommandSonarFront( const std::vector< std::string >& args );
 
+void CommandGps( const std::vector< std::string >& args );
+
 // ----------------------------------------------------------------------------------------
 
--- a/main/robots/cantool/jamfile	Thu Apr 17 21:56:02 2014 -0700
+++ b/main/robots/cantool/jamfile	Thu Apr 17 21:56:30 2014 -0700
@@ -5,7 +5,7 @@
 #   Bob Cook Development, Robotics Library
 #   http://www.bobcookdev.com/rl/
 #    
-#   Copyright (c) 2011 Bob Cook
+#   Copyright (c) 2011-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
@@ -37,7 +37,7 @@
 COMMON_SOURCES = 
     main.cpp
     CanToolApp.cpp
-    CmdEStop.cpp CmdListen.cpp CmdSpeed.cpp CmdSonarFront.cpp CmdSteer.cpp
+    CmdEStop.cpp CmdGps.cpp CmdListen.cpp CmdSpeed.cpp CmdSonarFront.cpp CmdSteer.cpp
     packages.common.can.pkg
     packages.linux.can.pkg
     ;