You can subscribe to this list here.
| 2008 |
Jan
|
Feb
(58) |
Mar
(15) |
Apr
(23) |
May
(8) |
Jun
(92) |
Jul
(66) |
Aug
(6) |
Sep
(9) |
Oct
(44) |
Nov
(8) |
Dec
(1) |
|---|---|---|---|---|---|---|---|---|---|---|---|---|
| 2009 |
Jan
(10) |
Feb
(8) |
Mar
(2) |
Apr
(8) |
May
(19) |
Jun
(11) |
Jul
(8) |
Aug
(6) |
Sep
(5) |
Oct
(4) |
Nov
(26) |
Dec
(4) |
| 2010 |
Jan
(5) |
Feb
(3) |
Mar
(4) |
Apr
(3) |
May
(4) |
Jun
|
Jul
(1) |
Aug
(16) |
Sep
(7) |
Oct
(3) |
Nov
(7) |
Dec
|
| 2011 |
Jan
|
Feb
|
Mar
|
Apr
|
May
|
Jun
(1) |
Jul
|
Aug
|
Sep
|
Oct
|
Nov
|
Dec
|
|
From: <to...@us...> - 2008-06-30 12:01:05
|
Revision: 232
http://gearbox.svn.sourceforge.net/gearbox/?rev=232&view=rev
Author: tobasco
Date: 2008-06-30 05:01:10 -0700 (Mon, 30 Jun 2008)
Log Message:
-----------
incorporated Geoff's comments
Modified Paths:
--------------
gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverparser.cpp
gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverparser.h
gearbox/trunk/submitted/gbxsmartbatteryacfr/smartbattery.cpp
gearbox/trunk/submitted/gbxsmartbatteryacfr/smartbatteryacfr.dox
gearbox/trunk/submitted/gbxsmartbatteryacfr/test/CMakeLists.txt
gearbox/trunk/submitted/gbxsmartbatteryacfr/test/example.cmake.in
gearbox/trunk/submitted/gbxsmartbatteryacfr/test/example.readme
Added Paths:
-----------
gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserversystem.cpp
gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserversystem.h
gearbox/trunk/submitted/gbxsmartbatteryacfr/test/darttest/
gearbox/trunk/submitted/gbxsmartbatteryacfr/test/darttest/CMakeLists.txt
gearbox/trunk/submitted/gbxsmartbatteryacfr/test/darttest/checksumtest.cpp
Removed Paths:
-------------
gearbox/trunk/submitted/gbxsmartbatteryacfr/test/checksumtest.cpp
Modified: gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverparser.cpp
===================================================================
--- gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverparser.cpp 2008-06-29 03:49:54 UTC (rev 231)
+++ gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverparser.cpp 2008-06-30 12:01:10 UTC (rev 232)
@@ -18,227 +18,6 @@
namespace gbxsmartbatteryacfr {
-//
-// Helper functions
-//
-string toString( const vector<bool> &flags )
-{
- stringstream ss;
- for (unsigned int i=0; i<flags.size(); i++)
- {
- ss << flags[i] << " ";
- }
- return ss.str();
-}
-
-string toLogString( const vector<bool> &flags )
-{
- stringstream ss;
- for (unsigned int i=0; i<flags.size(); i++)
- {
- ss << flags[i];
- }
- return ss.str();
-}
-
-
-//
-// OceanServerSystem non-member functions
-//
-
-string toString( const OceanServerSystem &system )
-{
- stringstream ss;
- ss << "Charge: \t" << system.percentCharge << endl;
- ss << "Minutes to empty:\t" << system.minToEmpty << endl;
- ss << "Available batt.: \t" << toString( system.availableBatteries ) << endl;
- ss << "Charging: \t" << toString( system.chargingStates ) << endl;
- ss << "Supplying power: \t" << toString( system.supplyingPowerStates ) << endl;
- ss << "Charge power: \t" << toString( system.chargePowerPresentStates ) << endl;
- ss << "Power no good: \t" << toString( system.powerNoGoodStates ) << endl;
- ss << "Charge inhibited:\t" << toString( system.chargeInhibitedStates ) << endl;
-
- map<int,SmartBattery>::const_iterator it;
- for (it=system.batteries().begin(); it!=system.batteries().end(); it++)
- {
- ss << "Data from battery number: " << it->first << endl;
- ss << toString( it->second ) << endl;
- }
-
- return ss.str();
-}
-
-string toLogString( const OceanServerSystem &system )
-{
- stringstream ss;
- ss << system.percentCharge << " ";
- ss << system.minToEmpty << " ";
- ss << toLogString( system.availableBatteries ) << " ";
- ss << toLogString( system.chargingStates ) << " ";
- ss << toLogString( system.supplyingPowerStates ) << " ";
- ss << toLogString( system.chargePowerPresentStates ) << " ";
- ss << toLogString( system.powerNoGoodStates ) << " ";
- ss << toLogString( system.chargeInhibitedStates ) << endl;
-
- ss << system.batteries().size();
-
- map<int,SmartBattery>::const_iterator it;
- for (it=system.batteries().begin(); it!=system.batteries().end(); it++)
- {
- ss << it->first << " " << toLogString( it->second ) << endl;
- }
-
- return ss.str();
-}
-
-void updateWithNewData( const OceanServerSystem &from,
- OceanServerSystem &to )
-{
- typedef map<int,SmartBattery>::const_iterator BatIt;
-
- to.availableBatteries = from.availableBatteries;
- to.percentCharge = from.percentCharge;
- to.minToEmpty = from.minToEmpty;
- to.messageToSystem = from.messageToSystem;
- to.chargingStates = from.chargingStates;
- to.supplyingPowerStates = from.supplyingPowerStates;
- to.chargePowerPresentStates = from.chargePowerPresentStates;
- to.powerNoGoodStates = from.powerNoGoodStates;
- to.chargeInhibitedStates = from.chargeInhibitedStates;
-
- for (BatIt it=from.batteries().begin(); it!=from.batteries().end(); it++)
- {
- const SmartBattery &fromB = from.battery( it->first );
- SmartBattery &toB = to.battery( it->first );
-
- if ( fromB.has( ManufacturerAccess ) ) toB.setManufacturerAccess ( fromB.manufacturerAccess () );
- if ( fromB.has( RemainingCapacityAlarm ) ) toB.setRemainingCapacityAlarm ( fromB.remainingCapacityAlarm () );
- if ( fromB.has( RemainingTimeAlarm ) ) toB.setRemainingTimeAlarm ( fromB.remainingTimeAlarm () );
- if ( fromB.has( BatteryMode ) ) toB.setBatteryMode ( fromB.batteryMode () );
- if ( fromB.has( AtRate ) ) toB.setAtRate ( fromB.atRate () );
- if ( fromB.has( AtRateTimeToFull ) ) toB.setAtRateTimeToFull ( fromB.atRateTimeToFull () );
- if ( fromB.has( AtRateTimeToEmpty ) ) toB.setAtRateTimeToEmpty ( fromB.atRateTimeToEmpty () );
- if ( fromB.has( AtRateOk ) ) toB.setAtRateOk ( fromB.atRateOk () );
- if ( fromB.has( Temperature ) ) toB.setTemperature ( fromB.temperature () );
- if ( fromB.has( Voltage ) ) toB.setVoltage ( fromB.voltage () );
- if ( fromB.has( Current ) ) toB.setCurrent ( fromB.current () );
- if ( fromB.has( AverageCurrent ) ) toB.setAverageCurrent ( fromB.averageCurrent () );
- if ( fromB.has( MaxError ) ) toB.setMaxError ( fromB.maxError () );
- if ( fromB.has( RelativeStateOfCharge ) ) toB.setRelativeStateOfCharge ( fromB.relativeStateOfCharge () );
- if ( fromB.has( AbsoluteStateOfCharge ) ) toB.setAbsoluteStateOfCharge ( fromB.absoluteStateOfCharge () );
- if ( fromB.has( RemainingCapacity ) ) toB.setRemainingCapacity ( fromB.remainingCapacity () );
- if ( fromB.has( FullChargeCapacity ) ) toB.setFullChargeCapacity ( fromB.fullChargeCapacity () );
- if ( fromB.has( RunTimeToEmpty ) ) toB.setRunTimeToEmpty ( fromB.runTimeToEmpty () );
- if ( fromB.has( AverageTimeToEmpty ) ) toB.setAverageTimeToEmpty ( fromB.averageTimeToEmpty () );
- if ( fromB.has( AverageTimeToFull ) ) toB.setAverageTimeToFull ( fromB.averageTimeToFull () );
- if ( fromB.has( ChargingCurrent ) ) toB.setChargingCurrent ( fromB.chargingCurrent () );
- if ( fromB.has( ChargingVoltage ) ) toB.setChargingVoltage ( fromB.chargingVoltage () );
- if ( fromB.has( BatteryStatus ) ) toB.setBatteryStatus ( fromB.batteryStatus () );
- if ( fromB.has( CycleCount ) ) toB.setCycleCount ( fromB.cycleCount () );
- if ( fromB.has( DesignCapacity ) ) toB.setDesignCapacity ( fromB.designCapacity () );
- if ( fromB.has( DesignVoltage ) ) toB.setDesignVoltage ( fromB.designVoltage () );
- if ( fromB.has( SpecificationInfo ) ) toB.setSpecificationInfo ( fromB.specificationInfo () );
- if ( fromB.has( ManufactureDate ) ) toB.setManufactureDate ( fromB.manufactureDate () );
- if ( fromB.has( SerialNumber ) ) toB.setSerialNumber ( fromB.serialNumber () );
- if ( fromB.has( ManufacturerName ) ) toB.setManufacturerName ( fromB.manufacturerName () );
- if ( fromB.has( DeviceName ) ) toB.setDeviceName ( fromB.deviceName () );
- if ( fromB.has( DeviceChemistry ) ) toB.setDeviceChemistry ( fromB.deviceChemistry () );
- if ( fromB.has( ManufacturerData ) ) toB.setManufacturerData ( fromB.manufacturerData () );
- }
-
- // check if reaping needs to be done, if not return
- if ( from.batteries().size() == to.batteries().size() )
- return;
-
- // store battery ids from batteries which need to be reaped in a vector
- vector<int> reapingIds;
-
- // go through all 'to' batteries and check if they are also in 'from'
- for (BatIt it=to.batteries().begin(); it!=to.batteries().end(); it++)
- {
- const int batId = it->first;
-
- BatIt itFrom = from.batteries().find( batId );
- if ( itFrom == from.batteries().end() ) {
- // battery is in 'to' but not in 'from' -> needs to be reaped
- reapingIds.push_back( batId );
- }
- }
-
- // reap batteries
- for (unsigned int i=0; i<reapingIds.size(); i++) {
- to.eraseBattery( reapingIds[i] );
- }
-
-}
-
-
-//
-// OceanServerSystem member functions
-//
-
-OceanServerSystem::OceanServerSystem()
- : percentCharge(0),
- minToEmpty(0),
- messageToSystem("")
-{
- // fixed number of slots for oceanserver system
- const int NUM_BATTERY_SLOTS = 8;
- availableBatteries.resize(NUM_BATTERY_SLOTS);
- chargingStates.resize(NUM_BATTERY_SLOTS);
- supplyingPowerStates.resize(NUM_BATTERY_SLOTS);
- chargePowerPresentStates.resize(NUM_BATTERY_SLOTS);
- powerNoGoodStates.resize(NUM_BATTERY_SLOTS);
- chargeInhibitedStates.resize(NUM_BATTERY_SLOTS);
-}
-
-// read access to all batteries
-const map<int,SmartBattery>&
-OceanServerSystem::batteries() const
-{
- return batteries_;
-}
-
-// write access to single battery
-SmartBattery&
-OceanServerSystem::battery( unsigned int batteryNumber )
-{
- map<int,SmartBattery>::iterator it = batteries_.find(batteryNumber);
- if ( it==batteries_.end() )
- {
- // we don't have it, so instantiate a new one
- SmartBattery b;
- batteries_[batteryNumber] = b;
- return batteries_[batteryNumber];
- }
-
- return it->second;
-}
-
-// read access to single battery
-const SmartBattery&
-OceanServerSystem::battery( unsigned int batteryNumber ) const
-{
- map<int,SmartBattery>::const_iterator it = batteries_.find(batteryNumber);
- if ( it==batteries_.end() )
- {
- stringstream ss;
- ss << "Trying to read from non-existent battery " << batteryNumber;
- throw ParsingException( ERROR_INFO, ss.str().c_str() );
- }
- return it->second;
-}
-
-void
-OceanServerSystem::eraseBattery( unsigned int batteryNumber )
-{
- batteries_.erase( batteries_.find( batteryNumber ) );
-}
-
-//
-// OceanServerParser member functions
-//
-
OceanServerParser::OceanServerParser( gbxutilacfr::Tracer &tracer )
: tracer_(tracer)
{
@@ -488,25 +267,29 @@
//
// Debugging output
//
- stringstream ss;
- ss << "OceanServerParser: Received the following input: " << endl;
- for (unsigned int i=0; i<stringList.size(); i++)
+ const int debugLevel = 10;
+ if (tracer_.verbosity( gbxutilacfr::Tracer::DebugTrace, gbxutilacfr::Tracer::ToAny ) >= debugLevel)
{
- const string &str = stringList[i];
- ss << i << ": " << str;
-
- // output in hex
- ss << i << " hex: ";
- for (unsigned k=0; k<str.size(); k++)
+ stringstream ss;
+ ss << "OceanServerParser: Received the following input: " << endl;
+ for (unsigned int i=0; i<stringList.size(); i++)
{
- unsigned int charValue = (unsigned int)str[k];
- ss << str[k] << ": " << std::hex << charValue << " ";
+ const string &str = stringList[i];
+ ss << i << ": " << str;
+
+ // output in hex
+ ss << i << " hex: ";
+ for (unsigned k=0; k<str.size(); k++)
+ {
+ unsigned int charValue = (unsigned int)str[k];
+ ss << str[k] << ": " << std::hex << charValue << " ";
+ }
+ ss << std::dec;
+
}
- ss << std::dec;
-
+ ss << endl;
+ tracer_.debug( ss.str(), debugLevel );
}
- ss << endl;
- tracer_.debug( ss.str(), 10 );
//
// Parsing
Modified: gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverparser.h
===================================================================
--- gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverparser.h 2008-06-29 03:49:54 UTC (rev 231)
+++ gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverparser.h 2008-06-30 12:01:10 UTC (rev 232)
@@ -13,69 +13,11 @@
#include <map>
#include <gbxutilacfr/tracer.h>
-#include <gbxsmartbatteryacfr/smartbattery.h>
+#include <gbxsmartbatteryacfr/oceanserversystem.h>
namespace gbxsmartbatteryacfr
{
-
-//!
-//! Class representing the OceanServer battery system data
-//! Contains average values of the whole system and values from individual batteries
-//!
-//! @author Tobias Kaupp
-//!
-class OceanServerSystem
-{
- public:
-
- OceanServerSystem();
- ~OceanServerSystem() {};
-
- //! Read access to all batteries
- const std::map<int,SmartBattery>& batteries() const;
-
- //! Easy write access to single battery, instantiates a new one if it doesn't exist
- SmartBattery& battery( unsigned int batteryNumber );
-
- //! Easy read access to single battery, battery must exist
- const SmartBattery& battery( unsigned int batteryNumber ) const;
-
- //! Erase a battery
- void eraseBattery( unsigned int batteryNumber );
-
- //! Average battery values
- int percentCharge;
- int minToEmpty;
- std::string messageToSystem;
-
- //! Battery module states
- //! Each vector is always of size 8 because oceanserver system has 8 slots
- std::vector<bool> availableBatteries;
- std::vector<bool> chargingStates;
- std::vector<bool> supplyingPowerStates;
- std::vector<bool> chargePowerPresentStates;
- std::vector<bool> powerNoGoodStates;
- std::vector<bool> chargeInhibitedStates;
-
- private:
-
- // key: slot number, data: a single smart battery module
- std::map<int,SmartBattery> batteries_;
-};
-
-//! Puts OceanServerSystem data into a human-readable string
-std::string toString( const OceanServerSystem &system );
-
-//! Puts OceanServerSystem data into a machine-readable ASCII string
-std::string toLogString( const OceanServerSystem &system );
-//! Updates all fields in 'to' with data from 'from'. Also reapes batteries in 'to' if they are not in 'from'.
-//! Has persistence capabilities: if fields in 'from' are not set and corresponding fields in 'to' are set, the ones in 'to' are kept.
-//! Use case: a class stores 'to' as a member variable, receives the latest records into 'from', calls this function to update 'to'.
-//! The reaping capability makes sure that battery modules which are no longer connected don't persist.
-void updateWithNewData( const OceanServerSystem &from,
- OceanServerSystem &to );
-
//!
//! Class to parse the hex data the oceanserver battery controller spits out
//!
Added: gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserversystem.cpp
===================================================================
--- gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserversystem.cpp (rev 0)
+++ gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserversystem.cpp 2008-06-30 12:01:10 UTC (rev 232)
@@ -0,0 +1,239 @@
+/*
+ * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
+ * http://gearbox.sf.net/
+ * Copyright (c) 2004-2008 Tobias Kaupp
+ *
+ * This distribution is licensed to you under the terms described in
+ * the LICENSE file included in this distribution.
+ *
+ */
+
+#include <sstream>
+#include <cstring>
+#include <gbxsmartbatteryacfr/exceptions.h>
+
+#include "oceanserversystem.h"
+
+using namespace std;
+
+namespace gbxsmartbatteryacfr {
+
+//
+// Helper functions
+//
+
+string toString( const vector<bool> &flags )
+{
+ stringstream ss;
+ for (unsigned int i=0; i<flags.size(); i++)
+ {
+ ss << flags[i] << " ";
+ }
+ return ss.str();
+}
+
+string toLogString( const vector<bool> &flags )
+{
+ stringstream ss;
+ for (unsigned int i=0; i<flags.size(); i++)
+ {
+ ss << flags[i];
+ }
+ return ss.str();
+}
+
+
+//
+// Non-member functions
+//
+
+string toString( const OceanServerSystem &system )
+{
+ stringstream ss;
+ ss << "Charge: \t" << system.percentCharge << endl;
+ ss << "Minutes to empty:\t" << system.minToEmpty << endl;
+ ss << "Available batt.: \t" << toString( system.availableBatteries ) << endl;
+ ss << "Charging: \t" << toString( system.chargingStates ) << endl;
+ ss << "Supplying power: \t" << toString( system.supplyingPowerStates ) << endl;
+ ss << "Charge power: \t" << toString( system.chargePowerPresentStates ) << endl;
+ ss << "Power no good: \t" << toString( system.powerNoGoodStates ) << endl;
+ ss << "Charge inhibited:\t" << toString( system.chargeInhibitedStates ) << endl;
+
+ map<int,SmartBattery>::const_iterator it;
+ for (it=system.batteries().begin(); it!=system.batteries().end(); it++)
+ {
+ ss << "Data from battery number: " << it->first << endl;
+ ss << toString( it->second ) << endl;
+ }
+
+ return ss.str();
+}
+
+string toLogString( const OceanServerSystem &system )
+{
+ stringstream ss;
+ ss << system.percentCharge << " ";
+ ss << system.minToEmpty << " ";
+ ss << toLogString( system.availableBatteries ) << " ";
+ ss << toLogString( system.chargingStates ) << " ";
+ ss << toLogString( system.supplyingPowerStates ) << " ";
+ ss << toLogString( system.chargePowerPresentStates ) << " ";
+ ss << toLogString( system.powerNoGoodStates ) << " ";
+ ss << toLogString( system.chargeInhibitedStates ) << endl;
+
+ ss << system.batteries().size();
+
+ map<int,SmartBattery>::const_iterator it;
+ for (it=system.batteries().begin(); it!=system.batteries().end(); it++)
+ {
+ ss << it->first << " " << toLogString( it->second ) << endl;
+ }
+
+ return ss.str();
+}
+
+void updateWithNewData( const OceanServerSystem &from,
+ OceanServerSystem &to )
+{
+ typedef map<int,SmartBattery>::const_iterator BatIt;
+
+ to.availableBatteries = from.availableBatteries;
+ to.percentCharge = from.percentCharge;
+ to.minToEmpty = from.minToEmpty;
+ to.messageToSystem = from.messageToSystem;
+ to.chargingStates = from.chargingStates;
+ to.supplyingPowerStates = from.supplyingPowerStates;
+ to.chargePowerPresentStates = from.chargePowerPresentStates;
+ to.powerNoGoodStates = from.powerNoGoodStates;
+ to.chargeInhibitedStates = from.chargeInhibitedStates;
+
+ for (BatIt it=from.batteries().begin(); it!=from.batteries().end(); it++)
+ {
+ const SmartBattery &fromB = from.battery( it->first );
+ SmartBattery &toB = to.battery( it->first );
+
+ if ( fromB.has( ManufacturerAccess ) ) toB.setManufacturerAccess ( fromB.manufacturerAccess () );
+ if ( fromB.has( RemainingCapacityAlarm ) ) toB.setRemainingCapacityAlarm ( fromB.remainingCapacityAlarm () );
+ if ( fromB.has( RemainingTimeAlarm ) ) toB.setRemainingTimeAlarm ( fromB.remainingTimeAlarm () );
+ if ( fromB.has( BatteryMode ) ) toB.setBatteryMode ( fromB.batteryMode () );
+ if ( fromB.has( AtRate ) ) toB.setAtRate ( fromB.atRate () );
+ if ( fromB.has( AtRateTimeToFull ) ) toB.setAtRateTimeToFull ( fromB.atRateTimeToFull () );
+ if ( fromB.has( AtRateTimeToEmpty ) ) toB.setAtRateTimeToEmpty ( fromB.atRateTimeToEmpty () );
+ if ( fromB.has( AtRateOk ) ) toB.setAtRateOk ( fromB.atRateOk () );
+ if ( fromB.has( Temperature ) ) toB.setTemperature ( fromB.temperature () );
+ if ( fromB.has( Voltage ) ) toB.setVoltage ( fromB.voltage () );
+ if ( fromB.has( Current ) ) toB.setCurrent ( fromB.current () );
+ if ( fromB.has( AverageCurrent ) ) toB.setAverageCurrent ( fromB.averageCurrent () );
+ if ( fromB.has( MaxError ) ) toB.setMaxError ( fromB.maxError () );
+ if ( fromB.has( RelativeStateOfCharge ) ) toB.setRelativeStateOfCharge ( fromB.relativeStateOfCharge () );
+ if ( fromB.has( AbsoluteStateOfCharge ) ) toB.setAbsoluteStateOfCharge ( fromB.absoluteStateOfCharge () );
+ if ( fromB.has( RemainingCapacity ) ) toB.setRemainingCapacity ( fromB.remainingCapacity () );
+ if ( fromB.has( FullChargeCapacity ) ) toB.setFullChargeCapacity ( fromB.fullChargeCapacity () );
+ if ( fromB.has( RunTimeToEmpty ) ) toB.setRunTimeToEmpty ( fromB.runTimeToEmpty () );
+ if ( fromB.has( AverageTimeToEmpty ) ) toB.setAverageTimeToEmpty ( fromB.averageTimeToEmpty () );
+ if ( fromB.has( AverageTimeToFull ) ) toB.setAverageTimeToFull ( fromB.averageTimeToFull () );
+ if ( fromB.has( ChargingCurrent ) ) toB.setChargingCurrent ( fromB.chargingCurrent () );
+ if ( fromB.has( ChargingVoltage ) ) toB.setChargingVoltage ( fromB.chargingVoltage () );
+ if ( fromB.has( BatteryStatus ) ) toB.setBatteryStatus ( fromB.batteryStatus () );
+ if ( fromB.has( CycleCount ) ) toB.setCycleCount ( fromB.cycleCount () );
+ if ( fromB.has( DesignCapacity ) ) toB.setDesignCapacity ( fromB.designCapacity () );
+ if ( fromB.has( DesignVoltage ) ) toB.setDesignVoltage ( fromB.designVoltage () );
+ if ( fromB.has( SpecificationInfo ) ) toB.setSpecificationInfo ( fromB.specificationInfo () );
+ if ( fromB.has( ManufactureDate ) ) toB.setManufactureDate ( fromB.manufactureDate () );
+ if ( fromB.has( SerialNumber ) ) toB.setSerialNumber ( fromB.serialNumber () );
+ if ( fromB.has( ManufacturerName ) ) toB.setManufacturerName ( fromB.manufacturerName () );
+ if ( fromB.has( DeviceName ) ) toB.setDeviceName ( fromB.deviceName () );
+ if ( fromB.has( DeviceChemistry ) ) toB.setDeviceChemistry ( fromB.deviceChemistry () );
+ if ( fromB.has( ManufacturerData ) ) toB.setManufacturerData ( fromB.manufacturerData () );
+ }
+
+ // check if reaping needs to be done, if not return
+ if ( from.batteries().size() == to.batteries().size() )
+ return;
+
+ // store battery ids from batteries which need to be reaped in a vector
+ vector<int> reapingIds;
+
+ // go through all 'to' batteries and check if they are also in 'from'
+ for (BatIt it=to.batteries().begin(); it!=to.batteries().end(); it++)
+ {
+ const int batId = it->first;
+
+ BatIt itFrom = from.batteries().find( batId );
+ if ( itFrom == from.batteries().end() ) {
+ // battery is in 'to' but not in 'from' -> needs to be reaped
+ reapingIds.push_back( batId );
+ }
+ }
+
+ // reap batteries
+ for (unsigned int i=0; i<reapingIds.size(); i++) {
+ to.eraseBattery( reapingIds[i] );
+ }
+
+}
+
+
+//
+// Member functions
+//
+
+OceanServerSystem::OceanServerSystem()
+ : percentCharge(0),
+ minToEmpty(0),
+ messageToSystem("")
+{
+ // fixed number of slots for oceanserver system
+ const int NUM_BATTERY_SLOTS = 8;
+ availableBatteries.resize(NUM_BATTERY_SLOTS);
+ chargingStates.resize(NUM_BATTERY_SLOTS);
+ supplyingPowerStates.resize(NUM_BATTERY_SLOTS);
+ chargePowerPresentStates.resize(NUM_BATTERY_SLOTS);
+ powerNoGoodStates.resize(NUM_BATTERY_SLOTS);
+ chargeInhibitedStates.resize(NUM_BATTERY_SLOTS);
+}
+
+// read access to all batteries
+const map<int,SmartBattery>&
+OceanServerSystem::batteries() const
+{
+ return batteries_;
+}
+
+// write access to single battery
+SmartBattery&
+OceanServerSystem::battery( unsigned int batteryNumber )
+{
+ map<int,SmartBattery>::iterator it = batteries_.find(batteryNumber);
+ if ( it==batteries_.end() )
+ {
+ // we don't have it, so instantiate a new one
+ SmartBattery b;
+ batteries_[batteryNumber] = b;
+ return batteries_[batteryNumber];
+ }
+
+ return it->second;
+}
+
+// read access to single battery
+const SmartBattery&
+OceanServerSystem::battery( unsigned int batteryNumber ) const
+{
+ map<int,SmartBattery>::const_iterator it = batteries_.find(batteryNumber);
+ if ( it==batteries_.end() )
+ {
+ stringstream ss;
+ ss << "Trying to read from non-existent battery " << batteryNumber;
+ throw ParsingException( ERROR_INFO, ss.str().c_str() );
+ }
+ return it->second;
+}
+
+void
+OceanServerSystem::eraseBattery( unsigned int batteryNumber )
+{
+ batteries_.erase( batteries_.find( batteryNumber ) );
+}
+
+} //namespace
\ No newline at end of file
Added: gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserversystem.h
===================================================================
--- gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserversystem.h (rev 0)
+++ gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserversystem.h 2008-06-30 12:01:10 UTC (rev 232)
@@ -0,0 +1,80 @@
+/*
+ * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
+ * http://gearbox.sf.net/
+ * Copyright (c) 2004-2008 Tobias Kaupp
+ *
+ * This distribution is licensed to you under the terms described in
+ * the LICENSE file included in this distribution.
+ *
+ */
+
+#ifndef GBX_OCEANSERVER_SYSTEM_H
+#define GBX_OCEANSERVER_SYSTEM_H
+
+#include <map>
+#include <gbxsmartbatteryacfr/smartbattery.h>
+
+namespace gbxsmartbatteryacfr
+{
+
+//!
+//! Class representing the OceanServer battery system data
+//! Contains average values of the whole system and values from individual batteries
+//!
+//! @author Tobias Kaupp
+//!
+class OceanServerSystem
+{
+ public:
+
+ OceanServerSystem();
+ ~OceanServerSystem() {};
+
+ //! Read access to all batteries
+ const std::map<int,SmartBattery>& batteries() const;
+
+ //! Easy write access to single battery, instantiates a new one if it doesn't exist
+ SmartBattery& battery( unsigned int batteryNumber );
+
+ //! Easy read access to single battery, battery must exist
+ const SmartBattery& battery( unsigned int batteryNumber ) const;
+
+ //! Erase a battery
+ void eraseBattery( unsigned int batteryNumber );
+
+ //! Average battery values
+ int percentCharge;
+ int minToEmpty;
+ std::string messageToSystem;
+
+ //! Battery module states
+ //! Each vector is always of size 8 because oceanserver system has 8 slots
+ std::vector<bool> availableBatteries;
+ std::vector<bool> chargingStates;
+ std::vector<bool> supplyingPowerStates;
+ std::vector<bool> chargePowerPresentStates;
+ std::vector<bool> powerNoGoodStates;
+ std::vector<bool> chargeInhibitedStates;
+
+ private:
+
+ // key: slot number, data: a single smart battery module
+ std::map<int,SmartBattery> batteries_;
+};
+
+//! Puts OceanServerSystem data into a human-readable string
+std::string toString( const OceanServerSystem &system );
+
+//! Puts OceanServerSystem data into a machine-readable ASCII string
+std::string toLogString( const OceanServerSystem &system );
+
+//! Updates all fields in 'to' with data from 'from'. Also reapes batteries in 'to' if they are not in 'from'.
+//! Has persistence capabilities: if fields in 'from' are not set and corresponding fields in 'to' are set, the ones in 'to' are kept.
+//! Use case: a class stores 'to' as a member variable, receives the latest records into 'from', calls this function to update 'to'.
+//! The reaping capability makes sure that battery modules which are no longer connected don't persist.
+void updateWithNewData( const OceanServerSystem &from,
+ OceanServerSystem &to );
+
+} // namespace
+
+#endif
Modified: gearbox/trunk/submitted/gbxsmartbatteryacfr/smartbattery.cpp
===================================================================
--- gearbox/trunk/submitted/gbxsmartbatteryacfr/smartbattery.cpp 2008-06-29 03:49:54 UTC (rev 231)
+++ gearbox/trunk/submitted/gbxsmartbatteryacfr/smartbattery.cpp 2008-06-30 12:01:10 UTC (rev 232)
@@ -10,6 +10,7 @@
#include <iostream>
#include <sstream>
+#include <gbxsmartbatteryacfr/exceptions.h>
#include "smartbattery.h"
@@ -52,8 +53,12 @@
else if (fieldStr=="21") return DeviceName;
else if (fieldStr=="22") return DeviceChemistry;
else if (fieldStr=="23") return ManufacturerData;
- else cout << "ERROR: Unknown field: " << fieldStr << endl;
-
+ else
+ {
+ stringstream ss;
+ ss << "Unknown field: " << fieldStr;
+ throw ParsingException( ERROR_INFO, ss.str().c_str() );
+ }
return NUM_SMARTBATTERY_FIELDS;
}
Modified: gearbox/trunk/submitted/gbxsmartbatteryacfr/smartbatteryacfr.dox
===================================================================
--- gearbox/trunk/submitted/gbxsmartbatteryacfr/smartbatteryacfr.dox 2008-06-29 03:49:54 UTC (rev 231)
+++ gearbox/trunk/submitted/gbxsmartbatteryacfr/smartbatteryacfr.dox 2008-06-30 12:01:10 UTC (rev 232)
@@ -24,8 +24,8 @@
@endverbatim
@par Examples
-- See 'test/simpletest.cpp' for a simple example of how to use the library.
-- See 'test/fancytest.cpp' for a more advanced example.
+- See 'test/simpletest.cpp' for a simple example of how to use the library. The test reads a few records from the oceanserver system and prints the results on the screen.
+- See 'test/fancytest.cpp' for a more advanced example. The test uses a class to read a few records from the oceanserver system and stores an internal (full) record which only gets updated with new data.
@par Style
See http://orca-robotics.sourceforge.net/orca/orca_doc_style.html
Modified: gearbox/trunk/submitted/gbxsmartbatteryacfr/test/CMakeLists.txt
===================================================================
--- gearbox/trunk/submitted/gbxsmartbatteryacfr/test/CMakeLists.txt 2008-06-29 03:49:54 UTC (rev 231)
+++ gearbox/trunk/submitted/gbxsmartbatteryacfr/test/CMakeLists.txt 2008-06-30 12:01:10 UTC (rev 232)
@@ -1,13 +1,15 @@
INCLUDE( ${GBX_CMAKE_DIR}/UseBasicRules.cmake )
-GBX_ADD_EXECUTABLE( gbxsmartbatterychecksumtest checksumtest.cpp )
-GBX_ADD_TEST( GbxSmartBatteryChecksumTest gbxsmartbatterychecksumtest )
-
GBX_ADD_EXECUTABLE( gbxsmartbatterysimpletest simpletest.cpp )
TARGET_LINK_LIBRARIES( gbxsmartbatterysimpletest GbxSmartBatteryAcfr )
GBX_ADD_EXECUTABLE( gbxsmartbatteryfancytest fancytest.cpp )
TARGET_LINK_LIBRARIES( gbxsmartbatteryfancytest GbxSmartBatteryAcfr )
-GBX_ADD_EXAMPLE( gbxsmartbatteryacfr example.cmake.in example.cmake checksumtest.cpp simpletest.cpp fancytest.cpp example.readme )
+GBX_ADD_EXAMPLE( gbxsmartbatteryacfr example.cmake.in example.cmake simpletest.cpp fancytest.cpp example.readme )
+IF( GBX_BUILD_TESTS )
+ ADD_SUBDIRECTORY( darttest )
+ENDIF( GBX_BUILD_TESTS )
+
+
Deleted: gearbox/trunk/submitted/gbxsmartbatteryacfr/test/checksumtest.cpp
===================================================================
--- gearbox/trunk/submitted/gbxsmartbatteryacfr/test/checksumtest.cpp 2008-06-29 03:49:54 UTC (rev 231)
+++ gearbox/trunk/submitted/gbxsmartbatteryacfr/test/checksumtest.cpp 2008-06-30 12:01:10 UTC (rev 232)
@@ -1,94 +0,0 @@
-#include <iostream>
-#include <sstream>
-
-using namespace std;
-
-template < typename T >
-inline T highbit(T& t)
-{
- return t = (((T)(-1)) >> 1) + 1;
-}
-
-template < typename T >
-std::ostream& bin(T& value, std::ostream &o)
-{
- for ( T bit = highbit(bit); bit; bit >>= 1 )
- {
- o << ( ( value & bit ) ? '1' : '0' );
- }
- return o;
-}
-
-// Simple checksum test using XOR
-int main( int argc, char **argv )
-{
- int opt;
- std::string input = "B18,0A,0000";
- std::string resultHex = "3A";
-
- // Get some options from the command line
- while ((opt = getopt(argc, argv, "i:r:")) != -1)
- {
- switch ( opt )
- {
- case 'i':
- input = optarg;
- break;
- case 'r':
- resultHex = optarg;
- break;
- default:
- cout << "Usage: " << argv[0] << " [-i input] [-r resultInHex]" << endl
- << "-i input\tInput string. E.g. B18,0A,0000" << endl
- << "-r resultInHex\tResult in hex. E.g. 3A "<< endl;
- return 1;
- }
- }
-
- cout << endl << "Checksum is computed using XOR" << endl;
- cout << endl << "Input string is:\t\t " << input << endl;
- cout << "Expected checksum result is:\t " << resultHex << endl << endl;
-
- unsigned int checksum = 0;
-
- cout << "Table: " << endl
- << "char\tdec\thex\tbin" << endl
- << "----------------------------------" << endl;
-
- for (unsigned int i=0; i<input.size(); i++)
- {
- unsigned int charValue = (unsigned int)input[i];
- cout << input[i] << "\t"
- << std::dec << charValue << "\t"
- << std::hex << charValue << "\t";
- bin(charValue, cout);
- cout << endl;
- //checksum computation
- checksum ^= charValue;
- }
-
- cout << endl;
- cout << "Computed checksum (dec,hex,bin): " << std::dec << checksum << ", " << std::hex << checksum << ", ";
- bin(checksum, cout); cout << endl << endl;
-
- stringstream ss;
- ss << std::hex << checksum;
-
- string checksumStr = ss.str();
- for (unsigned int i=0; i<checksumStr.size(); i++)
- checksumStr[i] = toupper( checksumStr[i] );
-
- cout << "Expected checksum result is:\t" << resultHex << endl;
- cout << "Computed checksum result is:\t" << checksumStr << endl << endl;
-
- int passTest = checksumStr.compare( resultHex );
- if (passTest!=0) {
- cout << "Test not passed" << endl;
- return 1;
- }
- cout << "Test passed" << endl;
-
- return 0;
-}
-
-
Added: gearbox/trunk/submitted/gbxsmartbatteryacfr/test/darttest/CMakeLists.txt
===================================================================
--- gearbox/trunk/submitted/gbxsmartbatteryacfr/test/darttest/CMakeLists.txt (rev 0)
+++ gearbox/trunk/submitted/gbxsmartbatteryacfr/test/darttest/CMakeLists.txt 2008-06-30 12:01:10 UTC (rev 232)
@@ -0,0 +1,4 @@
+INCLUDE( ${GBX_CMAKE_DIR}/UseBasicRules.cmake )
+
+GBX_ADD_EXECUTABLE( gbxsmartbatterychecksumtest checksumtest.cpp )
+GBX_ADD_TEST( GbxSmartBatteryChecksumTest gbxsmartbatterychecksumtest )
\ No newline at end of file
Added: gearbox/trunk/submitted/gbxsmartbatteryacfr/test/darttest/checksumtest.cpp
===================================================================
--- gearbox/trunk/submitted/gbxsmartbatteryacfr/test/darttest/checksumtest.cpp (rev 0)
+++ gearbox/trunk/submitted/gbxsmartbatteryacfr/test/darttest/checksumtest.cpp 2008-06-30 12:01:10 UTC (rev 232)
@@ -0,0 +1,94 @@
+#include <iostream>
+#include <sstream>
+
+using namespace std;
+
+template < typename T >
+inline T highbit(T& t)
+{
+ return t = (((T)(-1)) >> 1) + 1;
+}
+
+template < typename T >
+std::ostream& bin(T& value, std::ostream &o)
+{
+ for ( T bit = highbit(bit); bit; bit >>= 1 )
+ {
+ o << ( ( value & bit ) ? '1' : '0' );
+ }
+ return o;
+}
+
+// Simple checksum test using XOR
+int main( int argc, char **argv )
+{
+ int opt;
+ std::string input = "B18,0A,0000";
+ std::string resultHex = "3A";
+
+ // Get some options from the command line
+ while ((opt = getopt(argc, argv, "i:r:")) != -1)
+ {
+ switch ( opt )
+ {
+ case 'i':
+ input = optarg;
+ break;
+ case 'r':
+ resultHex = optarg;
+ break;
+ default:
+ cout << "Usage: " << argv[0] << " [-i input] [-r resultInHex]" << endl
+ << "-i input\tInput string. E.g. B18,0A,0000" << endl
+ << "-r resultInHex\tResult in hex. E.g. 3A "<< endl;
+ return 1;
+ }
+ }
+
+ cout << endl << "Checksum is computed using XOR" << endl;
+ cout << endl << "Input string is:\t\t " << input << endl;
+ cout << "Expected checksum result is:\t " << resultHex << endl << endl;
+
+ unsigned int checksum = 0;
+
+ cout << "Table: " << endl
+ << "char\tdec\thex\tbin" << endl
+ << "----------------------------------" << endl;
+
+ for (unsigned int i=0; i<input.size(); i++)
+ {
+ unsigned int charValue = (unsigned int)input[i];
+ cout << input[i] << "\t"
+ << std::dec << charValue << "\t"
+ << std::hex << charValue << "\t";
+ bin(charValue, cout);
+ cout << endl;
+ //checksum computation
+ checksum ^= charValue;
+ }
+
+ cout << endl;
+ cout << "Computed checksum (dec,hex,bin): " << std::dec << checksum << ", " << std::hex << checksum << ", ";
+ bin(checksum, cout); cout << endl << endl;
+
+ stringstream ss;
+ ss << std::hex << checksum;
+
+ string checksumStr = ss.str();
+ for (unsigned int i=0; i<checksumStr.size(); i++)
+ checksumStr[i] = toupper( checksumStr[i] );
+
+ cout << "Expected checksum result is:\t" << resultHex << endl;
+ cout << "Computed checksum result is:\t" << checksumStr << endl << endl;
+
+ int passTest = checksumStr.compare( resultHex );
+ if (passTest!=0) {
+ cout << "Test not passed" << endl;
+ return 1;
+ }
+ cout << "Test passed" << endl;
+
+ return 0;
+}
+
+
Modified: gearbox/trunk/submitted/gbxsmartbatteryacfr/test/example.cmake.in
===================================================================
--- gearbox/trunk/submitted/gbxsmartbatteryacfr/test/example.cmake.in 2008-06-29 03:49:54 UTC (rev 231)
+++ gearbox/trunk/submitted/gbxsmartbatteryacfr/test/example.cmake.in 2008-06-30 12:01:10 UTC (rev 232)
@@ -1,13 +1,6 @@
PROJECT( gbxsmartbatteryacfr_example )
INCLUDE_DIRECTORIES( @CMAKE_INSTALL_PREFIX@/include/gearbox )
-
-ADD_EXECUTABLE( gbxsmartbatteryacfrchecksum checksumtest.cpp )
-TARGET_LINK_LIBRARIES( gbxsmartbatteryacfrchecksum GbxSmartBatteryAcfr )
-SET_TARGET_PROPERTIES( gbxsmartbatteryacfrchecksum PROPERTIES
- LINK_FLAGS "-L@CMAKE_INSTALL_PREFIX@/lib/gearbox"
- INSTALL_RPATH "${INSTALL_RPATH};@CMAKE_INSTALL_PREFIX@/lib/gearbox"
- BUILD_WITH_INSTALL_RPATH TRUE )
ADD_EXECUTABLE( gbxsmartbatteryacfrsimple simpletest.cpp )
TARGET_LINK_LIBRARIES( gbxsmartbatteryacfrsimple GbxSmartBatteryAcfr )
Modified: gearbox/trunk/submitted/gbxsmartbatteryacfr/test/example.readme
===================================================================
--- gearbox/trunk/submitted/gbxsmartbatteryacfr/test/example.readme 2008-06-29 03:49:54 UTC (rev 231)
+++ gearbox/trunk/submitted/gbxsmartbatteryacfr/test/example.readme 2008-06-30 12:01:10 UTC (rev 232)
@@ -9,4 +9,10 @@
$ cd ~
$ mkdir gbxsmartbatteryacfr_example
$ cd gbxsmartbatteryacfr_example
-$ ccmake /usr/local/share/gearbox/gbxsmartbatteryacfr
+$ ccmake /usr/local/share/gearbox/gbxsmartbatteryacfr
+(press 'c' and 'g')
+$ make
+$ ./gbxsmartbatteryacfrsimple
+$ ./gbxsmartbatteryacfrfancy
+
+See the library documentation (smartbatteryacfr.dox) for a description of the tests.
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rum...@us...> - 2008-06-29 03:49:47
|
Revision: 231
http://gearbox.svn.sourceforge.net/gearbox/?rev=231&view=rev
Author: rumataxyz
Date: 2008-06-28 20:49:54 -0700 (Sat, 28 Jun 2008)
Log Message:
-----------
fixed logic error (operator precedence)
Modified Paths:
--------------
gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/receiverstatusdecoder.h
Modified: gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/receiverstatusdecoder.h
===================================================================
--- gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/receiverstatusdecoder.h 2008-06-27 08:33:39 UTC (rev 230)
+++ gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/receiverstatusdecoder.h 2008-06-29 03:49:54 UTC (rev 231)
@@ -15,7 +15,7 @@
#include <sstream>
namespace gbxnovatelutilacfr{
bool receiverStatusIsGood(uint32_t receiverStatus){
- return 0 == receiverStatus & 0xe1fe8fef;
+ return 0 == (receiverStatus & 0xe1fe8fef);
}
//bool receiverStatusIsWarning(uint32_t receiverStatus){
//}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rum...@us...> - 2008-06-27 08:33:33
|
Revision: 230
http://gearbox.svn.sourceforge.net/gearbox/?rev=230&view=rev
Author: rumataxyz
Date: 2008-06-27 01:33:39 -0700 (Fri, 27 Jun 2008)
Log Message:
-----------
another gcc4.3 fix
Modified Paths:
--------------
gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/serialconnectivity.h
Modified: gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/serialconnectivity.h
===================================================================
--- gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/serialconnectivity.h 2008-06-26 10:21:46 UTC (rev 229)
+++ gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/serialconnectivity.h 2008-06-27 08:33:39 UTC (rev 230)
@@ -7,7 +7,9 @@
* the LICENSE file included in this distribution.
*
*/
-class gbxserialacfr::Serial;
+namespace gbxserialacfr{
+ class Serial;
+}
// test connectivity to a [serial] device at a [baudrate];
// Assumes that you can figure out a [challenge] (command ...) to
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rum...@us...> - 2008-06-26 10:21:38
|
Revision: 229
http://gearbox.svn.sourceforge.net/gearbox/?rev=229&view=rev
Author: rumataxyz
Date: 2008-06-26 03:21:46 -0700 (Thu, 26 Jun 2008)
Log Message:
-----------
fixed logic bugs (assumed wrong precedence)
Modified Paths:
--------------
gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/imudecoder.cpp
gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/receiverstatusdecoder.h
Modified: gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/imudecoder.cpp
===================================================================
--- gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/imudecoder.cpp 2008-06-26 10:20:23 UTC (rev 228)
+++ gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/imudecoder.cpp 2008-06-26 10:21:46 UTC (rev 229)
@@ -18,7 +18,7 @@
public:
virtual ~ImuDecoderHg1700(){}
inline bool statusIsGood(const uint32_t imuStatus){
- return 0 == (imuStatus & 0xf8000010);
+ return 0 == ((imuStatus & 0xf8000010));
}
std::string statusToString(const uint32_t imuStatus);
};
@@ -42,25 +42,25 @@
ImuDecoderHg1700::statusToString(const uint32_t imuStatus){
std::stringstream ss;
ss << "IMU test: "
- << (( 0 == imuStatus & 0x00000010) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x00000010)) ? "pass;" : "fail;") << " ";
ss << "Z-gyro path-length control: "
- << (( 0 == imuStatus & 0x00000020) ? "good;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x00000020)) ? "good;" : "fail;") << " ";
ss << "Y-gyro path-length control: "
- << (( 0 == imuStatus & 0x00000040) ? "good;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x00000040)) ? "good;" : "fail;") << " ";
ss << "X-gyro path-length control: "
- << (( 0 == imuStatus & 0x00000080) ? "good;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x00000080)) ? "good;" : "fail;") << " ";
ss << "Gyro tests: "
- << (( 0 == imuStatus & 0x08000000) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x08000000)) ? "pass;" : "fail;") << " ";
ss << "Accelerometer tests: "
- << (( 0 == imuStatus & 0x10000000) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x10000000)) ? "pass;" : "fail;") << " ";
ss << "Other test: "
- << (( 0 == imuStatus & 0x20000000) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x20000000)) ? "pass;" : "fail;") << " ";
ss << "Memory test: "
- << (( 0 == imuStatus & 0x40000000) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x40000000)) ? "pass;" : "fail;") << " ";
ss << "Processor test: "
- << (( 0 == imuStatus & 0x80000000) ? "pass;" : "fail;") << " ";
- int temprAccel = (imuStatus & 0x0000ff00) >> 8;
- int softwVersion = (imuStatus & 0x00ff0000) >> 16;
+ << (( 0 == (imuStatus & 0x80000000)) ? "pass;" : "fail;") << " ";
+ int temprAccel = ((imuStatus & 0x0000ff00)) >> 8;
+ int softwVersion = ((imuStatus & 0x00ff0000)) >> 16;
ss << "Accelerometer temprature [C]: " << temprAccel << " ";
ss << "Software version number: " << softwVersion;
@@ -75,7 +75,7 @@
std::cout << "This driver has _not_ been tested with this IMU!\n";
};
inline bool statusIsGood(const uint32_t imuStatus){
- return 0 == (imuStatus & 0xefec9580);
+ return 0 == ((imuStatus & 0xefec9580));
}
std::string statusToString(const uint32_t imuStatus);
};
@@ -84,45 +84,45 @@
ImuDecoderImarFsas::statusToString(const uint32_t imuStatus){
std::stringstream ss;
ss << "Gyro warm-up: "
- << (( 0 == imuStatus & 0x00000010 ) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x00000010 )) ? "pass;" : "fail;") << " ";
ss << "Gyro self-test active: "
- << (( 0 == imuStatus & 0x00000020 ) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x00000020 )) ? "pass;" : "fail;") << " ";
ss << "Gyro status bit set: "
- << (( 0 == imuStatus & 0x00000040 ) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x00000040 )) ? "pass;" : "fail;") << " ";
ss << "Gyro time-out command interface: "
- << (( 0 == imuStatus & 0x00000080 ) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x00000080 )) ? "pass;" : "fail;") << " ";
ss << "Power-up built-in test (PBIT): "
- << (( 0 == imuStatus & 0x00000100 ) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x00000100 )) ? "pass;" : "fail;") << " ";
ss << "Interrupt: "
- << (( 0 == imuStatus & 0x00000400 ) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x00000400 )) ? "pass;" : "fail;") << " ";
ss << "Warm-up: "
- << (( 0 == imuStatus & 0x00001000 ) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x00001000 )) ? "pass;" : "fail;") << " ";
ss << "Initiated built-in test (IBIT): "
- << (( 0 == imuStatus & 0x00008000 ) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x00008000 )) ? "pass;" : "fail;") << " ";
ss << "Accelerometer: "
- << (( 0 == imuStatus & 0x00040000 ) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x00040000 )) ? "pass;" : "fail;") << " ";
ss << "Accelerometer time-out: "
- << (( 0 == imuStatus & 0x00080000 ) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x00080000 )) ? "pass;" : "fail;") << " ";
ss << "Gyro initiated BIT: "
- << (( 0 == imuStatus & 0x00200000 ) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x00200000 )) ? "pass;" : "fail;") << " ";
ss << "Gyro self-test: "
- << (( 0 == imuStatus & 0x00400000 ) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x00400000 )) ? "pass;" : "fail;") << " ";
ss << "Gyro time-out: "
- << (( 0 == imuStatus & 0x00800000 ) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x00800000 )) ? "pass;" : "fail;") << " ";
ss << "Analog-to-Digital (AD): "
- << (( 0 == imuStatus & 0x01000000 ) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x01000000 )) ? "pass;" : "fail;") << " ";
ss << "Testmode: "
- << (( 0 == imuStatus & 0x02000000 ) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x02000000 )) ? "pass;" : "fail;") << " ";
ss << "Software: "
- << (( 0 == imuStatus & 0x04000000 ) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x04000000 )) ? "pass;" : "fail;") << " ";
ss << "RAM/ROM: "
- << (( 0 == imuStatus & 0x08000000 ) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x08000000 )) ? "pass;" : "fail;") << " ";
ss << "Operational: "
- << (( 0 == imuStatus & 0x20000000 ) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x20000000 )) ? "pass;" : "fail;") << " ";
ss << "Interface: "
- << (( 0 == imuStatus & 0x40000000 ) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x40000000 )) ? "pass;" : "fail;") << " ";
ss << "Interface time-out: "
- << (( 0 == imuStatus & 0x80000000 ) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x80000000 )) ? "pass;" : "fail;") << " ";
return ss.str();
}
@@ -136,7 +136,7 @@
std::cout << "This driver has _not_ been tested with this IMU!\n";
};
inline bool statusIsGood(const uint32_t imuStatus){
- return 0 == (imuStatus & 0xf8000010);
+ return 0 == ((imuStatus & 0xf8000010));
}
std::string statusToString(const uint32_t imuStatus);
};
@@ -146,47 +146,47 @@
std::stringstream ss;
ss << "Delta_velocity_counter: "
- << (( 0 == imuStatus & 0x00000001 ) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x00000001 )) ? "pass;" : "fail;") << " ";
ss << "D/A_converter: "
- << (( 0 == imuStatus & 0x00000002 ) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x00000002 )) ? "pass;" : "fail;") << " ";
ss << "Gyro: "
- << (( 0 == imuStatus & 0x00000004 ) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x00000004 )) ? "pass;" : "fail;") << " ";
ss << "Accelerometer: "
- << (( 0 == imuStatus & 0x00000008 ) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x00000008 )) ? "pass;" : "fail;") << " ";
ss << "Gyro_loop_control: "
- << (( 0 == imuStatus & 0x00000010 ) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x00000010 )) ? "pass;" : "fail;") << " ";
ss << "Gyro_temperature_control: "
- << (( 0 == imuStatus & 0x00000020 ) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x00000020 )) ? "pass;" : "fail;") << " ";
ss << "Accelerometer_temperature: "
- << (( 0 == imuStatus & 0x00000040 ) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x00000040 )) ? "pass;" : "fail;") << " ";
ss << "Accelerometer_temperature: "
- << (( 0 == imuStatus & 0x00000040 ) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x00000040 )) ? "pass;" : "fail;") << " ";
ss << "A/D_converter: "
- << (( 0 == imuStatus & 0x00000100 ) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x00000100 )) ? "pass;" : "fail;") << " ";
ss << "Serial_I/O: "
- << (( 0 == imuStatus & 0x00000200 ) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x00000200 )) ? "pass;" : "fail;") << " ";
ss << "Serial_I/O: "
- << (( 0 == imuStatus & 0x00000200 ) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x00000200 )) ? "pass;" : "fail;") << " ";
ss << "Laser_diode: "
- << (( 0 == imuStatus & 0x00000800 ) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x00000800 )) ? "pass;" : "fail;") << " ";
ss << "Thermo-electric_cooler_(TEC): "
- << (( 0 == imuStatus & 0x00001000 ) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x00001000 )) ? "pass;" : "fail;") << " ";
ss << "Broadband_Fiber_Source_(BFS)_fiber_temperature: "
- << (( 0 == imuStatus & 0x00002000 ) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x00002000 )) ? "pass;" : "fail;") << " ";
ss << "Optical_receiver: "
- << (( 0 == imuStatus & 0x00004000 ) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x00004000 )) ? "pass;" : "fail;") << " ";
ss << "Gyro_accuracy: "
- << (( 0 == imuStatus & 0x01000000 ) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x01000000 )) ? "pass;" : "fail;") << " ";
ss << "Gyro: "
- << (( 0 == imuStatus & 0x02000000 ) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x02000000 )) ? "pass;" : "fail;") << " ";
ss << "Shut_down_on_failure: "
- << (( 0 == imuStatus & 0x04000000 ) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x04000000 )) ? "pass;" : "fail;") << " ";
ss << "Fast_start: "
- << (( 0 == imuStatus & 0x08000000 ) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x08000000 )) ? "pass;" : "fail;") << " ";
ss << "Commanded_bit_in_progress: "
- << (( 0 == imuStatus & 0x10000000 ) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x10000000 )) ? "pass;" : "fail;") << " ";
ss << "Accelerometer_data: "
- << (( 0 == imuStatus & 0x40000000 ) ? "pass;" : "fail;") << " ";
+ << (( 0 == (imuStatus & 0x40000000 )) ? "pass;" : "fail;") << " ";
return ss.str();
}
Modified: gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/receiverstatusdecoder.h
===================================================================
--- gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/receiverstatusdecoder.h 2008-06-26 10:20:23 UTC (rev 228)
+++ gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/receiverstatusdecoder.h 2008-06-26 10:21:46 UTC (rev 229)
@@ -24,51 +24,51 @@
std::string receiverStatusToString(uint32_t receiverStatus){
std::stringstream ss;
ss << "Error flag: "
- << ((0 == receiverStatus & 0x00000001) ? "No error" : "Error") << "; ";
+ << ((0 == (receiverStatus & 0x00000001)) ? "No error" : "Error") << "; ";
ss << "Temperature status: "
- << ((0 == receiverStatus & 0x00000002) ? "Within specifications" : "Warning") << "; ";
+ << ((0 == (receiverStatus & 0x00000002)) ? "Within specifications" : "Warning") << "; ";
ss << "Voltage supply status: "
- << ((0 == receiverStatus & 0x00000004) ? "OK" : "Warning") << "; ";
+ << ((0 == (receiverStatus & 0x00000004)) ? "OK" : "Warning") << "; ";
ss << "Antenna power status: "
- << ((0 == receiverStatus & 0x00000008) ? "Powered" : "Not powered") << "; ";
+ << ((0 == (receiverStatus & 0x00000008)) ? "Powered" : "Not powered") << "; ";
ss << "Antenna open flag: "
- << ((0 == receiverStatus & 0x00000020) ? "OK" : "Open") << "; ";
+ << ((0 == (receiverStatus & 0x00000020)) ? "OK" : "Open") << "; ";
ss << "Antenna shorted flag: "
- << ((0 == receiverStatus & 0x00000040) ? "OK" : "Shorted") << "; ";
+ << ((0 == (receiverStatus & 0x00000040)) ? "OK" : "Shorted") << "; ";
ss << "CPU overload flag: "
- << ((0 == receiverStatus & 0x00000080) ? "No overload" : "Overload") << "; ";
+ << ((0 == (receiverStatus & 0x00000080)) ? "No overload" : "Overload") << "; ";
ss << "COM1 buffer overrun flag: "
- << ((0 == receiverStatus & 0x00000100) ? "No overrun" : "Overrun") << "; ";
+ << ((0 == (receiverStatus & 0x00000100)) ? "No overrun" : "Overrun") << "; ";
ss << "COM2 buffer overrun flag: "
- << ((0 == receiverStatus & 0x00000200) ? "No overrun" : "Overrun") << "; ";
+ << ((0 == (receiverStatus & 0x00000200)) ? "No overrun" : "Overrun") << "; ";
ss << "COM3 buffer overrun flag: "
- << ((0 == receiverStatus & 0x00000400) ? "No overrun" : "Overrun") << "; ";
+ << ((0 == (receiverStatus & 0x00000400)) ? "No overrun" : "Overrun") << "; ";
ss << "USB buffer overrun flag: "
- << ((0 == receiverStatus & 0x00000800) ? "No overrun" : "Overrun") << "; ";
+ << ((0 == (receiverStatus & 0x00000800)) ? "No overrun" : "Overrun") << "; ";
ss << "RF1 AGC status: "
- << ((0 == receiverStatus & 0x00008000) ? "OK" : "Bad") << "; ";
+ << ((0 == (receiverStatus & 0x00008000)) ? "OK" : "Bad") << "; ";
ss << "RF2 AGC status: "
- << ((0 == receiverStatus & 0x00020000) ? "OK" : "Bad") << "; ";
+ << ((0 == (receiverStatus & 0x00020000)) ? "OK" : "Bad") << "; ";
ss << "Almanac flag/UTC known: "
- << ((0 == receiverStatus & 0x00040000) ? "Valid" : "Invalid") << "; ";
+ << ((0 == (receiverStatus & 0x00040000)) ? "Valid" : "Invalid") << "; ";
ss << "Position solution flag: "
- << ((0 == receiverStatus & 0x00080000) ? "Valid" : "Invalid") << "; ";
+ << ((0 == (receiverStatus & 0x00080000)) ? "Valid" : "Invalid") << "; ";
ss << "Position fixed flag: "
- << ((0 == receiverStatus & 0x00100000) ? "Not" : "fixed Fixed") << "; ";
+ << ((0 == (receiverStatus & 0x00100000)) ? "Not" : "fixed Fixed") << "; ";
ss << "Clock steering status: "
- << ((0 == receiverStatus & 0x00200000) ? "Enabled" : "Disabled") << "; ";
+ << ((0 == (receiverStatus & 0x00200000)) ? "Enabled" : "Disabled") << "; ";
ss << "Clock model flag: "
- << ((0 == receiverStatus & 0x00400000) ? "Valid" : "Invalid") << "; ";
+ << ((0 == (receiverStatus & 0x00400000)) ? "Valid" : "Invalid") << "; ";
ss << "OEMV card external oscillator flag: "
- << ((0 == receiverStatus & 0x00800000) ? "Disabled" : "Enabled") << "; ";
+ << ((0 == (receiverStatus & 0x00800000)) ? "Disabled" : "Enabled") << "; ";
ss << "Software resource: "
- << ((0 == receiverStatus & 0x01000000) ? "OK" : "Warning") << "; ";
+ << ((0 == (receiverStatus & 0x01000000)) ? "OK" : "Warning") << "; ";
ss << "Auxiliary 3 status event flag: "
- << ((0 == receiverStatus & 0x20000000) ? "No event" : "Event") << "; ";
+ << ((0 == (receiverStatus & 0x20000000)) ? "No event" : "Event") << "; ";
ss << "Auxiliary 2 status event flag: "
- << ((0 == receiverStatus & 0x40000000) ? "No event" : "Event") << "; ";
+ << ((0 == (receiverStatus & 0x40000000)) ? "No event" : "Event") << "; ";
ss << "Auxiliary 1 status event flag: "
- << ((0 == receiverStatus & 0x80000000) ? "No event" : "Event");
+ << ((0 == (receiverStatus & 0x80000000)) ? "No event" : "Event");
return ss.str();
}
}//namespace
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rum...@us...> - 2008-06-26 10:20:28
|
Revision: 228
http://gearbox.svn.sourceforge.net/gearbox/?rev=228&view=rev
Author: rumataxyz
Date: 2008-06-26 03:20:23 -0700 (Thu, 26 Jun 2008)
Log Message:
-----------
fixed array copy/print bug
Modified Paths:
--------------
gearbox/trunk/submitted/gbxnovatelacfr/driver.cpp
Modified: gearbox/trunk/submitted/gbxnovatelacfr/driver.cpp
===================================================================
--- gearbox/trunk/submitted/gbxnovatelacfr/driver.cpp 2008-06-26 10:18:37 UTC (rev 227)
+++ gearbox/trunk/submitted/gbxnovatelacfr/driver.cpp 2008-06-26 10:20:23 UTC (rev 228)
@@ -701,7 +701,12 @@
ss << "sigmaLatitude " << sigmaLatitude << " ";
ss << "sigmaLongitude " << sigmaLongitude << " ";
ss << "sigmaHeight " << sigmaHeight << " ";
- ss << "baseStationId[4] " << baseStationId[4] << " ";
+ ss << "baseStationId "
+ << baseStationId[0]
+ << baseStationId[1]
+ << baseStationId[2]
+ << baseStationId[3]
+ << " ";
ss << "diffAge " << diffAge << " ";
ss << "solutionAge " << solutionAge << " ";
ss << "numObservations " << numObservations << " ";
@@ -933,7 +938,10 @@
data->sigmaLatitude = bestGpsPos.data.sigmaLatitude;
data->sigmaLongitude = bestGpsPos.data.sigmaLongitude;
data->sigmaHeight = bestGpsPos.data.sigmaHeight;
- data->baseStationId[4] = bestGpsPos.data.baseStationId[4];
+ data->baseStationId[0] = bestGpsPos.data.baseStationId[0];
+ data->baseStationId[1] = bestGpsPos.data.baseStationId[1];
+ data->baseStationId[2] = bestGpsPos.data.baseStationId[2];
+ data->baseStationId[3] = bestGpsPos.data.baseStationId[3];
data->diffAge = bestGpsPos.data.diffAge;
data->solutionAge = bestGpsPos.data.solutionAge;
data->numObservations = bestGpsPos.data.numObservations;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rum...@us...> - 2008-06-26 10:18:31
|
Revision: 227
http://gearbox.svn.sourceforge.net/gearbox/?rev=227&view=rev
Author: rumataxyz
Date: 2008-06-26 03:18:37 -0700 (Thu, 26 Jun 2008)
Log Message:
-----------
fixed potential logice error
Modified Paths:
--------------
gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/test/crc32test.cpp
Modified: gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/test/crc32test.cpp
===================================================================
--- gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/test/crc32test.cpp 2008-06-26 09:36:06 UTC (rev 226)
+++ gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/test/crc32test.cpp 2008-06-26 10:18:37 UTC (rev 227)
@@ -135,8 +135,8 @@
do{
errorByteThree = (int) ((double)faultBufLen * (rand_r(&randSeed) / (RAND_MAX + 1.0)));
errorBitThree = (int) (8.0 * (rand_r(&randSeed) / (RAND_MAX + 1.0)));
- }while (errorByteThree==errorByte && errorBitThree==errorBit
- || errorByteThree==errorByteTwo && errorBitThree==errorBitTwo);// make sure we don't flip the same bit twice
+ }while ((errorByteThree==errorByte && errorBitThree==errorBit)
+ || (errorByteThree==errorByteTwo && errorBitThree==errorBitTwo));// make sure we don't flip the same bit twice
if(0 == faultBuf[errorByteThree] && (1<<errorBitThree))
faultBuf[errorByteThree] = faultBuf[errorByteThree] + (1<<errorBitThree);
else
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <to...@us...> - 2008-06-26 09:36:04
|
Revision: 226
http://gearbox.svn.sourceforge.net/gearbox/?rev=226&view=rev
Author: tobasco
Date: 2008-06-26 02:36:06 -0700 (Thu, 26 Jun 2008)
Log Message:
-----------
compiles cleanly with gcc-4.3
Modified Paths:
--------------
gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverreader.cpp
gearbox/trunk/submitted/gbxsmartbatteryacfr/test/fancytest.cpp
Modified: gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverreader.cpp
===================================================================
--- gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverreader.cpp 2008-06-26 08:46:20 UTC (rev 225)
+++ gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverreader.cpp 2008-06-26 09:36:06 UTC (rev 226)
@@ -9,6 +9,7 @@
*/
#include <sstream>
+#include <cstring>
#include <gbxsmartbatteryacfr/exceptions.h>
#include "oceanserverreader.h"
Modified: gearbox/trunk/submitted/gbxsmartbatteryacfr/test/fancytest.cpp
===================================================================
--- gearbox/trunk/submitted/gbxsmartbatteryacfr/test/fancytest.cpp 2008-06-26 08:46:20 UTC (rev 225)
+++ gearbox/trunk/submitted/gbxsmartbatteryacfr/test/fancytest.cpp 2008-06-26 09:36:06 UTC (rev 226)
@@ -1,4 +1,6 @@
#include <iostream>
+#include <memory>
+#include <cstdlib>
#include <gbxutilacfr/trivialtracer.h>
#include <gbxsmartbatteryacfr/gbxsmartbatteryacfr.h>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <to...@us...> - 2008-06-26 08:46:12
|
Revision: 225
http://gearbox.svn.sourceforge.net/gearbox/?rev=225&view=rev
Author: tobasco
Date: 2008-06-26 01:46:20 -0700 (Thu, 26 Jun 2008)
Log Message:
-----------
little things
Modified Paths:
--------------
gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverreader.cpp
gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverreader.h
gearbox/trunk/submitted/gbxsmartbatteryacfr/test/fancytest.cpp
gearbox/trunk/submitted/gbxsmartbatteryacfr/test/simpletest.cpp
Modified: gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverreader.cpp
===================================================================
--- gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverreader.cpp 2008-06-26 08:20:16 UTC (rev 224)
+++ gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverreader.cpp 2008-06-26 08:46:20 UTC (rev 225)
@@ -39,9 +39,7 @@
// send the command to start reading data
serial_.flush();
-
const char startReading = 'X';
-
serial_.write(&startReading, 1);
}
@@ -94,10 +92,6 @@
throw HardwareReadingException( ERROR_INFO, "Connected to the wrong serial port. Didn't recognize any of the strings.");
}
}
-}
-
-OceanServerReader::~OceanServerReader()
-{
}
void
@@ -143,7 +137,6 @@
}
else
{
-
tracer_.debug( "OceanServerReader: We already have the first line from the previous record", 5 );
stringList.push_back( beginningRecordLine_ );
}
@@ -163,8 +156,7 @@
stringList.push_back(serialData);
}
- // (4) Save the last line for next time, it's the S-record
- // Otherwise we'd miss a record
+ // (4) Save the last line: it is the beginning of the next record
beginningRecordLine_ = serialData;
if (firstTime_) {
firstTime_ = false;
Modified: gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverreader.h
===================================================================
--- gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverreader.h 2008-06-26 08:20:16 UTC (rev 224)
+++ gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverreader.h 2008-06-26 08:46:20 UTC (rev 225)
@@ -35,7 +35,7 @@
OceanServerReader( const std::string &device,
gbxutilacfr::Tracer &tracer );
- ~OceanServerReader();
+ ~OceanServerReader() {};
//! May throw HardwareReadingException
void read( OceanServerSystem &system );
Modified: gearbox/trunk/submitted/gbxsmartbatteryacfr/test/fancytest.cpp
===================================================================
--- gearbox/trunk/submitted/gbxsmartbatteryacfr/test/fancytest.cpp 2008-06-26 08:20:16 UTC (rev 224)
+++ gearbox/trunk/submitted/gbxsmartbatteryacfr/test/fancytest.cpp 2008-06-26 08:46:20 UTC (rev 225)
@@ -70,7 +70,7 @@
}
}
- unsigned int numRecords=10;
+ const unsigned int numRecords = 5;
MyClass myClass( port, debug );
for (unsigned int i=0; i<=numRecords; i++)
Modified: gearbox/trunk/submitted/gbxsmartbatteryacfr/test/simpletest.cpp
===================================================================
--- gearbox/trunk/submitted/gbxsmartbatteryacfr/test/simpletest.cpp 2008-06-26 08:20:16 UTC (rev 224)
+++ gearbox/trunk/submitted/gbxsmartbatteryacfr/test/simpletest.cpp 2008-06-26 08:46:20 UTC (rev 225)
@@ -28,7 +28,7 @@
}
}
- const unsigned int numRecords = 10;
+ const unsigned int numRecords = 5;
cout << "INFO(simple_test): The plan is to read " << numRecords << " records from the oceanserver system and display the results." << endl << endl;
// instantiate reader
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <to...@us...> - 2008-06-26 08:20:08
|
Revision: 224
http://gearbox.svn.sourceforge.net/gearbox/?rev=224&view=rev
Author: tobasco
Date: 2008-06-26 01:20:16 -0700 (Thu, 26 Jun 2008)
Log Message:
-----------
fixed tests
Modified Paths:
--------------
gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverreader.h
gearbox/trunk/submitted/gbxsmartbatteryacfr/test/CMakeLists.txt
gearbox/trunk/submitted/gbxsmartbatteryacfr/test/fancytest.cpp
gearbox/trunk/submitted/gbxsmartbatteryacfr/test/simpletest.cpp
Modified: gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverreader.h
===================================================================
--- gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverreader.h 2008-06-26 08:10:26 UTC (rev 223)
+++ gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverreader.h 2008-06-26 08:20:16 UTC (rev 224)
@@ -31,7 +31,7 @@
{
public:
- //! May throw SerialPortException
+ //! May throw HardwareReadingException
OceanServerReader( const std::string &device,
gbxutilacfr::Tracer &tracer );
Modified: gearbox/trunk/submitted/gbxsmartbatteryacfr/test/CMakeLists.txt
===================================================================
--- gearbox/trunk/submitted/gbxsmartbatteryacfr/test/CMakeLists.txt 2008-06-26 08:10:26 UTC (rev 223)
+++ gearbox/trunk/submitted/gbxsmartbatteryacfr/test/CMakeLists.txt 2008-06-26 08:20:16 UTC (rev 224)
@@ -1,13 +1,13 @@
INCLUDE( ${GBX_CMAKE_DIR}/UseBasicRules.cmake )
-GBX_ADD_EXECUTABLE( checksumtest checksumtest.cpp )
-TARGET_LINK_LIBRARIES( checksumtest )
+GBX_ADD_EXECUTABLE( gbxsmartbatterychecksumtest checksumtest.cpp )
+GBX_ADD_TEST( GbxSmartBatteryChecksumTest gbxsmartbatterychecksumtest )
-GBX_ADD_EXECUTABLE( simpletest simpletest.cpp )
-TARGET_LINK_LIBRARIES( simpletest GbxSmartBatteryAcfr )
+GBX_ADD_EXECUTABLE( gbxsmartbatterysimpletest simpletest.cpp )
+TARGET_LINK_LIBRARIES( gbxsmartbatterysimpletest GbxSmartBatteryAcfr )
-GBX_ADD_EXECUTABLE( fancytest fancytest.cpp )
-TARGET_LINK_LIBRARIES( fancytest GbxSmartBatteryAcfr )
+GBX_ADD_EXECUTABLE( gbxsmartbatteryfancytest fancytest.cpp )
+TARGET_LINK_LIBRARIES( gbxsmartbatteryfancytest GbxSmartBatteryAcfr )
GBX_ADD_EXAMPLE( gbxsmartbatteryacfr example.cmake.in example.cmake checksumtest.cpp simpletest.cpp fancytest.cpp example.readme )
Modified: gearbox/trunk/submitted/gbxsmartbatteryacfr/test/fancytest.cpp
===================================================================
--- gearbox/trunk/submitted/gbxsmartbatteryacfr/test/fancytest.cpp 2008-06-26 08:10:26 UTC (rev 223)
+++ gearbox/trunk/submitted/gbxsmartbatteryacfr/test/fancytest.cpp 2008-06-26 08:20:16 UTC (rev 224)
@@ -13,20 +13,29 @@
private:
gbxsmartbatteryacfr::OceanServerSystem data_;
gbxutilacfr::TrivialTracer tracer_;
- gbxsmartbatteryacfr::OceanServerReader reader_;
+ auto_ptr<gbxsmartbatteryacfr::OceanServerReader> reader_;
};
MyClass::MyClass(const std::string &port, bool debug)
- : tracer_(debug),
- reader_( port, tracer_ )
+ : tracer_(debug)
{
+ try
+ {
+ reader_.reset(new gbxsmartbatteryacfr::OceanServerReader( port, tracer_ ));
+ }
+ catch ( gbxsmartbatteryacfr::HardwareReadingException &e )
+ {
+ cout << "ERROR(fancy_test): Caught a hardware reading exception when initialising reader: "
+ << e.what() << endl;
+ exit(1);
+ }
}
void
MyClass::read()
{
gbxsmartbatteryacfr::OceanServerSystem data;
- reader_.read(data);
+ reader_->read(data);
gbxsmartbatteryacfr::updateWithNewData( data, data_ );
cout << "Current data:" << endl
Modified: gearbox/trunk/submitted/gbxsmartbatteryacfr/test/simpletest.cpp
===================================================================
--- gearbox/trunk/submitted/gbxsmartbatteryacfr/test/simpletest.cpp 2008-06-26 08:10:26 UTC (rev 223)
+++ gearbox/trunk/submitted/gbxsmartbatteryacfr/test/simpletest.cpp 2008-06-26 08:20:16 UTC (rev 224)
@@ -33,8 +33,20 @@
// instantiate reader
gbxutilacfr::TrivialTracer tracer( debug );
- gbxsmartbatteryacfr::OceanServerReader reader( port, tracer );
+ gbxsmartbatteryacfr::OceanServerReader *reader;
+
+ try
+ {
+ reader = new gbxsmartbatteryacfr::OceanServerReader( port, tracer );
+ }
+ catch ( gbxsmartbatteryacfr::HardwareReadingException &e )
+ {
+ cout << "ERROR(simple_test): Caught a hardware reading exception when initialising reader: "
+ << e.what() << endl;
+ return 1;
+ }
+
// data storage
gbxsmartbatteryacfr::OceanServerSystem data;
@@ -42,7 +54,7 @@
{
try
{
- reader.read( data );
+ reader->read( data );
cout << "TRACE(simple_test): Reading record " << i << ": " << endl
<< "=====================================" << endl << endl
<< gbxsmartbatteryacfr::toString( data ) << endl;
@@ -74,5 +86,6 @@
cout << "INFO(simple_test): Successfully read " << numRecords << " records." << endl;
+ delete reader;
return 0;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <gb...@us...> - 2008-06-26 08:10:24
|
Revision: 223
http://gearbox.svn.sourceforge.net/gearbox/?rev=223&view=rev
Author: gbiggs
Date: 2008-06-26 01:10:26 -0700 (Thu, 26 Jun 2008)
Log Message:
-----------
Added some missing const specifiers
Modified Paths:
--------------
gearbox/trunk/submitted/hokuyo_aist/hokuyo_aist.cpp
gearbox/trunk/submitted/hokuyo_aist/hokuyo_aist.h
Modified: gearbox/trunk/submitted/hokuyo_aist/hokuyo_aist.cpp
===================================================================
--- gearbox/trunk/submitted/hokuyo_aist/hokuyo_aist.cpp 2008-06-26 07:10:03 UTC (rev 222)
+++ gearbox/trunk/submitted/hokuyo_aist/hokuyo_aist.cpp 2008-06-26 08:10:26 UTC (rev 223)
@@ -174,7 +174,7 @@
}
// error must be null-terminated
-string SCIP2ErrorToString (char *error, char *cmd)
+string SCIP2ErrorToString (const char *error, const char *cmd)
{
stringstream ss;
@@ -1469,7 +1469,8 @@
// If paramLength is 0, no parameters will be sent or expected in the reply.
// extraOK must be a 1-byte string for SCIP1 and a 2-byte string for SCIP2.
// Return value is the status code returned for the command.
-int HokuyoLaser::SendCommand (char *cmd, char *param, int paramLength, char *extraOK)
+int HokuyoLaser::SendCommand (const char *cmd, const char *param,
+ int paramLength, const char *extraOK)
{
int statusCode = -1;
char response[16];
Modified: gearbox/trunk/submitted/hokuyo_aist/hokuyo_aist.h
===================================================================
--- gearbox/trunk/submitted/hokuyo_aist/hokuyo_aist.h 2008-06-26 07:10:03 UTC (rev 222)
+++ gearbox/trunk/submitted/hokuyo_aist/hokuyo_aist.h 2008-06-26 08:10:26 UTC (rev 223)
@@ -423,7 +423,7 @@
int ReadLine (char *buffer, int expectedLength = -1);
int ReadLineWithCheck (char *buffer, int expectedLength = -1, bool hasSemicolon = false);
void SkipLines (int count);
- int SendCommand (char *cmd, char *param, int paramLength, char *extraOK);
+ int SendCommand (const char *cmd, const char *param, int paramLength, const char *extraOK);
void GetAndSetSCIPVersion (void);
void GetDefaults (void);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rum...@us...> - 2008-06-26 07:09:57
|
Revision: 222
http://gearbox.svn.sourceforge.net/gearbox/?rev=222&view=rev
Author: rumataxyz
Date: 2008-06-26 00:10:03 -0700 (Thu, 26 Jun 2008)
Log Message:
-----------
stop gcc from whinging
Modified Paths:
--------------
gearbox/trunk/submitted/gbxnovatelacfr/test/test.cpp
Modified: gearbox/trunk/submitted/gbxnovatelacfr/test/test.cpp
===================================================================
--- gearbox/trunk/submitted/gbxnovatelacfr/test/test.cpp 2008-06-26 05:31:38 UTC (rev 221)
+++ gearbox/trunk/submitted/gbxnovatelacfr/test/test.cpp 2008-06-26 07:10:03 UTC (rev 222)
@@ -25,7 +25,7 @@
using namespace std;
namespace gna = gbxnovatelacfr;
-void usage(char *progName, char *optString){
+void usage(char *progName, const char *optString){
printf("Usage: %s -m [mode] -p [port] -b [baud] -i [imutype] -d -v -h\n", progName);
printf("\t[mode]:\t\"gps\" to set the system up in GPS only mode\n"
"\t\t\"ins\" to run INS/GPS mode (default)\n");
@@ -41,7 +41,7 @@
int main(int argc, char *argv[]){
int opt;
- char *optString = ":m:p:b:i:dvh";
+ const char *optString = ":m:p:b:i:dvh";
//use sensible defaults
string mode = "ins";
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <to...@us...> - 2008-06-26 05:31:29
|
Revision: 221
http://gearbox.svn.sourceforge.net/gearbox/?rev=221&view=rev
Author: tobasco
Date: 2008-06-25 22:31:38 -0700 (Wed, 25 Jun 2008)
Log Message:
-----------
Modified Paths:
--------------
gearbox/trunk/submitted/gbxsmartbatteryacfr/test/fancytest.cpp
Modified: gearbox/trunk/submitted/gbxsmartbatteryacfr/test/fancytest.cpp
===================================================================
--- gearbox/trunk/submitted/gbxsmartbatteryacfr/test/fancytest.cpp 2008-06-26 04:55:29 UTC (rev 220)
+++ gearbox/trunk/submitted/gbxsmartbatteryacfr/test/fancytest.cpp 2008-06-26 05:31:38 UTC (rev 221)
@@ -8,29 +8,25 @@
{
public:
MyClass(const std::string &port, bool debug);
- ~MyClass();
+ ~MyClass() {};
void read();
private:
gbxsmartbatteryacfr::OceanServerSystem data_;
- gbxsmartbatteryacfr::OceanServerReader *reader_;
+ gbxutilacfr::TrivialTracer tracer_;
+ gbxsmartbatteryacfr::OceanServerReader reader_;
};
MyClass::MyClass(const std::string &port, bool debug)
+ : tracer_(debug),
+ reader_( port, tracer_ )
{
- gbxutilacfr::TrivialTracer tracer( debug );
- reader_ = new gbxsmartbatteryacfr::OceanServerReader( port, tracer );
}
-MyClass::~MyClass()
-{
- delete reader_;
-}
-
void
MyClass::read()
{
gbxsmartbatteryacfr::OceanServerSystem data;
- reader_->read(data);
+ reader_.read(data);
gbxsmartbatteryacfr::updateWithNewData( data, data_ );
cout << "Current data:" << endl
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <to...@us...> - 2008-06-26 04:55:23
|
Revision: 220
http://gearbox.svn.sourceforge.net/gearbox/?rev=220&view=rev
Author: tobasco
Date: 2008-06-25 21:55:29 -0700 (Wed, 25 Jun 2008)
Log Message:
-----------
more cleaning up
Modified Paths:
--------------
gearbox/trunk/submitted/gbxsmartbatteryacfr/exceptions.h
gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverparser.cpp
gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverparser.h
gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverreader.cpp
gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverreader.h
gearbox/trunk/submitted/gbxsmartbatteryacfr/smartbattery.h
gearbox/trunk/submitted/gbxsmartbatteryacfr/smartbatteryacfr.dox
gearbox/trunk/submitted/gbxsmartbatteryacfr/smartbatteryparsing.cpp
gearbox/trunk/submitted/gbxsmartbatteryacfr/smartbatteryparsing.h
Modified: gearbox/trunk/submitted/gbxsmartbatteryacfr/exceptions.h
===================================================================
--- gearbox/trunk/submitted/gbxsmartbatteryacfr/exceptions.h 2008-06-26 04:51:03 UTC (rev 219)
+++ gearbox/trunk/submitted/gbxsmartbatteryacfr/exceptions.h 2008-06-26 04:55:29 UTC (rev 220)
@@ -8,47 +8,27 @@
*
*/
-#ifndef GBX_SMARTBATTERY_ACFR_EXCEPTIONS_H
-#define GBX_SMARTBATTERY_ACFR_EXCEPTIONS_H
+#ifndef GBX_SMARTBATTERYACFR_EXCEPTIONS_H
+#define GBX_SMARTBATTERYACFR_EXCEPTIONS_H
-#include <exception>
-#include <string>
+#include <gbxutilacfr/exceptions.h>
namespace gbxsmartbatteryacfr {
-//!
-//! Exceptions for gbxsmartbatteryacfr
-//!
-class Exception : public std::exception
-{
-public:
-
- Exception(const char *message)
- : message_(message) {}
- Exception(const std::string &message)
- : message_(message) {}
-
- virtual ~Exception() throw() {}
-
- virtual const char* what() const throw() { return message_.c_str(); }
-
-protected:
-
- std::string message_;
-};
-
-class HardwareReadingException : public Exception
+//! Exception for hardware reading problems
+class HardwareReadingException : public gbxutilacfr::Exception
{
public:
- HardwareReadingException( const char * message )
- : Exception( message ) {}
+ HardwareReadingException( const char *file, const char *line, const char *message )
+ : Exception( file, line, message ) {}
};
-class ParsingException : public Exception
+//! Exception for parsing problems
+class ParsingException : public gbxutilacfr::Exception
{
public:
- ParsingException( const char * message )
- : Exception( message ) {}
+ ParsingException( const char *file, const char *line, const char *message )
+ : Exception( file, line, message ) {}
};
}
Modified: gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverparser.cpp
===================================================================
--- gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverparser.cpp 2008-06-26 04:51:03 UTC (rev 219)
+++ gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverparser.cpp 2008-06-26 04:55:29 UTC (rev 220)
@@ -8,9 +8,7 @@
*
*/
-#include <iostream>
#include <sstream>
-
#include <gbxsmartbatteryacfr/exceptions.h>
#include <gbxsmartbatteryacfr/smartbatteryparsing.h>
@@ -148,11 +146,11 @@
if ( fromB.has( ManufacturerData ) ) toB.setManufacturerData ( fromB.manufacturerData () );
}
- // check if reaping needs to be done
+ // check if reaping needs to be done, if not return
if ( from.batteries().size() == to.batteries().size() )
return;
- // store batteries to be reaped in a vector
+ // store battery ids from batteries which need to be reaped in a vector
vector<int> reapingIds;
// go through all 'to' batteries and check if they are also in 'from'
@@ -162,12 +160,12 @@
BatIt itFrom = from.batteries().find( batId );
if ( itFrom == from.batteries().end() ) {
- // battery is in 'to' but not in 'from' -> needs to be deleted
+ // battery is in 'to' but not in 'from' -> needs to be reaped
reapingIds.push_back( batId );
}
}
- // erase batteries
+ // reap batteries
for (unsigned int i=0; i<reapingIds.size(); i++) {
to.eraseBattery( reapingIds[i] );
}
@@ -225,8 +223,8 @@
if ( it==batteries_.end() )
{
stringstream ss;
- ss << "ERROR(OceanServerParser.cpp): trying to read from non-existent battery " << batteryNumber;
- throw ParsingException( ss.str().c_str() );
+ ss << "Trying to read from non-existent battery " << batteryNumber;
+ throw ParsingException( ERROR_INFO, ss.str().c_str() );
}
return it->second;
}
@@ -269,8 +267,8 @@
else
{
stringstream ss;
- ss << "OceanServerParser: Unknown System key: " << it->first;
- throw ParsingException(ss.str().c_str());
+ ss << "Unknown System key: " << it->first;
+ throw ParsingException( ERROR_INFO, ss.str().c_str() );
}
}
}
@@ -315,8 +313,8 @@
else
{
stringstream ss;
- ss << "OceanServerParser: Unknown controller key: " << it->first;
- throw ParsingException(ss.str().c_str());
+ ss << "Unknown controller key: " << it->first;
+ throw ParsingException( ERROR_INFO, ss.str().c_str() );
}
}
@@ -407,8 +405,8 @@
bat.setManufacturerData( read16Flags( it->second ) ); break;
case NUM_SMARTBATTERY_FIELDS:
default:
- stringstream ss; ss << "OceanServerParser: Unknown Battery key: " << it->first;
- throw ParsingException( ss.str().c_str() );
+ stringstream ss; ss << "Unknown Battery key: " << it->first;
+ throw ParsingException( ERROR_INFO, ss.str().c_str() );
}
}
@@ -450,8 +448,8 @@
else
{
stringstream ss;
- ss << "OceanServerParser: Unknown message type: " << msgTypeKey;
- throw ParsingException(ss.str().c_str());
+ ss << "Unknown message type: " << msgTypeKey;
+ throw ParsingException( ERROR_INFO, ss.str().c_str() );
}
}
@@ -488,9 +486,8 @@
{
//
- // Uncomment for DEBUG information
+ // Debugging output
//
-
stringstream ss;
ss << "OceanServerParser: Received the following input: " << endl;
for (unsigned int i=0; i<stringList.size(); i++)
@@ -511,16 +508,20 @@
ss << endl;
tracer_.debug( ss.str(), 10 );
+ //
+ // Parsing
+ //
for (unsigned int i=0; i<stringList.size(); i++)
{
const string &line = stringList[i];
- // check for control characters in the line (sometimes they are accidently inserted)
- // don't check the last 2 characters: they're \0 and \n
+ // Known problem with oceanserver system: sometimes \0 is inserted in the middle of the string.
+ // To get around this, we check for 'control characters' in the string.
+ // Don't check the last 2 characters of the string: they're \0 and \n.
for (unsigned int k=0; k<line.size()-2; k++)
{
if ( iscntrl( line[k] ) )
- throw ParsingException("ERROR(oceanserverparser.cpp): Found a control character (binary) in the string!");
+ throw ParsingException( ERROR_INFO, "Found a control character (binary) in the string!" );
}
// divide the line into 2 parts: data and checksum (if present)
@@ -530,12 +531,12 @@
{
// we have a checksum, is it correct?
if (!isChecksumValid( checksumList[0], checksumList[1] ) )
- throw ParsingException("ERROR(oceanserverparser.cpp): Checksum failed!");
+ throw ParsingException( ERROR_INFO, "Checksum failed!" );
}
// divide the data into individual fields and parse
if (checksumList.size()==0)
- throw ParsingException("ERROR(oceanserverparser.cpp): String length is 0");
+ throw ParsingException( ERROR_INFO, "String length is 0" );
vector<string> fields;
splitIntoFields( checksumList[0], fields, ",");
parseFields( fields, batterySystem );
Modified: gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverparser.h
===================================================================
--- gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverparser.h 2008-06-26 04:51:03 UTC (rev 219)
+++ gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverparser.h 2008-06-26 04:55:29 UTC (rev 220)
@@ -8,8 +8,8 @@
*
*/
-#ifndef OCEANSERVER_PARSER_H
-#define OCEANSERVER_PARSER_H
+#ifndef GBX_OCEANSERVER_PARSER_H
+#define GBX_OCEANSERVER_PARSER_H
#include <map>
#include <gbxutilacfr/tracer.h>
@@ -31,25 +31,25 @@
OceanServerSystem();
~OceanServerSystem() {};
- // read access to all batteries
+ //! Read access to all batteries
const std::map<int,SmartBattery>& batteries() const;
- // easy write access to single battery, instantiates a new one if it doesn't exist
+ //! Easy write access to single battery, instantiates a new one if it doesn't exist
SmartBattery& battery( unsigned int batteryNumber );
- // easy read access to single battery, battery must exist
+ //! Easy read access to single battery, battery must exist
const SmartBattery& battery( unsigned int batteryNumber ) const;
- // erase a battery
+ //! Erase a battery
void eraseBattery( unsigned int batteryNumber );
- // average battery values
+ //! Average battery values
int percentCharge;
int minToEmpty;
std::string messageToSystem;
- // values from the controller
- // each vector is always of size 8 because oceanserver system has 8 slots
+ //! Battery module states
+ //! Each vector is always of size 8 because oceanserver system has 8 slots
std::vector<bool> availableBatteries;
std::vector<bool> chargingStates;
std::vector<bool> supplyingPowerStates;
@@ -58,13 +58,15 @@
std::vector<bool> chargeInhibitedStates;
private:
+
+ // key: slot number, data: a single smart battery module
std::map<int,SmartBattery> batteries_;
};
-//! Puts all available data into a human-readable string
+//! Puts OceanServerSystem data into a human-readable string
std::string toString( const OceanServerSystem &system );
-//! Puts all available data into a machine-readable ASCII string
+//! Puts OceanServerSystem data into a machine-readable ASCII string
std::string toLogString( const OceanServerSystem &system );
//! Updates all fields in 'to' with data from 'from'. Also reapes batteries in 'to' if they are not in 'from'.
Modified: gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverreader.cpp
===================================================================
--- gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverreader.cpp 2008-06-26 04:51:03 UTC (rev 219)
+++ gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverreader.cpp 2008-06-26 04:55:29 UTC (rev 220)
@@ -8,11 +8,8 @@
*
*/
-#include <iostream>
#include <sstream>
-
#include <gbxsmartbatteryacfr/exceptions.h>
-#include <gbxsmartbatteryacfr/oceanserverparser.h>
#include "oceanserverreader.h"
@@ -84,7 +81,7 @@
string serialData;
int ret = serial_.readLine( serialData );
if (ret<0) {
- throw HardwareReadingException("Connected to the wrong serial port. Timed out while trying to read a line.");
+ throw HardwareReadingException( ERROR_INFO, "Connected to the wrong serial port. Timed out while trying to read a line.");
}
if ( isOceanServerSystem(serialData.c_str()) ) {
tracer_.info( "Oceanserverreader.cpp: We are connected to an Oceanserver system. Good." );
@@ -94,7 +91,7 @@
ss.str(""); ss << "OceanServerReader: Trying to find out whether this is an oceanserver system. Attempt number " << numTries << "/" << maxTries << ".";
tracer_.info( ss.str() );
if (numTries>=maxTries) {
- throw HardwareReadingException("Connected to the wrong serial port. Didn't recognize any of the strings.");
+ throw HardwareReadingException( ERROR_INFO, "Connected to the wrong serial port. Didn't recognize any of the strings.");
}
}
}
@@ -119,7 +116,7 @@
if (numTries>=maxTries) {
stringstream ss;
ss << "Can't read data from serial port. Timed out and/or empty strings " << maxTries << " times in a row.";
- throw HardwareReadingException( ss.str().c_str() );
+ throw HardwareReadingException( ERROR_INFO, ss.str().c_str() );
}
}
}
Modified: gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverreader.h
===================================================================
--- gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverreader.h 2008-06-26 04:51:03 UTC (rev 219)
+++ gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverreader.h 2008-06-26 04:55:29 UTC (rev 220)
@@ -8,10 +8,9 @@
*
*/
-#ifndef OCEANSERVER_READER_H
-#define OCEANSERVER_READER_H
+#ifndef GBX_OCEANSERVER_READER_H
+#define GBX_OCEANSERVER_READER_H
-#include <string>
#include <gbxserialacfr/serial.h>
#include <gbxutilacfr/tracer.h>
Modified: gearbox/trunk/submitted/gbxsmartbatteryacfr/smartbattery.h
===================================================================
--- gearbox/trunk/submitted/gbxsmartbatteryacfr/smartbattery.h 2008-06-26 04:51:03 UTC (rev 219)
+++ gearbox/trunk/submitted/gbxsmartbatteryacfr/smartbattery.h 2008-06-26 04:55:29 UTC (rev 220)
@@ -8,8 +8,8 @@
*
*/
-#ifndef SMARTBATTERY_H
-#define SMARTBATTERY_H
+#ifndef GBX_SMARTBATTERY_H
+#define GBX_SMARTBATTERY_H
#include <vector>
#include <string>
@@ -18,7 +18,7 @@
namespace gbxsmartbatteryacfr {
//! Smart battery data specification
-//! Table of fields can be found at http://sbs-forum.org/specs/
+//! Specs can be found at http://sbs-forum.org/specs/
enum SmartBatteryDataField
{
ManufacturerAccess = 0,
@@ -60,7 +60,7 @@
//! Converts a string to a SmartBatteryDataField, returns NUM_SMARTBATTERY_FIELDS if string is not valid
SmartBatteryDataField stringToSmartField( const std::string &fieldStr );
-//! SmartBattery class holds all the data of a single smart battery
+//! SmartBattery class holds all the data of a single smart battery.
//! Since not all data is always present, access to data needs to be done as follows:
//! if (has(Temperature)) { myTemp = smartBattery.temperature(); }
class SmartBattery
@@ -211,10 +211,10 @@
};
-//! Prints the data the SmartBattery contains in a human-readable format
+//! Puts SmartBattery data into a human-readable string
std::string toString( const SmartBattery &b );
-//! Prints the data the SmartBattery contains in a machine-readable format (ASCII)
+//! Puts SmartBattery data into a machine-readable ASCII string
std::string toLogString( const SmartBattery &b );
}
Modified: gearbox/trunk/submitted/gbxsmartbatteryacfr/smartbatteryacfr.dox
===================================================================
--- gearbox/trunk/submitted/gbxsmartbatteryacfr/smartbatteryacfr.dox 2008-06-26 04:51:03 UTC (rev 219)
+++ gearbox/trunk/submitted/gbxsmartbatteryacfr/smartbatteryacfr.dox 2008-06-26 04:55:29 UTC (rev 220)
@@ -15,11 +15,8 @@
@defgroup gbx_library_gbxsmartbatteryacfr GbxSmartBatteryAcfr
@brief A library to read data from a smart battery system
-This library provides a set of classes and functions to read data from a Smart Battery System (SBS). SBS is a specification for determining accurate battery readings.
+This library provides a set of classes and functions to read data from a Smart Battery System (SBS). SBS is a specification for determining accurate battery readings. The library also contains classes which are specialised to a battery system provided by OceanServer Technologies Inc.
-The library also contains classes which are specialised to a battery system provided by OceanServer Technologies Inc.
-
-
@par Header file
@verbatim
@@ -55,10 +52,11 @@
- The library is tailored to the OceanServer battery system.
- Has been tested with the following firmware: V2.10 (2007)
-@par OceanServer Battery Management Module Layout
+@par OceanServer Layout
-The battery number reported by the driver corresponds to the following connectors on the board:
+The battery number reported by the driver corresponds to the following connectors on the Battery Management Module board:
+@verbatim
Level 1 (lower level):
==================
| |
@@ -73,6 +71,7 @@
(J22) 8 | | 6 (J21)
(J17) 7 | | 5 (J12)
==================
+@endverbatim
Note that the lower level needs to be powered (at least one battery module needs to be plugged in) to be able to read from the serial port.
Modified: gearbox/trunk/submitted/gbxsmartbatteryacfr/smartbatteryparsing.cpp
===================================================================
--- gearbox/trunk/submitted/gbxsmartbatteryacfr/smartbatteryparsing.cpp 2008-06-26 04:51:03 UTC (rev 219)
+++ gearbox/trunk/submitted/gbxsmartbatteryacfr/smartbatteryparsing.cpp 2008-06-26 04:55:29 UTC (rev 220)
@@ -8,7 +8,6 @@
*
*/
-#include <iostream>
#include <sstream>
#include <gbxsmartbatteryacfr/exceptions.h>
@@ -49,7 +48,7 @@
int readUnsignedInt16( const string &str )
{
if (str.size()!=4)
- throw ParsingException("ERROR(smartbatteryparsing.cpp): readUnsignedInt16 called with string size != 4");
+ throw ParsingException( ERROR_INFO, "readUnsignedInt16 called with string size != 4" );
return readUnsignedInt( str );
}
@@ -57,7 +56,7 @@
int readUnsignedInt8( const string &str )
{
if (str.size()!=2)
- throw ParsingException("ERROR(smartbatteryparsing.cpp): readUnsignedInt8 called with string size != 2");
+ throw ParsingException( ERROR_INFO, "readUnsignedInt8 called with string size != 2" );
return readUnsignedInt( str );
}
@@ -65,7 +64,7 @@
int16_t readSignedInt16( const string &str )
{
if (str.size()!=4)
- throw ParsingException("ERROR(smartbatteryparsing.cpp): readSignedInt16 called with string size != 4");
+ throw ParsingException( ERROR_INFO, "readSignedInt16 called with string size != 4" );
stringstream ss(str);
int value;
@@ -149,9 +148,8 @@
return readUnsignedInt16( str );
}
-bool
-isChecksumValid( const string &data,
- const string &expectedChecksumStr )
+bool isChecksumValid( const string &data,
+ const string &expectedChecksumStr )
{
int computedChecksum=0;
@@ -183,13 +181,13 @@
// make sure we have an equal number
if (fields.size()%2 != 0)
- throw ParsingException("ERROR(smartbatteryparsing.cpp): toKeyValuePairs: odd number of inputs");
+ throw ParsingException( ERROR_INFO, "toKeyValuePairs: odd number of inputs" );
unsigned int i=0;
while(true)
{
if (fields.size() <= i+1)
- throw ParsingException("ERROR(smartbatteryparsing.cpp): toKeyValuePairs: wrong number of inputs");
+ throw ParsingException( ERROR_INFO, "toKeyValuePairs: wrong number of inputs" );
pairs[fields[i]] = fields[i+1];
i=i+2;
if (fields.size()==i) break;
@@ -207,6 +205,7 @@
{
// Skip delimiters at beginning.
string::size_type lastPos = str.find_first_not_of(delimiter, 0);
+
// Find first "non-delimiter".
string::size_type pos = str.find_first_of(delimiter, lastPos);
@@ -217,6 +216,7 @@
// Skip delimiters. Note the "not_of"
lastPos = str.find_first_not_of(delimiter, pos);
+
// Find next "non-delimiter"
pos = str.find_first_of(delimiter, lastPos);
}
@@ -227,7 +227,7 @@
vector<bool> &flags )
{
if (str.size()!=2)
- throw ParsingException("ERROR(smartbatteryparsing.cpp): readSingleByte called with string size != 2");
+ throw ParsingException( ERROR_INFO, "readSingleByte called with string size != 2" );
stringstream ss(str);
int allFlags;
Modified: gearbox/trunk/submitted/gbxsmartbatteryacfr/smartbatteryparsing.h
===================================================================
--- gearbox/trunk/submitted/gbxsmartbatteryacfr/smartbatteryparsing.h 2008-06-26 04:51:03 UTC (rev 219)
+++ gearbox/trunk/submitted/gbxsmartbatteryacfr/smartbatteryparsing.h 2008-06-26 04:55:29 UTC (rev 220)
@@ -8,10 +8,9 @@
*
*/
-#ifndef SMARTBATTERY_PARSING_H
-#define SMARTBATTERY_PARSING_H
+#ifndef GBX_SMARTBATTERY_PARSING_H
+#define GBX_SMARTBATTERY_PARSING_H
-#include <string>
#include <vector>
#include <map>
@@ -21,7 +20,7 @@
{
//!
-//! Functions to parse data using the smart battery data standard
+//! Functions to parse SmartBattery data
//! and some helper functions
//!
//! @author Tobias Kaupp
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <to...@us...> - 2008-06-26 04:50:56
|
Revision: 219
http://gearbox.svn.sourceforge.net/gearbox/?rev=219&view=rev
Author: tobasco
Date: 2008-06-25 21:51:03 -0700 (Wed, 25 Jun 2008)
Log Message:
-----------
Modified Paths:
--------------
gearbox/trunk/src/gbxutilacfr/doc.dox
Modified: gearbox/trunk/src/gbxutilacfr/doc.dox
===================================================================
--- gearbox/trunk/src/gbxutilacfr/doc.dox 2008-06-26 04:40:08 UTC (rev 218)
+++ gearbox/trunk/src/gbxutilacfr/doc.dox 2008-06-26 04:51:03 UTC (rev 219)
@@ -13,7 +13,7 @@
@ingroup gbx_cpp
@ingroup gbx_linux
@defgroup gbx_library_gbxutilacfr GbxUtilAcfr
-@brief ACFR driver for SICK laser range-finder.
+@brief ACFR utility functions and drivers
Utility functions and objects used accross ACFR libraries and drivers.
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <bo...@us...> - 2008-06-26 04:39:59
|
Revision: 218
http://gearbox.svn.sourceforge.net/gearbox/?rev=218&view=rev
Author: borax00
Date: 2008-06-25 21:40:08 -0700 (Wed, 25 Jun 2008)
Log Message:
-----------
added hints about manually verifying connectivity.
Modified Paths:
--------------
gearbox/trunk/src/gbxgarminacfr/doc.dox
Modified: gearbox/trunk/src/gbxgarminacfr/doc.dox
===================================================================
--- gearbox/trunk/src/gbxgarminacfr/doc.dox 2008-06-26 00:41:44 UTC (rev 217)
+++ gearbox/trunk/src/gbxgarminacfr/doc.dox 2008-06-26 04:40:08 UTC (rev 218)
@@ -217,6 +217,24 @@
The hexadecimal value of the most significant and least significant 4 bits of the result are converted
to two ASCII characters (0-9, A-F) for transmission. The most significant character is transmitted first.
+@section gbx_library_gbxgarminacfr_troubleshooting Troubleshooting
+
+To verify connectivity to the GPS device manually:
+
+- Connect using a terminal emulator (eg minicom) at 4800 8N1.
+- type the following (and hit return): $PGRMO,GPGGA,1
+
+The second step enables output from the GPS module. You should see a bunch of stuff like:
+@verbatim
+$GPGGA,043932,3353.3699,S,15111.5817,E,1,11,0.8,47.2,M,19.8,M,,*64
+$GPGGA,043933,3353.3699,S,15111.5817,E,1,11,0.8,47.2,M,19.8,M,,*65
+$GPGGA,043934,3353.3699,S,15111.5817,E,1,11,0.8,47.1,M,19.8,M,,*61
+$GPGGA,043935,3353.3699,S,15111.5817,E,1,11,0.8,47.1,M,19.8,M,,*60
+$GPGGA,043936,3353.3699,S,15111.5817,E,1,11,0.8,47.0,M,19.8,M,,*62
+$GPGGA,043937,3353.3699,S,15111.5817,E,1,11,0.8,47.0,M,19.8,M,,*63
+@endverbatim
+scrolling past.
+
*/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rum...@us...> - 2008-06-26 00:41:37
|
Revision: 217
http://gearbox.svn.sourceforge.net/gearbox/?rev=217&view=rev
Author: rumataxyz
Date: 2008-06-25 17:41:44 -0700 (Wed, 25 Jun 2008)
Log Message:
-----------
more build-dependencies
Modified Paths:
--------------
gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/CMakeLists.txt
gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/test/CMakeLists.txt
Modified: gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/CMakeLists.txt
===================================================================
--- gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/CMakeLists.txt 2008-06-26 00:27:56 UTC (rev 216)
+++ gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/CMakeLists.txt 2008-06-26 00:41:44 UTC (rev 217)
@@ -12,6 +12,9 @@
# INCLUDE( ${GBX_CMAKE_DIR}/FindIceUtil.cmake )
# GBX_REQUIRE_VAR( build LIB ${lib_name} ICEUTIL_FOUND "libIceUtil not found" )
+SET( dep_libs GbxUtilAcfr GbxSerialAcfr )
+GBX_REQUIRE_TARGETS( build LIB ${lib_name} ${int_libs} )
+
IF( build )
INCLUDE( ${GBX_CMAKE_DIR}/UseBasicRules.cmake )
Modified: gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/test/CMakeLists.txt
===================================================================
--- gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/test/CMakeLists.txt 2008-06-26 00:27:56 UTC (rev 216)
+++ gearbox/trunk/submitted/gbxnovatelacfr/gbxnovatelutilacfr/test/CMakeLists.txt 2008-06-26 00:41:44 UTC (rev 217)
@@ -1,4 +1,4 @@
-LINK_LIBRARIES( GbxNovatelAcfr )
+LINK_LIBRARIES( GbxNovatelUtilAcfr )
ADD_EXECUTABLE( crc32test crc32test.cpp )
GBX_ADD_TEST( GbxNovatelUtilAcfrCrc32Test crc32test )
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rum...@us...> - 2008-06-26 00:27:48
|
Revision: 216
http://gearbox.svn.sourceforge.net/gearbox/?rev=216&view=rev
Author: rumataxyz
Date: 2008-06-25 17:27:56 -0700 (Wed, 25 Jun 2008)
Log Message:
-----------
removed IceUtil dependency, not needed anymore
Modified Paths:
--------------
gearbox/trunk/submitted/gbxnovatelacfr/CMakeLists.txt
Modified: gearbox/trunk/submitted/gbxnovatelacfr/CMakeLists.txt
===================================================================
--- gearbox/trunk/submitted/gbxnovatelacfr/CMakeLists.txt 2008-06-25 13:54:53 UTC (rev 215)
+++ gearbox/trunk/submitted/gbxnovatelacfr/CMakeLists.txt 2008-06-26 00:27:56 UTC (rev 216)
@@ -4,8 +4,8 @@
SET( build TRUE )
GBX_REQUIRE_OPTION( build LIB ${lib_name} ON )
-INCLUDE( ${GBX_CMAKE_DIR}/FindIceUtil.cmake )
-GBX_REQUIRE_VAR( build LIB ${lib_name} ICEUTIL_FOUND "libIceUtil not found" )
+#INCLUDE( ${GBX_CMAKE_DIR}/FindIceUtil.cmake )
+#GBX_REQUIRE_VAR( build LIB ${lib_name} ICEUTIL_FOUND "libIceUtil not found" )
SET( int_libs GbxUtilAcfr GbxSerialAcfr )
GBX_REQUIRE_TARGETS( build LIB ${lib_name} ${int_libs} )
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <gb...@us...> - 2008-06-25 13:55:33
|
Revision: 215
http://gearbox.svn.sourceforge.net/gearbox/?rev=215&view=rev
Author: gbiggs
Date: 2008-06-25 06:54:53 -0700 (Wed, 25 Jun 2008)
Log Message:
-----------
Fixed problem with not reading a complete error response message
Modified Paths:
--------------
gearbox/trunk/submitted/hokuyo_aist/hokuyo_aist.cpp
Modified: gearbox/trunk/submitted/hokuyo_aist/hokuyo_aist.cpp
===================================================================
--- gearbox/trunk/submitted/hokuyo_aist/hokuyo_aist.cpp 2008-06-25 09:07:15 UTC (rev 214)
+++ gearbox/trunk/submitted/hokuyo_aist/hokuyo_aist.cpp 2008-06-25 13:54:53 UTC (rev 215)
@@ -1523,6 +1523,8 @@
{
if (response[statusIndex] != extraOK[0])
{
+ // There is an extra line feed after an error status (signalling end of message)
+ SkipLines (1);
stringstream ss;
ss << "Bad response to " << cmd[0] << " command: " << " " <<
SCIP1ErrorToString (response[statusIndex], cmd[0]);
@@ -1531,6 +1533,8 @@
}
else
{
+ // There is an extra line feed after an error status (signalling end of message)
+ SkipLines (1);
stringstream ss;
ss << "Bad response to " << cmd[0] << " command: " << response[statusIndex] <<
" " << SCIP1ErrorToString (response[statusIndex], cmd[0]);
@@ -1593,6 +1597,8 @@
{
if (response[0] != extraOK[0] || response[1] != extraOK[1])
{
+ // There is an extra line feed after an error status (signalling end of message)
+ SkipLines (1);
stringstream ss;
ss << "Bad response to " << cmd << " command: " << response[0] << response[1] <<
" " << SCIP2ErrorToString (response, cmd);
@@ -1601,6 +1607,8 @@
}
else
{
+ // There is an extra line feed after an error status (signalling end of message)
+ SkipLines (1);
stringstream ss;
ss << "Bad response to " << cmd << " command: " << response[0] << response[1] <<
" " << SCIP2ErrorToString (response, cmd);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rum...@us...> - 2008-06-25 09:07:16
|
Revision: 214
http://gearbox.svn.sourceforge.net/gearbox/?rev=214&view=rev
Author: rumataxyz
Date: 2008-06-25 02:07:15 -0700 (Wed, 25 Jun 2008)
Log Message:
-----------
changes due to Alex Brook's code-review
Modified Paths:
--------------
gearbox/trunk/submitted/gbxnovatelacfr/driver.cpp
gearbox/trunk/submitted/gbxnovatelacfr/driver.h
gearbox/trunk/submitted/gbxnovatelacfr/novatel.dox
Modified: gearbox/trunk/submitted/gbxnovatelacfr/driver.cpp
===================================================================
--- gearbox/trunk/submitted/gbxnovatelacfr/driver.cpp 2008-06-25 07:27:56 UTC (rev 213)
+++ gearbox/trunk/submitted/gbxnovatelacfr/driver.cpp 2008-06-25 09:07:15 UTC (rev 214)
@@ -69,14 +69,13 @@
std::auto_ptr<gna::GenericData> createExternalMsg(gnua::RawImuLogSB &rawImu, struct timeval &timeStamp, gnua::ImuDecoder *imuDecoder);
//helper functions for the toString() gear
- std::string statusToString(gna::StatusMessagetype statusMessageType, std::string statusMessage);
+ std::string statusToString(gna::StatusMessageType statusMessageType, std::string statusMessage);
std::string doubleVectorToString(vector<double > &vec, std::string seperator = std::string(" "));
}
namespace gbxnovatelacfr
{
Driver::Driver( const Config& cfg) :
- serial_(0),
baud_(115200),
config_(cfg),
tracer_(new gbxutilacfr::TrivialTracer())
@@ -88,7 +87,6 @@
Driver::Driver( const Config& cfg,
gbxutilacfr::Tracer* tracer) :
- serial_(0),
baud_(115200),
config_(cfg),
tracer_(tracer)
@@ -109,9 +107,7 @@
std::string serialDevice = config_.serialDevice_;
serial_.reset(new Serial( serialDevice, baud_, Serial::Timeout(1,0) ));
serial_->setDebugLevel(0);
- if(0 != connectToHardware() ){
- throw (gua::Exception(ERROR_INFO, "failed to connect to receiver!"));
- }
+ connectToHardware();
// just in case something is running... stops the novatel logging any messages
serial_->writeString( "unlogall\r\n" );
@@ -139,7 +135,7 @@
}
}
-int
+void
Driver::connectToHardware() {
// baudrates we test for; this is
// _not_ all the baudrates the receiver
@@ -155,7 +151,7 @@
int currentBaudrate = 0;
bool correctBaudrate = false;
- std::cout << "Trying to hook up to receiver at different Baudrates\n";
+ tracer_->info( "Trying to hook up to receiver at different Baudrates" );
int maxTry = 4;
int successThresh = 4;
int timeOutMsec = 150;
@@ -168,25 +164,26 @@
i++;
}
if(false == correctBaudrate){
- std::cout << "\n!Failed to establish a connection to the receiver!\n";
- std::cout << "Check physical connections; Check manually (minicom) for Baudrates < 9600kb/s.\n\n";
- return -1;
+ std::stringstream ss;
+ ss << "!Failed to establish a connection to the receiver! Check physical connections; Check manually (minicom) for Baudrates < 9600kb/s.";
+ throw ( gua::Exception(ERROR_INFO, ss.str()) );
}
+ // ok, we've got a working link
+ std::stringstream ss;
+ ss << "Established connection at "
+ << currentBaudrate << "bps; "
+ << "Resetting to configured speed: "
+ << baud_ << "bps";
+ tracer_->info(ss.str());
char str[256];
sprintf( str,"com com1 %d n 8 1 n off on\r\n", baud_ );
serial_->writeString( str );
- std::cout << "*******************************\n"
- << "** Current Speed " << currentBaudrate << "\n"
- << "** Resetting to " << baud_ << "\n"
- << "*******************************\n";
- std::cout << "** Testing new setting\n** ";
- if(true == gnua::testConnectivity( challenge, ack, *(serial_.get()), timeOutMsec, maxTry, successThresh, baud_)){
- std::cout << "*******************************\n";
- return 0;
- }else{
- std::cout << "*******************************\n";
- return -1;
+ if(false == gnua::testConnectivity( challenge, ack, *(serial_.get()), timeOutMsec, maxTry, successThresh, baud_)){
+ std::stringstream ss;
+ ss << "!Failed to reset connection to configured baudrate!";
+ throw ( gua::Exception(ERROR_INFO, ss.str()) );
}
+ return;
}
void
@@ -400,7 +397,7 @@
if(config_.ignoreUnknownMessages_){
tracer_->warning(ss.str());
}else{
- gua::Exception(ERROR_INFO, ss.str() );
+ throw ( gua::Exception(ERROR_INFO, ss.str()) );
}
}
break;
@@ -410,7 +407,7 @@
std::stringstream ss;
ss << "Warning("<<__FILE__<<":"<< __LINE__
<< "Timed out while waiting for data";
- gua::Exception(ERROR_INFO, ss.str());
+ throw ( gua::Exception(ERROR_INFO, ss.str()) );
}
}while(NULL == data.get()); // repeat till we get valid data
@@ -836,7 +833,7 @@
if(in_crc != crc) {
fprintf( stderr,"CRC Error: 0x%lx, 0x%lx\n",in_crc,crc );
- gua::Exception(ERROR_INFO, "CRC Error" );
+ throw ( gua::Exception(ERROR_INFO, "CRC Error" ) );
return -1;
}
@@ -1072,7 +1069,7 @@
return ss.str();
}
- std::string statusToString(gna::StatusMessagetype statusMessageType, std::string statusMessage){
+ std::string statusToString(gna::StatusMessageType statusMessageType, std::string statusMessage){
std::stringstream ss;
switch(statusMessageType){
case gna::NoMsg:
Modified: gearbox/trunk/submitted/gbxnovatelacfr/driver.h
===================================================================
--- gearbox/trunk/submitted/gbxnovatelacfr/driver.h 2008-06-25 07:27:56 UTC (rev 213)
+++ gearbox/trunk/submitted/gbxnovatelacfr/driver.h 2008-06-25 09:07:15 UTC (rev 214)
@@ -188,7 +188,7 @@
};
//! possible Status Messages GenericData can contain
-enum StatusMessagetype {
+enum StatusMessageType {
NoMsg, //!< Nothing new, no message
Initialising,//!< Nothing wrong, just not quite ready
Ok, //!< All good, but something to say
@@ -285,8 +285,8 @@
return InsPva;
}
std::string toString();
- int gpsWeekNr; //
- double secIntoWeek; //
+ int gpsWeekNr; //!< number of full weeks since midnight 05/Jan/1980 (UTC)
+ double secIntoWeek; //!< yields GPS-time (together with @ref gpsWeekNr); continous (contrary to UTC which uses leapseconds)
double latitude; //!< [deg] north positive WGS84
double longitude; //!< [deg] east positive WGS84
double height; //!< [m] above ellipsoid WGS84 (heigth_ellipsoid - undulation == height_geoid (aka AMSL)
@@ -311,7 +311,7 @@
//@}
- StatusMessagetype statusMessageType;
+ StatusMessageType statusMessageType;
std::string statusMessage;
int timeStampSec; //!< in Computer time, beginning of message at serial port
@@ -325,8 +325,8 @@
return BestGpsPos;
}
std::string toString();
- int gpsWeekNr; //
- unsigned int msIntoWeek; //!< milliseconds from beginning of week
+ int gpsWeekNr; //!< number of full weeks since midnight 05/Jan/1980 (UTC)
+ unsigned int msIntoWeek; //!< yields GPS-time (together with @ref gpsWeekNr); continous (contrary to UTC which uses leapseconds)
GpsSolutionStatusType solutionStatus; //
GpsPosVelType positionType; //
double latitude; //!< [deg] north positive
@@ -345,7 +345,7 @@
int numL1RangesRTK; //!< number of L1 ranges above the RTK mask angle (??) number of L1 carrier ranges used?
int numL2RangesRTK; //!< number of L2 ranges above the RTK mask angle (??) number of L2 carrier ranges used?
- StatusMessagetype statusMessageType;
+ StatusMessageType statusMessageType;
std::string statusMessage;
int timeStampSec; //!< in Computer time, beginning of message at serial port
@@ -359,8 +359,8 @@
return BestGpsVel;
}
std::string toString();
- int gpsWeekNr; //
- unsigned int msIntoWeek; //!< milliseconds from beginning of week
+ int gpsWeekNr; //!< number of full weeks since midnight 05/Jan/1980 (UTC)
+ unsigned int msIntoWeek; //!< yields GPS-time (together with @ref gpsWeekNr); continous (contrary to UTC which uses leapseconds)
GpsSolutionStatusType solutionStatus; //
GpsPosVelType positionType; //
float latency; //!< [s] gps speed can be calculated from instantanious or integrated doppler. The latter refers to the average speed over the last interval -> is delayed by half an interval
@@ -369,7 +369,7 @@
double trackOverGround; //!< [deg] "heading" of the speed vector w. respect to true North
double verticalSpeed; //!< [m/s]
- StatusMessagetype statusMessageType;
+ StatusMessageType statusMessageType;
std::string statusMessage;
int timeStampSec; //!< in Computer time, beginning of message at serial port
@@ -383,8 +383,8 @@
return RawImu;
}
std::string toString();
- int gpsWeekNr;
- double secIntoWeek;
+ int gpsWeekNr; //!< number of full weeks since midnight 05/Jan/1980 (UTC)
+ double secIntoWeek; //!< yields GPS-time (together with @ref gpsWeekNr); continous (contrary to UTC which uses leapseconds)
//!@name Change in speed
//!Divide by dt to get accelerations.
//!The default IMU axis definitions are: Y - forward, Z - up, X - right hand side
@@ -407,7 +407,7 @@
//@}
- StatusMessagetype statusMessageType;
+ StatusMessageType statusMessageType;
std::string statusMessage;
int timeStampSec; //!< in Computer time, beginning of message at serial port
@@ -438,49 +438,22 @@
Throws gbxutilacfr::Exception when a problem is encountered (derives from std::exception).
Throws gbxutilacfr::HardwareException when a (fatal) problem with the hardware is encountered
- @verbatim
- std::auto_ptr<gbxnovatelacfr::GenericData> data;
-
- while(1) { // read forever
- try {
- data = device->read();
- }
- catch ( const gbxutilacfr::HardwareException& e ) {
- cout <<"Something wrong with the hardware: "<<e.what()<<endl;
- cout <<"Giving up!\n";
- throw e;
- }
- catch ( const std::exception& e ) {
- cout <<"Failed to read data: "<<e.what()<<endl;
- continue;
- }
- switch( data.type() ){
- case InsPvaData:
- InsData *insData = dynamic_cast<InsData *>(data.get());
- assert(insData);
- // process insData
- break;
- default:
- // don't handle the other guys
- break;
- }
- }
- @endverbatim */
+ */
std::auto_ptr<GenericData> read();
private:
- //! does the leg-work for the constructor (via the following guys)
+ // does the leg-work for the constructor (via the following guys)
void configure();
- //! establish a serial connection to the receiver
- int connectToHardware();
- //! set parameters related to the IMU
+ // establish a serial connection to the receiver
+ void connectToHardware();
+ // set parameters related to the IMU
void configureImu();
- //! set parameters related to the INS
+ // set parameters related to the INS
void configureIns();
- //! set parameters related to GPS
+ // set parameters related to GPS
void configureGps();
- //! turn on data messages we are interested in
+ // turn on data messages we are interested in
void requestData();
std::auto_ptr<gbxnovatelutilacfr::ImuDecoder> imuDecoder_;
Modified: gearbox/trunk/submitted/gbxnovatelacfr/novatel.dox
===================================================================
--- gearbox/trunk/submitted/gbxnovatelacfr/novatel.dox 2008-06-25 07:27:56 UTC (rev 213)
+++ gearbox/trunk/submitted/gbxnovatelacfr/novatel.dox 2008-06-25 09:07:15 UTC (rev 214)
@@ -122,7 +122,13 @@
}
break;
default:
- std::cout << "Got unknown message!\n";
+ if(0 == generic.get()){
+ std::cout << "Got NULL message!\n";
+ }
+ else{
+ std::cout << "Got unknown message!\n";
+ std::cout << generic->toString() << "\n"; // yes this works, since toString() is a member of the base class
+ }
break;
}
}
@@ -147,6 +153,7 @@
- This is a Linux-only implementation (because of the serial library and the system-calls for timestamps)
- Only supports a subset of the messages a NovatelSPAN system can provide.
- Driver currently treats the hardware as data-source only, communication _to_ the hardware is possible only during initialization.
+- Driver doesn't check if configuring the receiver was successful/as-intended
*/
/*!
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <to...@us...> - 2008-06-25 07:27:52
|
Revision: 213
http://gearbox.svn.sourceforge.net/gearbox/?rev=213&view=rev
Author: tobasco
Date: 2008-06-25 00:27:56 -0700 (Wed, 25 Jun 2008)
Log Message:
-----------
first commit of gbxsmartbatteryacfr
Modified Paths:
--------------
gearbox/trunk/submitted/CMakeLists.txt
Added Paths:
-----------
gearbox/trunk/submitted/gbxsmartbatteryacfr/
gearbox/trunk/submitted/gbxsmartbatteryacfr/CMakeLists.txt
gearbox/trunk/submitted/gbxsmartbatteryacfr/exceptions.h
gearbox/trunk/submitted/gbxsmartbatteryacfr/gbxsmartbatteryacfr.h
gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverparser.cpp
gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverparser.h
gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverreader.cpp
gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverreader.h
gearbox/trunk/submitted/gbxsmartbatteryacfr/smartbattery.cpp
gearbox/trunk/submitted/gbxsmartbatteryacfr/smartbattery.h
gearbox/trunk/submitted/gbxsmartbatteryacfr/smartbatteryacfr.dox
gearbox/trunk/submitted/gbxsmartbatteryacfr/smartbatteryparsing.cpp
gearbox/trunk/submitted/gbxsmartbatteryacfr/smartbatteryparsing.h
gearbox/trunk/submitted/gbxsmartbatteryacfr/test/
gearbox/trunk/submitted/gbxsmartbatteryacfr/test/CMakeLists.txt
gearbox/trunk/submitted/gbxsmartbatteryacfr/test/checksumtest.cpp
gearbox/trunk/submitted/gbxsmartbatteryacfr/test/example.cmake.in
gearbox/trunk/submitted/gbxsmartbatteryacfr/test/example.readme
gearbox/trunk/submitted/gbxsmartbatteryacfr/test/fancytest.cpp
gearbox/trunk/submitted/gbxsmartbatteryacfr/test/serialtest.cpp
gearbox/trunk/submitted/gbxsmartbatteryacfr/test/simpletest.cpp
Modified: gearbox/trunk/submitted/CMakeLists.txt
===================================================================
--- gearbox/trunk/submitted/CMakeLists.txt 2008-06-24 14:57:10 UTC (rev 212)
+++ gearbox/trunk/submitted/CMakeLists.txt 2008-06-25 07:27:56 UTC (rev 213)
@@ -11,6 +11,7 @@
# Otherwise, maintain alphabetical order.
# E.g. ADD_SUBDIRECTORY( mydir )
ADD_SUBDIRECTORY( flexiport )
+ ADD_SUBDIRECTORY( gbxsmartbatteryacfr )
ADD_SUBDIRECTORY( gbxnovatelacfr )
ADD_SUBDIRECTORY( hokuyo_aist )
Added: gearbox/trunk/submitted/gbxsmartbatteryacfr/CMakeLists.txt
===================================================================
--- gearbox/trunk/submitted/gbxsmartbatteryacfr/CMakeLists.txt (rev 0)
+++ gearbox/trunk/submitted/gbxsmartbatteryacfr/CMakeLists.txt 2008-06-25 07:27:56 UTC (rev 213)
@@ -0,0 +1,28 @@
+SET( lib_name GbxSmartBatteryAcfr )
+GBX_ADD_LICENSE( LGPL )
+
+SET( build TRUE )
+GBX_REQUIRE_OPTION( build LIB ${lib_name} ON )
+GBX_REQUIRE_VAR( build LIB ${lib_name} GBX_OS_LINUX "only Linux OS is supported" )
+
+SET( int_libs GbxUtilAcfr GbxSerialAcfr )
+GBX_REQUIRE_TARGETS( build LIB ${lib_name} ${int_libs} )
+
+IF( build )
+
+ INCLUDE( ${GBX_CMAKE_DIR}/UseBasicRules.cmake )
+
+ FILE( GLOB hdrs *.h )
+ FILE( GLOB srcs *.cpp )
+ SET( dep_libs ${int_libs} )
+
+ GBX_ADD_LIBRARY( ${lib_name} SHARED ${srcs} )
+ TARGET_LINK_LIBRARIES( ${lib_name} ${dep_libs} )
+
+ GBX_ADD_HEADERS( gbxsmartbatteryacfr ${hdrs} )
+
+ IF( GBX_BUILD_TESTS )
+ ADD_SUBDIRECTORY( test )
+ ENDIF( GBX_BUILD_TESTS )
+
+ENDIF( build )
Added: gearbox/trunk/submitted/gbxsmartbatteryacfr/exceptions.h
===================================================================
--- gearbox/trunk/submitted/gbxsmartbatteryacfr/exceptions.h (rev 0)
+++ gearbox/trunk/submitted/gbxsmartbatteryacfr/exceptions.h 2008-06-25 07:27:56 UTC (rev 213)
@@ -0,0 +1,57 @@
+/*
+ * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
+ * http://gearbox.sf.net/
+ * Copyright (c) 2004-2008 Tobias Kaupp
+ *
+ * This distribution is licensed to you under the terms described in
+ * the LICENSE file included in this distribution.
+ *
+ */
+
+#ifndef GBX_SMARTBATTERY_ACFR_EXCEPTIONS_H
+#define GBX_SMARTBATTERY_ACFR_EXCEPTIONS_H
+
+#include <exception>
+#include <string>
+
+namespace gbxsmartbatteryacfr {
+
+//!
+//! Exceptions for gbxsmartbatteryacfr
+//!
+class Exception : public std::exception
+{
+public:
+
+ Exception(const char *message)
+ : message_(message) {}
+ Exception(const std::string &message)
+ : message_(message) {}
+
+ virtual ~Exception() throw() {}
+
+ virtual const char* what() const throw() { return message_.c_str(); }
+
+protected:
+
+ std::string message_;
+};
+
+class HardwareReadingException : public Exception
+{
+ public:
+ HardwareReadingException( const char * message )
+ : Exception( message ) {}
+};
+
+class ParsingException : public Exception
+{
+ public:
+ ParsingException( const char * message )
+ : Exception( message ) {}
+};
+
+}
+
+#endif
+
Added: gearbox/trunk/submitted/gbxsmartbatteryacfr/gbxsmartbatteryacfr.h
===================================================================
--- gearbox/trunk/submitted/gbxsmartbatteryacfr/gbxsmartbatteryacfr.h (rev 0)
+++ gearbox/trunk/submitted/gbxsmartbatteryacfr/gbxsmartbatteryacfr.h 2008-06-25 07:27:56 UTC (rev 213)
@@ -0,0 +1,21 @@
+/*
+ * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
+ * http://gearbox.sf.net/
+ * Copyright (c) 2004-2008 Tobias Kaupp
+ *
+ * This distribution is licensed to you under the terms described in
+ * the LICENSE file included in this distribution.
+ *
+ */
+
+#ifndef GBX_SMARTBATTERY_ACFR_H
+#define GBX_SMARTBATTERY_ACFR_H
+
+#include <gbxsmartbatteryacfr/oceanserverparser.h>
+#include <gbxsmartbatteryacfr/oceanserverreader.h>
+#include <gbxsmartbatteryacfr/exceptions.h>
+#include <gbxsmartbatteryacfr/smartbattery.h>
+#include <gbxsmartbatteryacfr/smartbatteryparsing.h>
+
+
+#endif
Added: gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverparser.cpp
===================================================================
--- gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverparser.cpp (rev 0)
+++ gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverparser.cpp 2008-06-25 07:27:56 UTC (rev 213)
@@ -0,0 +1,546 @@
+/*
+ * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
+ * http://gearbox.sf.net/
+ * Copyright (c) 2004-2008 Tobias Kaupp
+ *
+ * This distribution is licensed to you under the terms described in
+ * the LICENSE file included in this distribution.
+ *
+ */
+
+#include <iostream>
+#include <sstream>
+
+#include <gbxsmartbatteryacfr/exceptions.h>
+#include <gbxsmartbatteryacfr/smartbatteryparsing.h>
+
+#include "oceanserverparser.h"
+
+using namespace std;
+
+namespace gbxsmartbatteryacfr {
+
+//
+// Helper functions
+//
+string toString( const vector<bool> &flags )
+{
+ stringstream ss;
+ for (unsigned int i=0; i<flags.size(); i++)
+ {
+ ss << flags[i] << " ";
+ }
+ return ss.str();
+}
+
+string toLogString( const vector<bool> &flags )
+{
+ stringstream ss;
+ for (unsigned int i=0; i<flags.size(); i++)
+ {
+ ss << flags[i];
+ }
+ return ss.str();
+}
+
+
+//
+// OceanServerSystem non-member functions
+//
+
+string toString( const OceanServerSystem &system )
+{
+ stringstream ss;
+ ss << "Charge: \t" << system.percentCharge << endl;
+ ss << "Minutes to empty:\t" << system.minToEmpty << endl;
+ ss << "Available batt.: \t" << toString( system.availableBatteries ) << endl;
+ ss << "Charging: \t" << toString( system.chargingStates ) << endl;
+ ss << "Supplying power: \t" << toString( system.supplyingPowerStates ) << endl;
+ ss << "Charge power: \t" << toString( system.chargePowerPresentStates ) << endl;
+ ss << "Power no good: \t" << toString( system.powerNoGoodStates ) << endl;
+ ss << "Charge inhibited:\t" << toString( system.chargeInhibitedStates ) << endl;
+
+ map<int,SmartBattery>::const_iterator it;
+ for (it=system.batteries().begin(); it!=system.batteries().end(); it++)
+ {
+ ss << "Data from battery number: " << it->first << endl;
+ ss << toString( it->second ) << endl;
+ }
+
+ return ss.str();
+}
+
+string toLogString( const OceanServerSystem &system )
+{
+ stringstream ss;
+ ss << system.percentCharge << " ";
+ ss << system.minToEmpty << " ";
+ ss << toLogString( system.availableBatteries ) << " ";
+ ss << toLogString( system.chargingStates ) << " ";
+ ss << toLogString( system.supplyingPowerStates ) << " ";
+ ss << toLogString( system.chargePowerPresentStates ) << " ";
+ ss << toLogString( system.powerNoGoodStates ) << " ";
+ ss << toLogString( system.chargeInhibitedStates ) << endl;
+
+ ss << system.batteries().size();
+
+ map<int,SmartBattery>::const_iterator it;
+ for (it=system.batteries().begin(); it!=system.batteries().end(); it++)
+ {
+ ss << it->first << " " << toLogString( it->second ) << endl;
+ }
+
+ return ss.str();
+}
+
+void updateWithNewData( const OceanServerSystem &from,
+ OceanServerSystem &to )
+{
+ typedef map<int,SmartBattery>::const_iterator BatIt;
+
+ to.availableBatteries = from.availableBatteries;
+ to.percentCharge = from.percentCharge;
+ to.minToEmpty = from.minToEmpty;
+ to.messageToSystem = from.messageToSystem;
+ to.chargingStates = from.chargingStates;
+ to.supplyingPowerStates = from.supplyingPowerStates;
+ to.chargePowerPresentStates = from.chargePowerPresentStates;
+ to.powerNoGoodStates = from.powerNoGoodStates;
+ to.chargeInhibitedStates = from.chargeInhibitedStates;
+
+ for (BatIt it=from.batteries().begin(); it!=from.batteries().end(); it++)
+ {
+ const SmartBattery &fromB = from.battery( it->first );
+ SmartBattery &toB = to.battery( it->first );
+
+ if ( fromB.has( ManufacturerAccess ) ) toB.setManufacturerAccess ( fromB.manufacturerAccess () );
+ if ( fromB.has( RemainingCapacityAlarm ) ) toB.setRemainingCapacityAlarm ( fromB.remainingCapacityAlarm () );
+ if ( fromB.has( RemainingTimeAlarm ) ) toB.setRemainingTimeAlarm ( fromB.remainingTimeAlarm () );
+ if ( fromB.has( BatteryMode ) ) toB.setBatteryMode ( fromB.batteryMode () );
+ if ( fromB.has( AtRate ) ) toB.setAtRate ( fromB.atRate () );
+ if ( fromB.has( AtRateTimeToFull ) ) toB.setAtRateTimeToFull ( fromB.atRateTimeToFull () );
+ if ( fromB.has( AtRateTimeToEmpty ) ) toB.setAtRateTimeToEmpty ( fromB.atRateTimeToEmpty () );
+ if ( fromB.has( AtRateOk ) ) toB.setAtRateOk ( fromB.atRateOk () );
+ if ( fromB.has( Temperature ) ) toB.setTemperature ( fromB.temperature () );
+ if ( fromB.has( Voltage ) ) toB.setVoltage ( fromB.voltage () );
+ if ( fromB.has( Current ) ) toB.setCurrent ( fromB.current () );
+ if ( fromB.has( AverageCurrent ) ) toB.setAverageCurrent ( fromB.averageCurrent () );
+ if ( fromB.has( MaxError ) ) toB.setMaxError ( fromB.maxError () );
+ if ( fromB.has( RelativeStateOfCharge ) ) toB.setRelativeStateOfCharge ( fromB.relativeStateOfCharge () );
+ if ( fromB.has( AbsoluteStateOfCharge ) ) toB.setAbsoluteStateOfCharge ( fromB.absoluteStateOfCharge () );
+ if ( fromB.has( RemainingCapacity ) ) toB.setRemainingCapacity ( fromB.remainingCapacity () );
+ if ( fromB.has( FullChargeCapacity ) ) toB.setFullChargeCapacity ( fromB.fullChargeCapacity () );
+ if ( fromB.has( RunTimeToEmpty ) ) toB.setRunTimeToEmpty ( fromB.runTimeToEmpty () );
+ if ( fromB.has( AverageTimeToEmpty ) ) toB.setAverageTimeToEmpty ( fromB.averageTimeToEmpty () );
+ if ( fromB.has( AverageTimeToFull ) ) toB.setAverageTimeToFull ( fromB.averageTimeToFull () );
+ if ( fromB.has( ChargingCurrent ) ) toB.setChargingCurrent ( fromB.chargingCurrent () );
+ if ( fromB.has( ChargingVoltage ) ) toB.setChargingVoltage ( fromB.chargingVoltage () );
+ if ( fromB.has( BatteryStatus ) ) toB.setBatteryStatus ( fromB.batteryStatus () );
+ if ( fromB.has( CycleCount ) ) toB.setCycleCount ( fromB.cycleCount () );
+ if ( fromB.has( DesignCapacity ) ) toB.setDesignCapacity ( fromB.designCapacity () );
+ if ( fromB.has( DesignVoltage ) ) toB.setDesignVoltage ( fromB.designVoltage () );
+ if ( fromB.has( SpecificationInfo ) ) toB.setSpecificationInfo ( fromB.specificationInfo () );
+ if ( fromB.has( ManufactureDate ) ) toB.setManufactureDate ( fromB.manufactureDate () );
+ if ( fromB.has( SerialNumber ) ) toB.setSerialNumber ( fromB.serialNumber () );
+ if ( fromB.has( ManufacturerName ) ) toB.setManufacturerName ( fromB.manufacturerName () );
+ if ( fromB.has( DeviceName ) ) toB.setDeviceName ( fromB.deviceName () );
+ if ( fromB.has( DeviceChemistry ) ) toB.setDeviceChemistry ( fromB.deviceChemistry () );
+ if ( fromB.has( ManufacturerData ) ) toB.setManufacturerData ( fromB.manufacturerData () );
+ }
+
+ // check if reaping needs to be done
+ if ( from.batteries().size() == to.batteries().size() )
+ return;
+
+ // store batteries to be reaped in a vector
+ vector<int> reapingIds;
+
+ // go through all 'to' batteries and check if they are also in 'from'
+ for (BatIt it=to.batteries().begin(); it!=to.batteries().end(); it++)
+ {
+ const int batId = it->first;
+
+ BatIt itFrom = from.batteries().find( batId );
+ if ( itFrom == from.batteries().end() ) {
+ // battery is in 'to' but not in 'from' -> needs to be deleted
+ reapingIds.push_back( batId );
+ }
+ }
+
+ // erase batteries
+ for (unsigned int i=0; i<reapingIds.size(); i++) {
+ to.eraseBattery( reapingIds[i] );
+ }
+
+}
+
+
+//
+// OceanServerSystem member functions
+//
+
+OceanServerSystem::OceanServerSystem()
+ : percentCharge(0),
+ minToEmpty(0),
+ messageToSystem("")
+{
+ // fixed number of slots for oceanserver system
+ const int NUM_BATTERY_SLOTS = 8;
+ availableBatteries.resize(NUM_BATTERY_SLOTS);
+ chargingStates.resize(NUM_BATTERY_SLOTS);
+ supplyingPowerStates.resize(NUM_BATTERY_SLOTS);
+ chargePowerPresentStates.resize(NUM_BATTERY_SLOTS);
+ powerNoGoodStates.resize(NUM_BATTERY_SLOTS);
+ chargeInhibitedStates.resize(NUM_BATTERY_SLOTS);
+}
+
+// read access to all batteries
+const map<int,SmartBattery>&
+OceanServerSystem::batteries() const
+{
+ return batteries_;
+}
+
+// write access to single battery
+SmartBattery&
+OceanServerSystem::battery( unsigned int batteryNumber )
+{
+ map<int,SmartBattery>::iterator it = batteries_.find(batteryNumber);
+ if ( it==batteries_.end() )
+ {
+ // we don't have it, so instantiate a new one
+ SmartBattery b;
+ batteries_[batteryNumber] = b;
+ return batteries_[batteryNumber];
+ }
+
+ return it->second;
+}
+
+// read access to single battery
+const SmartBattery&
+OceanServerSystem::battery( unsigned int batteryNumber ) const
+{
+ map<int,SmartBattery>::const_iterator it = batteries_.find(batteryNumber);
+ if ( it==batteries_.end() )
+ {
+ stringstream ss;
+ ss << "ERROR(OceanServerParser.cpp): trying to read from non-existent battery " << batteryNumber;
+ throw ParsingException( ss.str().c_str() );
+ }
+ return it->second;
+}
+
+void
+OceanServerSystem::eraseBattery( unsigned int batteryNumber )
+{
+ batteries_.erase( batteries_.find( batteryNumber ) );
+}
+
+//
+// OceanServerParser member functions
+//
+
+OceanServerParser::OceanServerParser( gbxutilacfr::Tracer &tracer )
+ : tracer_(tracer)
+{
+}
+
+void
+OceanServerParser::parseSystemData( const map<string,string> &keyValuePairs,
+ OceanServerSystem &batterySystem )
+{
+ map<string,string>::const_iterator it;
+
+ for (it=keyValuePairs.begin(); it!=keyValuePairs.end(); it++)
+ {
+ if (it->first=="01") {
+ batterySystem.minToEmpty = readMinutes(it->second);
+ }
+ else if (it->first=="02") {
+ // reserved, do nothing
+ }
+ else if (it->first=="03") {
+ batterySystem.messageToSystem = it->second;
+ }
+ else if (it->first=="04") {
+ batterySystem.percentCharge = readPercentByte(it->second);
+ }
+ else
+ {
+ stringstream ss;
+ ss << "OceanServerParser: Unknown System key: " << it->first;
+ throw ParsingException(ss.str().c_str());
+ }
+ }
+}
+
+void
+OceanServerParser::parseControllerData( const map<string,string> &keyValuePairs,
+ OceanServerSystem &batterySystem )
+{
+ map<string,string>::const_iterator it;
+
+ for (it=keyValuePairs.begin(); it!=keyValuePairs.end(); it++)
+ {
+ vector<bool> states;
+
+ if (it->first=="01") {
+ readSingleByte(it->second, states);
+ batterySystem.availableBatteries = states;
+ }
+ else if (it->first=="02") {
+ readSingleByte(it->second, states);
+ batterySystem.chargingStates = states;
+ }
+ else if (it->first=="03") {
+ readSingleByte(it->second, states);
+ batterySystem.supplyingPowerStates = states;
+ }
+ else if (it->first=="04") {
+ // reserved, do nothing
+ }
+ else if (it->first=="05") {
+ readSingleByte(it->second, states);
+ batterySystem.chargePowerPresentStates = states;
+ }
+ else if (it->first=="06") {
+ readSingleByte(it->second, states);
+ batterySystem.powerNoGoodStates = states;
+ }
+ else if (it->first=="07") {
+ readSingleByte(it->second, states);
+ batterySystem.chargeInhibitedStates = states;
+ }
+ else
+ {
+ stringstream ss;
+ ss << "OceanServerParser: Unknown controller key: " << it->first;
+ throw ParsingException(ss.str().c_str());
+ }
+
+ }
+
+}
+
+void
+OceanServerParser::parseSingleBatteryData( const map<string,string> &keyValuePairs,
+ unsigned int batteryNum,
+ OceanServerSystem &batterySystem )
+{
+ map<string,string>::const_iterator it;
+
+ // get a reference to the battery whose fields we're updating
+ SmartBattery &bat = batterySystem.battery( batteryNum );
+
+ for (it=keyValuePairs.begin(); it!=keyValuePairs.end(); it++)
+ {
+ SmartBatteryDataField smartField = stringToSmartField( it->first );
+
+ switch( smartField )
+ {
+ case ManufacturerAccess:
+ bat.setManufacturerAccess( read16Flags( it->second ) ); break;
+ case RemainingCapacityAlarm:
+ bat.setRemainingCapacityAlarm( readCapacity( it->second ) ); break;
+ case RemainingTimeAlarm:
+ bat.setRemainingTimeAlarm( readMinutes( it->second ) ); break;
+ case BatteryMode:
+ bat.setBatteryMode( read16Flags( it->second ) ); break;
+ case AtRate:
+ bat.setAtRate( readRate( it->second ) ); break;
+ case AtRateTimeToFull:
+ bat.setAtRateTimeToFull( readMinutes( it->second ) ); break;
+ case AtRateTimeToEmpty:
+ bat.setAtRateTimeToEmpty( readMinutes( it->second ) ); break;
+ case AtRateOk:
+ bat.setAtRateOk( readBool( it->second ) ); break;
+ case Temperature:
+ bat.setTemperature( readTemperature( it->second ) ); break;
+ case Voltage:
+ bat.setVoltage( readVoltage( it->second ) ); break;
+ case Current:
+ bat.setCurrent( readCurrent( it->second ) ); break;
+ case AverageCurrent:
+ bat.setAverageCurrent( readCurrent( it->second ) ); break;
+ case MaxError:
+ bat.setMaxError( readPercentWord( it->second ) ); break;
+ case RelativeStateOfCharge:
+ bat.setRelativeStateOfCharge( readPercentWord( it->second ) ); break;
+ case AbsoluteStateOfCharge:
+ bat.setAbsoluteStateOfCharge( readPercentWord( it->second ) ); break;
+ case RemainingCapacity:
+ bat.setRemainingCapacity( readCapacity( it->second ) ); break;
+ case FullChargeCapacity:
+ bat.setFullChargeCapacity( readCapacity( it->second ) ); break;
+ case RunTimeToEmpty:
+ bat.setRunTimeToEmpty( readMinutes( it->second ) ); break;
+ case AverageTimeToEmpty:
+ bat.setAverageTimeToEmpty( readMinutes( it->second ) ); break;
+ case AverageTimeToFull:
+ bat.setAverageTimeToFull( readMinutes( it->second ) ); break;
+ case ChargingCurrent:
+ bat.setChargingCurrent( readCurrent( it->second ) ); break;
+ case ChargingVoltage:
+ bat.setChargingVoltage( readVoltage( it->second ) ); break;
+ case BatteryStatus:
+ bat.setBatteryStatus( read16Flags( it->second ) ); break;
+ case CycleCount:
+ bat.setCycleCount( readCount (it->second) ); break;
+ case DesignCapacity:
+ bat.setDesignCapacity( readCapacity( it->second ) ); break;
+ case DesignVoltage:
+ bat.setDesignVoltage( readVoltage( it->second ) ); break;
+ case SpecificationInfo:
+ bat.setSpecificationInfo( read16Flags( it->second ) ); break;
+ case ManufactureDate:
+ bat.setManufactureDate( read16Flags( it->second ) ); break;
+ case SerialNumber:
+ bat.setSerialNumber( readNumber( it->second ) ); break;
+ case ManufacturerName:
+ bat.setManufacturerName( it->second ); break;
+ case DeviceName:
+ bat.setDeviceName( it->second ); break;
+ case DeviceChemistry:
+ bat.setDeviceChemistry( it->second ); break;
+ case ManufacturerData:
+ bat.setManufacturerData( read16Flags( it->second ) ); break;
+ case NUM_SMARTBATTERY_FIELDS:
+ default:
+ stringstream ss; ss << "OceanServerParser: Unknown Battery key: " << it->first;
+ throw ParsingException( ss.str().c_str() );
+ }
+ }
+
+}
+
+void
+OceanServerParser::parseFields( vector<string> &fields,
+ OceanServerSystem &batterySystem )
+{
+ if (fields.size()==0) return;
+
+ // save the msgType string and remove from vector
+ string msgType = fields[0];
+ vector<string>::iterator it = fields.begin();
+ fields.erase( it );
+
+ // get the msg type key (S,C,B)
+ const string &msgTypeKey = msgType.substr(0,2);
+
+ // make key-value pairs
+ map<string,string> keyValuePairs;
+ toKeyValuePairs( fields, keyValuePairs, tracer_ );
+
+ if (msgTypeKey=="$S")
+ {
+ parseSystemData( keyValuePairs, batterySystem );
+ }
+ else if (msgTypeKey=="$C")
+ {
+ parseControllerData( keyValuePairs, batterySystem );
+ }
+ else if (msgTypeKey=="$B")
+ {
+ stringstream ss(msgType.substr(3));
+ int batteryNum;
+ ss >> batteryNum;
+ parseSingleBatteryData( keyValuePairs, batteryNum, batterySystem );
+ }
+ else
+ {
+ stringstream ss;
+ ss << "OceanServerParser: Unknown message type: " << msgTypeKey;
+ throw ParsingException(ss.str().c_str());
+ }
+}
+
+bool
+OceanServerParser::atBeginningOfRecord( const char* line )
+{
+ vector<string> tokens;
+ splitIntoFields( line, tokens, ",");
+
+ if (tokens.size()>0) {
+ if (tokens[0]!="$S") {
+ return false;
+ }
+ }
+ return true;
+}
+
+bool
+OceanServerParser::atEndOfRecord( const char* line )
+{
+ vector<string> tokens;
+ splitIntoFields( line, tokens, ",");
+
+ if (tokens.size()>0) {
+ if (tokens[0]=="$S") return true;
+ }
+ return false;
+}
+
+
+void
+OceanServerParser::parse( vector<string> &stringList,
+ OceanServerSystem &batterySystem )
+{
+
+ //
+ // Uncomment for DEBUG information
+ //
+
+ stringstream ss;
+ ss << "OceanServerParser: Received the following input: " << endl;
+ for (unsigned int i=0; i<stringList.size(); i++)
+ {
+ const string &str = stringList[i];
+ ss << i << ": " << str;
+
+ // output in hex
+ ss << i << " hex: ";
+ for (unsigned k=0; k<str.size(); k++)
+ {
+ unsigned int charValue = (unsigned int)str[k];
+ ss << str[k] << ": " << std::hex << charValue << " ";
+ }
+ ss << std::dec;
+
+ }
+ ss << endl;
+ tracer_.debug( ss.str(), 10 );
+
+ for (unsigned int i=0; i<stringList.size(); i++)
+ {
+ const string &line = stringList[i];
+
+ // check for control characters in the line (sometimes they are accidently inserted)
+ // don't check the last 2 characters: they're \0 and \n
+ for (unsigned int k=0; k<line.size()-2; k++)
+ {
+ if ( iscntrl( line[k] ) )
+ throw ParsingException("ERROR(oceanserverparser.cpp): Found a control character (binary) in the string!");
+ }
+
+ // divide the line into 2 parts: data and checksum (if present)
+ vector<string> checksumList;
+ splitIntoFields(line, checksumList, "%" );
+ if (checksumList.size()==2)
+ {
+ // we have a checksum, is it correct?
+ if (!isChecksumValid( checksumList[0], checksumList[1] ) )
+ throw ParsingException("ERROR(oceanserverparser.cpp): Checksum failed!");
+ }
+
+ // divide the data into individual fields and parse
+ if (checksumList.size()==0)
+ throw ParsingException("ERROR(oceanserverparser.cpp): String length is 0");
+ vector<string> fields;
+ splitIntoFields( checksumList[0], fields, ",");
+ parseFields( fields, batterySystem );
+ }
+}
+
+}
+
Added: gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverparser.h
===================================================================
--- gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverparser.h (rev 0)
+++ gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverparser.h 2008-06-25 07:27:56 UTC (rev 213)
@@ -0,0 +1,123 @@
+/*
+ * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
+ * http://gearbox.sf.net/
+ * Copyright (c) 2004-2008 Tobias Kaupp
+ *
+ * This distribution is licensed to you under the terms described in
+ * the LICENSE file included in this distribution.
+ *
+ */
+
+#ifndef OCEANSERVER_PARSER_H
+#define OCEANSERVER_PARSER_H
+
+#include <map>
+#include <gbxutilacfr/tracer.h>
+#include <gbxsmartbatteryacfr/smartbattery.h>
+
+namespace gbxsmartbatteryacfr
+{
+
+//!
+//! Class representing the OceanServer battery system data
+//! Contains average values of the whole system and values from individual batteries
+//!
+//! @author Tobias Kaupp
+//!
+class OceanServerSystem
+{
+ public:
+
+ OceanServerSystem();
+ ~OceanServerSystem() {};
+
+ // read access to all batteries
+ const std::map<int,SmartBattery>& batteries() const;
+
+ // easy write access to single battery, instantiates a new one if it doesn't exist
+ SmartBattery& battery( unsigned int batteryNumber );
+
+ // easy read access to single battery, battery must exist
+ const SmartBattery& battery( unsigned int batteryNumber ) const;
+
+ // erase a battery
+ void eraseBattery( unsigned int batteryNumber );
+
+ // average battery values
+ int percentCharge;
+ int minToEmpty;
+ std::string messageToSystem;
+
+ // values from the controller
+ // each vector is always of size 8 because oceanserver system has 8 slots
+ std::vector<bool> availableBatteries;
+ std::vector<bool> chargingStates;
+ std::vector<bool> supplyingPowerStates;
+ std::vector<bool> chargePowerPresentStates;
+ std::vector<bool> powerNoGoodStates;
+ std::vector<bool> chargeInhibitedStates;
+
+ private:
+ std::map<int,SmartBattery> batteries_;
+};
+
+//! Puts all available data into a human-readable string
+std::string toString( const OceanServerSystem &system );
+
+//! Puts all available data into a machine-readable ASCII string
+std::string toLogString( const OceanServerSystem &system );
+
+//! Updates all fields in 'to' with data from 'from'. Also reapes batteries in 'to' if they are not in 'from'.
+//! Has persistence capabilities: if fields in 'from' are not set and corresponding fields in 'to' are set, the ones in 'to' are kept.
+//! Use case: a class stores 'to' as a member variable, receives the latest records into 'from', calls this function to update 'to'.
+//! The reaping capability makes sure that battery modules which are no longer connected don't persist.
+void updateWithNewData( const OceanServerSystem &from,
+ OceanServerSystem &to );
+
+//!
+//! Class to parse the hex data the oceanserver battery controller spits out
+//!
+//! @author Tobias Kaupp
+//!
+class OceanServerParser
+{
+public:
+
+ OceanServerParser( gbxutilacfr::Tracer &tracer );
+ ~OceanServerParser() {};
+
+ //! Expects a full record of batterydata as a stringList (one line per string) produced by the oceanserver controller.
+ //! Parses each line and sets corresponding fields in batterySystem
+ void parse( std::vector<std::string> &stringList,
+ OceanServerSystem &batterySystem );
+
+ //! Checks whether the passed string (one line) is the first line of the record
+ bool atBeginningOfRecord( const char *string );
+
+ //! Checks whether the passed string (one line) is the end of the record
+ bool atEndOfRecord( const char *string );
+
+private:
+
+ gbxutilacfr::Tracer &tracer_;
+
+ // parsing functions
+ void parseFields( std::vector<std::string> &fields,
+ OceanServerSystem &batterySystem );
+
+ void parseSystemData( const std::map<std::string,std::string> &keyValuePairs,
+ OceanServerSystem &batterySystem);
+
+ void parseControllerData( const std::map<std::string,std::string> &keyValuePairs,
+ OceanServerSystem &batterySystem);
+
+ void parseSingleBatteryData( const std::map<std::string,std::string> &keyValuePairs,
+ unsigned int batteryNum,
+ OceanServerSystem &batterySystem);
+
+};
+
+
+} // namespace
+
+#endif
Added: gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverreader.cpp
===================================================================
--- gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverreader.cpp (rev 0)
+++ gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverreader.cpp 2008-06-25 07:27:56 UTC (rev 213)
@@ -0,0 +1,190 @@
+/*
+ * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
+ * http://gearbox.sf.net/
+ * Copyright (c) 2004-2008 Tobias Kaupp
+ *
+ * This distribution is licensed to you under the terms described in
+ * the LICENSE file included in this distribution.
+ *
+ */
+
+#include <iostream>
+#include <sstream>
+
+#include <gbxsmartbatteryacfr/exceptions.h>
+#include <gbxsmartbatteryacfr/oceanserverparser.h>
+
+#include "oceanserverreader.h"
+
+using namespace std;
+
+namespace gbxsmartbatteryacfr {
+
+static const int BAUDRATE = 19200;
+static const int TIMEOUT_SEC = 2;
+
+OceanServerReader::OceanServerReader( const string &device,
+ gbxutilacfr::Tracer &tracer )
+ : serial_( device, BAUDRATE, gbxserialacfr::Serial::Timeout(TIMEOUT_SEC,0) ),
+ tracer_(tracer),
+ parser_(tracer),
+ firstTime_(true)
+{
+ // some menu entries from the OceanServer system - used to recognize whether
+ // we are connected to the right device
+ oceanServerStrings_.push_back(" S - Setup Controller");
+ oceanServerStrings_.push_back(" B - Battery Status");
+ oceanServerStrings_.push_back(" X - Host HEX");
+ oceanServerStrings_.push_back(" H - Help");
+ oceanServerStrings_.push_back(" www.ocean-server.com");
+
+ checkConnection();
+
+ // send the command to start reading data
+ serial_.flush();
+
+ const char startReading = 'X';
+
+ serial_.write(&startReading, 1);
+}
+
+bool
+OceanServerReader::isOceanServerSystem( const char* oceanServerString )
+{
+ for (unsigned int i=0; i<oceanServerStrings_.size(); i++)
+ {
+ // if the first 8 characters agree we are pretty sure we have an OceanServerSystem
+ if (strncmp(oceanServerStrings_[i].c_str(),oceanServerString,8)==0) return true;
+ }
+ return false;
+}
+
+void
+OceanServerReader::checkConnection()
+{
+ tracer_.info( "OceanServerReader: Checking connection to serial port" );
+
+ // a blank will get us into menu mode
+ const char menuMode = ' ';
+ serial_.write(&menuMode, 1);
+
+ // if we were in battery reading mode before, we might have to skip quite
+ // a few lines until we get something from the menu
+ const int maxTries = 40;
+ int numTries=0;
+
+ // for tracer output
+ stringstream ss;
+
+ while(true)
+ {
+ ss.str(""); ss << "OceanServerReader: Trying to read from serial port with timeout of " << TIMEOUT_SEC << "s" << endl;
+ tracer_.info( ss.str() );
+
+ string serialData;
+ int ret = serial_.readLine( serialData );
+ if (ret<0) {
+ throw HardwareReadingException("Connected to the wrong serial port. Timed out while trying to read a line.");
+ }
+ if ( isOceanServerSystem(serialData.c_str()) ) {
+ tracer_.info( "Oceanserverreader.cpp: We are connected to an Oceanserver system. Good." );
+ break;
+ }
+ numTries++;
+ ss.str(""); ss << "OceanServerReader: Trying to find out whether this is an oceanserver system. Attempt number " << numTries << "/" << maxTries << ".";
+ tracer_.info( ss.str() );
+ if (numTries>=maxTries) {
+ throw HardwareReadingException("Connected to the wrong serial port. Didn't recognize any of the strings.");
+ }
+ }
+}
+
+OceanServerReader::~OceanServerReader()
+{
+}
+
+void
+OceanServerReader::tryToReadLineFromSerialPort( std::string &serialData )
+{
+ const int maxTries=5;
+ int numTries=0;
+
+ while(true)
+ {
+ int ret = serial_.readLine( serialData );
+
+ if (ret>0) break;
+
+ numTries++;
+ if (numTries>=maxTries) {
+ stringstream ss;
+ ss << "Can't read data from serial port. Timed out and/or empty strings " << maxTries << " times in a row.";
+ throw HardwareReadingException( ss.str().c_str() );
+ }
+ }
+}
+
+void
+OceanServerReader::read( OceanServerSystem &system )
+{
+ string serialData;
+ vector<string> stringList;
+
+ if (firstTime_)
+ {
+ // (1) Wait until we got the beginning of the record
+ while(true)
+ {
+ tryToReadLineFromSerialPort( serialData );
+ if (parser_.atBeginningOfRecord( serialData.c_str() )) break;
+ }
+
+ tracer_.debug( "OceanServerReader: Beginning of a new record", 5 );
+
+ // (2) Add the first line to the stringlist
+ stringList.push_back( serialData );
+ }
+ else
+ {
+
+ tracer_.debug( "OceanServerReader: We already have the first line from the previous record", 5 );
+ stringList.push_back( beginningRecordLine_ );
+ }
+
+ try {
+
+ // (3) Read the rest of the record line-by-line
+ while(true)
+ {
+ tryToReadLineFromSerialPort( serialData );
+ if ( parser_.atEndOfRecord( serialData.c_str() ) )
+ {
+ tracer_.debug( "OceanServerReader: End of record", 5 );
+ parser_.parse( stringList, system );
+ break;
+ }
+ stringList.push_back(serialData);
+ }
+
+ // (4) Save the last line for next time, it's the S-record
+ // Otherwise we'd miss a record
+ beginningRecordLine_ = serialData;
+ if (firstTime_) {
+ firstTime_ = false;
+ }
+ }
+ catch (ParsingException &e)
+ {
+ stringstream ss;
+ ss << "OceanServerReader: Caught ParsingException: " << e.what() << ". ";
+ ss << "It's not critical, we are trying to find the beginning of a new record.";
+ tracer_.warning( ss.str() );
+ firstTime_ = true;
+
+ // we have to rethrow, so that the caller knows that it may receive a corrupt record
+ throw;
+ }
+}
+
+}
+
Added: gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverreader.h
===================================================================
--- gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverreader.h (rev 0)
+++ gearbox/trunk/submitted/gbxsmartbatteryacfr/oceanserverreader.h 2008-06-25 07:27:56 UTC (rev 213)
@@ -0,0 +1,62 @@
+/*
+ * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
+ * http://gearbox.sf.net/
+ * Copyright (c) 2004-2008 Tobias Kaupp
+ *
+ * This distribution is licensed to you under the terms described in
+ * the LICENSE file included in this distribution.
+ *
+ */
+
+#ifndef OCEANSERVER_READER_H
+#define OCEANSERVER_READER_H
+
+#include <string>
+#include <gbxserialacfr/serial.h>
+#include <gbxutilacfr/tracer.h>
+
+#include <gbxsmartbatteryacfr/oceanserverparser.h>
+
+
+namespace gbxsmartbatteryacfr
+{
+
+//!
+//! Class to read data from the oceanserver battery system:
+//! (1) connects to the serial port
+//! (2) reads from the serial port and parses data
+//!
+//! @author Tobias Kaupp
+//!
+class OceanServerReader
+{
+public:
+
+ //! May throw SerialPortException
+ OceanServerReader( const std::string &device,
+ gbxutilacfr::Tracer &tracer );
+
+ ~OceanServerReader();
+
+ //! May throw HardwareReadingException
+ void read( OceanServerSystem &system );
+
+private:
+
+ bool isOceanServerSystem( const char* oceanServerString );
+ std::vector<std::string> oceanServerStrings_;
+
+ gbxserialacfr::Serial serial_;
+ gbxutilacfr::Tracer& tracer_;
+ gbxsmartbatteryacfr::OceanServerParser parser_;
+
+ void checkConnection();
+ void tryToReadLineFromSerialPort( std::string &serialData );
+
+ std::string beginningRecordLine_;
+ bool firstTime_;
+};
+
+} // namespace
+
+#endif
Added: gearbox/trunk/submitted/gbxsmartbatteryacfr/smartbattery.cpp
===================================================================
--- gearbox/trunk/submitted/gbxsmartbatteryacfr/smartbattery.cpp (rev 0)
+++ gearbox/trunk/submitted/gbxsmartbatteryacfr/smartbattery.cpp 2008-06-25 07:27:56 UTC (rev 213)
@@ -0,0 +1,257 @@
+/*
+ * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
+ * http://gearbox.sf.net/
+ * Copyright (c) 2004-2008 Tobias Kaupp
+ *
+ * This distribution is licensed to you under the terms described in
+ * the LICENSE file included in this distribution.
+ *
+ */
+
+#include <iostream>
+#include <sstream>
+
+#include "smartbattery.h"
+
+using namespace std;
+
+namespace gbxsmartbatteryacfr {
+
+SmartBatteryDataField stringToSmartField( const string &fieldStr )
+{
+ if (fieldStr=="00") return ManufacturerAccess;
+ else if (fieldStr=="01") return RemainingCapacityAlarm;
+ else if (fieldStr=="02") return RemainingTimeAlarm;
+ else if (fieldStr=="03") return BatteryMode;
+ else if (fieldStr=="04") return AtRate;
+ else if (fieldStr=="05") return AtRateTimeToFull;
+ else if (fieldStr=="06") return AtRateTimeToEmpty;
+ else if (fieldStr=="07") return AtRateOk;
+ else if (fieldStr=="08") return Temperature;
+ else if (fieldStr=="09") return Voltage;
+ else if (fieldStr=="0A") return Current;
+ else if (fieldStr=="0B") return AverageCurrent;
+ else if (fieldStr=="0C") return MaxError;
+ else if (fieldStr=="0D") return RelativeStateOfCharge;
+ else if (fieldStr=="0E") return AbsoluteStateOfCharge;
+ else if (fieldStr=="0F") return RemainingCapacity;
+ else if (fieldStr=="10") return FullChargeCapacity;
+ else if (fieldStr=="11") return RunTimeToEmpty;
+ else if (fieldStr=="12") return AverageTimeToEmpty;
+ else if (fieldStr=="13") return AverageTimeToFull;
+ else if (fieldStr=="14") return ChargingCurrent;
+ else if (fieldStr=="15") return ChargingVoltage;
+ else if (fieldStr=="16") return BatteryStatus;
+ else if (fieldStr=="17") return CycleCount;
+ else if (fieldStr=="18") return DesignCapacity;
+ else if (fieldStr=="19") return DesignVoltage;
+ else if (fieldStr=="1A") return SpecificationInfo;
+ else if (fieldStr=="1B") return ManufactureDate;
+ else if (fieldStr=="1C") return SerialNumber;
+ else if (fieldStr=="20") return ManufacturerName;
+ else if (fieldStr=="21") return DeviceName;
+ else if (fieldStr=="22") return DeviceChemistry;
+ else if (fieldStr=="23") return ManufacturerData;
+ else cout << "ERROR: Unknown field: " << fieldStr << endl;
+
+ return NUM_SMARTBATTERY_FIELDS;
+}
+
+string toString( const SmartBattery &b )
+{
+ stringstream ss;
+
+ if ( b.has( ManufacturerAccess ) )
+ ss << "ManufacturerAccess (16 bits): " << std::hex << b.manufacturerAccess() << endl;
+ if ( b.has( RemainingCapacityAlarm ) )
+ ss << "RemainingCapacityAlarm (mAh or 10mWh): " << std::dec << b.remainingCapacityAlarm() << endl;
+ if ( b.has( RemainingTimeAlarm ) )
+ ss << "RemainingTimeAlarm (min): " << std::dec << b.remainingTimeAlarm() << endl;
+ if ( b.has( BatteryMode ) )
+ ss << "BatteryMode (16 bits): " << std::hex << b.batteryMode() << endl;
+ if ( b.has( AtRate ) )
+ ss << "AtRate (mA or 10mW): " << std::dec << b.atRate() << endl;
+ if ( b.has( AtRateTimeToFull ) )
+ ss << "AtRateTimeToFull (min): " << std::dec << b.atRateTimeToFull() << endl;
+ if ( b.has( AtRateTimeToEmpty ) )
+ ss << "AtRateTimeToEmpty (min): " << std::dec << b.atRateTimeToEmpty() << endl;
+ if ( b.has( AtRateOk ) )
+ ss << "AtRateOk (bool): " << std::dec << b.atRateOk() << endl;
+ if ( b.has( Temperature ) )
+ ss << "Temperature (degC): " << std::dec << b.temperature() << endl;
+ if ( b.has( Voltage ) )
+ ss << "Voltage (V): " << std::dec << b.voltage() << endl;
+ if ( b.has( Current ) )
+ ss << "Current (A): " << std::dec << b.current() << endl;
+ if ( b.has( AverageCurrent ) )
+ ss << "AverageCurrent (A): " << std::dec << b.averageCurrent() << endl;
+ if ( b.has( MaxError ) )
+ ss << "MaxError (%): " << std::dec << b.maxError() << endl;
+ if ( b.has( RelativeStateOfCharge ) )
+ ss << "RelativeStateOfCharge (%): " << std::dec << b.relativeStateOfCharge() << endl;
+ if ( b.has( AbsoluteStateOfCharge ) )
+ ss << "AbsoluteStateOfCharge (%): " << std::dec << b.absoluteStateOfCharge() << endl;
+ if ( b.has( RemainingCapacity ) )
+ ss << "RemainingCapacity (mAh or 10 mWh): " << std::dec << b.remainingCapacity() << endl;
+ if ( b.has( FullChargeCapacity ) )
+ ss << "FullChargeCapacity (mAh or 10 mWh): " << std::dec << b.fullChargeCapacity() << endl;
+ if ( b.has( RunTimeToEmpty ) )
+ ss << "RunTimeToEmpty (min): " << std::dec << b.runTimeToEmpty() << endl;
+ if ( b.has( AverageTimeToEmpty ) )
+ ss << "AverageTimeToEmpty (min): " << std::dec << b.averageTimeToEmpty() << endl;
+ if ( b.has( AverageTimeToFull ) )
+ ss << "AverageTimeToFull (min): " << std::dec << b.averageTimeToFull() << endl;
+ if ( b.has( ChargingCurrent ) )
+ ss << "ChargingCurrent (A): " << std::dec << b.chargingCurrent() << endl;
+ if ( b.has( ChargingVoltage ) )
+ ss << "ChargingVoltage (V): " << std::dec << b.chargingVoltage() << endl;
+ if ( b.has( BatteryStatus ) )
+ ss << "BatteryStatus (16 bits): " << std::hex << b.batteryStatus() << endl;
+ if ( b.has( CycleCount ) )
+ ss << "CycleCount (number): " << std::dec << b.cycleCount() << endl;
+ if ( b.has( DesignCapacity ) )
+ ss << "DesignCapacity (mAh or 10 mWh): " << std::dec << b.designCapacity() << endl;
+ if ( b.has( DesignVoltage ) )
+ ss << "DesignVoltage (V): " << std::dec << b.designVoltage() << endl;
+ if ( b.has( SpecificationInfo ) )
+ ss << "SpecificationInfo (16 bits): " << std::hex << b.specificationInfo() << endl;
+ if ( b.has( ManufactureDate ) )
+ ss << "ManufactureDate (16 bits): " << std::hex << b.manufactureDate() << endl;
+ if ( b.has( SerialNumber ) )
+ ss << "SerialNumber (number): " << std::dec << b.serialNumber() << endl;
+ if ( b.has( ManufacturerName ) )
+ ss << "ManufacturerName: " << b.manufacturerName() << endl;
+ if ( b.has( DeviceName ) )
+ ss << "DeviceName: " << b.deviceName() << endl;
+ if ( b.has( DeviceChemistry ) )
+ ss << "DeviceChemistry: " << b.deviceChemistry() << endl;
+ if ( b.has( ManufacturerData ) )
+ ss << "ManufacturerData (16 bits): " << std::hex << b.manufacturerData() << endl;
+
+ return ss.str();
+}
+
+// For logging, we want a data entry per field, so it's easier to parse.
+// For cases where data is not available (NA), we define "special" data entries for each type.
+// This is a bit sketchy but ok for the purpose of simpler logging.
+const int NA_DEC = -999;
+const uint16_t NA_HEX = 11111;
+const std::string NA_STR = "XXXX";
+
+string toLogString( const SmartBattery &b )
+{
+ stringstream ss;
+
+ if ( b.has( ManufacturerAccess ) ) ss << std::hex << b.manufacturerAccess();
+ else ss << std::hex << NA_HEX; ss << " ";
+
+ if ( b.has( RemainingCapacityAlarm ) ) ss << std::dec << b.remainingCapacityAlarm();
+ else ss << std::dec << NA_DEC; ss << " ";
+
+ if ( b.has( RemainingTimeAlarm ) ) ss << std::dec << b.remainingTimeAlarm();
+ else ss << std::dec << NA_DEC; ss << " ";
+
+ if ( b.has( BatteryMode ) ) ss << std::hex << b.batteryMode();
+ else ss << std::hex << NA_HEX; ss << " ";
+
+ if ( b.has( AtRate ) ) ss << std::dec << b.atRate();
+ else ss << std::dec << NA_DEC; ss << " ";
+
+ if ( b.has( AtRateTimeToFull ) ) ss << std::dec << b.atRateTimeToFull();
+ else ss << std::dec << NA_DEC; ss << " ";
+
+ if ( b.has( AtRateTimeToEmpty ) ) ss << std::dec << b.atRateTimeToEmpty();
+ else ss << std::dec << NA_DEC; ss << " ";
+
+ if ( b.has( AtRateOk ) ) ss << std::dec << b.atRateOk();
+ else ss << std::dec << NA_DEC; ss << " ";
+
+ if ( b.has( Temperature ) ) ss << std::dec << b.temperature();
+ else ss << std::dec << NA_DEC; ss << " ";
+
+ if ( b.has( Voltage ) ) ss << std::dec << b.voltage();
+ else ss << std::dec << NA_DEC; ss << " ";
+
+ if ( b.has( Current ) ) ss << std::dec << b.current();
+ else ss << std::dec << NA_DEC; ss << " ";
+
+ if ( b.has( AverageCurrent ) ) ss << std::dec << b.averageCurrent();
+ else ss << std::dec << NA_DEC; ss << " ";
+
+ if ( b.has( MaxError ) ) ss << std::dec << b.maxError();
+ else ss << std::dec << NA_DEC; ss << " ";
+
+ if ( b.has( RelativeStateOfCharge ) ) ss << std::dec << b.relativeStateOfCharge();
+ else ss << std::dec << NA_DEC; ss << " ";
+
+ if ( b.has( AbsoluteStateOfCharge ) ) ss << std::dec << b.absoluteStateOfCharge();
+ else ss << std::dec << NA_DEC; ss << " ";
+
+ if ( b.has( RemainingCapacity ) ) ss << std::dec << b.remainingCapacity();
+ else ss << std::dec << NA_DEC; ss << " ";
+
+ if ( b.has( FullChargeCapacity ) ) ss << std::dec << b.fullChargeCapacity();
+ else ss << std::dec << NA_DEC; ss << " ";
+
+ if ( b.has( RunTimeToEmpty ) ) ss << std::dec << b.runTimeToEmpty();
+ else ss << std::dec << NA_DEC; ss << " ";
+
+ if ( b.has( AverageTimeToEmpty ) ) ss << std::dec << b.averageTimeToEmpty();
+ else ss << std::dec << NA_DEC; ss << " ";
+
+ if ( b.has( AverageTimeToFull ) ) ss << std::dec << b.averageTimeToFull();
+ else ss << std::dec << NA_DEC; ss << " ";
+
+ if ( b.has( ChargingCurrent ) ) ss << std::dec << b.chargingCurrent();
+ else ss << std::dec << NA_DEC; ss << " ";
+
+ if ( b.has( ChargingVoltage ) ) ss << std::dec << b.chargingVoltage();
+ else ss << std::dec << NA_DEC; ss << " ";
+
+ if ( b.has( BatteryStatus ) ) ss << std::hex << b.batteryStatus();
+ else ss << std::hex << NA_HEX; ss << " ";
+
+ if ( b.has( CycleCount ) ) ss << std::dec << b.cycleCount();
+ else ss << std::dec << NA_DEC; ss << " ";
+
+ if ( b.has( DesignCapacity ) ) ss << std::dec << b.designCapacity();
+ else ss << std::dec << NA_DEC; ss << " ";
+
+ if ( b.has( DesignVoltage ) ) ss << std::dec << b.designVoltage();
+ else ss << std::dec << NA_DEC; ss << " ";
+
+ if ( b.has( SpecificationInfo ) ) ss << std::hex << b.specificationInfo();
+ else ss << std::hex << NA_HEX; ss << " ";
+
+ if ( b.has( ManufactureDate ) ) ss << std::hex << b.manufactureDate();
+ else ss << std::hex << NA_HEX; ss << " ";
+
+ if ( b.has( SerialNumber ) ) ss << std::dec << b.serialNumber();
+ else ss << std::dec << NA_DEC; ss << " ";
+
+ if ( b.has( ManufacturerName ) ) ss << b.manufacturerName();
+ else ss << std::dec << NA_STR; ss << " ";
+
+ if ( b.has( DeviceName ) ) ss << b.deviceName();
+ else ss << std::dec << NA_STR; ss << " ";
+
+ if ( b.has( DeviceChemistry ) ) ss << b.deviceChemistry();
+ else ss << std::dec << NA_STR; ss << " ";
+
+ if ( b.has( ManufacturerData ) ) ss << std::hex << b.manufacturerData();
+ else ss << std::hex << NA_HEX;
+
+ return ss.str();
+}
+
+bool
+SmartBattery::has( SmartBatteryDataField field ) const
+{
+ if ( (int)field >= (int)(has_.size()) ) {
+ cout << "has(): field=" << field << ", has_.size=" << has_.size() << endl;
+ }
+ assert( (int)field < (int)(has_.size()) );
+ return has_[field];
+};
+
+}
Added: gearbox/trunk/submitted/gbxsmartbatteryacfr/smartbattery.h
===================================================================
--- gearbox/trunk/submitted/gbxsmartbatteryacfr/smartbattery.h (rev 0)
+++ gearbox/trunk/submitted/gbxsmartbatteryacfr/smartbattery.h 2008-06-25 07:27:56 UTC (rev 213)
@@ -0,0 +1,223 @@
+/*
+ * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
+ * http://gearbox.sf.net/
+ * Copyright (c) 2004-2008 Tobias Kaupp
+ *
+ * This distribution is licensed to you under the terms described in
+ * the LICENSE file included in this distribution.
+ *
+ */
+
+#ifndef SMARTBATTERY_H
+#define SMARTBATTERY_H
+
+#include <vector>
+#include <string>
+#include <assert.h>
+
+namespace gbxsmartbatteryacfr {
+
+//! Smart battery data specification
+//! Table of fields can be found at http://sbs-forum.org/specs/
+enum SmartBatteryDataField
+{
+ ManufacturerAccess = 0,
+ RemainingCapacityAlarm,
+ RemainingTimeAlarm,
+ BatteryMode,
+ AtRate,
+ AtRateTimeToFull,
+ AtRateTimeToEmpty,
+ AtRateOk,
+ Temperature,
+ Voltage,
+ Current,
+ AverageCurrent,
+ MaxError,
+ RelativeStateOfCharge,
+ AbsoluteStateOfCharge,
+ RemainingCapacity,
+ FullChargeCapacity,
+ RunTimeToEmpty,
+ AverageTimeToEmpty,
+ AverageTimeToFull,
+ ChargingCurrent,
+ ChargingVoltage,
+ BatteryStatus,
+ CycleCount,
+ DesignCapacity,
+ DesignVoltage,
+ SpecificationInfo,
+ ManufactureDate,
+ SerialNumber,
+ ManufacturerName,
+ DeviceName,
+ DeviceChemistry,
+ ManufacturerData,
+ NUM_SMARTBATTERY_FIELDS
+};
+
+//! Converts a string to a SmartBatteryDataField, returns NUM_SMARTBATTERY_FIELDS if string is not valid
+SmartBatteryDataField stringToSmartField( const std::string &fieldStr );
+
+//! SmartBattery class holds all the data of a single smart battery
+//! Since not all data is always present, access to data needs to be done as follows:
+//! if (has(Temperature)) { myTemp = smartBattery.temperature(); }
+class SmartBattery
+{
+ public:
+ SmartBattery() : has_(NUM_SMARTBATTERY_FIELDS)
+ { std::fill( has_.begin(), has_.end(), false ); };
+
+ bool has( SmartBatteryDataField field ) const;
+
+ uint16_t manufacturerAccess() const { assert(has_[ManufacturerAccess]); return manufacturerAccess_; };
+ void setManufacturerAccess( uint16_t manufacturerAccess ) { has_[ManufacturerAccess] = true; manufacturerAccess_ = manufacturerAccess; };
+
+ int remainingCapacityAlarm() const { assert(has_[RemainingCapacityAlarm]); return remainingCapacityAlarm_; };
+ void setRemainingCapacityAlarm( int remainingCapacityAlarm ) { has_[RemainingCapacityAlarm] = true; remainingCapacityAlarm_ = remainingCapacityAlarm; };
+
+ int remainingTimeAlarm() const { assert(has_[RemainingTimeAlarm]); return remainingTimeAlarm_; };
+ void setRemainingTimeAlarm( int remainingTimeAlarm ) { has_[RemainingTimeAlarm] = true; remainingTimeAlarm_ = remainingTimeAlarm; };
+
+ uint16_t batteryMode() const { assert(has_[BatteryMode]); return batteryMode_; };
+ void setBatteryMode( uint16_t batteryMode ) { has_[BatteryMode] = true; batteryMode_ = batteryMode; };
+
+ int atRate() const { assert(has_[AtRate]); return atRate_; };
+ void setAtRate( int atRate ) { has_[AtRate] = true; atRate_ = atRate; };
+
+ int atRateTimeToFull() const { assert(has_[AtRateTimeToFull]); return atRateTimeToFull_; };
+ void setAtRateTimeToFull( int atRateTimeToFull ) { has_[AtRateTimeToFull] = true; atRateTimeToFull_ = atRateTimeToFull; };
+
+ int atRateTimeToEmpty() const { assert(has_[AtRateTimeToEmpty]); return atRateTimeToEmpty_; };
+ void setAtRateTimeToEmpty( int atRateTimeToEmpty ) { has_[AtRateTimeToEmpty] = true; atRateTimeToEmpty_ = atRateTimeToEmpty; };
+
+ bool atRateOk() const { assert(has_[AtRateOk]); return atRateOk_; };
+ void setAtRateOk( bool atRateOk ) { has_[AtRateOk] = true; atRateOk_ = atRateOk; };
+
+ double temperature() const { assert(has_[Temperature]); return temperature_; };
+ void setTemperature( double temperature ) { has_[Temperature] = true; temperature_ = temperature; };
+
+ double voltage() const { assert(has_[Voltage]); return voltage_; };
+ void setVoltage( double voltage ) { has_[Voltage] = true; voltage_ = voltage; };
+
+ double current() const { assert(has_[Current]); return current_; };
+ void setCurrent( double current ) { has_[Current] = true; current_ = current; };
+
+ double averageCurrent() const { assert(has_[AverageCurrent]); return averageCurrent_; };
+ void setAverageCurrent( double averageCurrent ) { has_[AverageCurrent] = true; averageCurrent_ = averageCurrent; };
+
+ int maxError() const { assert(has_[MaxError]); return maxError_; };
+ void setMaxError( int maxError ) { has_[MaxError] = true; maxError_ = maxError; };
+
+ int relativeStateOfCharge() const { assert(has_[RelativeStateOfCharge]); return relativeStateOfCharge_; };
+ void setRelativeStateOfCharge( int relativeStateOfCharge ) { has_[RelativeStateOfCharge] = true; relativeStateOfCharge_ = relativeStateOfCharge; };
+
+ int absoluteStateOfCharge() const { assert(has_[AbsoluteStateOfCharge]); return absoluteStateOfCharge_; };
+ void setAbsoluteStateOfCharge( int absoluteStateOfCharge ) { has_[AbsoluteStateOfCharge] = true; absoluteStateOfCharge_ = absoluteStateOfCharge; };
+
+ int remainingCapacity() const { assert(has_[RemainingCapacity]); return remainingCapacity_; };
+ void setRemainingCapacity( int remainingCapacity ) { has_[RemainingCapacity] = true; remainingCapacity_ = remainingCapacity; };
+
+ int fullChargeCapacity() const { assert(has_[FullChargeCapacity]); return fullChargeCapacity_; };
+ void setFullChargeCapacity( int fullChargeCapacity ) { has_[FullChargeCapacity] = true; fullChargeCapacity_ = fullChargeCapacity; };
+
+ int runTimeToEmpty() const { assert(has_[RunTimeToEmpty]); return runTimeToEmpty_; };
+ void setRunTimeToEmpty( int runTimeToEmpty ) { has_[RunTimeToEmpty] = true; runTimeToEmpty_ = runTimeToEmpty; };
+
+ int averageTimeToEmpty() const { assert(has_[AverageTimeToEmpty]); return averageTimeToEmpty_; };
+ void setAverageTimeToEmpty( int averageTimeToEmpty ) { has_[AverageTimeToEmpty] = true; averageTimeToEmpty_ = averageTimeToEmpty; };
+
+ int averageTimeToFull() const { assert(has_[AverageTimeToFull]); return averageTimeToFull_; };
+ void setAverageTimeToFull( int averageTimeToFull ) { has_[AverageTimeToFull] = true; averageTimeToFull_ = averageTimeToFull; };
+
+ double chargingCurrent() const { assert(has_[ChargingCurrent]); return chargingCurrent_; };
+ void setChargingCurrent( double chargingCurrent ) { has_[ChargingCurrent] = true; chargingCurrent_ = chargingCurrent; };
+
+ double chargingVoltage() const { assert(has_[ChargingVoltage]); return chargingVoltage_; };
+ void setChargingVoltage( double chargingVoltage ) { has_[ChargingVoltage] = true; chargingVoltage_ = chargingVoltage; };
+
+ uint16_t batteryStatus() const { assert(has_[BatteryStatus]); return batteryStatus_; };
+ void setBatteryStatus( uint16_t batteryStatus ) { has_[BatteryStatus] = true; batteryStatus_ = batteryStatus; };
+
+ int cycleCount() const { assert(has_[CycleCount]); return cycleCount_; };
+ void setCycleCount( int cycleCount ) { has_[CycleCount] = true; cycleCount_ = cycleCount; };
+
+ int designCapacity() const { assert(has_[DesignCapacity]); return designCapacity_; };
+ void setDesignCapacity( int designCapacity ) { has_[DesignCapacity] = true; designCapacity_ = designCapacity; };
+
+ double designVoltage() const { assert(has_[DesignVoltage]); return designVoltage_; };
+ void setDesignVoltage( double designVoltage ) { has_[DesignVoltage] = true; designVoltage_ = designVoltage; };
+
+ uint16_t specificationInfo() const { assert(has_[SpecificationInfo]); return specificationInfo_; };
+ void setSpecificationInfo( uint16_t specificationInfo ) { has_[SpecificationInfo] = true; specificationInfo_ = specificationInfo; };
+
+ ui...
[truncated message content] |
|
From: <gb...@us...> - 2008-06-24 14:57:35
|
Revision: 212
http://gearbox.svn.sourceforge.net/gearbox/?rev=212&view=rev
Author: gbiggs
Date: 2008-06-24 07:57:10 -0700 (Tue, 24 Jun 2008)
Log Message:
-----------
Renaming urg_nz to hokuyo_aist.
Modified Paths:
--------------
gearbox/trunk/submitted/flexiport/test/CMakeLists.txt
gearbox/trunk/submitted/flexiport/test/example.cmake.in
gearbox/trunk/submitted/hokuyo_aist/CMakeLists.txt
gearbox/trunk/submitted/hokuyo_aist/doc.dox
gearbox/trunk/submitted/hokuyo_aist/hokuyo_aist.cpp
gearbox/trunk/submitted/hokuyo_aist/hokuyo_aist.h
gearbox/trunk/submitted/hokuyo_aist/test/CMakeLists.txt
gearbox/trunk/submitted/hokuyo_aist/test/example.cmake.in
gearbox/trunk/submitted/hokuyo_aist/test/example.cpp
gearbox/trunk/submitted/hokuyo_aist/test/example.readme
Removed Paths:
-------------
gearbox/trunk/submitted/hokuyo_aist/urg_nz.cpp
gearbox/trunk/submitted/hokuyo_aist/urg_nz.h
Modified: gearbox/trunk/submitted/flexiport/test/CMakeLists.txt
===================================================================
--- gearbox/trunk/submitted/flexiport/test/CMakeLists.txt 2008-06-24 14:24:38 UTC (rev 211)
+++ gearbox/trunk/submitted/flexiport/test/CMakeLists.txt 2008-06-24 14:57:10 UTC (rev 212)
@@ -7,4 +7,4 @@
TARGET_LINK_LIBRARIES (serial_example flexiport)
GBX_ADD_EXAMPLE (flexiport/example example.cmake.in example.cmake
- tcp_example.cpp serial_example.cpp example.readme)
+ tcp_example.cpp serial_example.cpp example.readme example.logr example.logw)
Modified: gearbox/trunk/submitted/flexiport/test/example.cmake.in
===================================================================
--- gearbox/trunk/submitted/flexiport/test/example.cmake.in 2008-06-24 14:24:38 UTC (rev 211)
+++ gearbox/trunk/submitted/flexiport/test/example.cmake.in 2008-06-24 14:57:10 UTC (rev 212)
@@ -1,3 +1,5 @@
+CMAKE_MINIMUM_REQUIRED (VERSION 2.4)
+
PROJECT (FlexiPortExample)
INCLUDE_DIRECTORIES (@CMAKE_INSTALL_PREFIX@)
@@ -15,4 +17,3 @@
LINK_FLAGS "-L@CMAKE_INSTALL_PREFIX@/lib/gearbox"
INSTALL_RPATH "${INSTALL_RPATH};@CMAKE_INSTALL_PREFIX@/lib/gearbox"
BUILD_WITH_INSTALL_RPATH TRUE)
-
\ No newline at end of file
Modified: gearbox/trunk/submitted/hokuyo_aist/CMakeLists.txt
===================================================================
--- gearbox/trunk/submitted/hokuyo_aist/CMakeLists.txt 2008-06-24 14:24:38 UTC (rev 211)
+++ gearbox/trunk/submitted/hokuyo_aist/CMakeLists.txt 2008-06-24 14:57:10 UTC (rev 212)
@@ -1,5 +1,5 @@
-SET (libName urg_nz)
-set (libDesc "Hokuyo URG laser scanner driver")
+SET (libName hokuyo_aist)
+set (libDesc "Hokuyo laser scanner driver")
GBX_ADD_LICENSE (GPL)
SET (build TRUE)
@@ -11,8 +11,8 @@
IF (build)
INCLUDE (${GBX_CMAKE_DIR}/UseBasicRules.cmake)
- SET (hdrs urg_nz.h)
- SET (srcs urg_nz.cpp)
+ SET (hdrs hokuyo_aist.h)
+ SET (srcs hokuyo_aist.cpp)
IF (WIN32)
ADD_DEFINITIONS (-DURG_NZ_EXPORTS)
Modified: gearbox/trunk/submitted/hokuyo_aist/doc.dox
===================================================================
--- gearbox/trunk/submitted/hokuyo_aist/doc.dox 2008-06-24 14:24:38 UTC (rev 211)
+++ gearbox/trunk/submitted/hokuyo_aist/doc.dox 2008-06-24 14:57:10 UTC (rev 212)
@@ -3,16 +3,17 @@
@ingroup gbx_libs
@ingroup gbx_hardware
@ingroup gbx_cpp
-@defgroup gbx_library_urg_nz liburg_nz
+@defgroup gbx_library_hokuyo_aist libhokuyo_aist
@brief Hokuyo URG laser scanner driver.
This library provides a driver for Hokuyo laser scanner devices using the SCIP protocol version 1 or
-2. It has been tested with the Hokuyo URG-04LX, but it should work with any scanner that conforms to
-these protocol versions. For a full list of functions and classes see @ref urg_nz.
+2. It has been tested with the Hokuyo URG-04LX and UHG-08LX, but it should work with any scanner
+that conforms to these protocol versions. For a full list of functions and classes see
+@ref hokuyo_aist.
Header file:
@verbatim
-#include <urg_nz/urg_nz.h>
+#include <hokuyo_aist/hokuyo_aist.h>
@endverbatim
@par Example
@@ -44,15 +45,15 @@
LGPL
@par Dependencies
- @ref FlexiPort
+ @ref Flexiport
*/
/*!
-@namespace urg_nz
-@brief URG laser scanner driver name space.
+@namespace hokuyo_aist
+@brief Hokuyo laser scanner driver name space.
-This namespace is part of a library which provides a driver for the URG laser scanner driver.
+This namespace is part of a library which provides a driver for the Hokuyo laser scanner driver.
-@see @ref gbx_library_urg_nz
+@see @ref gbx_library_hokuyo_aist
*/
Modified: gearbox/trunk/submitted/hokuyo_aist/hokuyo_aist.cpp
===================================================================
--- gearbox/trunk/submitted/hokuyo_aist/hokuyo_aist.cpp 2008-06-24 14:24:38 UTC (rev 211)
+++ gearbox/trunk/submitted/hokuyo_aist/hokuyo_aist.cpp 2008-06-24 14:57:10 UTC (rev 212)
@@ -3,7 +3,7 @@
* http://gearbox.sf.net/
* Copyright (c) 2008 Geoffrey Biggs
*
- * urg_nz Hokuyo URG laser scanner driver.
+ * hokuyo_aist Hokuyo laser scanner driver.
*
* This distribution is licensed to you under the terms described in the LICENSE file included in
* this distribution.
@@ -11,22 +11,22 @@
* This work is a product of the National Institute of Advanced Industrial Science and Technology,
* Japan. Registration number: ___
*
- * This file is part of urg_nz.
+ * This file is part of hokuyo_aist.
*
- * urg_nz is free software: you can redistribute it and/or modify it under the terms of the GNU
+ * hokuyo_aist is free software: you can redistribute it and/or modify it under the terms of the GNU
* Lesser General Public License as published by the Free Software Foundation, either version 3 of
* the License, or (at your option) any later version.
*
- * urg_nz is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even
- * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser
- * General Public License for more details.
+ * hokuyo_aist is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without
+ * even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
*
- * You should have received a copy of the GNU Lesser General Public License along with urg_nz. If
- * not, see <http://www.gnu.org/licenses/>.
+ * You should have received a copy of the GNU Lesser General Public License along with hokuyo_aist.
+ * If not, see <http://www.gnu.org/licenses/>.
*/
-#include "urg_nz.h"
-using namespace urg_nz;
+#include "hokuyo_aist.h"
+using namespace hokuyo_aist;
#include <flexiport/flexiport.h>
#include <flexiport/port.h>
@@ -44,7 +44,7 @@
#define __func__ __FUNCTION__
#endif
-namespace urg_nz
+namespace hokuyo_aist
{
#ifndef M_PI
@@ -412,50 +412,50 @@
}
////////////////////////////////////////////////////////////////////////////////////////////////////
-// URGError class
+// HokuyoError class
////////////////////////////////////////////////////////////////////////////////////////////////////
-string URGError::AsString (void) const throw ()
+string HokuyoError::AsString (void) const throw ()
{
switch (_errorCode)
{
- case URG_ERR_READ:
- return "URG_ERR_READ";
- case URG_ERR_WRITE:
- return "URG_ERR_WRITE";
- case URG_ERR_PROTOCOL:
- return "URG_ERR_PROTOCOL";
- case URG_ERR_CHANGEBAUD:
- return "URG_ERR_CHANGEBAUD";
- case URG_ERR_CONNECT_FAILED:
- return "URG_ERR_CONNECT_FAILED";
- case URG_ERR_CLOSE_FAILED:
- return "URG_ERR_CLOSE_FAILED";
- case URG_ERR_NODESTINATION:
- return "URG_ERR_NODESTINATION";
- case URG_ERR_BADFIRMWARE:
- return "URG_ERR_BADFIRMWARE";
- case URG_ERR_SCIPVERSION:
- return "URG_ERR_SCIPVERSION";
- case URG_ERR_MEMORY:
- return "URG_ERR_MEMORY";
- case URG_ERR_UNSUPPORTED:
- return "URG_ERR_UNSUPPORTED";
- case URG_ERR_BADARG:
- return "URG_ERR_BADARG";
- case URG_ERR_NODATA:
- return "URG_ERR_NODATA";
- case URG_ERR_NOTSERIAL:
- return "URG_ERR_NOTSERIAL";
+ case HOKUYO_ERR_READ:
+ return "HOKUYO_ERR_READ";
+ case HOKUYO_ERR_WRITE:
+ return "HOKUYO_ERR_WRITE";
+ case HOKUYO_ERR_PROTOCOL:
+ return "HOKUYO_ERR_PROTOCOL";
+ case HOKUYO_ERR_CHANGEBAUD:
+ return "HOKUYO_ERR_CHANGEBAUD";
+ case HOKUYO_ERR_CONNECT_FAILED:
+ return "HOKUYO_ERR_CONNECT_FAILED";
+ case HOKUYO_ERR_CLOSE_FAILED:
+ return "HOKUYO_ERR_CLOSE_FAILED";
+ case HOKUYO_ERR_NODESTINATION:
+ return "HOKUYO_ERR_NODESTINATION";
+ case HOKUYO_ERR_BADFIRMWARE:
+ return "HOKUYO_ERR_BADFIRMWARE";
+ case HOKUYO_ERR_SCIPVERSION:
+ return "HOKUYO_ERR_SCIPVERSION";
+ case HOKUYO_ERR_MEMORY:
+ return "HOKUYO_ERR_MEMORY";
+ case HOKUYO_ERR_UNSUPPORTED:
+ return "HOKUYO_ERR_UNSUPPORTED";
+ case HOKUYO_ERR_BADARG:
+ return "HOKUYO_ERR_BADARG";
+ case HOKUYO_ERR_NODATA:
+ return "HOKUYO_ERR_NODATA";
+ case HOKUYO_ERR_NOTSERIAL:
+ return "HOKUYO_ERR_NOTSERIAL";
}
return "UNKNOWN_ERROR";
}
////////////////////////////////////////////////////////////////////////////////////////////////////
-// URGSensorInfo class
+// HokuyoSensorInfo class
////////////////////////////////////////////////////////////////////////////////////////////////////
-URGSensorInfo::URGSensorInfo (void)
+HokuyoSensorInfo::HokuyoSensorInfo (void)
: minRange (0), maxRange (0), steps (0), firstStep (0), lastStep (0), frontStep (0),
standardSpeed (0), power (false), speed (0), baud (0), time (0), minAngle (0.0), maxAngle (0.0),
resolution (0.0), scanableSteps (0)
@@ -463,7 +463,7 @@
}
// Set various known values based on what the manual says
-void URGSensorInfo::SetDefaults (void)
+void HokuyoSensorInfo::SetDefaults (void)
{
minRange = 20;
maxRange = 4095;
@@ -473,7 +473,7 @@
frontStep = 384;
}
-void URGSensorInfo::CalculateValues (void)
+void HokuyoSensorInfo::CalculateValues (void)
{
resolution = DTOR (360.0) / steps;
// If any of the steps are beyond INT_MAX, we have problems.
@@ -483,7 +483,7 @@
scanableSteps = lastStep - firstStep + 1;
}
-string URGSensorInfo::AsString (void)
+string HokuyoSensorInfo::AsString (void)
{
stringstream ss;
@@ -513,15 +513,15 @@
}
////////////////////////////////////////////////////////////////////////////////////////////////////
-// URGData class
+// HokuyoData class
////////////////////////////////////////////////////////////////////////////////////////////////////
-URGData::URGData (void)
+HokuyoData::HokuyoData (void)
: _data (NULL), _length (0), _error (-1), _time (0)
{
}
-URGData::URGData (unsigned short *data, unsigned int length, short error, unsigned int time)
+HokuyoData::HokuyoData (unsigned short *data, unsigned int length, short error, unsigned int time)
: _error (error), _time (time)
{
_length = length;
@@ -532,13 +532,13 @@
if ((_data = new unsigned short[_length]) == NULL)
{
_length = 0;
- throw URGError (URG_ERR_MEMORY, "Failed to allocate space to copy data.");
+ throw HokuyoError (HOKUYO_ERR_MEMORY, "Failed to allocate space to copy data.");
}
memcpy (_data, data, sizeof (unsigned short) * _length);
}
}
-URGData::URGData (const URGData &rhs)
+HokuyoData::HokuyoData (const HokuyoData &rhs)
{
_length = rhs.Length ();
if (_length == 0)
@@ -548,7 +548,7 @@
if ((_data = new unsigned short[_length]) == NULL)
{
_length = 0;
- throw URGError (URG_ERR_MEMORY, "Failed to allocate space to copy data.");
+ throw HokuyoError (HOKUYO_ERR_MEMORY, "Failed to allocate space to copy data.");
}
memcpy (_data, rhs.Ranges (), sizeof (unsigned short) * _length);
}
@@ -556,13 +556,13 @@
_time = rhs.TimeStamp ();
}
-URGData::~URGData (void)
+HokuyoData::~HokuyoData (void)
{
if (_data != NULL)
delete[] _data;
}
-string URGData::ErrorCodeToString (void)
+string HokuyoData::ErrorCodeToString (void)
{
switch (_error)
{
@@ -615,7 +615,7 @@
}
}
-URGData& URGData::operator= (const URGData &rhs)
+HokuyoData& HokuyoData::operator= (const HokuyoData &rhs)
{
if (rhs.Length () == 0)
{
@@ -635,7 +635,7 @@
// Copy the data into a temporary variable pointing to new space (prevents dangling
// pointers on allocation error and prevents self-assignment making a mess).
if ((newData = new unsigned short[rhsLength]) == NULL)
- throw URGError (URG_ERR_MEMORY, "Failed to allocate space to copy data.");
+ throw HokuyoError (HOKUYO_ERR_MEMORY, "Failed to allocate space to copy data.");
memcpy (newData, rhs.Ranges (), sizeof (unsigned short) * rhsLength);
if (_data != NULL)
delete[] _data;
@@ -655,14 +655,14 @@
return *this;
}
-unsigned short URGData::operator[] (unsigned int index)
+unsigned short HokuyoData::operator[] (unsigned int index)
{
if (index >= _length)
- throw URGError (URG_ERR_BADARG, "Invalid data index.");
+ throw HokuyoError (HOKUYO_ERR_BADARG, "Invalid data index.");
return _data[index];
}
-string URGData::AsString (void)
+string HokuyoData::AsString (void)
{
stringstream ss;
@@ -675,7 +675,7 @@
return ss.str ();
}
-void URGData::CleanUp (void)
+void HokuyoData::CleanUp (void)
{
if (_data != NULL)
delete[] _data;
@@ -685,7 +685,7 @@
_time = 0;
}
-void URGData::AllocateData (unsigned int length)
+void HokuyoData::AllocateData (unsigned int length)
{
// If no data yet, allocate new
if (_data == NULL)
@@ -693,7 +693,7 @@
if ((_data = new unsigned short[length]) == NULL)
{
_length = 0;
- throw URGError (URG_ERR_MEMORY, "Failed to allocate space to copy data.");
+ throw HokuyoError (HOKUYO_ERR_MEMORY, "Failed to allocate space to copy data.");
}
_length = length;
}
@@ -704,7 +704,7 @@
if ((_data = new unsigned short[length]) == NULL)
{
_length = 0;
- throw URGError (URG_ERR_MEMORY, "Failed to allocate space to copy data.");
+ throw HokuyoError (HOKUYO_ERR_MEMORY, "Failed to allocate space to copy data.");
}
_length = length;
}
@@ -712,29 +712,29 @@
}
////////////////////////////////////////////////////////////////////////////////////////////////////
-// URGLaser class
+// HokuyoLaser class
////////////////////////////////////////////////////////////////////////////////////////////////////
// Public API
////////////////////////////////////////////////////////////////////////////////////////////////////
-URGLaser::URGLaser (void)
+HokuyoLaser::HokuyoLaser (void)
: _port (NULL), _scipVersion (1), _verbose (false), _minAngle (0.0), _maxAngle (0.0),
_resolution (0.0), _firstStep (0), _lastStep (0), _frontStep (0)
{
}
-URGLaser::~URGLaser (void)
+HokuyoLaser::~HokuyoLaser (void)
{
if (_port != NULL)
delete _port;
}
-void URGLaser::Open (string portOptions)
+void HokuyoLaser::Open (string portOptions)
{
if (_verbose)
{
- cerr << "URGLaser::" << __func__ << "() Creating and opening port using options: " <<
+ cerr << "HokuyoLaser::" << __func__ << "() Creating and opening port using options: " <<
portOptions << endl;
}
_port = flexiport::CreatePort (portOptions);
@@ -742,7 +742,7 @@
if (_verbose)
{
- cerr << "URGLaser::" << __func__ << "() Connected using " << _port->GetPortType () <<
+ cerr << "HokuyoLaser::" << __func__ << "() Connected using " << _port->GetPortType () <<
" connection." << endl;
cerr << _port->GetStatus ();
}
@@ -754,37 +754,37 @@
GetDefaults ();
}
-void URGLaser::Close (void)
+void HokuyoLaser::Close (void)
{
if (!_port)
- throw URGError (URG_ERR_CLOSE_FAILED, "Port is not open.");
+ throw HokuyoError (HOKUYO_ERR_CLOSE_FAILED, "Port is not open.");
if (_verbose)
- cerr << "URGLaser::" << __func__ << "() Closing connection." << endl;
+ cerr << "HokuyoLaser::" << __func__ << "() Closing connection." << endl;
delete _port;
_port = NULL;
}
-bool URGLaser::IsOpen (void) const
+bool HokuyoLaser::IsOpen (void) const
{
if (_port != NULL)
return _port->IsOpen ();
return false;
}
-void URGLaser::SetPower (bool on)
+void HokuyoLaser::SetPower (bool on)
{
if (_scipVersion == 1)
{
if (on)
{
if (_verbose)
- cerr << "URGLaser::" << __func__ << "() Turning laser on." << endl;
+ cerr << "HokuyoLaser::" << __func__ << "() Turning laser on." << endl;
SendCommand ("L", "1", 1, NULL);
}
else
{
if (_verbose)
- cerr << "URGLaser::" << __func__ << "() Turning laser off." << endl;
+ cerr << "HokuyoLaser::" << __func__ << "() Turning laser off." << endl;
SendCommand ("L", "0", 1, NULL);
}
SkipLines (1);
@@ -794,26 +794,29 @@
if (on)
{
if (_verbose)
- cerr << "URGLaser::" << __func__ << "() Turning laser on." << endl;
+ cerr << "HokuyoLaser::" << __func__ << "() Turning laser on." << endl;
SendCommand ("BM", NULL, 0, "02");
}
else
{
if (_verbose)
- cerr << "URGLaser::" << __func__ << "() Turning laser off." << endl;
+ cerr << "HokuyoLaser::" << __func__ << "() Turning laser off." << endl;
SendCommand ("QT", NULL, 0, "02");
}
SkipLines (1);
}
else
- throw URGError (URG_ERR_SCIPVERSION, "Unknown SCIP version.");
+ throw HokuyoError (HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
}
// This function assumes that both the port and the laser scanner are already set to the same baud.
-void URGLaser::SetBaud (unsigned int baud)
+void HokuyoLaser::SetBaud (unsigned int baud)
{
if (_port->GetPortType () != "serial")
- throw URGError (URG_ERR_NOTSERIAL, "Cannot change baud rate of non-serial connection.");
+ {
+ throw HokuyoError (HOKUYO_ERR_NOTSERIAL,
+ "Cannot change baud rate of non-serial connection.");
+ }
char newBaud[13];
memset (newBaud, 0, sizeof (char) * 13);
@@ -823,7 +826,7 @@
{
stringstream ss;
ss << "Bad baud rate: " << baud << endl;
- throw URGError (URG_ERR_BADARG, ss.str ());
+ throw HokuyoError (HOKUYO_ERR_BADARG, ss.str ());
}
NumberToString (baud, newBaud, 6);
@@ -844,42 +847,48 @@
reinterpret_cast<SerialPort*> (_port)->SetBaudRate (baud);
}
else
- throw URGError (URG_ERR_SCIPVERSION, "Unknown SCIP version.");
+ throw HokuyoError (HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
}
-void URGLaser::Reset (void)
+void HokuyoLaser::Reset (void)
{
if (_scipVersion == 1)
- throw URGError (URG_ERR_UNSUPPORTED, "SCIP version 1 does not support the reset command.");
+ {
+ throw HokuyoError (HOKUYO_ERR_UNSUPPORTED,
+ "SCIP version 1 does not support the reset command.");
+ }
else if (_scipVersion == 2)
{
if (_verbose)
- cerr << "URGLaser::" << __func__ << "() Resetting laser." << endl;
+ cerr << "HokuyoLaser::" << __func__ << "() Resetting laser." << endl;
SendCommand ("RS", NULL, 0, NULL);
SkipLines (1);
}
else
- throw URGError (URG_ERR_SCIPVERSION, "Unknown SCIP version.");
+ throw HokuyoError (HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
}
-void URGLaser::SetMotorSpeed (unsigned int speed)
+void HokuyoLaser::SetMotorSpeed (unsigned int speed)
{
if (_scipVersion == 1)
{
- throw URGError (URG_ERR_UNSUPPORTED,
+ throw HokuyoError (HOKUYO_ERR_UNSUPPORTED,
"SCIP version 1 does not support the set motor speed command.");
}
else if (_scipVersion == 2)
{
// Sanity check the value
if ((speed > 600 || speed < 540 || (speed % 6) != 0) && speed != 0)
- throw URGError (URG_ERR_BADARG, "Invalid motor speed.");
+ throw HokuyoError (HOKUYO_ERR_BADARG, "Invalid motor speed.");
char buffer[3];
buffer[2] = '\0';
if (speed == 0)
{
if (_verbose)
- cerr << "URGLaser::" << __func__ << "() Reseting motor speed to default." << endl;
+ {
+ cerr << "HokuyoLaser::" << __func__ << "() Reseting motor speed to default." <<
+ endl;
+ }
buffer[0] = '9';
buffer[1] = '9';
}
@@ -887,7 +896,7 @@
{
if (_verbose)
{
- cerr << "URGLaser::" << __func__ << "() Setting motor speed to " <<
+ cerr << "HokuyoLaser::" << __func__ << "() Setting motor speed to " <<
speed << "rpm." << endl;
}
if (speed == 540)
@@ -905,19 +914,19 @@
SkipLines (1);
}
else
- throw URGError (URG_ERR_SCIPVERSION, "Unknown SCIP version.");
+ throw HokuyoError (HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
}
-void URGLaser::GetSensorInfo (URGSensorInfo *info)
+void HokuyoLaser::GetSensorInfo (HokuyoSensorInfo *info)
{
if (info == NULL)
- throw URGError (URG_ERR_NODESTINATION, "No info object provided.");
+ throw HokuyoError (HOKUYO_ERR_NODESTINATION, "No info object provided.");
if (_scipVersion == 1)
{
if (_verbose)
{
- cerr << "URGLaser::" << __func__ <<
+ cerr << "HokuyoLaser::" << __func__ <<
"() Getting sensor information using SCIP version 1." << endl;
}
@@ -1009,7 +1018,7 @@
{
if (_verbose)
{
- cerr << "URGLaser::" << __func__ <<
+ cerr << "HokuyoLaser::" << __func__ <<
"() Getting sensor information using SCIP version 2." << endl;
}
@@ -1073,7 +1082,7 @@
ReadLineWithCheck (buffer, -1, true);
// TODO: check if the format of this line changes if the motor speed is changed
if (sscanf (buffer, "SCSP:%*7s(%d[rpm]", &info->speed) != 1)
- throw URGError (URG_ERR_PROTOCOL, "Motor speed line parse failed.");
+ throw HokuyoError (HOKUYO_ERR_PROTOCOL, "Motor speed line parse failed.");
// Measuring state
ReadLineWithCheck (buffer, -1, true);
info->measureState = &buffer[5];
@@ -1085,11 +1094,11 @@
info->baud = 0;
}
else if (sscanf (buffer, "SBPS:%d[bps]", &info->baud) != 1)
- throw URGError (URG_ERR_PROTOCOL, "Baud rate line parse failed.");
+ throw HokuyoError (HOKUYO_ERR_PROTOCOL, "Baud rate line parse failed.");
// Time stamp
ReadLineWithCheck (buffer, -1, true);
if (sscanf (buffer, "TIME:%x", &info->time) != 1)
- throw URGError (URG_ERR_PROTOCOL, "Timestamp line parse failed.");
+ throw HokuyoError (HOKUYO_ERR_PROTOCOL, "Timestamp line parse failed.");
// Diagnostic
ReadLineWithCheck (buffer, -1, true);
info->sensorDiagnostic = &buffer[5];
@@ -1104,20 +1113,20 @@
}
}
else
- throw URGError (URG_ERR_SCIPVERSION, "Unknown SCIP version.");
+ throw HokuyoError (HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
}
-unsigned int URGLaser::GetTime (void)
+unsigned int HokuyoLaser::GetTime (void)
{
if (_scipVersion == 1)
{
- throw URGError (URG_ERR_UNSUPPORTED,
+ throw HokuyoError (HOKUYO_ERR_UNSUPPORTED,
"SCIP version 1 does not support the get time command.");
}
else if (_scipVersion == 2)
{
if (_verbose)
- cerr << "URGLaser::" << __func__ << "() Retrieving time from laser." << endl;
+ cerr << "HokuyoLaser::" << __func__ << "() Retrieving time from laser." << endl;
SendCommand ("TM", "0", 1, NULL);
SendCommand ("TM", "1", 1, NULL);
char buffer[7];
@@ -1128,16 +1137,16 @@
return Decode4ByteValue (buffer);
}
else
- throw URGError (URG_ERR_SCIPVERSION, "Unknown SCIP version.");
+ throw HokuyoError (HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
return 0;
}
-unsigned int URGLaser::GetRanges (URGData *data, int startStep, int endStep,
+unsigned int HokuyoLaser::GetRanges (HokuyoData *data, int startStep, int endStep,
unsigned int clusterCount)
{
if (data == NULL)
- throw URGError (URG_ERR_NODESTINATION, "No data destination provided.");
+ throw HokuyoError (HOKUYO_ERR_NODESTINATION, "No data destination provided.");
char buffer[11];
memset (buffer, 0, sizeof (char) * 11);
@@ -1150,7 +1159,7 @@
unsigned int numSteps = (endStep - startStep + 1) / clusterCount;
if (_verbose)
{
- cerr << "URGLaser::" << __func__ << "() Reading " << numSteps << " ranges between " <<
+ cerr << "HokuyoLaser::" << __func__ << "() Reading " << numSteps << " ranges between " <<
startStep << " and " << endStep << " with a cluster count of " << clusterCount << endl;
}
@@ -1175,22 +1184,22 @@
// Normally we would send 6 for the expected length, but we may get no timestamp back if
// there was no data.
if (ReadLineWithCheck (buffer) == 0)
- throw URGError (URG_ERR_NODATA, "No data received. Check data error code.");
+ throw HokuyoError (HOKUYO_ERR_NODATA, "No data received. Check data error code.");
data->_time = Decode4ByteValue (buffer);
// In SCIP2 mode we're going to get back 3-byte data because we're sending the GD command
Read3ByteRangeData (data, numSteps);
}
else
- throw URGError (URG_ERR_SCIPVERSION, "Unknown SCIP version.");
+ throw HokuyoError (HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
return data->_length;
}
-unsigned int URGLaser::GetRanges (URGData *data, double startAngle,
+unsigned int HokuyoLaser::GetRanges (HokuyoData *data, double startAngle,
double endAngle, unsigned int clusterCount)
{
if (data == NULL)
- throw URGError (URG_ERR_NODESTINATION, "No data destination provided.");
+ throw HokuyoError (HOKUYO_ERR_NODESTINATION, "No data destination provided.");
// Calculate the given angles in steps, rounding towards _frontStep
int startStep, endStep;
@@ -1199,13 +1208,13 @@
// Check the steps are within the allowable range
if (startStep < _firstStep || startStep > _lastStep)
- throw URGError (URG_ERR_BADARG, "Start step is out of range.");
+ throw HokuyoError (HOKUYO_ERR_BADARG, "Start step is out of range.");
if (endStep < _firstStep || endStep > _lastStep)
- throw URGError (URG_ERR_BADARG, "End step is out of range.");
+ throw HokuyoError (HOKUYO_ERR_BADARG, "End step is out of range.");
if (_verbose)
{
- cerr << "URGLaser::" << __func__ << "() Start angle " << startAngle << " is step " <<
+ cerr << "HokuyoLaser::" << __func__ << "() Start angle " << startAngle << " is step " <<
startStep << ", end angle " << endAngle << " is step " << endStep << endl;
}
@@ -1213,15 +1222,15 @@
return GetRanges (data, startStep, endStep, clusterCount);
}
-unsigned int URGLaser::GetNewRanges (URGData *data, int startStep, int endStep,
+unsigned int HokuyoLaser::GetNewRanges (HokuyoData *data, int startStep, int endStep,
unsigned int clusterCount)
{
if (data == NULL)
- throw URGError (URG_ERR_NODESTINATION, "No data destination provided.");
+ throw HokuyoError (HOKUYO_ERR_NODESTINATION, "No data destination provided.");
if (_scipVersion == 1)
{
- throw URGError (URG_ERR_UNSUPPORTED,
+ throw HokuyoError (HOKUYO_ERR_UNSUPPORTED,
"SCIP version 1 does not support the get new ranges command.");
}
else if (_scipVersion == 2)
@@ -1237,7 +1246,7 @@
unsigned int numSteps = (endStep - startStep + 1) / clusterCount;
if (_verbose)
{
- cerr << "URGLaser::" << __func__ << "() Reading " << numSteps <<
+ cerr << "HokuyoLaser::" << __func__ << "() Reading " << numSteps <<
" new ranges between " << startStep << " and " << endStep <<
" with a cluster count of " << clusterCount << endl;
}
@@ -1253,25 +1262,25 @@
// Normally we would send 6 for the expected length, but we may get no timestamp back if
// there was no data.
if (ReadLineWithCheck (buffer) == 0)
- throw URGError (URG_ERR_NODATA, "No data received. Check data error code.");
+ throw HokuyoError (HOKUYO_ERR_NODATA, "No data received. Check data error code.");
data->_time = Decode4ByteValue (buffer);
// In SCIP2 mode we're going to get back 3-byte data because we're sending the MD command
Read3ByteRangeData (data, numSteps);
}
else
- throw URGError (URG_ERR_SCIPVERSION, "Unknown SCIP version.");
+ throw HokuyoError (HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
return data->_length;
}
-unsigned int URGLaser::GetNewRanges (URGData *data, double startAngle, double endAngle,
+unsigned int HokuyoLaser::GetNewRanges (HokuyoData *data, double startAngle, double endAngle,
unsigned int clusterCount)
{
if (data == NULL)
- throw URGError (URG_ERR_NODESTINATION, "No data destination provided.");
+ throw HokuyoError (HOKUYO_ERR_NODESTINATION, "No data destination provided.");
if (_scipVersion == 1)
{
- throw URGError (URG_ERR_UNSUPPORTED,
+ throw HokuyoError (HOKUYO_ERR_UNSUPPORTED,
"SCIP version 1 does not support the get new ranges command.");
}
@@ -1282,13 +1291,13 @@
// Check the steps are within the allowable range
if (startStep < _firstStep || startStep > _lastStep)
- throw URGError (URG_ERR_BADARG, "Start step is out of range.");
+ throw HokuyoError (HOKUYO_ERR_BADARG, "Start step is out of range.");
if (endStep < _firstStep || endStep > _lastStep)
- throw URGError (URG_ERR_BADARG, "End step is out of range.");
+ throw HokuyoError (HOKUYO_ERR_BADARG, "End step is out of range.");
if (_verbose)
{
- cerr << "URGLaser::" << __func__ << "() Start angle " << startAngle << " is step " <<
+ cerr << "HokuyoLaser::" << __func__ << "() Start angle " << startAngle << " is step " <<
startStep << ", end angle " << endAngle << " is step " << endStep << endl;
}
@@ -1296,12 +1305,12 @@
return GetNewRanges (data, startStep, endStep, clusterCount);
}
-double URGLaser::StepToAngle (unsigned int step)
+double HokuyoLaser::StepToAngle (unsigned int step)
{
return (static_cast<int> (step) - static_cast<int> (_frontStep)) * _resolution;
}
-unsigned int URGLaser::AngleToStep (double angle)
+unsigned int HokuyoLaser::AngleToStep (double angle)
{
unsigned int result;
double resultF;
@@ -1325,7 +1334,7 @@
// maximum line length to be read. See SCIP1_LINE_LENGTH and SCIP2_LINE_LENGTH.
// The line feed that terminates a line will be replaced with a NULL.
// The return value is the number of bytes received, not including the NULL byte or the line feed.
-int URGLaser::ReadLine (char *buffer, int expectedLength)
+int HokuyoLaser::ReadLine (char *buffer, int expectedLength)
{
int lineLength = 0;
@@ -1334,14 +1343,14 @@
int maxLength = (_scipVersion == 1) ? SCIP1_LINE_LENGTH : SCIP2_LINE_LENGTH;
if (_verbose)
{
- cerr << "URGLaser::" << __func__ << "() Reading up to " << maxLength << " bytes." <<
+ cerr << "HokuyoLaser::" << __func__ << "() Reading up to " << maxLength << " bytes." <<
endl;
}
// We need to get at least 1 byte in a line: the line feed.
if ((lineLength = _port->ReadLine (buffer, maxLength)) < 0)
- throw URGError (URG_ERR_READ, "Timed out trying to read a line.");
+ throw HokuyoError (HOKUYO_ERR_READ, "Timed out trying to read a line.");
else if (lineLength == 0)
- throw URGError (URG_ERR_READ, "No data received when trying to read a line.");
+ throw HokuyoError (HOKUYO_ERR_READ, "No data received when trying to read a line.");
// Replace the line feed with a NULL
buffer[lineLength - 1] = '\0';
}
@@ -1349,19 +1358,19 @@
{
if (_verbose)
{
- cerr << "URGLaser::" << __func__ << "() Reading exactly " << expectedLength <<
+ cerr << "HokuyoLaser::" << __func__ << "() Reading exactly " << expectedLength <<
" bytes." << endl;
}
if ((lineLength = _port->ReadLine (buffer, expectedLength + 1)) < 0) // +1 for the NULL
- throw URGError (URG_ERR_READ, "Timed out trying to read a line.");
+ throw HokuyoError (HOKUYO_ERR_READ, "Timed out trying to read a line.");
else if (lineLength == 0)
- throw URGError (URG_ERR_READ, "No data received when trying to read a line.");
+ throw HokuyoError (HOKUYO_ERR_READ, "No data received when trying to read a line.");
else if (lineLength < expectedLength)
{
stringstream ss;
- ss << "URGLaser::" << __func__ << "() Got an incorrect line length: " << lineLength <<
- " != " << expectedLength;
- throw URGError (URG_ERR_PROTOCOL, ss.str ());
+ ss << "HokuyoLaser::" << __func__ << "() Got an incorrect line length: " <<
+ lineLength << " != " << expectedLength;
+ throw HokuyoError (HOKUYO_ERR_PROTOCOL, ss.str ());
}
// Replace the line feed with a NULL
buffer[lineLength - 1] = '\0';
@@ -1369,8 +1378,8 @@
if (_verbose)
{
- cerr << "URGLaser::" << __func__ << "() Read " << lineLength << " bytes." << endl;
- cerr << "URGLaser::" << __func__ << "() Line is " << buffer << endl;
+ cerr << "HokuyoLaser::" << __func__ << "() Read " << lineLength << " bytes." << endl;
+ cerr << "HokuyoLaser::" << __func__ << "() Line is " << buffer << endl;
}
return lineLength - 1; // Line feed not included
}
@@ -1383,7 +1392,7 @@
// Empty lines (i.e. a line that is just the line feed, as at the end of the message) will result in
// a return value of zero and no checksum check will be performed. Otherwise the number of actual
// data bytes (i.e. excluding the checksum and semicolon) will be returned.
-int URGLaser::ReadLineWithCheck (char *buffer, int expectedLength, bool hasSemicolon)
+int HokuyoLaser::ReadLineWithCheck (char *buffer, int expectedLength, bool hasSemicolon)
{
int lineLength = ReadLine (buffer, expectedLength);
if (_scipVersion == 1)
@@ -1403,7 +1412,7 @@
int checksumIndex = bytesToConsider + (hasSemicolon ? 1 : 0);
if (_verbose)
{
- cerr << "URGLaser::" << __func__ << "() Considering " << bytesToConsider <<
+ cerr << "HokuyoLaser::" << __func__ << "() Considering " << bytesToConsider <<
" bytes for checksum from a line length of " << lineLength << " bytes." << endl;
}
if (bytesToConsider < 1)
@@ -1411,7 +1420,7 @@
stringstream ss;
ss << "Not enough bytes to calculate checksum with: " << bytesToConsider <<
" bytes (line length is " << lineLength << " bytes).";
- throw URGError (URG_ERR_PROTOCOL, ss.str ());
+ throw HokuyoError (HOKUYO_ERR_PROTOCOL, ss.str ());
}
int checkSum = 0;
@@ -1425,7 +1434,7 @@
if (_verbose)
{
- cerr << "URGLaser::" << __func__ << "() Calculated checksum = " << checkSum << " (" <<
+ cerr << "HokuyoLaser::" << __func__ << "() Calculated checksum = " << checkSum << " (" <<
static_cast<char> (checkSum) << "), given checksum = " <<
static_cast<int> (buffer[checksumIndex]) << " (" << buffer[checksumIndex] <<
")" << endl;
@@ -1435,7 +1444,7 @@
stringstream ss;
ss << "Invalid checksum - given: " << static_cast<int> (buffer[checksumIndex]) <<
", calculated: " << checkSum;
- throw URGError (URG_ERR_PROTOCOL, ss.str ());
+ throw HokuyoError (HOKUYO_ERR_PROTOCOL, ss.str ());
}
// Null out the semi-colon (if there) and checksum
@@ -1445,12 +1454,12 @@
}
// Reads lines until the number specified has passed.
-void URGLaser::SkipLines (int count)
+void HokuyoLaser::SkipLines (int count)
{
if (_verbose)
- cerr << "URGLaser::" << __func__ << "() Skipping " << count << " lines." << endl;
+ cerr << "HokuyoLaser::" << __func__ << "() Skipping " << count << " lines." << endl;
if (_port->SkipUntil (0x0A, count) < 0)
- throw URGError (URG_ERR_READ, "Timed out while skipping.");
+ throw HokuyoError (HOKUYO_ERR_READ, "Timed out while skipping.");
}
// Sends a command with optional parameters and checks that the echo of the command and parameters
@@ -1460,7 +1469,7 @@
// If paramLength is 0, no parameters will be sent or expected in the reply.
// extraOK must be a 1-byte string for SCIP1 and a 2-byte string for SCIP2.
// Return value is the status code returned for the command.
-int URGLaser::SendCommand (char *cmd, char *param, int paramLength, char *extraOK)
+int HokuyoLaser::SendCommand (char *cmd, char *param, int paramLength, char *extraOK)
{
int statusCode = -1;
char response[16];
@@ -1468,19 +1477,19 @@
{
if (_verbose)
{
- cerr << "URGLaser::" << __func__ << "() Writing in SCIP1 mode. Command is " <<
+ cerr << "HokuyoLaser::" << __func__ << "() Writing in SCIP1 mode. Command is " <<
cmd[0] << ", parameters length is " << paramLength << endl;
}
// Write the command
if (_port->Write (cmd, 1) < 1)
- throw URGError (URG_ERR_WRITE, "Failed to write command byte.");
+ throw HokuyoError (HOKUYO_ERR_WRITE, "Failed to write command byte.");
if (paramLength > 0)
{
if (_port->Write (param, paramLength) < paramLength)
- throw URGError (URG_ERR_WRITE, "Failed to write command parameters.");
+ throw HokuyoError (HOKUYO_ERR_WRITE, "Failed to write command parameters.");
}
if (_port->Write ("\n", 1) < 1)
- throw URGError (URG_ERR_WRITE, "Failed to write termination character.");
+ throw HokuyoError (HOKUYO_ERR_WRITE, "Failed to write termination character.");
// Read back the response (should get at least 4 bytes , possibly up to 16 including \n's
// depending on the parameters): cmd[0] params \n status \n
@@ -1490,20 +1499,22 @@
// First make sure that the echoed command matches
if (response[0] != cmd[0])
{
- throw URGError (URG_ERR_PROTOCOL, string ("Incorrect command echo: ") + cmd[0] +
+ throw HokuyoError (HOKUYO_ERR_PROTOCOL, string ("Incorrect command echo: ") + cmd[0] +
string (" != ") + response[0]);
}
// Then compare the parameters
if (paramLength > 0)
{
if (memcmp (&response[1], param, paramLength) != 0)
- throw URGError (URG_ERR_PROTOCOL, string ("Incorrect paramaters echo for command ")
- + cmd[0]);
+ {
+ throw HokuyoError (HOKUYO_ERR_PROTOCOL,
+ string ("Incorrect paramaters echo for command ") + cmd[0]);
+ }
}
// Next up, check the status byte
if (_verbose)
{
- cerr << "URGLaser::" << __func__ << "() Command response status: " <<
+ cerr << "HokuyoLaser::" << __func__ << "() Command response status: " <<
response[statusIndex] << endl;
}
if (response[statusIndex] != '0')
@@ -1515,7 +1526,7 @@
stringstream ss;
ss << "Bad response to " << cmd[0] << " command: " << " " <<
SCIP1ErrorToString (response[statusIndex], cmd[0]);
- throw URGError (URG_ERR_PROTOCOL, ss.str ());
+ throw HokuyoError (HOKUYO_ERR_PROTOCOL, ss.str ());
}
}
else
@@ -1523,7 +1534,7 @@
stringstream ss;
ss << "Bad response to " << cmd[0] << " command: " << response[statusIndex] <<
" " << SCIP1ErrorToString (response[statusIndex], cmd[0]);
- throw URGError (URG_ERR_PROTOCOL, ss.str ());
+ throw HokuyoError (HOKUYO_ERR_PROTOCOL, ss.str ());
}
}
statusCode = atoi (&response[statusIndex]);
@@ -1533,19 +1544,19 @@
{
if (_verbose)
{
- cerr << "URGLaser::" << __func__ << "() Writing in SCIP2 mode. Command is " <<
+ cerr << "HokuyoLaser::" << __func__ << "() Writing in SCIP2 mode. Command is " <<
cmd << ", parameters length is " << paramLength << endl;
}
// Write the command
if (_port->Write (cmd, 2) < 2)
- throw URGError (URG_ERR_WRITE, "Failed to write command byte.");
+ throw HokuyoError (HOKUYO_ERR_WRITE, "Failed to write command byte.");
if (paramLength > 0)
{
if (_port->Write (param, paramLength) < paramLength)
- throw URGError (URG_ERR_WRITE, "Failed to write command parameters.");
+ throw HokuyoError (HOKUYO_ERR_WRITE, "Failed to write command parameters.");
}
if (_port->Write ("\n", 1) < 1)
- throw URGError (URG_ERR_WRITE, "Failed to write termination character.");
+ throw HokuyoError (HOKUYO_ERR_WRITE, "Failed to write termination character.");
// Read back the command echo (minimum of 3 bytes, maximum of 16 bytes)
ReadLine (response, 3 + paramLength);
@@ -1554,21 +1565,23 @@
{
stringstream ss;
ss << "Incorrect command echo: " << cmd << " != " << response[0] << response[1];
- throw URGError (URG_ERR_PROTOCOL, ss.str ());
+ throw HokuyoError (HOKUYO_ERR_PROTOCOL, ss.str ());
}
// Then compare the parameters
if (paramLength > 0)
{
if (memcmp (&response[2], param, paramLength) != 0)
- throw URGError (URG_ERR_PROTOCOL, string ("Incorrect paramaters echo for command ")
- + cmd);
+ {
+ throw HokuyoError (HOKUYO_ERR_PROTOCOL,
+ string ("Incorrect paramaters echo for command ") + cmd);
+ }
}
// The next line should be the status line
ReadLineWithCheck (response, 4);
if (_verbose)
{
- cerr << "URGLaser::" << __func__ << "() Command response status: " << response[0] <<
+ cerr << "HokuyoLaser::" << __func__ << "() Command response status: " << response[0] <<
response[1] << endl;
}
// Check the status code is OK
@@ -1583,7 +1596,7 @@
stringstream ss;
ss << "Bad response to " << cmd << " command: " << response[0] << response[1] <<
" " << SCIP2ErrorToString (response, cmd);
- throw URGError (URG_ERR_PROTOCOL, ss.str ());
+ throw HokuyoError (HOKUYO_ERR_PROTOCOL, ss.str ());
}
}
else
@@ -1591,34 +1604,34 @@
stringstream ss;
ss << "Bad response to " << cmd << " command: " << response[0] << response[1] <<
" " << SCIP2ErrorToString (response, cmd);
- throw URGError (URG_ERR_PROTOCOL, ss.str ());
+ throw HokuyoError (HOKUYO_ERR_PROTOCOL, ss.str ());
}
}
statusCode = atoi (response);
// All OK, data starts at beginning of port's buffer
}
else
- throw URGError (URG_ERR_SCIPVERSION, "Unknown SCIP version.");
+ throw HokuyoError (HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
return statusCode;
}
-void URGLaser::GetAndSetSCIPVersion (void)
+void HokuyoLaser::GetAndSetSCIPVersion (void)
{
bool scip1Failed = false;
if (_verbose)
- cerr << "URGLaser::" << __func__ << "() Testing SCIP protocol version." << endl;
+ cerr << "HokuyoLaser::" << __func__ << "() Testing SCIP protocol version." << endl;
// Try SCIP version 1 first by sending an info command
try
{
SendCommand ("V", NULL, 0, NULL);
}
- catch (URGError &e)
+ catch (HokuyoError &e)
{
// That didn't work too well...
if (_verbose)
- cerr << "URGLaser::" << __func__ << "() Initial SCIP version 1 test failed." << endl;
+ cerr << "HokuyoLaser::" << __func__ << "() Initial SCIP version 1 test failed." << endl;
scip1Failed = true;
}
@@ -1633,15 +1646,15 @@
{
SendCommand ("VV", NULL, 0, NULL);
}
- catch (URGError &e)
+ catch (HokuyoError &e)
{
- throw URGError (URG_ERR_SCIPVERSION, "SCIP versions 1 and 2 failed.");
+ throw HokuyoError (HOKUYO_ERR_SCIPVERSION, "SCIP versions 1 and 2 failed.");
}
// Otherwise all OK, dump the rest of the result
SkipLines (6);
if (_verbose)
- cerr << "URGLaser::" << __func__ << "() Using SCIP version 2." << endl;
+ cerr << "HokuyoLaser::" << __func__ << "() Using SCIP version 2." << endl;
return;
}
else
@@ -1662,13 +1675,13 @@
// that we sent to get the info).
ReadLine (buffer);
}
- catch (URGError e)
+ catch (HokuyoError e)
{
- if (e.Code () != URG_ERR_READ) // We're only interested in timeouts
+ if (e.Code () != HOKUYO_ERR_READ) // We're only interested in timeouts
throw;
if (_verbose)
{
- cerr << "URGLaser::" << __func__ <<
+ cerr << "HokuyoLaser::" << __func__ <<
"() Timed out trying SCIP version 1, trying SCIP version 2." << endl;
}
// Already in SCIP version 2 mode.
@@ -1679,29 +1692,32 @@
{
SendCommand ("VV", NULL, 0, NULL);
}
- catch (URGError &e)
+ catch (HokuyoError &e)
{
cout << "error: " << e.Code() << " " << e.what() << endl;
- throw URGError (URG_ERR_SCIPVERSION, "SCIP versions 1 and 2 failed.");
+ throw HokuyoError (HOKUYO_ERR_SCIPVERSION, "SCIP versions 1 and 2 failed.");
}
// Otherwise all OK, dump the rest of the result
SkipLines (6);
if (_verbose)
- cerr << "URGLaser::" << __func__ << "() Using SCIP version 2." << endl;
+ cerr << "HokuyoLaser::" << __func__ << "() Using SCIP version 2." << endl;
return;
}
if (strncmp (buffer, "FIRM:", 5) != 0)
{
- throw URGError (URG_ERR_PROTOCOL,
+ throw HokuyoError (HOKUYO_ERR_PROTOCOL,
"'FIRM:' was not found when checking firmware version.");
}
// Pull out the major version number
int majorVer = strtol (&buffer[5], NULL, 10);
if (errno == ERANGE)
- throw URGError (URG_ERR_BADFIRMWARE, "Out-of-range firmware version.");
+ throw HokuyoError (HOKUYO_ERR_BADFIRMWARE, "Out-of-range firmware version.");
if (_verbose)
- cerr << "URGLaser::" << __func__ << "() Firmware major version is " << majorVer << endl;
+ {
+ cerr << "HokuyoLaser::" << __func__ << "() Firmware major version is " <<
+ majorVer << endl;
+ }
// Dump the rest of the V command result (one of these will be the empty last line)
SkipLines (3);
@@ -1709,7 +1725,7 @@
if (majorVer < 3)
{
if (_verbose)
- cerr << "URGLaser::" << __func__ <<
+ cerr << "HokuyoLaser::" << __func__ <<
"() Firmware does not support SCIP version 2; using SCIP version 1." << endl;
return;
}
@@ -1725,10 +1741,10 @@
{
SendCommand ("S", "CIP2.0", 6, NULL);
}
- catch (URGError &e)
+ catch (HokuyoError &e)
{
if (_verbose)
- cerr << "URGLaser::" << __func__ <<
+ cerr << "HokuyoLaser::" << __func__ <<
"() Could not change to SCIP version 2; using SCIP version 1." << endl;
return;
}
@@ -1737,23 +1753,23 @@
// Changed to SCIP version 2
if (_verbose)
- cerr << "URGLaser::" << __func__ << "() Using SCIP version 2." << endl;
+ cerr << "HokuyoLaser::" << __func__ << "() Using SCIP version 2." << endl;
_scipVersion = 2;
return;
}
}
// Fallback case if didn't find a good SCIP version and return above
- throw URGError (URG_ERR_SCIPVERSION, "Unknown SCIP version.");
+ throw HokuyoError (HOKUYO_ERR_SCIPVERSION, "Unknown SCIP version.");
}
-void URGLaser::GetDefaults (void)
+void HokuyoLaser::GetDefaults (void)
{
if (_verbose)
- cerr << "URGLaser::" << __func__ << "() Getting default values." << endl;
+ cerr << "HokuyoLaser::" << __func__ << "() Getting default values." << endl;
// Get the laser's info
- URGSensorInfo info;
+ HokuyoSensorInfo info;
GetSensorInfo (&info);
_minAngle = info.minAngle;
@@ -1765,16 +1781,16 @@
_maxRange = info.maxRange;
if (_verbose)
{
- cerr << "URGLaser::" << __func__ <<
+ cerr << "HokuyoLaser::" << __func__ <<
"() Got default values: " << _minAngle << " " << _maxAngle << " " << _resolution <<
" " << _firstStep << " " << _lastStep << " " << _frontStep << " " << _maxRange << endl;
}
}
-void URGLaser::Read2ByteRangeData (URGData *data, unsigned int numSteps)
+void HokuyoLaser::Read2ByteRangeData (HokuyoData *data, unsigned int numSteps)
{
if (_verbose)
- cerr << "URGLaser::" << __func__ << "() Reading " << numSteps << " ranges." << endl;
+ cerr << "HokuyoLaser::" << __func__ << "() Reading " << numSteps << " ranges." << endl;
// This will automatically take care of whether it actually needs to (re)allocate or not.
data->AllocateData (numSteps);
@@ -1796,7 +1812,7 @@
if (buffer[ii] == '\n' || buffer[ii + 1] == '\n')
{
// Line feed in the middle of a line? Why?
- throw URGError (URG_ERR_PROTOCOL, "Found line feed in a data block.");
+ throw HokuyoError (HOKUYO_ERR_PROTOCOL, "Found line feed in a data block.");
}
data->_data[currentStep] = Decode2ByteValue (&buffer[ii]);
}
@@ -1804,15 +1820,15 @@
}
if (_verbose)
- cerr << "URGLaser::" << __func__ << "() Read " << currentStep << " ranges." << endl;
+ cerr << "HokuyoLaser::" << __func__ << "() Read " << currentStep << " ranges." << endl;
if (currentStep != numSteps)
- throw URGError (URG_ERR_PROTOCOL, "Read less range readings than were asked for.");
+ throw HokuyoError (HOKUYO_ERR_PROTOCOL, "Read less range readings than were asked for.");
}
-void URGLaser::Read3ByteRangeData (URGData *data, unsigned int numSteps)
+void HokuyoLaser::Read3ByteRangeData (HokuyoData *data, unsigned int numSteps)
{
if (_verbose)
- cerr << "URGLaser::" << __func__ << "() Reading " << numSteps << " ranges." << endl;
+ cerr << "HokuyoLaser::" << __func__ << "() Reading " << numSteps << " ranges." << endl;
// This will automatically take care of whether it actually needs to (re)allocate or not.
data->AllocateData (numSteps);
@@ -1835,7 +1851,7 @@
if (buffer[ii] == '\n' || buffer[ii + 1] == '\n')
{
// Line feed in the middle of a line? Why?
- throw URGError (URG_ERR_PROTOCOL, "Found line feed in a data block.");
+ throw HokuyoError (HOKUYO_ERR_PROTOCOL, "Found line feed in a data block.");
}
if (ii == numBytesInLine - 2) // Short 1 byte
{
@@ -1870,7 +1886,7 @@
}
if (data->_data[currentStep] > _maxRange)
{
- cerr << "WARNING: URGLaser::" << __func__ <<
+ cerr << "WARNING: HokuyoLaser::" << __func__ <<
"() Value at step " << currentStep << " beyond maximum range: " <<
data->_data[currentStep] << " (raw bytes: ";
if (splitCount != 0)
@@ -1886,12 +1902,12 @@
}
if (_verbose)
- cerr << "URGLaser::" << __func__ << "() Read " << currentStep << " ranges." << endl;
+ cerr << "HokuyoLaser::" << __func__ << "() Read " << currentStep << " ranges." << endl;
if (currentStep != numSteps)
{
- throw URGError (URG_ERR_PROTOCOL,
+ throw HokuyoError (HOKUYO_ERR_PROTOCOL,
"Read a different number of range readings than were asked for.");
}
}
-} // namespace urg_nz
+} // namespace hokuyo_aist
Modified: gearbox/trunk/submitted/hokuyo_aist/hokuyo_aist.h
===================================================================
--- gearbox/trunk/submitted/hokuyo_aist/hokuyo_aist.h 2008-06-24 14:24:38 UTC (rev 211)
+++ gearbox/trunk/submitted/hokuyo_aist/hokuyo_aist.h 2008-06-24 14:57:10 UTC (rev 212)
@@ -3,7 +3,7 @@
* http://gearbox.sf.net/
* Copyright (c) 2008 Geoffrey Biggs
*
- * urg_nz Hokuyo URG laser scanner driver.
+ * hokuyo_aist Hokuyo laser scanner driver.
*
* This distribution is licensed to you under the terms described in the LICENSE file included in
* this distribution.
@@ -11,57 +11,57 @@
* This work is a product of the National Institute of Advanced Industrial Science and Technology,
* Japan. Registration number: ___
*
- * This file is part of urg_nz.
+ * This file is part of hokuyo_aist.
*
- * urg_nz is free software: you can redistribute it and/or modify it under the terms of the GNU
+ * hokuyo_aist is free software: you can redistribute it and/or modify it under the terms of the GNU
* Lesser General Public License as published by the Free Software Foundation, either version 3 of
* the License, or (at your option) any later version.
*
- * urg_nz is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even
- * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser
- * General Public License for more details.
+ * hokuyo_aist is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without
+ * even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
*
- * You should have received a copy of the GNU Lesser General Public License along with urg_nz. If
- * not, see <http://www.gnu.org/licenses/>.
+ * You should have received a copy of the GNU Lesser General Public License along with hokuyo_aist.
+ * If not, see <http://www.gnu.org/licenses/>.
*/
-#ifndef __URG_NZ_H
-#define __URG_NZ_H
+#ifndef __HOKUYO_AIST_H
+#define __HOKUYO_AIST_H
#include <flexiport/port.h>
#include <string>
#if defined (WIN32)
typedef unsigned char uint8_t;
- #if defined (URG_NZ_EXPORTS)
- #define URG_NZ_EXPORT __declspec (dllexport)
+ #if defined (HOKUYO_AIST_EXPORTS)
+ #define HOKUYO_AIST_EXPORT __declspec (dllexport)
#else
- #define URG_NZ_EXPORT __declspec (dllimport)
+ #define HOKUYO_AIST_EXPORT __declspec (dllimport)
#endif
#else
#include <stdint.h>
- #define URG_NZ_EXPORT
+ #define HOKUYO_AIST_EXPORT
#endif
-/** @ingroup gbx_library_urg_nz
+/** @ingroup gbx_library_hokuyo_aist
@{
*/
-namespace urg_nz
+namespace hokuyo_aist
{
-/// URG general error class.
-class URG_NZ_EXPORT URGError : public std::exception
+/// Hokuyo general error class.
+class HOKUYO_AIST_EXPORT HokuyoError : public std::exception
{
public:
- /** @brief URG error constructor.
+ /** @brief Hokuyo error constructor.
@param code Error code of the error.
@param desc Description of the error. */
- URGError (unsigned int code, std::string desc)
+ HokuyoError (unsigned int code, std::string desc)
: _errorCode (code), _errorDesc (desc)
{}
- virtual ~URGError (void) throw () {};
+ virtual ~HokuyoError (void) throw () {};
virtual unsigned int Code (void) const throw ()
{
@@ -85,78 +85,78 @@
// Exporting data members on Windows is a bloody pain (they have to be initialised somewhere else),
// so we'll use #define's on Win32 and lose the type safety.
/// Read error while reading from the laser.
-#define URG_ERR_READ 1
+#define HOKUYO_ERR_READ 1
/// Write error while writing to the laser.
-#define URG_ERR_WRITE 2
+#define HOKUYO_ERR_WRITE 2
/// Error in the SCIP protocol.
-#define URG_ERR_PROTOCOL 3
+#define HOKUYO_ERR_PROTOCOL 3
/// Error while changing baud rate.
-#define URG_ERR_CHANGEBAUD 4
+#define HOKUYO_ERR_CHANGEBAUD 4
/// Failed to connect to the laser.
-#define URG_ERR_CONNECT_FAILED 5
+#define HOKUYO_ERR_CONNECT_FAILED 5
/// Failed to close a port.
-#define URG_ERR_CLOSE_FAILED 6
+#define HOKUYO_ERR_CLOSE_FAILED 6
/// No destination buffer provided for range readings.
-#define URG_ERR_NODESTINATION 7
+#define HOKUYO_ERR_NODESTINATION 7
/// Bad firmware version.
-#define URG_ERR_BADFIRMWARE 8
+#define HOKUYO_ERR_BADFIRMWARE 8
/// Unknown/unsupported SCIP protocol version.
-#define URG_ERR_SCIPVERSION 9
+#define HOKUYO_ERR_SCIPVERSION 9
/// Memory allocation error.
-#define URG_ERR_MEMORY 10
+#define HOKUYO_ERR_MEMORY 10
/// Unsupported function error.
-#define URG_ERR_UNSUPPORTED 11
+#define HOKUYO_ERR_UNSUPPORTED 11
/// Argument error
-#define URG_ERR_BADARG 12
+#define HOKUYO_ERR_BADARG 12
/// No data received error
-#define URG_ERR_NODATA 13
+#define HOKUYO_ERR_NODATA 13
/// Not a serial connection error
-#define URG_ERR_NOTSERIAL 14
+#define HOKUYO_ERR_NOTSERIAL 14
#else
/// Read error while reading from the laser.
-const unsigned int URG_ERR_READ = 1;
+const unsigned int HOKUYO_ERR_READ = 1;
/// Write error while writing to the laser.
-const unsigned int URG_ERR_WRITE = 2;
+const unsigned int HOKUYO_ERR_WRITE = 2;
/// Error in the SCIP protocol.
-const unsigned int URG_ERR_PROTOCOL = 3;
+const unsigned int HOKUYO_ERR_PROTOCOL = 3;
/// Error while changing baud rate.
-const unsigned int URG_ERR_CHANGEBAUD = 4;
+const unsigned int HOKUYO_ERR_CHANGEBAUD = 4;
/// Failed to connect to the laser.
-const unsigned int URG_ERR_CONNECT_FAILED = 5;
+const unsigned int HOKUYO_ERR_CONNECT_FAILED = 5;
/// Failed to close a port.
-const unsigned int URG_ERR_CLOSE_FAILED = 6;
+const unsigned int HOKUYO_ERR_CLOSE_FAILED = 6;
/// No destination buffer provided for range readings.
-const unsigned int URG_ERR_NODESTINATION = 7;
+const unsigned int HOKUYO_ERR_NODESTINATION = 7;
/// Bad firmware version.
-const unsigned int URG_ERR_BADFIRMWARE = 8;
+const unsigned int HOKUYO_ERR_BADFIRMWARE = 8;
/// Unknown/unsupported SCIP protocol version.
-const unsigned int URG_ERR_SCIPVERSION = 9;
+const unsigned int HOKUYO_ERR_SCIPVERSION = 9;
/// Memory allocation error.
-const unsigned int URG_ERR_MEMORY = 10;
+const unsigned int HOKUYO_ERR_MEMORY = 10;
/// Unsupported function error.
-const unsigned int URG_ERR_UNSUPPORTED = 11;
+const unsigned int HOKUYO_ERR_UNSUPPORTED = 11;
/// Argument error
-const unsigned int URG_ERR_BADARG = 12;
+const unsigned int HOKUYO_ERR_BADARG = 12;
/// No data received error
-const unsigned int URG_ERR_NODATA = 13;
+const unsigned int HOKUYO_ERR_NODATA = 13;
/// Not a serial connection error
-const unsigned int URG_ERR_NOTSERIAL = 14;
+const unsigned int HOKUYO_ERR_NOTSERIAL = 14;
#endif // defined (WIN32)
/** @brief Sensor information.
Returned from a call to @GetSensorInfo. Contains various information about the laser scanner such as
firmware version and maximum possible range. */
-class URG_NZ_EXPORT URGSensorInfo
+class HOKUYO_AIST_EXPORT HokuyoSensorInfo
{
public:
- friend class URGLaser;
+ friend class HokuyoLaser;
- URGSensorInfo (void);
- URGSensorInfo (const URGSensorInfo &rhs);
+ HokuyoSensorInfo (void);
+ HokuyoSensorInfo (const HokuyoSensorInfo &rhs);
/// @brief Assignment operator.
- URGSensorInfo& operator= (const URGSensorInfo &rhs);
+ HokuyoSensorInfo& operator= (const HokuyoSensorInfo &rhs);
/// @brief Format the entire object into a string.
std::string AsString (void);
@@ -223,18 +223,18 @@
};
/** @brief Structure to store data returned from the laser scanner. */
-class URG_NZ_EXPORT URGData
+class HOKUYO_AIST_EXPORT HokuyoData
{
public:
- friend class URGLaser;
+ friend class HokuyoLaser;
- /// This constructor creates an empty URGData with no data currently allocated.
- URGData (void);
+ /// This constructor creates an empty HokuyoData with no data currently allocated.
+ HokuyoData (void);
/// This constructor performs a deep copy of data.
- URGData (unsigned short *data, unsigned int length, short error, unsigned int time);
+ HokuyoData (unsigned short *data, unsigned int length, short error, unsigned int time);
/// This copy constructor performs a deep copy of data.
- URGData (const URGData &rhs);
- ~URGData (void);
+ HokuyoData (const HokuyoData &rhs);
+ ~HokuyoData (void);
/** @brief Return a pointer to an array of range readings in millimetres.
@@ -254,7 +254,7 @@
unsigned int TimeStamp (void) const { return _time; }
/// @brief Assignment operator.
- URGData& operator= (const URGData &rhs);
+ HokuyoData& operator= (const HokuyoData &rhs);
/// @brief Subscript operator.
unsigned short operator[] (unsigned int index);
@@ -273,22 +273,22 @@
void AllocateData (unsigned int length);
};
-/** @brief URG laser scanner class.
+/** @brief Hokuyo laser scanner class.
-Provides an interface for interacting with a Hokuyo URG laser scanner using SCIP protocol version 1
+Provides an interface for interacting with a Hokuyo laser scanner using SCIP protocol version 1
or 2. The FlexiPort library is used to implement the data communications with the scanner. See its
documentation for details on controlling the connection.
To use a serial connection, ensure that you do not also have a USB cable connected, as this will
force the scanner into USB mode, preventing the serial connection from functioning correctly.
-All functions may throw instances of @ref URGError or its children. Exceptions from @ref FlexiPort
-may also occur. */
-class URG_NZ_EXPORT URGLaser
+All functions may throw instances of @ref HokuyoError or its children. Exceptions from
+@ref FlexiPort may also occur. */
+class HOKUYO_AIST_EXPORT HokuyoLaser
{
public:
- URGLaser (void);
- ~URGLaser (void);
+ HokuyoLaser (void);
+ ~HokuyoLaser (void);
/// @brief Open the laser scanner and begin scanning.
void Open (std::string portOptions);
@@ -324,7 +324,7 @@
/** @brief Get various information about the scanner.
Much of the information is not available with the SCIP v1 protocol. */
- void GetSensorInfo (URGSensorInfo *info);
+ void GetSensorInfo (HokuyoSensorInfo *info);
/** @brief Get the current value of the scanner's clock in milliseconds.
@@ -333,30 +333,30 @@
/** @brief Get the latest scan data from the scanner.
- This function requires a pointer to a @ref URGData object. It will allocate space in this
- object as necessary for storing range data. If the passed-in @ref URGData object already has
- the correct quantity of space to store the range data, it will not be re-allocated. If it
- does not have any space, it will be allocated. If it has space, but it is the wrong size, it
- will be re-allocated. This means you can repeatedly send the same @ref URGData object
+ This function requires a pointer to a @ref HokuyoData object. It will allocate space in this
+ object as necessary for storing range data. If the passed-in @ref HokuyoData object already
+ has the correct quantity of space to store the range data, it will not be re-allocated. If
+ it does not have any space, it will be allocated. If it has space, but it is the wrong size,
+ it will be re-allocated. This means you can repeatedly send the same @ref HokuyoData object
without having to worry about allocating its data, whether it will change or not, while also
avoiding excessive allocations.
...
[truncated message content] |
|
From: <gb...@us...> - 2008-06-24 14:25:02
|
Revision: 211
http://gearbox.svn.sourceforge.net/gearbox/?rev=211&view=rev
Author: gbiggs
Date: 2008-06-24 07:24:38 -0700 (Tue, 24 Jun 2008)
Log Message:
-----------
Renaming urg_nz to hokuyo_aist
Added Paths:
-----------
gearbox/trunk/submitted/hokuyo_aist/hokuyo_aist.cpp
gearbox/trunk/submitted/hokuyo_aist/hokuyo_aist.h
Copied: gearbox/trunk/submitted/hokuyo_aist/hokuyo_aist.cpp (from rev 210, gearbox/trunk/submitted/hokuyo_aist/urg_nz.cpp)
===================================================================
--- gearbox/trunk/submitted/hokuyo_aist/hokuyo_aist.cpp (rev 0)
+++ gearbox/trunk/submitted/hokuyo_aist/hokuyo_aist.cpp 2008-06-24 14:24:38 UTC (rev 211)
@@ -0,0 +1,1897 @@
+/*
+ * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
+ * http://gearbox.sf.net/
+ * Copyright (c) 2008 Geoffrey Biggs
+ *
+ * urg_nz Hokuyo URG laser scanner driver.
+ *
+ * This distribution is licensed to you under the terms described in the LICENSE file included in
+ * this distribution.
+ *
+ * This work is a product of the National Institute of Advanced Industrial Science and Technology,
+ * Japan. Registration number: ___
+ *
+ * This file is part of urg_nz.
+ *
+ * urg_nz is free software: you can redistribute it and/or modify it under the terms of the GNU
+ * Lesser General Public License as published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ *
+ * urg_nz is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even
+ * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License along with urg_nz. If
+ * not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "urg_nz.h"
+using namespace urg_nz;
+
+#include <flexiport/flexiport.h>
+#include <flexiport/port.h>
+#include <flexiport/serialport.h>
+#include <stdarg.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <math.h>
+#include <sstream>
+#include <iostream>
+using namespace std;
+using namespace flexiport;
+
+#if defined (WIN32)
+ #define __func__ __FUNCTION__
+#endif
+
+namespace urg_nz
+{
+
+#ifndef M_PI
+ const double M_PI = 3.14159265358979323846;
+#endif
+// Convert radians to degrees
+#ifndef RTOD
+ inline double RTOD (double rad)
+ {
+ return rad * 180.0 / M_PI;
+ }
+#endif
+// Convert degrees to radians
+#ifndef DTOR
+ inline double DTOR (double deg)
+ {
+ return deg * M_PI / 180.0;
+ }
+#endif
+
+// SCIP1: 66 bytes (64 bytes of data + line feed + NULL)
+const unsigned int SCIP1_LINE_LENGTH = 66;
+// SCIP2: 67 bytes (64 bytes of data + checksum byte + line feed + NULL)
+const unsigned int SCIP2_LINE_LENGTH = 67;
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// SCIP protocol version 1 notes
+////////////////////////////////////////////////////////////////////////////////////////////////////
+
+/* | = byte boundary, ... indicates variable byte block (max 64 bytes), (x) = x byte block
+ - No checksum
+ - Host to sensor: Command | Parameters... | LF
+ - Sensor to host: Command | Parameters... | LF | Status | LF | Data... | LF | LF
+ - Where a block of data would take more than 64 bytes, a line feed is inserted every 64 bytes.
+ - Status 0 is OK, anything else is an error.
+
+L Power
+ L|Control code|LF
+ L|Control code|LF|Status|LF|LF
+ 3 byte command block
+G Get data
+ G|Start(3)|End(3)|Cluster(2)|LF
+ G|Start(3)|End(3)|Cluster(2)|LF|Status|LF|Data...|LF|LF
+ 10 byte command block
+S Set baud rate
+ S|Baud rate(6)|Reserved(7)|LF
+ S|Baud rate(6)|Reserved(7)|Status|LF|LF|
+ 16 byte command block
+V Version info
+ V|LF
+ V|LF|Status...|LF|Vendor...|LF|Product...|LF|Firmware...|LF|Protocol...|LF|Serial...|LF|LF
+ 2 byte command block
+*/
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// SCIP protocol version 2 notes
+////////////////////////////////////////////////////////////////////////////////////////////////////
+
+/* | = byte boundary, ... indicates variable byte block (max 64 bytes), (x) = x byte block
+ - We don't use the string block (which can be up to 16 bytes) so it's marked as size 0 and ignored
+ in the command definitions below.
+ - Host to sensor: Command(2) | Parameters... | String(0) | LF
+ - Sensor to host: Command(2) | Parameters... | String(0) | LF | Status(2) | Sum | LF
+ - Each data row: Data (max 64) | Sum | LF
+ - Data rows are broken after a maximum of 64 bytes, each one having a checksum and a line feed.
+ - Status codes 00 and 99 are OK, anything else is an error.
+ - Checksum is calculated by... well, see the code.
+
+VV Version info
+ V|V|LF
+ V|V|LF|Status(2)|Sum|LF|Vendor...|;|Sum|LF|Product...|;|Sum|LF|Firmware...|;|Sum|LF|
+ Protocol...|;|Sum|LF|Serial...|;|Sum|LF|LF
+ 3 byte command block
+PP Specification info
+ P|P|LF
+ P|P|LF|Status(2)|Sum|LF|Model...|;|Sum|LF|MaxRange...|;|Sum|LF|MinRange...|;|Sum|LF|
+ TotalSteps...|;|Sum|LF|FirstStep...|;|Sum|LF|LastStep...|;|Sum|LF|FrontStep...|;|Sum|LF|
+ MotorSpeed...|;|Sum|LF|LF
+ 3 byte command block
+II Status info
+ I|I|LF
+ I|I|LF|Status(2)|Sum|LF|Model...|;|Sum|LF|Power...|;|Sum|LF|MotorSpeed...|;|Sum|LF|
+ Mode...|;|Sum|LF|Baud...|;|Sum|LF|Time...|;|Sum|LF|Diagnostic...|;|Sum|LF|LF
+ 3 byte command block
+BM Power on
+ B|M|LF
+ B|M|LF|Status(2)|Sum|LF|LF
+ 3 byte command block
+QT Power off
+ Q|T|LF
+ Q|T|LF|Status(2)|Sum|LF|LF
+ 3 byte command block
+SS Set baud rate
+ S|S|Baud(6)|LF
+ S|S|Baud(6)|LF|Status(2)|Sum|LF|LF
+ 9 byte command block
+MDMS Get new data
+ M|D/S|Start(4)|End(4)|Cluster(2)|Interval(1)|Number(2)|LF
+ M|D/S|Start(4)|End(4)|Cluster(2)|Interval(1)|Number(2)|LF|Status(2)|Sum|LF|Data...|LF|LF
+ 16 byte command block
+GDGS Get latest data
+ G|D/S|Start(4)|End(4)|Cluster(2)|LF
+ G|D/S|Start(4)|End(4)|Cluster(2)|LF|Status(2)|Sum|LF|Data...|LF|LF
+ 12 byte command block
+CR Set motor speed
+ C|R|Speed(2)|LF
+ C|R|Speed(2)|LF|Status(2)|Sum|LF|LF
+ 5 byte command block
+TM Get sensor time
+ T|M|Code|LF
+ T|M|Code|LF|Status(2)|Sum|LF[|Time(4)|Sum|LF|LF]
+ 4 byte command block
+ Optional part only comes back for control code 1.
+RS Reset
+ R|S|LF
+ R|S|LF|Status(2)|Sum|LF|LF
+ 3 byte command block
+*/
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// Utility functions
+////////////////////////////////////////////////////////////////////////////////////////////////////
+
+string SCIP1ErrorToString (char error, char cmd)
+{
+ return string ("No error descriptions available");
+}
+
+// error must be null-terminated
+string SCIP2ErrorToString (char *error, char *cmd)
+{
+ stringstream ss;
+
+ // Check for universal errors first
+ if (error[1] == 'A')
+ return "Unable to create transmission data or reply command internally";
+ else if (error[1] == 'B')
+ return "Buffer shortage or command repeated that is already processed";
+ else if (error[1] == 'C')
+ return "Command with insufficient parameters 1";
+ else if (error[1] == 'D')
+ return "Undefined command 1";
+ else if (error[1] == 'E')
+ return "Undefined command 2";
+ else if (error[1] == 'F')
+ return "Command with insufficient parameters 2";
+ else if (error[1] == 'G')
+ return "String character in command exceeds 16 letters";
+ else if (error[1] == 'H')
+ return "String character has invalid letters";
+ else if (error[0] == '0' && error[1] == 'I')
+ return "Sensor is now in firmware update mode";
+
+ int errorCode = atoi (error);
+
+ if (cmd[0] == 'B' && cmd[1] == 'M')
+ {
+ switch (errorCode)
+ {
+ case 1:
+ return "Unable to control due to laser malfunction";
+ case 2:
+ return "Laser is already on";
+ }
+ }
+// No info in the manual for this.
+// else if (cmd[0] == 'Q' && cmd[1] == 'T')
+// {
+// switch (errorCode)
+// {
+// default:
+// stringstream ss;
+// ss << "Unknown error code " << errorCode << " for command " << cmd[0] << cmd[1];
+// return ss.str ();
+// }
+// }
+ else if ((cmd[0] == 'G' && cmd[1] == 'D') ||
+ (cmd[0] == 'G' && cmd[1] == 'S'))
+ {
+ switch (errorCode)
+ {
+ case 1:
+ return "Starting step has non-numeric value";
+ case 2:
+ return "Ending step has non-numeric value";
+ case 3:
+ return "Cluster count has non-numeric value";
+ case 4:
+ return "Ending step is out of range";
+ case 5:
+ return "Ending step is smaller than start step";
+ case 6:
+ return "Scan interval is greater than 9";
+ case 7:
+ return "Number of scans is greater than 99";
+ default:
+ if (errorCode >= 50)
+ ss << "Hardware error: " << errorCode;
+ else
+ ss << "Unknown error code " << errorCode << " for command " << cmd[0] << cmd[1];
+
+ return ss.str ();
+ }
+ }
+ else if ((cmd[0] == 'M' && cmd[1] == 'D') ||
+ (cmd[0] == 'M' && cmd[1] == 'S'))
+ {
+ switch (errorCode)
+ {
+ case 1:
+ return "Starting step has non-numeric value";
+ case 2:
+ return "Ending step has non-numeric value";
+ case 3:
+ return "Cluster count has non-numeric value";
+ case 4:
+ return "Ending step is out of range";
+ case 5:
+ return "Ending step is smaller than start step";
+ case 6:
+ return "Scan interval is greater than 9";
+ case 7:
+ return "Number of scans is greater than 99";
+ default:
+ if (errorCode >= 50)
+ ss << "Hardware error: " << errorCode;
+ else
+ ss << "Unknown error code " << errorCode << " for command " << cmd[0] << cmd[1];
+
+ return ss.str ();
+ }
+ }
+ else if (cmd[0] == 'T' && cmd[1] == 'M')
+ {
+ switch (errorCode)
+ {
+ case 1:
+ return "Invalid control code";
+ case 2:
+ return "Adjust mode on command received when sensor's adjust mode is already on";
+ case 3:
+ return "Adjust mode off command received when sensor's adjust mode is already off";
+ case 4:
+ return "Adjust mode is off when requested time";
+ }
+ }
+ else if (cmd[0] == 'S' && cmd[1] == 'S')
+ {
+ switch (errorCode)
+ {
+ case 1:
+ return "Baud rate has non-numeric value";
+ case 2:
+ return "Invalid baud rate";
+ case 3:
+ return "Sensor is already running at that baud rate";
+ }
+ }
+ else if (cmd[0] == 'C' && cmd[1] == 'R')
+ {
+ switch (errorCode)
+ {
+ case 1:
+ return "Invalid speed";
+ case 2:
+ return "Speed is out of range";
+ case 3:
+ return "Motor is already running at that speed";
+ }
+ }
+// No info in the manual for this.
+// else if (cmd[0] == 'R' && cmd[1] == 'S')
+// {
+// switch (errorCode)
+// {
+// case :
+// return "";
+// default:
+// stringstream ss;
+// ss << "Unknown error code " << errorCode << " for command " << cmd[0] << cmd[1];
+// return ss.str ();
+// }
+// }
+// No info in the manual for this.
+// else if (cmd[0] == 'V' && cmd[1] == 'V')
+// {
+// switch (errorCode)
+// {
+// case :
+// return "";
+// }
+// }
+// No info in the manual for this.
+// else if (cmd[0] == 'P' && cmd[1] == 'P')
+// {
+// switch (errorCode)
+// {
+// case :
+// return "";
+// }
+// }
+// No info in the manual for this.
+// else if (cmd[0] == 'I' && cmd[1] == 'I')
+// {
+// switch (errorCode)
+// {
+// case :
+// return "";
+// }
+// }
+ else
+ {
+ ss << "Unknown command: " << cmd[0] << cmd[1];
+ return ss.str ();
+ }
+
+ // Known commands with unknown error codes fall through to here
+ ss << "Unknown error code " << errorCode << " for command " << cmd[0] << cmd[1];
+ return ss.str ();
+}
+
+unsigned int Decode2ByteValue (char *data)
+{
+ unsigned int byte1, byte2;
+
+ byte1 = data[0] - 0x30;
+ byte2 = data[1] - 0x30;
+
+ return (byte1 << 6) + (byte2);
+}
+
+unsigned int Decode3ByteValue (char *data)
+{
+ unsigned int byte1, byte2, byte3;
+
+ byte1 = data[0] - 0x30;
+ byte2 = data[1] - 0x30;
+ byte3 = data[2] - 0x30;
+
+ return (byte1 << 12) + (byte2 << 6) + (byte3);
+}
+
+unsigned int Decode4ByteValue (char *data)
+{
+ unsigned int byte1, byte2, byte3, byte4;
+
+ byte1 = data[0] - 0x30;
+ byte2 = data[1] - 0x30;
+ byte3 = data[2] - 0x30;
+ byte4 = data[3] - 0x30;
+
+ return (byte1 << 18) + (byte2 << 12) + (byte3 << 6) + (byte4);
+}
+
+void NumberToString (unsigned int num, char *dest, int length)
+{
+#if defined (WIN32)
+ _snprintf (dest, length + 1, "%*d", length, num);
+#else
+ snprintf (dest, length + 1, "%*d", length, num);
+#endif
+ // Replace all leading spaces with '0'
+ for (int ii = 0; ii < length && dest[ii] == ' '; ii++)
+ dest[ii] = '0';
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// URGError class
+////////////////////////////////////////////////////////////////////////////////////////////////////
+
+string URGError::AsString (void) const throw ()
+{
+ switch (_errorCode)
+ {
+ case URG_ERR_READ:
+ return "URG_ERR_READ";
+ case URG_ERR_WRITE:
+ return "URG_ERR_WRITE";
+ case URG_ERR_PROTOCOL:
+ return "URG_ERR_PROTOCOL";
+ case URG_ERR_CHANGEBAUD:
+ return "URG_ERR_CHANGEBAUD";
+ case URG_ERR_CONNECT_FAILED:
+ return "URG_ERR_CONNECT_FAILED";
+ case URG_ERR_CLOSE_FAILED:
+ return "URG_ERR_CLOSE_FAILED";
+ case URG_ERR_NODESTINATION:
+ return "URG_ERR_NODESTINATION";
+ case URG_ERR_BADFIRMWARE:
+ return "URG_ERR_BADFIRMWARE";
+ case URG_ERR_SCIPVERSION:
+ return "URG_ERR_SCIPVERSION";
+ case URG_ERR_MEMORY:
+ return "URG_ERR_MEMORY";
+ case URG_ERR_UNSUPPORTED:
+ return "URG_ERR_UNSUPPORTED";
+ case URG_ERR_BADARG:
+ return "URG_ERR_BADARG";
+ case URG_ERR_NODATA:
+ return "URG_ERR_NODATA";
+ case URG_ERR_NOTSERIAL:
+ return "URG_ERR_NOTSERIAL";
+ }
+ return "UNKNOWN_ERROR";
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// URGSensorInfo class
+////////////////////////////////////////////////////////////////////////////////////////////////////
+
+URGSensorInfo::URGSensorInfo (void)
+ : minRange (0), maxRange (0), steps (0), firstStep (0), lastStep (0), frontStep (0),
+ standardSpeed (0), power (false), speed (0), baud (0), time (0), minAngle (0.0), maxAngle (0.0),
+ resolution (0.0), scanableSteps (0)
+{
+}
+
+// Set various known values based on what the manual says
+void URGSensorInfo::SetDefaults (void)
+{
+ minRange = 20;
+ maxRange = 4095;
+ steps = 1024;
+ firstStep = 44;
+ lastStep = 725;
+ frontStep = 384;
+}
+
+void URGSensorInfo::CalculateValues (void)
+{
+ resolution = DTOR (360.0) / steps;
+ // If any of the steps are beyond INT_MAX, we have problems.
+ // We also have an incredibly high-resolution sensor.
+ minAngle = (static_cast<int> (firstStep) - static_cast<int> (frontStep)) * resolution;
+ maxAngle = (static_cast<int> (lastStep) - static_cast<int> (frontStep)) * resolution;
+ scanableSteps = lastStep - firstStep + 1;
+}
+
+string URGSensorInfo::AsString (void)
+{
+ stringstream ss;
+
+ ss << "Vendor: " << vendor << endl;
+ ss << "Product: " << product << endl;
+ ss << "Firmware: " << firmware << endl;
+ ss << "Protocol: " << protocol << endl;
+ ss << "Serial: " << serial << endl;
+ ss << "Model: " << model << endl;
+
+ ss << "Mininum range: " << minRange << "mm\tMaximum range: " << maxRange << "mm" << endl;
+ ss << "Steps in 360 degrees: " << steps << "\tScanable steps: " << scanableSteps << endl;
+ ss << "First step: " << firstStep << "\tFront step: " << frontStep << "\tLast step: " <<
+ lastStep << endl;
+ ss << "Resolution: " << resolution << " radians/step" << endl;
+ ss << "Minimum angle: " << minAngle << " radians\tMaximum angle: " << maxAngle <<
+ " radians" << endl;
+ ss << "Standard motor speed: " << standardSpeed << "rpm" << endl;
+
+ ss << "Power status: " << (power ? "On" : "Off") << "\tMeasurement state: " <<
+ measureState << endl;
+ ss << "Motor speed: " << speed << "rpm\tBaud rate: " << baud << "bps" << endl;
+ ss << "Time stamp: " << time << "ms" << endl;
+ ss << "Sensor diagnostic: " << sensorDiagnostic << endl;
+
+ return ss.str ();
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// URGData class
+////////////////////////////////////////////////////////////////////////////////////////////////////
+
+URGData::URGData (void)
+ : _data (NULL), _length (0), _error (-1), _time (0)
+{
+}
+
+URGData::URGData (unsigned short *data, unsigned int length, short error, unsigned int time)
+ : _error (error), _time (time)
+{
+ _length = length;
+ if (_length == 0)
+ _data = NULL;
+ else
+ {
+ if ((_data = new unsigned short[_length]) == NULL)
+ {
+ _length = 0;
+ throw URGError (URG_ERR_MEMORY, "Failed to allocate space to copy data.");
+ }
+ memcpy (_data, data, sizeof (unsigned short) * _length);
+ }
+}
+
+URGData::URGData (const URGData &rhs)
+{
+ _length = rhs.Length ();
+ if (_length == 0)
+ _data = NULL;
+ else
+ {
+ if ((_data = new unsigned short[_length]) == NULL)
+ {
+ _length = 0;
+ throw URGError (URG_ERR_MEMORY, "Failed to allocate space to copy data.");
+ }
+ memcpy (_data, rhs.Ranges (), sizeof (unsigned short) * _length);
+ }
+ _error = rhs.GetErrorCode ();
+ _time = rhs.TimeStamp ();
+}
+
+URGData::~URGData (void)
+{
+ if (_data != NULL)
+ delete[] _data;
+}
+
+string URGData::ErrorCodeToString (void)
+{
+ switch (_error)
+ {
+ case -1:
+ return "No error.";
+ case 0:
+ return "Possibility of detected object is at 22m.";
+ case 1:
+ return "Reflected light has low intensity.";
+ case 2:
+ return "Reflected light has low intensity.";
+ case 3:
+ return "Reflected light has low intensity.";
+ case 4:
+ return "Reflected light has low intensity.";
+ case 5:
+ return "Reflected light has low intensity.";
+ case 6:
+ return "Possibility of detected object is at 5.7m.";
+ case 7:
+ return "Distance data on the preceding and succeeding steps have errors.";
+ case 8:
+ return "Others.";
+ case 9:
+ return "The same step had error in the last two scan.";
+ case 10:
+ return "Others.";
+ case 11:
+ return "Others.";
+ case 12:
+ return "Others.";
+ case 13:
+ return "Others.";
+ case 14:
+ return "Others.";
+ case 15:
+ return "Others.";
+ case 16:
+ return "Possibility of detected object is in the range 4096mm.";
+ case 17:
+ return "Others.";
+ case 18:
+ return "Unspecified.";
+ case 19:
+ return "Non-measurable distance.";
+ default:
+ stringstream ss;
+ ss << "Unknown error code: " << _error;
+ return ss.str ();
+ }
+}
+
+URGData& URGData::operator= (const URGData &rhs)
+{
+ if (rhs.Length () == 0)
+ {
+ _length = 0;
+ if (_data != NULL)
+ delete[] _data;
+ _data = NULL;
+ _error = rhs.GetErrorCode ();
+ _time = rhs.TimeStamp ();
+ }
+ else
+ {
+ unsigned int rhsLength = rhs.Length ();
+ unsigned short *newData;
+ if (rhsLength != _length)
+ {
+ // Copy the data into a temporary variable pointing to new space (prevents dangling
+ // pointers on allocation error and prevents self-assignment making a mess).
+ if ((newData = new unsigned short[rhsLength]) == NULL)
+ throw URGError (URG_ERR_MEMORY, "Failed to allocate space to copy data.");
+ memcpy (newData, rhs.Ranges (), sizeof (unsigned short) * rhsLength);
+ if (_data != NULL)
+ delete[] _data;
+ _data = newData;
+ _length = rhs.Length ();
+ }
+ else
+ {
+ // If lengths are the same, no need to reallocate
+ memcpy (_data, rhs.Ranges (), sizeof (unsigned short) * _length);
+ }
+
+ _error = rhs.GetErrorCode ();
+ _time = rhs.TimeStamp ();
+ }
+
+ return *this;
+}
+
+unsigned short URGData::operator[] (unsigned int index)
+{
+ if (index >= _length)
+ throw URGError (URG_ERR_BADARG, "Invalid data index.");
+ return _data[index];
+}
+
+string URGData::AsString (void)
+{
+ stringstream ss;
+
+ ss << _length << " readings:" << endl;
+ for (unsigned int ii = 0; ii < _length; ii++)
+ ss << _data[ii] << "\t";
+ ss << endl << "Data error: (" << _error << ") " << ErrorCodeToString () << endl;
+ ss << "Time stamp: " << _time << endl;
+
+ return ss.str ();
+}
+
+void URGData::CleanUp (void)
+{
+ if (_data != NULL)
+ delete[] _data;
+ _data = NULL;
+ _length = 0;
+ _error = 0;
+ _time = 0;
+}
+
+void URGData::AllocateData (unsigned int length)
+{
+ // If no data yet, allocate new
+ if (_data == NULL)
+ {
+ if ((_data = new unsigned short[length]) == NULL)
+ {
+ _length = 0;
+ throw URGError (URG_ERR_MEMORY, "Failed to allocate space to copy data.");
+ }
+ _length = length;
+ }
+ // If there is data, reallocate only if the length is different
+ else if (length != _length)
+ {
+ delete[] _data;
+ if ((_data = new unsigned short[length]) == NULL)
+ {
+ _length = 0;
+ throw URGError (URG_ERR_MEMORY, "Failed to allocate space to copy data.");
+ }
+ _length = length;
+ }
+ // Else data is already allocated to the right length, so do nothing
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// URGLaser class
+////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// Public API
+////////////////////////////////////////////////////////////////////////////////////////////////////
+
+URGLaser::URGLaser (void)
+ : _port (NULL), _scipVersion (1), _verbose (false), _minAngle (0.0), _maxAngle (0.0),
+ _resolution (0.0), _firstStep (0), _lastStep (0), _frontStep (0)
+{
+}
+
+URGLaser::~URGLaser (void)
+{
+ if (_port != NULL)
+ delete _port;
+}
+
+void URGLaser::Open (string portOptions)
+{
+ if (_verbose)
+ {
+ cerr << "URGLaser::" << __func__ << "() Creating and opening port using options: " <<
+ portOptions << endl;
+ }
+ _port = flexiport::CreatePort (portOptions);
+ _port->Open ();
+
+ if (_verbose)
+ {
+ cerr << "URGLaser::" << __func__ << "() Connected using " << _port->GetPortType () <<
+ " connection." << endl;
+ cerr << _port->GetStatus ();
+ }
+ _port->Flush ();
+
+ // Figure out the SCIP version currently in use and switch to a higher one if possible
+ GetAndSetSCIPVersion ();
+ // Get some values we need for providing default ranges
+ GetDefaults ();
+}
+
+void URGLaser::Close (void)
+{
+ if (!_port)
+ throw URGError (URG_ERR_CLOSE_FAILED, "Port is not open.");
+ if (_verbose)
+ cerr << "URGLaser::" << __func__ << "() Closing connection." << endl;
+ delete _port;
+ _port = NULL;
+}
+
+bool URGLaser::IsOpen (void) const
+{
+ if (_port != NULL)
+ return _port->IsOpen ();
+ return false;
+}
+
+void URGLaser::SetPower (bool on)
+{
+ if (_scipVersion == 1)
+ {
+ if (on)
+ {
+ if (_verbose)
+ cerr << "URGLaser::" << __func__ << "() Turning laser on." << endl;
+ SendCommand ("L", "1", 1, NULL);
+ }
+ else
+ {
+ if (_verbose)
+ cerr << "URGLaser::" << __func__ << "() Turning laser off." << endl;
+ SendCommand ("L", "0", 1, NULL);
+ }
+ SkipLines (1);
+ }
+ else if (_scipVersion == 2)
+ {
+ if (on)
+ {
+ if (_verbose)
+ cerr << "URGLaser::" << __func__ << "() Turning laser on." << endl;
+ SendCommand ("BM", NULL, 0, "02");
+ }
+ else
+ {
+ if (_verbose)
+ cerr << "URGLaser::" << __func__ << "() Turning laser off." << endl;
+ SendCommand ("QT", NULL, 0, "02");
+ }
+ SkipLines (1);
+ }
+ else
+ throw URGError (URG_ERR_SCIPVERSION, "Unknown SCIP version.");
+}
+
+// This function assumes that both the port and the laser scanner are already set to the same baud.
+void URGLaser::SetBaud (unsigned int baud)
+{
+ if (_port->GetPortType () != "serial")
+ throw URGError (URG_ERR_NOTSERIAL, "Cannot change baud rate of non-serial connection.");
+
+ char newBaud[13];
+ memset (newBaud, 0, sizeof (char) * 13);
+
+ if (baud != 19200 && baud != 57600 && baud != 115200 &&
+ baud != 250000 && baud != 500000 && baud != 750000)
+ {
+ stringstream ss;
+ ss << "Bad baud rate: " << baud << endl;
+ throw URGError (URG_ERR_BADARG, ss.str ());
+ }
+ NumberToString (baud, newBaud, 6);
+
+ if (_scipVersion == 1)
+ {
+ // Send the command to change baud rate
+ SendCommand ("S", newBaud, 13, NULL);
+ SkipLines (1);
+ // Change the port's baud rate
+ reinterpret_cast<SerialPort*> (_port)->SetBaudRate (baud);
+ }
+ else if (_scipVersion == 2)
+ {
+ // Send the command to change baud rate
+ SendCommand ("SS", newBaud, 6, "03");
+ SkipLines (1);
+ // Change the port's baud rate
+ reinterpret_cast<SerialPort*> (_port)->SetBaudRate (baud);
+ }
+ else
+ throw URGError (URG_ERR_SCIPVERSION, "Unknown SCIP version.");
+}
+
+void URGLaser::Reset (void)
+{
+ if (_scipVersion == 1)
+ throw URGError (URG_ERR_UNSUPPORTED, "SCIP version 1 does not support the reset command.");
+ else if (_scipVersion == 2)
+ {
+ if (_verbose)
+ cerr << "URGLaser::" << __func__ << "() Resetting laser." << endl;
+ SendCommand ("RS", NULL, 0, NULL);
+ SkipLines (1);
+ }
+ else
+ throw URGError (URG_ERR_SCIPVERSION, "Unknown SCIP version.");
+}
+
+void URGLaser::SetMotorSpeed (unsigned int speed)
+{
+ if (_scipVersion == 1)
+ {
+ throw URGError (URG_ERR_UNSUPPORTED,
+ "SCIP version 1 does not support the set motor speed command.");
+ }
+ else if (_scipVersion == 2)
+ {
+ // Sanity check the value
+ if ((speed > 600 || speed < 540 || (speed % 6) != 0) && speed != 0)
+ throw URGError (URG_ERR_BADARG, "Invalid motor speed.");
+ char buffer[3];
+ buffer[2] = '\0';
+ if (speed == 0)
+ {
+ if (_verbose)
+ cerr << "URGLaser::" << __func__ << "() Reseting motor speed to default." << endl;
+ buffer[0] = '9';
+ buffer[1] = '9';
+ }
+ else
+ {
+ if (_verbose)
+ {
+ cerr << "URGLaser::" << __func__ << "() Setting motor speed to " <<
+ speed << "rpm." << endl;
+ }
+ if (speed == 540)
+ {
+ buffer[0] = '1';
+ buffer[1] = '0';
+ }
+ else
+ {
+ buffer[0] = '0';
+ buffer[1] = 100 - (speed / 6) + 0x30;
+ }
+ }
+ SendCommand ("CR", buffer, 2, "03");
+ SkipLines (1);
+ }
+ else
+ throw URGError (URG_ERR_SCIPVERSION, "Unknown SCIP version.");
+}
+
+void URGLaser::GetSensorInfo (URGSensorInfo *info)
+{
+ if (info == NULL)
+ throw URGError (URG_ERR_NODESTINATION, "No info object provided.");
+
+ if (_scipVersion == 1)
+ {
+ if (_verbose)
+ {
+ cerr << "URGLaser::" << __func__ <<
+ "() Getting sensor information using SCIP version 1." << endl;
+ }
+
+ info->SetDefaults ();
+
+ char buffer[SCIP1_LINE_LENGTH];
+ memset (buffer, 0, sizeof (char) * SCIP1_LINE_LENGTH);
+
+ SendCommand ("V", NULL, 0, NULL);
+ // Get the vendor info line
+ ReadLine (buffer);
+ info->vendor = &buffer[5]; // Chop off the "VEND:" tag
+ // Get the product info line
+ ReadLine (buffer);
+ info->product = &buffer[5];
+ // Get the firmware line
+ ReadLine (buffer);
+ info->firmware = &buffer[5];
+ // Get the protocol version line
+ ReadLine (buffer);
+ info->protocol = &buffer[5];
+ // Get the serial number
+ ReadLine (buffer);
+ info->serial = &buffer[5];
+ // Get either the status line or the end of message
+ ReadLine (buffer);
+ if (buffer[0] != '\n')
+ {
+ // Got a status line
+ info->sensorDiagnostic = &buffer[5];
+ SkipLines (1);
+ }
+
+ // Check the firmware version major number. If it's >=3 there is probably some extra info
+ // in the firmware line.
+ // e.g.: FIRM:3.1.04,07/08/02(20-4095[mm],240[deg],44-725[step],600[rpm])
+ // Note that this example is right up against the maximum SCIP v1 line length of 64 bytes.
+ if (atoi (info->firmware.c_str ()) >= 3)
+ {
+ if (_verbose)
+ cerr << "SCIP1 Firmware line for parsing: " << info->firmware << endl;
+ // Now the fun part: parsing the line. It would be nice if we could use the POSIX regex
+ // functions, but since MS doesn't believe in POSIX we get to do it the hard way.
+ // Start by finding the first (
+ char *valueStart;
+ if ((valueStart = strchr (info->firmware.c_str (), '(')) == NULL)
+ {
+ // No bracket? Crud. Fail and use the hard-coded values from the manual.
+ info->CalculateValues ();
+ }
+ // Now put it through sscanf and hope...
+ int aperture;
+ int numFound = sscanf (valueStart, "%d-%d[mm],%d[deg],%d-%d[step],%d[rpm]",
+ &info->minRange, &info->maxRange, &aperture,
+ &info->firstStep, &info->lastStep, &info->speed);
+ if (numFound != 6)
+ {
+ // Didn't get enough values out, assume unknown format and fall back on the defaults
+ info->SetDefaults ();
+ info->CalculateValues ();
+ if (_verbose)
+ {
+ cerr << "Retrieved sensor info (hard-coded):" << endl;
+ cerr << info->AsString ();
+ }
+ }
+
+ // Need to calculate stuff differently since it gave us an aperture value
+ info->resolution = static_cast<double> (aperture) /
+ static_cast<double> (info->lastStep - info->firstStep);
+ // Assume that the range is evenly spread
+ info->scanableSteps = info->lastStep - info->firstStep + 1;
+ info->frontStep = info->scanableSteps / 2;
+ info->minAngle = (info->firstStep - info->frontStep) * info->resolution;
+ info->maxAngle = (info->lastStep - info->frontStep) * info->resolution;
+ }
+ else
+ {
+ // We're stuck with hard-coded defaults from the manual (already set earlier).
+ info->CalculateValues ();
+ if (_verbose)
+ {
+ cerr << "Retrieved sensor info (hard-coded):" << endl;
+ cerr << info->AsString ();
+ }
+ }
+ }
+ else if (_scipVersion == 2)
+ {
+ if (_verbose)
+ {
+ cerr << "URGLaser::" << __func__ <<
+ "() Getting sensor information using SCIP version 2." << endl;
+ }
+
+ char buffer[SCIP2_LINE_LENGTH];
+ memset (buffer, 0, sizeof (char) * SCIP2_LINE_LENGTH);
+
+ // We need to send three commands to get all the info we want: VV, PP and II
+ SendCommand ("VV", NULL, 0, NULL);
+ // Get the vendor info line
+ ReadLineWithCheck (buffer, -1, true);
+ info->vendor = &buffer[5]; // Chop off the "VEND:" tag
+ // Get the product info line
+ ReadLineWithCheck (buffer, -1, true);
+ info->product = &buffer[5];
+ // Get the firmware line
+ ReadLineWithCheck (buffer, -1, true);
+ info->firmware = &buffer[5];
+ // Get the protocol version line
+ ReadLineWithCheck (buffer, -1, true);
+ info->protocol = &buffer[5];
+ // Get the serial number
+ ReadLineWithCheck (buffer, -1, true);
+ info->serial = &buffer[5];
+ // Skip the end-of-message
+ SkipLines (1);
+
+ // Next up, PP
+ SendCommand ("PP", NULL, 0, NULL);
+ // Get the model line
+ ReadLineWithCheck (buffer, -1, true);
+ info->model = &buffer[5];
+ // On to the fun ones that require parsing
+ ReadLineWithCheck (buffer, -1, true);
+ info->minRange = atoi (&buffer[5]);
+ ReadLineWithCheck (buffer, -1, true);
+ info->maxRange = atoi (&buffer[5]);
+ ReadLineWithCheck (buffer, -1, true);
+ info->steps = atoi (&buffer[5]);
+ ReadLineWithCheck (buffer, -1, true);
+ info->firstStep = atoi (&buffer[5]);
+ ReadLineWithCheck (buffer, -1, true);
+ info->lastStep = atoi (&buffer[5]);
+ ReadLineWithCheck (buffer, -1, true);
+ info->frontStep = atoi (&buffer[5]);
+ ReadLineWithCheck (buffer, -1, true);
+ info->standardSpeed = atoi (&buffer[5]);
+ // Skip the end-of-message
+ SkipLines (1);
+
+ // Command II: Revenge of the Commands.
+ SendCommand ("II", NULL, 0, NULL);
+ // Skip the model line (we already have it from PP)
+ SkipLines (1);
+ // Get and parse the power state
+ ReadLineWithCheck (buffer, -1, true);
+ if (strncmp (&buffer[5], "OFF", 3) == 0)
+ info->power = false;
+ else
+ info->power = true;
+ // Motor speed
+ ReadLineWithCheck (buffer, -1, true);
+ // TODO: check if the format of this line changes if the motor speed is changed
+ if (sscanf (buffer, "SCSP:%*7s(%d[rpm]", &info->speed) != 1)
+ throw URGError (URG_ERR_PROTOCOL, "Motor speed line parse failed.");
+ // Measuring state
+ ReadLineWithCheck (buffer, -1, true);
+ info->measureState = &buffer[5];
+ // Baud rate
+ ReadLineWithCheck (buffer, -1, true);
+ if (strncmp (&buffer[5], "USB only", 8) == 0)
+ {
+ // No baud rate for USB-only devices such as the UHG-08LX
+ info->baud = 0;
+ }
+ else if (sscanf (buffer, "SBPS:%d[bps]", &info->baud) != 1)
+ throw URGError (URG_ERR_PROTOCOL, "Baud rate line parse failed.");
+ // Time stamp
+ ReadLineWithCheck (buffer, -1, true);
+ if (sscanf (buffer, "TIME:%x", &info->time) != 1)
+ throw URGError (URG_ERR_PROTOCOL, "Timestamp line parse failed.");
+ // Diagnostic
+ ReadLineWithCheck (buffer, -1, true);
+ info->sensorDiagnostic = &buffer[5];
+ // Skip the end-of-message
+ SkipLines (1);
+
+ info->CalculateValues ();
+ if (_verbose)
+ {
+ cerr << "Retrieved sensor info:" << endl;
+ cerr << info->AsString ();
+ }
+ }
+ else
+ throw URGError (URG_ERR_SCIPVERSION, "Unknown SCIP version.");
+}
+
+unsigned int URGLaser::GetTime (void)
+{
+ if (_scipVersion == 1)
+ {
+ throw URGError (URG_ERR_UNSUPPORTED,
+ "SCIP version 1 does not support the get time command.");
+ }
+ else if (_scipVersion == 2)
+ {
+ if (_verbose)
+ cerr << "URGLaser::" << __func__ << "() Retrieving time from laser." << endl;
+ SendCommand ("TM", "0", 1, NULL);
+ SendCommand ("TM", "1", 1, NULL);
+ char buffer[7];
+ ReadLineWithCheck (buffer, 6);
+ SendCommand ("TM", "2", 1, NULL);
+ SkipLines (1);
+ // We need to decode the time value that's in the buffer
+ return Decode4ByteValue (buffer);
+ }
+ else
+ throw URGError (URG_ERR_SCIPVERSION, "Unknown SCIP version.");
+
+ return 0;
+}
+
+unsigned int URGLaser::GetRanges (URGData *data, int startStep, int endStep,
+ unsigned int clusterCount)
+{
+ if (data == NULL)
+ throw URGError (URG_ERR_NODESTINATION, "No data destination provided.");
+
+ char buffer[11];
+ memset (buffer, 0, sizeof (char) * 11);
+
+ if (startStep < 0)
+ startStep = _firstStep;
+ if (endStep < 0)
+ endStep = _lastStep;
+
+ unsigned int numSteps = (endStep - startStep + 1) / clusterCount;
+ if (_verbose)
+ {
+ cerr << "URGLaser::" << __func__ << "() Reading " << numSteps << " ranges between " <<
+ startStep << " and " << endStep << " with a cluster count of " << clusterCount << endl;
+ }
+
+ if (_scipVersion == 1)
+ {
+ // Send the command to ask for the most recent range data from startStep to endStep
+ NumberToString (startStep, buffer, 3);
+ NumberToString (endStep, &buffer[3], 3);
+ NumberToString (clusterCount, &buffer[6], 2);
+ data->_error = SendCommand ("G", buffer, 8, NULL);
+ // In SCIP1 mode we're going to get back 2-byte data
+ Read2ByteRangeData (data, numSteps);
+ }
+ else if (_scipVersion == 2)
+ {
+ // Send the command to ask for the most recent range data from startStep to endStep
+ NumberToString (startStep, buffer, 4);
+ NumberToString (endStep, &buffer[4], 4);
+ NumberToString (clusterCount, &buffer[8], 2);
+ data->_error = SendCommand ("GD", buffer, 10, NULL);
+ // There will be a timestamp before the data (if there is data)
+ // Normally we would send 6 for the expected length, but we may get no timestamp back if
+ // there was no data.
+ if (ReadLineWithCheck (buffer) == 0)
+ throw URGError (URG_ERR_NODATA, "No data received. Check data error code.");
+ data->_time = Decode4ByteValue (buffer);
+ // In SCIP2 mode we're going to get back 3-byte data because we're sending the GD command
+ Read3ByteRangeData (data, numSteps);
+ }
+ else
+ throw URGError (URG_ERR_SCIPVERSION, "Unknown SCIP version.");
+
+ return data->_length;
+}
+
+unsigned int URGLaser::GetRanges (URGData *data, double startAngle,
+ double endAngle, unsigned int clusterCount)
+{
+ if (data == NULL)
+ throw URGError (URG_ERR_NODESTINATION, "No data destination provided.");
+
+ // Calculate the given angles in steps, rounding towards _frontStep
+ int startStep, endStep;
+ startStep = AngleToStep (startAngle);
+ endStep = AngleToStep (endAngle);
+
+ // Check the steps are within the allowable range
+ if (startStep < _firstStep || startStep > _lastStep)
+ throw URGError (URG_ERR_BADARG, "Start step is out of range.");
+ if (endStep < _firstStep || endStep > _lastStep)
+ throw URGError (URG_ERR_BADARG, "End step is out of range.");
+
+ if (_verbose)
+ {
+ cerr << "URGLaser::" << __func__ << "() Start angle " << startAngle << " is step " <<
+ startStep << ", end angle " << endAngle << " is step " << endStep << endl;
+ }
+
+ // Get the data
+ return GetRanges (data, startStep, endStep, clusterCount);
+}
+
+unsigned int URGLaser::GetNewRanges (URGData *data, int startStep, int endStep,
+ unsigned int clusterCount)
+{
+ if (data == NULL)
+ throw URGError (URG_ERR_NODESTINATION, "No data destination provided.");
+
+ if (_scipVersion == 1)
+ {
+ throw URGError (URG_ERR_UNSUPPORTED,
+ "SCIP version 1 does not support the get new ranges command.");
+ }
+ else if (_scipVersion == 2)
+ {
+ char buffer[14];
+ memset (buffer, 0, sizeof (char) * 14);
+
+ if (startStep < 0)
+ startStep = _firstStep;
+ if (endStep < 0)
+ endStep = _lastStep;
+
+ unsigned int numSteps = (endStep - startStep + 1) / clusterCount;
+ if (_verbose)
+ {
+ cerr << "URGLaser::" << __func__ << "() Reading " << numSteps <<
+ " new ranges between " << startStep << " and " << endStep <<
+ " with a cluster count of " << clusterCount << endl;
+ }
+
+ // Send the command to ask for the most recent range data from startStep to endStep
+ NumberToString (startStep, buffer, 4);
+ NumberToString (endStep, &buffer[4], 4);
+ NumberToString (clusterCount, &buffer[8], 2);
+ NumberToString (1, &buffer[10], 1);
+ NumberToString (1, &buffer[11], 2);
+ data->_error = SendCommand ("MD", buffer, 13, NULL);
+ // There will be a timestamp before the data (if there is data)
+ // Normally we would send 6 for the expected length, but we may get no timestamp back if
+ // there was no data.
+ if (ReadLineWithCheck (buffer) == 0)
+ throw URGError (URG_ERR_NODATA, "No data received. Check data error code.");
+ data->_time = Decode4ByteValue (buffer);
+ // In SCIP2 mode we're going to get back 3-byte data because we're sending the MD command
+ Read3ByteRangeData (data, numSteps);
+ }
+ else
+ throw URGError (URG_ERR_SCIPVERSION, "Unknown SCIP version.");
+
+ return data->_length;
+}
+
+unsigned int URGLaser::GetNewRanges (URGData *data, double startAngle, double endAngle,
+ unsigned int clusterCount)
+{
+ if (data == NULL)
+ throw URGError (URG_ERR_NODESTINATION, "No data destination provided.");
+ if (_scipVersion == 1)
+ {
+ throw URGError (URG_ERR_UNSUPPORTED,
+ "SCIP version 1 does not support the get new ranges command.");
+ }
+
+ // Calculate the given angles in steps, rounding towards _frontStep
+ int startStep, endStep;
+ startStep = AngleToStep (startAngle);
+ endStep = AngleToStep (endAngle);
+
+ // Check the steps are within the allowable range
+ if (startStep < _firstStep || startStep > _lastStep)
+ throw URGError (URG_ERR_BADARG, "Start step is out of range.");
+ if (endStep < _firstStep || endStep > _lastStep)
+ throw URGError (URG_ERR_BADARG, "End step is out of range.");
+
+ if (_verbose)
+ {
+ cerr << "URGLaser::" << __func__ << "() Start angle " << startAngle << " is step " <<
+ startStep << ", end angle " << endAngle << " is step " << endStep << endl;
+ }
+
+ // Get the data
+ return GetNewRanges (data, startStep, endStep, clusterCount);
+}
+
+double URGLaser::StepToAngle (unsigned int step)
+{
+ return (static_cast<int> (step) - static_cast<int> (_frontStep)) * _resolution;
+}
+
+unsigned int URGLaser::AngleToStep (double angle)
+{
+ unsigned int result;
+ double resultF;
+ resultF = _frontStep +
+ (static_cast<double> (angle) / static_cast<double> (_resolution));
+ // Round towards _frontStep so that the step values are always inside the angles given
+ if (resultF < _frontStep)
+ result = static_cast<int> (ceil (resultF));
+ else
+ result = static_cast<int> (floor (resultF));
+
+ return result;
+}
+
+// Private functions
+////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// If expectedLength is not -1, it should include the terminating line feed but not the NULL
+// (although the buffer still has to include this).
+// If expectedLenght is -1, this function expects buffer to be a certain length to allow up to the
+// maximum line length to be read. See SCIP1_LINE_LENGTH and SCIP2_LINE_LENGTH.
+// The line feed that terminates a line will be replaced with a NULL.
+// The return value is the number of bytes received, not including the NULL byte or the line feed.
+int URGLaser::ReadLine (char *buffer, int expectedLength)
+{
+ int lineLength = 0;
+
+ if (expectedLength == -1)
+ {
+ int maxLength = (_scipVersion == 1) ? SCIP1_LINE_LENGTH : SCIP2_LINE_LENGTH;
+ if (_verbose)
+ {
+ cerr << "URGLaser::" << __func__ << "() Reading up to " << maxLength << " bytes." <<
+ endl;
+ }
+ // We need to get at least 1 byte in a line: the line feed.
+ if ((lineLength = _port->ReadLine (buffer, maxLength)) < 0)
+ throw URGError (URG_ERR_READ, "Timed out trying to read a line.");
+ else if (lineLength == 0)
+ throw URGError (URG_ERR_READ, "No data received when trying to read a line.");
+ // Replace the line feed with a NULL
+ buffer[lineLength - 1] = '\0';
+ }
+ else
+ {
+ if (_verbose)
+ {
+ cerr << "URGLaser::" << __func__ << "() Reading exactly " << expectedLength <<
+ " bytes." << endl;
+ }
+ if ((lineLength = _port->ReadLine (buffer, expectedLength + 1)) < 0) // +1 for the NULL
+ throw URGError (URG_ERR_READ, "Timed out trying to read a line.");
+ else if (lineLength == 0)
+ throw URGError (URG_ERR_READ, "No data received when trying to read a line.");
+ else if (lineLength < expectedLength)
+ {
+ stringstream ss;
+ ss << "URGLaser::" << __func__ << "() Got an incorrect line length: " << lineLength <<
+ " != " << expectedLength;
+ throw URGError (URG_ERR_PROTOCOL, ss.str ());
+ }
+ // Replace the line feed with a NULL
+ buffer[lineLength - 1] = '\0';
+ }
+
+ if (_verbose)
+ {
+ cerr << "URGLaser::" << __func__ << "() Read " << lineLength << " bytes." << endl;
+ cerr << "URGLaser::" << __func__ << "() Line is " << buffer << endl;
+ }
+ return lineLength - 1; // Line feed not included
+}
+
+// This function will read a line and then calculate its checksum, comparing it with the checksum
+// at the end of the line. The checksum will be removed (along with the semi-colon, if present).
+// buffer and expectedLength args are as for ReadLine().
+// If hasSemicolon is true, the byte before the checksum is assumed to be the semi-colon separator
+// and so not a part of the checksum. If it's not a semi-colon, an exception is thrown.
+// Empty lines (i.e. a line that is just the line feed, as at the end of the message) will result in
+// a return value of zero and no checksum check will be performed. Otherwise the number of actual
+// data bytes (i.e. excluding the checksum and semicolon) will be returned.
+int URGLaser::ReadLineWithCheck (char *buffer, int expectedLength, bool hasSemicolon)
+{
+ int lineLength = ReadLine (buffer, expectedLength);
+ if (_scipVersion == 1)
+ {
+ // No checksums in SCIP version 1
+ return lineLength;
+ }
+
+ // If the line is empty, assume it was a line-feed message terminator, in which case there is no
+ // checksum to check.
+ if (lineLength == 0)
+ return 0;
+
+ // Ignore the checksum itself, and possibly a semicolon (ReadLine has already chopped off the
+ // line feed for us).
+ int bytesToConsider = lineLength - 1 - (hasSemicolon ? 1 : 0);
+ int checksumIndex = bytesToConsider + (hasSemicolon ? 1 : 0);
+ if (_verbose)
+ {
+ cerr << "URGLaser::" << __func__ << "() Considering " << bytesToConsider <<
+ " bytes for checksum from a line length of " << lineLength << " bytes." << endl;
+ }
+ if (bytesToConsider < 1)
+ {
+ stringstream ss;
+ ss << "Not enough bytes to calculate checksum with: " << bytesToConsider <<
+ " bytes (line length is " << lineLength << " bytes).";
+ throw URGError (URG_ERR_PROTOCOL, ss.str ());
+ }
+
+ int checkSum = 0;
+ // Start by adding the byte values
+ for (int ii = 0; ii < bytesToConsider; ii++)
+ checkSum += buffer[ii];
+ // Take the lowest 6 bits
+ checkSum &= 0x3F;
+ // Add 0x30
+ checkSum += 0x30;
+
+ if (_verbose)
+ {
+ cerr << "URGLaser::" << __func__ << "() Calculated checksum = " << checkSum << " (" <<
+ static_cast<char> (checkSum) << "), given checksum = " <<
+ static_cast<int> (buffer[checksumIndex]) << " (" << buffer[checksumIndex] <<
+ ")" << endl;
+ }
+ if (checkSum != static_cast<int> (buffer[checksumIndex]))
+ {
+ stringstream ss;
+ ss << "Invalid checksum - given: " << static_cast<int> (buffer[checksumIndex]) <<
+ ", calculated: " << checkSum;
+ throw URGError (URG_ERR_PROTOCOL, ss.str ());
+ }
+
+ // Null out the semi-colon (if there) and checksum
+ buffer[bytesToConsider] = '\0';
+
+ return bytesToConsider;
+}
+
+// Reads lines until the number specified has passed.
+void URGLaser::SkipLines (int count)
+{
+ if (_verbose)
+ cerr << "URGLaser::" << __func__ << "() Skipping " << count << " lines." << endl;
+ if (_port->SkipUntil (0x0A, count) < 0)
+ throw URGError (URG_ERR_READ, "Timed out while skipping.");
+}
+
+// Sends a command with optional parameters and checks that the echo of the command and parameters
+// sent are correct, and that the returned status code is 0 or the first byte of extraOK (for
+// SCIP1), or 00, 99 or the first two bytes of extraOK (for SCIP2).
+// cmd must be a 1 byte string for SCIP1 and a 2-byte NULL-terminated string for SCIP2.
+// If paramLength is 0, no parameters will be sent or expected in the reply.
+// extraOK must be a 1-byte string for SCIP1 and a 2-byte string for SCIP2.
+// Return value is the status code returned for the command.
+int URGLaser::SendCommand (char *cmd, char *param, int paramLength, char *extraOK)
+{
+ int statusCode = -1;
+ char response[16];
+ if (_scipVersion == 1)
+ {
+ if (_verbose)
+ {
+ cerr << "URGLaser::" << __func__ << "() Writing in SCIP1 mode. Command is " <<
+ cmd[0] << ", parameters length is " << paramLength << endl;
+ }
+ // Write the command
+ if (_port->Write (cmd, 1) < 1)
+ throw URGError (URG_ERR_WRITE, "Failed to write command byte.");
+ if (paramLength > 0)
+ {
+ if (_port->Write (param, paramLength) < paramLength)
+ throw URGError (URG_ERR_WRITE, "Failed to write command parameters.");
+ }
+ if (_port->Write ("\n", 1) < 1)
+ throw URGError (URG_ERR_WRITE, "Failed to write termination character.");
+
+ // Read back the response (should get at least 4 bytes , possibly up to 16 including \n's
+ // depending on the parameters): cmd[0] params \n status \n
+ int statusIndex = 2 + paramLength;
+ ReadLine (response, 2 + paramLength);
+ ReadLine (&response[statusIndex], 2);
+ // First make sure that the echoed command matches
+ if (response[0] != cmd[0])
+ {
+ throw URGError (URG_ERR_PROTOCOL, string ("Incorrect command echo: ") + cmd[0] +
+ string (" != ") + response[0]);
+ }
+ // Then compare the parameters
+ if (paramLength > 0)
+ {
+ if (memcmp (&response[1], param, paramLength) != 0)
+ throw URGError (URG_ERR_PROTOCOL, string ("Incorrect paramaters echo for command ")
+ + cmd[0]);
+ }
+ // Next up, check the status byte
+ if (_verbose)
+ {
+ cerr << "URGLaser::" << __func__ << "() Command response status: " <<
+ response[statusIndex] << endl;
+ }
+ if (response[statusIndex] != '0')
+ {
+ if (extraOK != NULL)
+ {
+ if (response[statusIndex] != extraOK[0])
+ {
+ stringstream ss;
+ ss << "Bad response to " << cmd[0] << " command: " << " " <<
+ SCIP1ErrorToString (response[statusIndex], cmd[0]);
+ throw URGError (URG_ERR_PROTOCOL, ss.str ());
+ }
+ }
+ else
+ {
+ stringstream ss;
+ ss << "Bad response to " << cmd[0] << " command: " << response[statusIndex] <<
+ " " << SCIP1ErrorToString (response[statusIndex], cmd[0]);
+ throw URGError (URG_ERR_PROTOCOL, ss.str ());
+ }
+ }
+ statusCode = atoi (&response[statusIndex]);
+ // All OK, data starts at beginning of port's buffer
+ }
+ else if (_scipVersion == 2)
+ {
+ if (_verbose)
+ {
+ cerr << "URGLaser::" << __func__ << "() Writing in SCIP2 mode. Command is " <<
+ cmd << ", parameters length is " << paramLength << endl;
+ }
+ // Write the command
+ if (_port->Write (cmd, 2) < 2)
+ throw URGError (URG_ERR_WRITE, "Failed to write command byte.");
+ if (paramLength > 0)
+ {
+ if (_port->Write (param, paramLength) < paramLength)
+ throw URGError (URG_ERR_WRITE, "Failed to write command parameters.");
+ }
+ if (_port->Write ("\n", 1) < 1)
+ throw URGError (URG_ERR_WRITE, "Failed to write termination character.");
+
+ // Read back the command echo (minimum of 3 bytes, maximum of 16 bytes)
+ ReadLine (response, 3 + paramLength);
+ // Check the echo is correct
+ if (response[0] != cmd[0] || response[1] != cmd[1])
+ {
+ stringstream ss;
+ ss << "Incorrect command echo: " << cmd << " != " << response[0] << response[1];
+ throw URGError (URG_ERR_PROTOCOL, ss.str ());
+ }
+ // Then compare the parameters
+ if (paramLength > 0)
+ {
+ if (memcmp (&response[2], param, paramLength) != 0)
+ throw URGError (URG_ERR_PROTOCOL, string ("Incorrect paramaters echo for command ")
+ + cmd);
+ }
+
+ // The next line should be the status line
+ ReadLineWithCheck (response, 4);
+ if (_verbose)
+ {
+ cerr << "URGLaser::" << __func__ << "() Command response status: " << response[0] <<
+ response[1] << endl;
+ }
+ // Check the status code is OK
+ response[2] = '\0';
+ if (!(response[0] == '0' && response[1] == '0') &&
+ !(response[0] == '9' && response[1] == '9'))
+ {
+ if (extraOK != NULL)
+ {
+ if (response[0] != extraOK[0] || response[1] != extraOK[1])
+ {
+ stringstream ss;
+ ss << "Bad response to " << cmd << " command: " << response[0] << response[1] <<
+ " " << SCIP2ErrorToString (response, cmd);
+ throw URGError (URG_ERR_PROTOCOL, ss.str ());
+ }
+ }
+ else
+ {
+ stringstream ss;
+ ss << "Bad response to " << cmd << " command: " << response[0] << response[1] <<
+ " " << SCIP2ErrorToString (response, cmd);
+ throw URGError (URG_ERR_PROTOCOL, ss.str ());
+ }
+ }
+ statusCode = atoi (response);
+ // All OK, data starts at beginning of port's buffer
+ }
+ else
+ throw URGError (URG_ERR_SCIPVERSION, "Unknown SCIP version.");
+
+ return statusCode;
+}
+
+void URGLaser::GetAndSetSCIPVersion (void)
+{
+ bool scip1Failed = false;
+
+ if (_verbose)
+ cerr << "URGLaser::" << __func__ << "() Testing SCIP protocol version." << endl;
+ // Try SCIP version 1 first by sending an info command
+ try
+ {
+ SendCommand ("V", NULL, 0, NULL);
+ }
+ catch (URGError &e)
+ {
+ // That didn't work too well...
+ if (_verbose)
+ cerr << "URGLaser::" << __func__ << "() Initial SCIP version 1 test failed." << endl;
+ scip1Failed = true;
+ }
+
+ if (scip1Failed)
+ {
+ // Currently using SCIP version 2
+ _scipVersion = 2;
+
+ _port->Flush ();
+ // Make sure by sending a VV command
+ try
+ {
+ SendCommand ("VV", NULL, 0, NULL);
+ }
+ catch (URGError &e)
+ {
+ throw URGError (URG_ERR_SCIPVERSION, "SCIP versions 1 and 2 failed.");
+ }
+
+ // Otherwise all OK, dump the rest of the result
+ SkipLines (6);
+ if (_verbose)
+ cerr << "URGLaser::" << __func__ << "() Using SCIP version 2." << endl;
+ return;
+ }
+ else
+ {
+ // Currently using SCIP version 1
+ // Get the firmware version and check if we can move to SCIP version 2
+ _scipVersion = 1;
+
+ // Skip the vendor and product info
+ SkipLines (2);
+ // Get the firmware line
+ char buffer[SCIP1_LINE_LENGTH];
+ memset (buffer, 0, sizeof (char) * SCIP1_LINE_LENGTH);
+ try
+ {
+ // If the laser is already in SCIP2 mode this has a tendency to time out (rather than
+ // the laser doing what the manual says and giving us an error code to the command
+ // that we sent to get the info).
+ ReadLine (buffer);
+ }
+ catch (URGError e)
+ {
+ if (e.Code () != URG_ERR_READ) // We're only interested in timeouts
+ throw;
+ if (_verbose)
+ {
+ cerr << "URGLaser::" << __func__ <<
+ "() Timed out trying SCIP version 1, trying SCIP version 2." << endl;
+ }
+ // Already in SCIP version 2 mode.
+ _scipVersion = 2;
+ _port->Flush ();
+ // Make sure by sending a VV command
+ try
+ {
+ SendCommand ("VV", NULL, 0, NULL);
+ }
+ catch (URGError &e)
+ {
+ cout << "error: " << e.Code() << " " << e.what() << endl;
+ throw URGError (URG_ERR_SCIPVERSION, "SCIP versions 1 and 2 failed.");
+ }
+ // Otherwise all OK, dump the rest of the result
+ SkipLines (6);
+ if (_verbose)
+ cerr << "URGLaser::" << __func__ << "() Using SCIP version 2." << endl;
+ return;
+ }
+
+ if (strncmp (buffer, "FIRM:", 5) != 0)
+ {
+ throw URGError (URG_ERR_PROTOCOL,
+ "'FIRM:' was not found when checking firmware version.");
+ }
+ // Pull out the major version number
+ int majorVer = strtol (&buffer[5], NULL, 10);
+ if (errno == ERANGE)
+ throw URGError (URG_ERR_BADFIRMWARE, "Out-of-range firmware version.");
+ if (_verbose)
+ cerr << "URGLaser::" << __func__ << "() Firmware major version is " << majorVer << endl;
+ // Dump the rest of the V command result (one of these will be the empty last line)
+ SkipLines (3);
+
+ // If the firmware version is less than 3, we're stuck with SCIP version 1.
+ if (majorVer < 3)
+ {
+ if (_verbose)
+ cerr << "URGLaser::" << __func__ <<
+ "() Firmware does not support SCIP version 2; using SCIP version 1." << endl;
+ return;
+ }
+ // Otherwise we can try SCIP version 2
+ else
+ {
+ _port->Flush ();
+ // We'll hijack the SendCommand function a bit here. Normally it takes 1-byte commands,
+ // (we're currently using SCIP version 1, remember), but the command to change to SCIP
+ // version 2 is 7 bytes long (why did they have to do it that way?). So send the first
+ // byte as the command and the other 6 as parameters.
+ try
+ {
+ SendCommand ("S", "CIP2.0", 6, NULL);
+ }
+ catch (URGError &e)
+ {
+ if (_verbose)
+ cerr << "URGLaser::" << __func__ <<
+ "() Could not change to SCIP version 2; using SCIP version 1." << endl;
+ return;
+ }
+ // There'll be a trailing line on the end
+ SkipLines (1);
+
+ // Changed to SCIP version 2
+ if (_verbose)
+ cerr << "URGLaser::" << __func__ << "() Using SCIP version 2." << endl;
+ _scipVersion = 2;
+ return;
+ }
+ }
+
+ // Fallback case if didn't find a good SCIP version and return above
+ throw URGError (URG_ERR_SCIPVERSION, "Unknown SCIP version.");
+}
+
+void URGLaser::GetDefaults (void)
+{
+ if (_verbose)
+ cerr << "URGLaser::" << __func__ << "() Getting default values." << endl;
+
+ // Get the laser's info
+ URGSensorInfo info;
+ GetSensorInfo (&info);
+
+ _minAngle = info.minAngle;
+ _maxAngle = info.maxAngle;
+ _resolution = info.resolution;
+ _firstStep = info.firstStep;
+ _lastStep = info.lastStep;
+ _frontStep = info.frontStep;
+ _maxRange = info.maxRange;
+ if (_verbose)
+ {
+ cerr << "URGLaser::" << __func__ <<
+ "() Got default values: " << _minAngle << " " << _maxAngle << " " << _resolution <<
+ " " << _firstStep << " " << _lastStep << " " << _frontStep << " " << _maxRange << endl;
+ }
+}
+
+void URGLaser::Read2ByteRangeData (URGData *data, unsigned int numSteps)
+{
+ if (_verbose)
+ cerr << "URGLaser::" << __func__ << "() Reading " << numSteps << " ranges." << endl;
+
+ // This will automatically take care of whether it actually needs to (re)allocate or not.
+ data->AllocateData (numSteps);
+
+ // 2 byte data is easy since it fits neatly in a 64-byte block
+ char buffer[SCIP2_LINE_LENGTH];
+ unsigned int currentStep = 0;
+ int numBytesInLine = 0;
+ while (true)
+ {
+ // Read a line of data
+ numBytesInLine = ReadLineWithCheck (buffer);
+ // Check if we've reached the end of the data
+ if (numBytesInLine == 0)
+ break;
+ // Process pairs of bytes until we encounter the end of the line
+ for (int ii = 0; ii < numBytesInLine; ii += 2, currentStep++)
+ {
+ if (buffer[ii] == '\n' || buffer[ii + 1] == '\n')
+ {
+ // Line feed in the middle of a line? Why?
+ throw URGError (URG_ERR_PROTOCOL, "Found line feed in a data block.");
+ }
+ data->_data[currentStep] = Decode2ByteValue (&buffer[ii]);
+ }
+ // End of this line. Go around again.
+ }
+
+ if (_verbose)
+ cerr << "URGLaser::" << __func__ << "() Read " << currentStep << " ranges." << endl;
+ if (currentStep != numSteps)
+ throw URGError (URG_ERR_PROTOCOL, "Read less range readings than were asked for.");
+}
+
+void URGLaser::Read3ByteRangeData (URGData *data, unsigned int numSteps)
+{
+ if (_verbose)
+ cerr << "URGLaser::" << __func__ << "() Reading " << numSteps << " ranges." << endl;
+
+ // This will automatically take care of whether it actually needs to (re)allocate or not.
+ data->AllocateData (numSteps);
+
+ // 3 byte data is a pain because it crosses the line boundary, it may overlap by 0, 1 or 2 bytes
+ char buffer[SCIP2_LINE_LENGTH];
+ unsigned int currentStep = 0;
+ int numBytesInLine = 0, splitCount = 0;
+ char splitValue[3];
+ while (true)
+ {
+ // Read a line of data
+ numBytesInLine = ReadLineWithCheck (buffer);
+ // Check if we've reached the end of the data
+ if (numBytesInLine == 0)
+ break;
+ // Process triplets of bytes until we encounter or overrun the end of the line
+ for (int ii = 0; ii < numBytesInLine;)
+ {
+ if (buffer[ii] == '\n' || buffer[ii + 1] == '\n')
+ {
+ // Line feed in the middle of a line? Why?
+ throw URGError (URG_ERR_PROTOCOL, "Found line feed in a data block.");
+ }
+ if (ii == numBytesInLine - 2) // Short 1 byte
+ {
+ splitValue[0] = buffer[ii];
+ splitValue[1] = buffer[ii + 1];
+ splitCount = 1; // Will be reset on the next iteration, after it's used
+ ii += 2;
+ }
+ else if (ii == numBytesInLine - 1) // Short 2 bytes
+ {
+ splitValue[0] = buffer[ii];
+ splitCount = 2; // Will be reset on the next iteration, after it's used
+ ii += 1;
+ }
+ else
+ {
+ if (splitCount == 1)
+ {
+ splitValue[2] = buffer[ii++];
+ data->_data[currentStep] = Decode3ByteValue (splitValue);
+ }
+ else if (splitCount == 2)
+ {
+ splitValue[1] = buffer[ii++];
+ splitValue[2] = buffer[ii++];
+ data->_data[currentStep] = Decode3ByteValue (splitValue);
+ }
+ else
+ {
+ data->_data[currentStep] = Decode3ByteValue (&buffer[ii]);
+ ii += 3;
+ }
+ if (data->_data[currentStep] > _maxRange)
+ {
+ cerr << "WARNING: URGLaser::" << __func__ <<
+ "() Value at step " << currentStep << " beyond maximum range: " <<
+ data->_data[currentStep] << " (raw bytes: ";
+ if (splitCount != 0)
+ cerr << splitValue[0] << splitValue[1] << splitValue[2] << ")" << endl;
+ else
+ cerr << buffer[0] << buffer[1] << buffer[2] << ")" << endl;
+ }
+ currentStep++;
+ splitCount = 0; // Reset this here now that it's been used
+ }
+ }
+ // End of this line. Go around again.
+ }
+
+ if (_verbose)
+ cerr << "URGLaser::" << __func__ << "() Read " << currentStep << " ranges." << endl;
+ if (currentStep != numSteps)
+ {
+ throw URGError (URG_ERR_PROTOCOL,
+ "Read a different number of range readings than were asked for.");
+ }
+}
+
+} // namespace urg_nz
Copied: gearbox/trunk/submitted/hokuyo_aist/hokuyo_aist.h (from rev 210, gearbox/trunk/submitted/hokuyo_aist/urg_nz.h)
===================================================================
--- gearbox/trunk/submitted/hokuyo_aist/hokuyo_aist.h (rev 0)
+++ gearbox/trunk/submitted/hokuyo_aist/hokuyo_aist.h 2008-06-24 14:24:38 UTC (rev 211)
@@ -0,0 +1,438 @@
+/*
+ * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
+ * http://gearbox.sf.net/
+ * Copyright (c) 2008 Geoffrey Biggs
+ *
+ * urg_nz Hokuyo URG laser scanner driver.
+ *
+ * This distribution is licensed to you under the terms described in the LICENSE file included in
+ * this distribution.
+ *
+ * This work is a product of the National Institute of Advanced Industrial Science and Technology,
+ * Japan. Registration number: ___
+ *
+ * This file is part of urg_nz.
+ *
+ * urg_nz is free software: you can redistribute it and/or modify it under the terms of the GNU
+ * Lesser General Public License as published by the Free Software Foundation, either version 3 of
+ * the ...
[truncated message content] |
|
From: <gb...@us...> - 2008-06-24 14:20:25
|
Revision: 210
http://gearbox.svn.sourceforge.net/gearbox/?rev=210&view=rev
Author: gbiggs
Date: 2008-06-24 07:18:59 -0700 (Tue, 24 Jun 2008)
Log Message:
-----------
Renaming urg_nz to hokuyo_aist
Modified Paths:
--------------
gearbox/trunk/submitted/CMakeLists.txt
Added Paths:
-----------
gearbox/trunk/submitted/hokuyo_aist/
gearbox/trunk/submitted/hokuyo_aist/CMakeLists.txt
gearbox/trunk/submitted/hokuyo_aist/doc.dox
gearbox/trunk/submitted/hokuyo_aist/test/
gearbox/trunk/submitted/hokuyo_aist/urg_nz.cpp
gearbox/trunk/submitted/hokuyo_aist/urg_nz.h
Removed Paths:
-------------
gearbox/trunk/submitted/hokuyo_aist/CMakeLists.txt
gearbox/trunk/submitted/hokuyo_aist/doc.dox
gearbox/trunk/submitted/hokuyo_aist/test/
gearbox/trunk/submitted/hokuyo_aist/urg_nz.cpp
gearbox/trunk/submitted/hokuyo_aist/urg_nz.h
gearbox/trunk/submitted/urg_nz/
Modified: gearbox/trunk/submitted/CMakeLists.txt
===================================================================
--- gearbox/trunk/submitted/CMakeLists.txt 2008-06-24 14:03:59 UTC (rev 209)
+++ gearbox/trunk/submitted/CMakeLists.txt 2008-06-24 14:18:59 UTC (rev 210)
@@ -12,6 +12,6 @@
# E.g. ADD_SUBDIRECTORY( mydir )
ADD_SUBDIRECTORY( flexiport )
ADD_SUBDIRECTORY( gbxnovatelacfr )
- ADD_SUBDIRECTORY( urg_nz )
+ ADD_SUBDIRECTORY( hokuyo_aist )
ENDIF( GBX_BUILD_SUBMITTED )
Copied: gearbox/trunk/submitted/hokuyo_aist (from rev 203, gearbox/trunk/submitted/urg_nz)
Deleted: gearbox/trunk/submitted/hokuyo_aist/CMakeLists.txt
===================================================================
--- gearbox/trunk/submitted/urg_nz/CMakeLists.txt 2008-06-23 02:26:41 UTC (rev 203)
+++ gearbox/trunk/submitted/hokuyo_aist/CMakeLists.txt 2008-06-24 14:18:59 UTC (rev 210)
@@ -1,29 +0,0 @@
-SET (libName urg_nz)
-set (libDesc "Hokuyo URG laser scanner driver")
-GBX_ADD_LICENSE (GPL)
-
-SET (build TRUE)
-GBX_REQUIRE_OPTION (build LIB ${libName} ON)
-
-SET (reqLibs flexiport)
-GBX_REQUIRE_TARGETS (build LIB ${libName} ${reqLibs})
-
-IF (build)
- INCLUDE (${GBX_CMAKE_DIR}/UseBasicRules.cmake)
-
- SET (hdrs urg_nz.h)
- SET (srcs urg_nz.cpp)
-
- IF (WIN32)
- ADD_DEFINITIONS (-DURG_NZ_EXPORTS)
- ENDIF (WIN32)
- GBX_ADD_LIBRARY (${libName} SHARED ${srcs})
- TARGET_LINK_LIBRARIES (${libName} ${reqLibs})
- GBX_ADD_PKGCONFIG (${libName} ${libDesc} ${reqLibs} "" "" "")
-
- GBX_ADD_HEADERS (${libName} ${hdrs})
-
- IF (GBX_BUILD_TESTS)
- ADD_SUBDIRECTORY (test)
- ENDIF (GBX_BUILD_TESTS)
-ENDIF( build )
Copied: gearbox/trunk/submitted/hokuyo_aist/CMakeLists.txt (from rev 209, gearbox/trunk/submitted/urg_nz/CMakeLists.txt)
===================================================================
--- gearbox/trunk/submitted/hokuyo_aist/CMakeLists.txt (rev 0)
+++ gearbox/trunk/submitted/hokuyo_aist/CMakeLists.txt 2008-06-24 14:18:59 UTC (rev 210)
@@ -0,0 +1,29 @@
+SET (libName urg_nz)
+set (libDesc "Hokuyo URG laser scanner driver")
+GBX_ADD_LICENSE (GPL)
+
+SET (build TRUE)
+GBX_REQUIRE_OPTION (build LIB ${libName} ON)
+
+SET (reqLibs flexiport)
+GBX_REQUIRE_TARGETS (build LIB ${libName} ${reqLibs})
+
+IF (build)
+ INCLUDE (${GBX_CMAKE_DIR}/UseBasicRules.cmake)
+
+ SET (hdrs urg_nz.h)
+ SET (srcs urg_nz.cpp)
+
+ IF (WIN32)
+ ADD_DEFINITIONS (-DURG_NZ_EXPORTS)
+ ENDIF (WIN32)
+ GBX_ADD_LIBRARY (${libName} SHARED ${srcs})
+ TARGET_LINK_LIBRARIES (${libName} ${reqLibs})
+ GBX_ADD_PKGCONFIG (${libName} ${libDesc} ${reqLibs} "" "" "")
+
+ GBX_ADD_HEADERS (${libName} ${hdrs})
+
+ IF (GBX_BUILD_TESTS)
+ ADD_SUBDIRECTORY (test)
+ ENDIF (GBX_BUILD_TESTS)
+ENDIF( build )
Deleted: gearbox/trunk/submitted/hokuyo_aist/doc.dox
===================================================================
--- gearbox/trunk/submitted/urg_nz/doc.dox 2008-06-23 02:26:41 UTC (rev 203)
+++ gearbox/trunk/submitted/hokuyo_aist/doc.dox 2008-06-24 14:18:59 UTC (rev 210)
@@ -1,58 +0,0 @@
-/*!
-
-@ingroup gbx_libs
-@ingroup gbx_hardware
-@ingroup gbx_cpp
-@defgroup gbx_library_urg_nz liburg_nz
-@brief Hokuyo URG laser scanner driver.
-
-This library provides a driver for Hokuyo laser scanner devices using the SCIP protocol version 1 or
-2. It has been tested with the Hokuyo URG-04LX, but it should work with any scanner that conforms to
-these protocol versions. For a full list of functions and classes see @ref urg_nz.
-
-Header file:
-@verbatim
-#include <urg_nz/urg_nz.h>
-@endverbatim
-
-@par Example
- See test/example.cpp
-
-@par Style guidelines
-
-- Naming conventions:
- - Class methods start with a capital letter.
- - VariableNamesLikeThis.
- - Private member variables start with an underscore _like _this.
- - Avoid using \#defines. \#define'd values in all capitals when necessary.
-- Formatting:
- - 1 tab indentation at start of lines, spaces only for inside lines (e.g. before commands).
- - Function declarations on one line.
- - Space between function name and arguments.
-- C++ API.
- - Functionality provided through classes.
-- Units:
- - All internal units are in millimetres and radians.
-
-@par Copyright
- Geoffrey Biggs
-
-@par Responsible Developer
- Geoffrey Biggs
-
-@par License
- LGPL
-
-@par Dependencies
- @ref FlexiPort
-
-*/
-
-/*!
-@namespace urg_nz
-@brief URG laser scanner driver name space.
-
-This namespace is part of a library which provides a driver for the URG laser scanner driver.
-
-@see @ref gbx_library_urg_nz
-*/
Copied: gearbox/trunk/submitted/hokuyo_aist/doc.dox (from rev 209, gearbox/trunk/submitted/urg_nz/doc.dox)
===================================================================
--- gearbox/trunk/submitted/hokuyo_aist/doc.dox (rev 0)
+++ gearbox/trunk/submitted/hokuyo_aist/doc.dox 2008-06-24 14:18:59 UTC (rev 210)
@@ -0,0 +1,58 @@
+/*!
+
+@ingroup gbx_libs
+@ingroup gbx_hardware
+@ingroup gbx_cpp
+@defgroup gbx_library_urg_nz liburg_nz
+@brief Hokuyo URG laser scanner driver.
+
+This library provides a driver for Hokuyo laser scanner devices using the SCIP protocol version 1 or
+2. It has been tested with the Hokuyo URG-04LX, but it should work with any scanner that conforms to
+these protocol versions. For a full list of functions and classes see @ref urg_nz.
+
+Header file:
+@verbatim
+#include <urg_nz/urg_nz.h>
+@endverbatim
+
+@par Example
+ See test/example.cpp
+
+@par Style guidelines
+
+- Naming conventions:
+ - Class methods start with a capital letter.
+ - VariableNamesLikeThis.
+ - Private member variables start with an underscore _like _this.
+ - Avoid using \#defines. \#define'd values in all capitals when necessary.
+- Formatting:
+ - 1 tab indentation at start of lines, spaces only for inside lines (e.g. before commands).
+ - Function declarations on one line.
+ - Space between function name and arguments.
+- C++ API.
+ - Functionality provided through classes.
+- Units:
+ - All internal units are in millimetres and radians.
+
+@par Copyright
+ Geoffrey Biggs
+
+@par Responsible Developer
+ Geoffrey Biggs
+
+@par License
+ LGPL
+
+@par Dependencies
+ @ref FlexiPort
+
+*/
+
+/*!
+@namespace urg_nz
+@brief URG laser scanner driver name space.
+
+This namespace is part of a library which provides a driver for the URG laser scanner driver.
+
+@see @ref gbx_library_urg_nz
+*/
Copied: gearbox/trunk/submitted/hokuyo_aist/test (from rev 209, gearbox/trunk/submitted/urg_nz/test)
Deleted: gearbox/trunk/submitted/hokuyo_aist/urg_nz.cpp
===================================================================
--- gearbox/trunk/submitted/urg_nz/urg_nz.cpp 2008-06-23 02:26:41 UTC (rev 203)
+++ gearbox/trunk/submitted/hokuyo_aist/urg_nz.cpp 2008-06-24 14:18:59 UTC (rev 210)
@@ -1,2030 +0,0 @@
-/*
- * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
- * http://gearbox.sf.net/
- * Copyright (c) 2008 Geoffrey Biggs
- *
- * urg_nz Hokuyo URG laser scanner driver.
- *
- * This distribution is licensed to you under the terms described in the LICENSE file included in
- * this distribution.
- *
- * This work is a product of the National Institute of Advanced Industrial Science and Technology,
- * Japan. Registration number: ___
- *
- * This file is part of urg_nz.
- *
- * urg_nz is free software: you can redistribute it and/or modify it under the terms of the GNU
- * Lesser General Public License as published by the Free Software Foundation, either version 3 of
- * the License, or (at your option) any later version.
- *
- * urg_nz is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even
- * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser
- * General Public License for more details.
- *
- * You should have received a copy of the GNU Lesser General Public License along with urg_nz. If
- * not, see <http://www.gnu.org/licenses/>.
- */
-
-#include "urg_nz.h"
-using namespace urg_nz;
-
-#include <flexiport/flexiport.h>
-#include <flexiport/port.h>
-#include <flexiport/serialport.h>
-#include <stdarg.h>
-#include <stdlib.h>
-#include <errno.h>
-#include <math.h>
-#include <sstream>
-#include <iostream>
-using namespace std;
-using namespace flexiport;
-
-#if defined (WIN32)
- #define __func__ __FUNCTION__
-#endif
-
-namespace urg_nz
-{
-
-#ifndef M_PI
- const double M_PI = 3.14159265358979323846;
-#endif
-// Convert radians to degrees
-#ifndef RTOD
- inline double RTOD (double rad)
- {
- return rad * 180.0 / M_PI;
- }
-#endif
-// Convert degrees to radians
-#ifndef DTOR
- inline double DTOR (double deg)
- {
- return deg * M_PI / 180.0;
- }
-#endif
-
-// SCIP1: 66 bytes (64 bytes of data + line feed + NULL)
-const unsigned int SCIP1_LINE_LENGTH = 66;
-// SCIP2: 67 bytes (64 bytes of data + checksum byte + line feed + NULL)
-const unsigned int SCIP2_LINE_LENGTH = 67;
-
-////////////////////////////////////////////////////////////////////////////////////////////////////
-// SCIP protocol version 1 notes
-////////////////////////////////////////////////////////////////////////////////////////////////////
-
-/* | = byte boundary, ... indicates variable byte block (max 64 bytes), (x) = x byte block
- - No checksum
- - Host to sensor: Command | Parameters... | LF
- - Sensor to host: Command | Parameters... | LF | Status | LF | Data... | LF | LF
- - Where a block of data would take more than 64 bytes, a line feed is inserted every 64 bytes.
- - Status 0 is OK, anything else is an error.
-
-L Power
- L|Control code|LF
- L|Control code|LF|Status|LF|LF
- 3 byte command block
-G Get data
- G|Start(3)|End(3)|Cluster(2)|LF
- G|Start(3)|End(3)|Cluster(2)|LF|Status|LF|Data...|LF|LF
- 10 byte command block
-S Set baud rate
- S|Baud rate(6)|Reserved(7)|LF
- S|Baud rate(6)|Reserved(7)|Status|LF|LF|
- 16 byte command block
-V Version info
- V|LF
- V|LF|Status...|LF|Vendor...|LF|Product...|LF|Firmware...|LF|Protocol...|LF|Serial...|LF|LF
- 2 byte command block
-*/
-
-////////////////////////////////////////////////////////////////////////////////////////////////////
-// SCIP protocol version 2 notes
-////////////////////////////////////////////////////////////////////////////////////////////////////
-
-/* | = byte boundary, ... indicates variable byte block (max 64 bytes), (x) = x byte block
- - We don't use the string block (which can be up to 16 bytes) so it's marked as size 0 and ignored
- in the command definitions below.
- - Host to sensor: Command(2) | Parameters... | String(0) | LF
- - Sensor to host: Command(2) | Parameters... | String(0) | LF | Status(2) | Sum | LF
- - Each data row: Data (max 64) | Sum | LF
- - Data rows are broken after a maximum of 64 bytes, each one having a checksum and a line feed.
- - Status codes 00 and 99 are OK, anything else is an error.
- - Checksum is calculated by... well, see the code.
-
-VV Version info
- V|V|LF
- V|V|LF|Status(2)|Sum|LF|Vendor...|;|Sum|LF|Product...|;|Sum|LF|Firmware...|;|Sum|LF|
- Protocol...|;|Sum|LF|Serial...|;|Sum|LF|LF
- 3 byte command block
-PP Specification info
- P|P|LF
- P|P|LF|Status(2)|Sum|LF|Model...|;|Sum|LF|MaxRange...|;|Sum|LF|MinRange...|;|Sum|LF|
- TotalSteps...|;|Sum|LF|FirstStep...|;|Sum|LF|LastStep...|;|Sum|LF|FrontStep...|;|Sum|LF|
- MotorSpeed...|;|Sum|LF|LF
- 3 byte command block
-II Status info
- I|I|LF
- I|I|LF|Status(2)|Sum|LF|Model...|;|Sum|LF|Power...|;|Sum|LF|MotorSpeed...|;|Sum|LF|
- Mode...|;|Sum|LF|Baud...|;|Sum|LF|Time...|;|Sum|LF|Diagnostic...|;|Sum|LF|LF
- 3 byte command block
-BM Power on
- B|M|LF
- B|M|LF|Status(2)|Sum|LF|LF
- 3 byte command block
-QT Power off
- Q|T|LF
- Q|T|LF|Status(2)|Sum|LF|LF
- 3 byte command block
-SS Set baud rate
- S|S|Baud(6)|LF
- S|S|Baud(6)|LF|Status(2)|Sum|LF|LF
- 9 byte command block
-MDMS Get new data
- M|D/S|Start(4)|End(4)|Cluster(2)|Interval(1)|Number(2)|LF
- M|D/S|Start(4)|End(4)|Cluster(2)|Interval(1)|Number(2)|LF|Status(2)|Sum|LF|Data...|LF|LF
- 16 byte command block
-GDGS Get latest data
- G|D/S|Start(4)|End(4)|Cluster(2)|LF
- G|D/S|Start(4)|End(4)|Cluster(2)|LF|Status(2)|Sum|LF|Data...|LF|LF
- 12 byte command block
-CR Set motor speed
- C|R|Speed(2)|LF
- C|R|Speed(2)|LF|Status(2)|Sum|LF|LF
- 5 byte command block
-TM Get sensor time
- T|M|Code|LF
- T|M|Code|LF|Status(2)|Sum|LF[|Time(4)|Sum|LF|LF]
- 4 byte command block
- Optional part only comes back for control code 1.
-RS Reset
- R|S|LF
- R|S|LF|Status(2)|Sum|LF|LF
- 3 byte command block
-*/
-
-////////////////////////////////////////////////////////////////////////////////////////////////////
-// Utility functions
-////////////////////////////////////////////////////////////////////////////////////////////////////
-
-string SCIP1ErrorToString (char error, char cmd)
-{
- return string ("No error descriptions available");
-}
-
-// error must be null-terminated
-string SCIP2ErrorToString (char *error, char *cmd)
-{
- stringstream ss;
-
- // Check for universal errors first
- if (error[1] == 'A')
- return "Unable to create transmission data or reply command internally";
- else if (error[1] == 'B')
- return "Buffer shortage or command repeated that is already processed";
- else if (error[1] == 'C')
- return "Command with insufficient parameters 1";
- else if (error[1] == 'D')
- return "Undefined command 1";
- else if (error[1] == 'E')
- return "Undefined command 2";
- else if (error[1] == 'F')
- return "Command with insufficient parameters 2";
- else if (error[1] == 'G')
- return "String character in command exceeds 16 letters";
- else if (error[1] == 'H')
- return "String character has invalid letters";
- else if (error[0] == '0' && error[1] == 'I')
- return "Sensor is now in firmware update mode";
-
- int errorCode = atoi (error);
-
- if (cmd[0] == 'B' && cmd[1] == 'M')
- {
- switch (errorCode)
- {
- case 1:
- return "Unable to control due to laser malfunction";
- case 2:
- return "Laser is already on";
- }
- }
-// else if (cmd[0] == 'Q' && cmd[1] == 'T')
-// {
-// switch (errorCode)
-// {
-// default:
-// stringstream ss;
-// ss << "Unknown error code " << errorCode << " for command " << cmd[0] << cmd[1];
-// return ss.str ();
-// }
-// }
- else if ((cmd[0] == 'G' && cmd[1] == 'D') ||
- (cmd[0] == 'G' && cmd[1] == 'S'))
- {
- switch (errorCode)
- {
- case 1:
- return "Starting step has non-numeric value";
- case 2:
- return "Ending step has non-numeric value";
- case 3:
- return "Cluster count has non-numeric value";
- case 4:
- return "Ending step is out of range";
- case 5:
- return "Ending step is smaller than start step";
- case 6:
- return "Scan interval is greater than 9";
- case 7:
- return "Number of scans is greater than 99";
- default:
- if (errorCode >= 50)
- ss << "Hardware error: " << errorCode;
- else
- ss << "Unknown error code " << errorCode << " for command " << cmd[0] << cmd[1];
-
- return ss.str ();
- }
- }
- else if ((cmd[0] == 'M' && cmd[1] == 'D') ||
- (cmd[0] == 'M' && cmd[1] == 'S'))
- {
- switch (errorCode)
- {
- case 1:
- return "Starting step has non-numeric value";
- case 2:
- return "Ending step has non-numeric value";
- case 3:
- return "Cluster count has non-numeric value";
- case 4:
- return "Ending step is out of range";
- case 5:
- return "Ending step is smaller than start step";
- case 6:
- return "Scan interval is greater than 9";
- case 7:
- return "Number of scans is greater than 99";
- default:
- if (errorCode >= 50)
- ss << "Hardware error: " << errorCode;
- else
- ss << "Unknown error code " << errorCode << " for command " << cmd[0] << cmd[1];
-
- return ss.str ();
- }
- }
- else if (cmd[0] == 'T' && cmd[1] == 'M')
- {
- switch (errorCode)
- {
- case 1:
- return "Invalid control code";
- case 2:
- return "Adjust mode on command received when sensor's adjust mode is already on";
- case 3:
- return "Adjust mode off command received when sensor's adjust mode is already off";
- case 4:
- return "Adjust mode is off when requested time";
- }
- }
- else if (cmd[0] == 'S' && cmd[1] == 'S')
- {
- switch (errorCode)
- {
- case 1:
- return "Baud rate has non-numeric value";
- case 2:
- return "Invalid baud rate";
- case 3:
- return "Sensor is already running at that baud rate";
- }
- }
- else if (cmd[0] == 'C' && cmd[1] == 'R')
- {
- switch (errorCode)
- {
- case 1:
- return "Invalid speed";
- case 2:
- return "Speed is out of range";
- case 3:
- return "Motor is already running at that speed";
- }
- }
-// else if (cmd[0] == 'R' && cmd[1] == 'S')
-// {
-// switch (errorCode)
-// {
-// case :
-// return "";
-// default:
-// stringstream ss;
-// ss << "Unknown error code " << errorCode << " for command " << cmd[0] << cmd[1];
-// return ss.str ();
-// }
-// }
-// else if (cmd[0] == 'V' && cmd[1] == 'V')
-// {
-// switch (errorCode)
-// {
-// case :
-// return "";
-// }
-// }
-// else if (cmd[0] == 'P' && cmd[1] == 'P')
-// {
-// switch (errorCode)
-// {
-// case :
-// return "";
-// }
-// }
-// else if (cmd[0] == 'I' && cmd[1] == 'I')
-// {
-// switch (errorCode)
-// {
-// case :
-// return "";
-// }
-// }
- else
- {
- ss << "Unknown command: " << cmd[0] << cmd[1];
- return ss.str ();
- }
-
- // Known commands with unknown error codes fall through to here
- ss << "Unknown error code " << errorCode << " for command " << cmd[0] << cmd[1];
- return ss.str ();
-}
-
-unsigned int Decode2ByteValue (char *data)
-{
- unsigned int byte1, byte2;
-
- byte1 = data[0] - 0x30;
- byte2 = data[1] - 0x30;
-
- return (byte1 << 6) + (byte2);
-}
-
-unsigned int Decode3ByteValue (char *data)
-{
- unsigned int byte1, byte2, byte3;
-
- byte1 = data[0] - 0x30;
- byte2 = data[1] - 0x30;
- byte3 = data[2] - 0x30;
-
- return (byte1 << 12) + (byte2 << 6) + (byte3);
-}
-
-unsigned int Decode4ByteValue (char *data)
-{
- unsigned int byte1, byte2, byte3, byte4;
-
- byte1 = data[0] - 0x30;
- byte2 = data[1] - 0x30;
- byte3 = data[2] - 0x30;
- byte4 = data[3] - 0x30;
-
- return (byte1 << 18) + (byte2 << 12) + (byte3 << 6) + (byte4);
-}
-
-void NumberToString (unsigned int num, char *dest, int length)
-{
-#if defined (WIN32)
- _snprintf (dest, length + 1, "%*d", length, num);
-#else
- snprintf (dest, length + 1, "%*d", length, num);
-#endif
- // Replace all leading spaces with '0'
- for (int ii = 0; ii < length && dest[ii] == ' '; ii++)
- dest[ii] = '0';
-}
-
-////////////////////////////////////////////////////////////////////////////////////////////////////
-// URGError class
-////////////////////////////////////////////////////////////////////////////////////////////////////
-
-string URGError::AsString (void) const throw ()
-{
- switch (_errorCode)
- {
- case URG_ERR_READ:
- return "URG_ERR_READ";
- case URG_ERR_WRITE:
- return "URG_ERR_WRITE";
- case URG_ERR_PROTOCOL:
- return "URG_ERR_PROTOCOL";
- case URG_ERR_CHANGEBAUD:
- return "URG_ERR_CHANGEBAUD";
- case URG_ERR_CONNECT_FAILED:
- return "URG_ERR_CONNECT_FAILED";
- case URG_ERR_CLOSE_FAILED:
- return "URG_ERR_CLOSE_FAILED";
- case URG_ERR_NODESTINATION:
- return "URG_ERR_NODESTINATION";
- case URG_ERR_BADFIRMWARE:
- return "URG_ERR_BADFIRMWARE";
- case URG_ERR_SCIPVERSION:
- return "URG_ERR_SCIPVERSION";
- case URG_ERR_MEMORY:
- return "URG_ERR_MEMORY";
- case URG_ERR_UNSUPPORTED:
- return "URG_ERR_UNSUPPORTED";
- case URG_ERR_BADARG:
- return "URG_ERR_BADARG";
- case URG_ERR_NODATA:
- return "URG_ERR_NODATA";
- case URG_ERR_NOTSERIAL:
- return "URG_ERR_NOTSERIAL";
- }
- return "UNKNOWN_ERROR";
-}
-
-////////////////////////////////////////////////////////////////////////////////////////////////////
-// URGSensorInfo class
-////////////////////////////////////////////////////////////////////////////////////////////////////
-
-URGSensorInfo::URGSensorInfo (void)
- : minRange (0), maxRange (0), steps (0), firstStep (0), lastStep (0), frontStep (0),
- standardSpeed (0), power (false), speed (0), baud (0), time (0), minAngle (0.0), maxAngle (0.0),
- resolution (0.0), scanableSteps (0)
-{
-}
-
-URGSensorInfo::URGSensorInfo (const URGSensorInfo &rhs)
-{
- vendor = rhs.vendor;
- product = rhs.product;
- firmware = rhs.firmware;
- protocol = rhs.protocol;
- serial = rhs.serial;
-
- model = rhs.model;
- minRange = rhs.minRange;
- maxRange = rhs.maxRange;
- steps = rhs.steps;
- firstStep = rhs.firstStep;
- lastStep = rhs.lastStep;
- frontStep = rhs.frontStep;
- standardSpeed = rhs.standardSpeed;
-
- power = rhs.power;
- speed = rhs.speed;
- measureState = rhs.measureState;
- baud = rhs.baud;
- time = rhs.time;
- sensorDiagnostic = rhs.sensorDiagnostic;
-
- minAngle = rhs.minAngle;
- maxAngle = rhs.maxAngle;
- resolution = rhs.resolution;
- scanableSteps = rhs.scanableSteps;
-}
-
-// Set various known values based on what the manual says
-void URGSensorInfo::SetDefaults (void)
-{
- minRange = 20;
- maxRange = 4095;
- steps = 1024;
- firstStep = 44;
- lastStep = 725;
- frontStep = 384;
-}
-
-void URGSensorInfo::CalculateValues (void)
-{
- resolution = DTOR (360.0) / steps;
- // If any of the steps are beyond INT_MAX, we have problems.
- // We also have an incredibly high-resolution sensor.
- minAngle = (static_cast<int> (firstStep) - static_cast<int> (frontStep)) * resolution;
- maxAngle = (static_cast<int> (lastStep) - static_cast<int> (frontStep)) * resolution;
- scanableSteps = lastStep - firstStep;
-}
-
-URGSensorInfo& URGSensorInfo::operator= (const URGSensorInfo &rhs)
-{
- if (this == &rhs)
- return *this;
-
- vendor = rhs.vendor;
- product = rhs.product;
- firmware = rhs.firmware;
- protocol = rhs.protocol;
- serial = rhs.serial;
-
- model = rhs.model;
- minRange = rhs.minRange;
- maxRange = rhs.maxRange;
- steps = rhs.steps;
- firstStep = rhs.firstStep;
- lastStep = rhs.lastStep;
- frontStep = rhs.frontStep;
- standardSpeed = rhs.standardSpeed;
-
- power = rhs.power;
- speed = rhs.speed;
- measureState = rhs.measureState;
- baud = rhs.baud;
- time = rhs.time;
- sensorDiagnostic = rhs.sensorDiagnostic;
-
- minAngle = rhs.minAngle;
- maxAngle = rhs.maxAngle;
- resolution = rhs.resolution;
- scanableSteps = rhs.scanableSteps;
-
- return *this;
-}
-
-string URGSensorInfo::AsString (void)
-{
- stringstream ss;
-
- ss << "Vendor: " << vendor << endl;
- ss << "Product: " << product << endl;
- ss << "Firmware: " << firmware << endl;
- ss << "Protocol: " << protocol << endl;
- ss << "Serial: " << serial << endl;
- ss << "Model: " << model << endl;
-
- ss << "Mininum range: " << minRange << "mm\tMaximum range: " << maxRange << "mm" << endl;
- ss << "Steps in 360 degrees: " << steps << "\tScanable steps: " << scanableSteps << endl;
- ss << "First step: " << firstStep << "\tFront step: " << frontStep << "\tLast step: " <<
- lastStep << endl;
- ss << "Resolution: " << resolution << " radians/step" << endl;
- ss << "Minimum angle: " << minAngle << " radians\tMaximum angle: " << maxAngle <<
- " radians" << endl;
- ss << "Standard motor speed: " << standardSpeed << "rpm" << endl;
-
- ss << "Power status: " << (power ? "On" : "Off") << "\tMeasurement state: " <<
- measureState << endl;
- ss << "Motor speed: " << speed << "rpm\tBaud rate: " << baud << "bps" << endl;
- ss << "Time stamp: " << time << "ms" << endl;
- ss << "Sensor diagnostic: " << sensorDiagnostic << endl;
-
- return ss.str ();
-}
-
-////////////////////////////////////////////////////////////////////////////////////////////////////
-// URGData class
-////////////////////////////////////////////////////////////////////////////////////////////////////
-
-URGData::URGData (void)
- : _data (NULL), _length (0), _error (-1), _time (0)
-{
-}
-
-URGData::URGData (unsigned short *data, unsigned int length, short error, unsigned int time)
- : _error (error), _time (time)
-{
- _length = length;
- if (_length == 0)
- _data = NULL;
- else
- {
- if ((_data = new unsigned short[_length]) == NULL)
- {
- _length = 0;
- throw URGError (URG_ERR_MEMORY, "Failed to allocate space to copy data.");
- }
- memcpy (_data, data, sizeof (unsigned short) * _length);
- }
-}
-
-URGData::URGData (const URGData &rhs)
-{
- _length = rhs.Length ();
- if (_length == 0)
- _data = NULL;
- else
- {
- if ((_data = new unsigned short[_length]) == NULL)
- {
- _length = 0;
- throw URGError (URG_ERR_MEMORY, "Failed to allocate space to copy data.");
- }
- memcpy (_data, rhs.Ranges (), sizeof (unsigned short) * _length);
- }
- _error = rhs.GetErrorCode ();
- _time = rhs.TimeStamp ();
-}
-
-URGData::~URGData (void)
-{
- if (_data != NULL)
- delete[] _data;
-}
-
-string URGData::ErrorCodeToString (void)
-{
- switch (_error)
- {
- case -1:
- return "No error.";
- case 0:
- return "Possibility of detected object is at 22m.";
- case 1:
- return "Reflected light has low intensity.";
- case 2:
- return "Reflected light has low intensity.";
- case 3:
- return "Reflected light has low intensity.";
- case 4:
- return "Reflected light has low intensity.";
- case 5:
- return "Reflected light has low intensity.";
- case 6:
- return "Possibility of detected object is at 5.7m.";
- case 7:
- return "Distance data on the preceding and succeeding steps have errors.";
- case 8:
- return "Others.";
- case 9:
- return "The same step had error in the last two scan.";
- case 10:
- return "Others.";
- case 11:
- return "Others.";
- case 12:
- return "Others.";
- case 13:
- return "Others.";
- case 14:
- return "Others.";
- case 15:
- return "Others.";
- case 16:
- return "Possibility of detected object is in the range 4096mm.";
- case 17:
- return "Others.";
- case 18:
- return "Unspecified.";
- case 19:
- return "Non-measurable distance.";
- default:
- stringstream ss;
- ss << "Unknown error code: " << _error;
- return ss.str ();
- }
-}
-
-URGData& URGData::operator= (const URGData &rhs)
-{
- if (rhs.Length () == 0)
- {
- _length = 0;
- if (_data != NULL)
- delete[] _data;
- _data = NULL;
- _error = rhs.GetErrorCode ();
- _time = rhs.TimeStamp ();
- }
- else
- {
- unsigned int rhsLength = rhs.Length ();
- unsigned short *newData;
- if (rhsLength != _length)
- {
- // Copy the data into a temporary variable pointing to new space (prevents dangling
- // pointers on allocation error and prevents self-assignment making a mess).
- if ((newData = new unsigned short[rhsLength]) == NULL)
- throw URGError (URG_ERR_MEMORY, "Failed to allocate space to copy data.");
- memcpy (newData, rhs.Ranges (), sizeof (unsigned short) * rhsLength);
- if (_data != NULL)
- delete[] _data;
- _data = newData;
- _length = rhs.Length ();
- }
- else
- {
- // If lengths are the same, no need to reallocate
- memcpy (_data, rhs.Ranges (), sizeof (unsigned short) * _length);
- }
-
- _error = rhs.GetErrorCode ();
- _time = rhs.TimeStamp ();
- }
-
- return *this;
-}
-
-unsigned short URGData::operator[] (unsigned int index)
-{
- if (index >= _length)
- throw URGError (URG_ERR_BADARG, "Invalid data index.");
- return _data[index];
-}
-
-string URGData::AsString (void)
-{
- stringstream ss;
-
- ss << _length << " readings:" << endl;
- for (unsigned int ii = 0; ii < _length; ii++)
- ss << _data[ii] << "\t";
- ss << endl << "Data error: (" << _error << ") " << ErrorCodeToString () << endl;
- ss << "Time stamp: " << _time << endl;
-
- return ss.str ();
-}
-
-void URGData::CleanUp (void)
-{
- if (_data != NULL)
- delete[] _data;
- _data = NULL;
- _length = 0;
- _error = 0;
- _time = 0;
-}
-
-void URGData::AllocateData (unsigned int length)
-{
- // If no data yet, allocate new
- if (_data == NULL)
- {
- if ((_data = new unsigned short[length]) == NULL)
- {
- _length = 0;
- throw URGError (URG_ERR_MEMORY, "Failed to allocate space to copy data.");
- }
- _length = length;
- }
- // If there is data, reallocate only if the length is different
- else if (length != _length)
- {
- delete[] _data;
- if ((_data = new unsigned short[length]) == NULL)
- {
- _length = 0;
- throw URGError (URG_ERR_MEMORY, "Failed to allocate space to copy data.");
- }
- _length = length;
- }
- // Else data is already allocated to the right length, so do nothing
-}
-
-////////////////////////////////////////////////////////////////////////////////////////////////////
-// URGLaser class
-////////////////////////////////////////////////////////////////////////////////////////////////////
-
-// Public API
-////////////////////////////////////////////////////////////////////////////////////////////////////
-
-URGLaser::URGLaser (void)
- : _port (NULL), _scipVersion (1), _verbose (false), _minAngle (0.0), _maxAngle (0.0),
- _resolution (0.0), _firstStep (0), _lastStep (0), _frontStep (0)
-{
-}
-
-URGLaser::~URGLaser (void)
-{
- if (_port != NULL)
- delete _port;
-}
-
-void URGLaser::Open (string portOptions)
-{
- if (_verbose)
- {
- cerr << "URGLaser::" << __func__ << "() Creating and opening port using options: " <<
- portOptions << endl;
- }
- _port = flexiport::CreatePort (portOptions);
- _port->Open ();
-
-// if (_port->GetPortType () == "serial")
-// {
-// if (_verbose)
-// {
-// cerr << "URGLaser::" << __func__ << "() Connected using serial connection." << endl;
-// cerr << _port->GetStatus ();
-// }
-// // Need to work our way down the baud rates until we get the one that the scanner is
-// // actually using. Start with the one the port is set to (hopefully the user set
-// // the port's baud option to the same as the scanner).
-// if (_verbose)
-// {
-// cerr << "URGLaser::" << __func__ << "() Trying to connect at " <<
-// reinterpret_cast<SerialPort*> (_port)->GetBaudRate () << "bps." << endl;
-// }
-// if (SetBaud (reinterpret_cast<SerialPort*> (_port)->GetBaudRate ()))
-// {
-// if (_verbose)
-// {
-// cerr << "URGLaser::" << __func__ << "() Successfully connected at " <<
-// reinterpret_cast<SerialPort*> (_port)->GetBaudRate () << "bps." << endl;
-// }
-// }
-// else
-// {
-// unsigned int bauds[] = {19200, 57600, 115200, 250000, 500000, 750000};
-// int ii;
-// for (ii = 0; ii < 6; ii++)
-// {
-// if (SetBaud (bauds[ii]))
-// {
-// if (_verbose)
-// {
-// cerr << "URGLaser::" << __func__ << "() Successfully connected at " <<
-// bauds[ii] << "bps." << endl;
-// }
-// break;
-// }
-// }
-// if (ii == 6)
-// {
-// // Did not connect at any speed
-// throw URGError (URG_ERR_CONNECT_FAILED, "Failed to connect at any baud.");
-// }
-// }
-// }
-// else if (_verbose)
-// {
-// cerr << "URGLaser::" << __func__ << "() Connected using " << _port->GetPortType () <<
-// " connection." << endl;
-// cerr << _port->GetStatus ();
-// }
- if (_verbose)
- {
- cerr << "URGLaser::" << __func__ << "() Connected using " << _port->GetPortType () <<
- " connection." << endl;
- cerr << _port->GetStatus ();
- }
- _port->Flush ();
-
- // Figure out the SCIP version currently in use and switch to a higher one if possible
- GetAndSetSCIPVersion ();
- // Get some values we need for providing default ranges
- GetDefaults ();
-}
-
-void URGLaser::Close (void)
-{
- if (!_port)
- throw URGError (URG_ERR_CLOSE_FAILED, "Port is not open.");
- if (_verbose)
- cerr << "URGLaser::" << __func__ << "() Closing connection." << endl;
- delete _port;
- _port = NULL;
-}
-
-bool URGLaser::IsOpen (void) const
-{
- if (_port != NULL)
- return _port->IsOpen ();
- return false;
-}
-
-void URGLaser::SetPower (bool on)
-{
- if (_scipVersion == 1)
- {
- if (on)
- {
- if (_verbose)
- cerr << "URGLaser::" << __func__ << "() Turning laser on." << endl;
- SendCommand ("L", "1", 1, NULL);
- }
- else
- {
- if (_verbose)
- cerr << "URGLaser::" << __func__ << "() Turning laser off." << endl;
- SendCommand ("L", "0", 1, NULL);
- }
- SkipLines (1);
- }
- else if (_scipVersion == 2)
- {
- if (on)
- {
- if (_verbose)
- cerr << "URGLaser::" << __func__ << "() Turning laser on." << endl;
- SendCommand ("BM", NULL, 0, "02");
- }
- else
- {
- if (_verbose)
- cerr << "URGLaser::" << __func__ << "() Turning laser off." << endl;
- SendCommand ("QT", NULL, 0, "02");
- }
- SkipLines (1);
- }
- else
- throw URGError (URG_ERR_SCIPVERSION, "Unknown SCIP version.");
-}
-
-// This function assumes that both the port and the laser scanner are already set to the same baud.
-void URGLaser::SetBaud (unsigned int baud)
-{
- if (_port->GetPortType () != "serial")
- throw URGError (URG_ERR_NOTSERIAL, "Cannot change baud rate of non-serial connection.");
-
- char newBaud[13];
- memset (newBaud, 0, sizeof (char) * 13);
-
- if (baud != 19200 && baud != 57600 && baud != 115200 &&
- baud != 250000 && baud != 500000 && baud != 750000)
- {
- stringstream ss;
- ss << "Bad baud rate: " << baud << endl;
- throw URGError (URG_ERR_BADARG, ss.str ());
- }
- NumberToString (baud, newBaud, 6);
-
- if (_scipVersion == 1)
- {
- // Send the command to change baud rate
- SendCommand ("S", newBaud, 13, NULL);
- SkipLines (1);
- // Change the port's baud rate
- reinterpret_cast<SerialPort*> (_port)->SetBaudRate (baud);
- }
- else if (_scipVersion == 2)
- {
- // Send the command to change baud rate
- SendCommand ("SS", newBaud, 6, "03");
- SkipLines (1);
- // Change the port's baud rate
- reinterpret_cast<SerialPort*> (_port)->SetBaudRate (baud);
- }
- else
- throw URGError (URG_ERR_SCIPVERSION, "Unknown SCIP version.");
-}
-
-void URGLaser::Reset (void)
-{
- if (_scipVersion == 1)
- throw URGError (URG_ERR_UNSUPPORTED, "SCIP version 1 does not support the reset command.");
- else if (_scipVersion == 2)
- {
- if (_verbose)
- cerr << "URGLaser::" << __func__ << "() Resetting laser." << endl;
- SendCommand ("RS", NULL, 0, NULL);
- SkipLines (1);
- }
- else
- throw URGError (URG_ERR_SCIPVERSION, "Unknown SCIP version.");
-}
-
-void URGLaser::SetMotorSpeed (unsigned int speed)
-{
- if (_scipVersion == 1)
- {
- throw URGError (URG_ERR_UNSUPPORTED,
- "SCIP version 1 does not support the set motor speed command.");
- }
- else if (_scipVersion == 2)
- {
- // Sanity check the value
- if ((speed > 600 || speed < 540 || (speed % 6) != 0) && speed != 0)
- throw URGError (URG_ERR_BADARG, "Invalid motor speed.");
- char buffer[3];
- buffer[2] = '\0';
- if (speed == 0)
- {
- if (_verbose)
- cerr << "URGLaser::" << __func__ << "() Reseting motor speed to default." << endl;
- buffer[0] = '9';
- buffer[1] = '9';
- }
- else
- {
- if (_verbose)
- {
- cerr << "URGLaser::" << __func__ << "() Setting motor speed to " <<
- speed << "rpm." << endl;
- }
- if (speed == 540)
- {
- buffer[0] = '1';
- buffer[1] = '0';
- }
- else
- {
- buffer[0] = '0';
- buffer[1] = 100 - (speed / 6) + 0x30;
- }
- }
- SendCommand ("CR", buffer, 2, "03");
- SkipLines (1);
- }
- else
- throw URGError (URG_ERR_SCIPVERSION, "Unknown SCIP version.");
-}
-
-void URGLaser::GetSensorInfo (URGSensorInfo *info)
-{
- if (info == NULL)
- throw URGError (URG_ERR_NODESTINATION, "No info object provided.");
-
- if (_scipVersion == 1)
- {
- if (_verbose)
- {
- cerr << "URGLaser::" << __func__ <<
- "() Getting sensor information using SCIP version 1." << endl;
- }
-
- info->SetDefaults ();
-
- char buffer[SCIP1_LINE_LENGTH];
- memset (buffer, 0, sizeof (char) * SCIP1_LINE_LENGTH);
-
- SendCommand ("V", NULL, 0, NULL);
- // Get the vendor info line
- ReadLine (buffer);
- info->vendor = &buffer[5]; // Chop off the "VEND:" tag
- // Get the product info line
- ReadLine (buffer);
- info->product = &buffer[5];
- // Get the firmware line
- ReadLine (buffer);
- info->firmware = &buffer[5];
- // Get the protocol version line
- ReadLine (buffer);
- info->protocol = &buffer[5];
- // Get the serial number
- ReadLine (buffer);
- info->serial = &buffer[5];
- // Get either the status line or the end of message
- ReadLine (buffer);
- if (buffer[0] != '\n')
- {
- // Got a status line
- info->sensorDiagnostic = &buffer[5];
- SkipLines (1);
- }
-
- // Check the firmware version major number. If it's >=3 there is probably some extra info
- // in the firmware line.
- // e.g.: FIRM:3.1.04,07/08/02(20-4095[mm],240[deg],44-725[step],600[rpm])
- // Note that this example is right up against the maximum SCIP v1 line length of 64 bytes.
- if (atoi (info->firmware.c_str ()) >= 3)
- {
- if (_verbose)
- cerr << "SCIP1 Firmware line for parsing: " << info->firmware << endl;
- // Now the fun part: parsing the line. It would be nice if we could use the POSIX regex
- // functions, but since MS doesn't believe in POSIX we get to do it the hard way.
- // Start by finding the first (
- char *valueStart;
- if ((valueStart = strchr (info->firmware.c_str (), '(')) == NULL)
- {
- // No bracket? Crud. Fail and use the hard-coded values from the manual.
- info->CalculateValues ();
- }
- // Now put it through sscanf and hope...
- int aperture;
- int numFound = sscanf (valueStart, "%d-%d[mm],%d[deg],%d-%d[step],%d[rpm]",
- &info->minRange, &info->maxRange, &aperture,
- &info->firstStep, &info->lastStep, &info->speed);
- if (numFound != 6)
- {
- // Didn't get enough values out, assume unknown format and fall back on the defaults
- info->SetDefaults ();
- info->CalculateValues ();
- if (_verbose)
- {
- cerr << "Retrieved sensor info (hard-coded):" << endl;
- cerr << info->AsString ();
- }
- }
-
- // Need to calculate stuff differently since it gave us an aperture value
- info->resolution = static_cast<double> (aperture) /
- static_cast<double> (info->lastStep - info->firstStep);
- // Assume that the range is evenly spread
- info->scanableSteps = info->lastStep - info->firstStep;
- info->frontStep = info->scanableSteps / 2;
- info->minAngle = (info->firstStep - info->frontStep) * info->resolution;
- info->maxAngle = (info->lastStep - info->frontStep) * info->resolution;
- }
- else
- {
- // We're stuck with hard-coded defaults from the manual (already set earlier).
- info->CalculateValues ();
- if (_verbose)
- {
- cerr << "Retrieved sensor info (hard-coded):" << endl;
- cerr << info->AsString ();
- }
- }
- }
- else if (_scipVersion == 2)
- {
- if (_verbose)
- {
- cerr << "URGLaser::" << __func__ <<
- "() Getting sensor information using SCIP version 2." << endl;
- }
-
- char buffer[SCIP2_LINE_LENGTH];
- memset (buffer, 0, sizeof (char) * SCIP2_LINE_LENGTH);
-
- // We need to send three commands to get all the info we want: VV, PP and II
- SendCommand ("VV", NULL, 0, NULL);
- // Get the vendor info line
- ReadLineWithCheck (buffer, -1, true);
- info->vendor = &buffer[5]; // Chop off the "VEND:" tag
- // Get the product info line
- ReadLineWithCheck (buffer, -1, true);
- info->product = &buffer[5];
- // Get the firmware line
- ReadLineWithCheck (buffer, -1, true);
- info->firmware = &buffer[5];
- // Get the protocol version line
- ReadLineWithCheck (buffer, -1, true);
- info->protocol = &buffer[5];
- // Get the serial number
- ReadLineWithCheck (buffer, -1, true);
- info->serial = &buffer[5];
- // Skip the end-of-message
- SkipLines (1);
-
- // Next up, PP
- SendCommand ("PP", NULL, 0, NULL);
- // Get the model line
- ReadLineWithCheck (buffer, -1, true);
- info->model = &buffer[5];
- // On to the fun ones that require parsing
- ReadLineWithCheck (buffer, -1, true);
- info->minRange = atoi (&buffer[5]);
- ReadLineWithCheck (buffer, -1, true);
- info->maxRange = atoi (&buffer[5]);
- ReadLineWithCheck (buffer, -1, true);
- info->steps = atoi (&buffer[5]);
- ReadLineWithCheck (buffer, -1, true);
- info->firstStep = atoi (&buffer[5]);
- ReadLineWithCheck (buffer, -1, true);
- info->lastStep = atoi (&buffer[5]);
- ReadLineWithCheck (buffer, -1, true);
- info->frontStep = atoi (&buffer[5]);
- ReadLineWithCheck (buffer, -1, true);
- info->standardSpeed = atoi (&buffer[5]);
- // Skip the end-of-message
- SkipLines (1);
-
- // Command II: Revenge of the Commands.
- SendCommand ("II", NULL, 0, NULL);
- // Skip the model line (we already have it from PP)
- SkipLines (1);
- // Get and parse the power state
- ReadLineWithCheck (buffer, -1, true);
- if (strncmp (&buffer[5], "OFF", 3) == 0)
- info->power = false;
- else
- info->power = true;
- // Motor speed
- ReadLineWithCheck (buffer, -1, true);
- // TODO: check if the format of this line changes if the motor speed is changed
- if (sscanf (buffer, "SCSP:%*7s(%d[rpm]", &info->speed) != 1)
- throw URGError (URG_ERR_PROTOCOL, "Motor speed line parse failed.");
- // Measuring state
- ReadLineWithCheck (buffer, -1, true);
- info->measureState = &buffer[5];
- // Baud rate
- ReadLineWithCheck (buffer, -1, true);
- if (sscanf (buffer, "SBPS:%d[bps]", &info->baud) != 1)
- throw URGError (URG_ERR_PROTOCOL, "Baud rate line parse failed.");
- // Time stamp
- ReadLineWithCheck (buffer, -1, true);
- if (sscanf (buffer, "TIME:%x", &info->time) != 1)
- throw URGError (URG_ERR_PROTOCOL, "Timestamp line parse failed.");
- // Diagnostic
- ReadLineWithCheck (buffer, -1, true);
- info->sensorDiagnostic = &buffer[5];
- // Skip the end-of-message
- SkipLines (1);
-
- info->CalculateValues ();
- if (_verbose)
- {
- cerr << "Retrieved sensor info:" << endl;
- cerr << info->AsString ();
- }
- }
- else
- throw URGError (URG_ERR_SCIPVERSION, "Unknown SCIP version.");
-}
-
-unsigned int URGLaser::GetTime (void)
-{
- if (_scipVersion == 1)
- {
- throw URGError (URG_ERR_UNSUPPORTED,
- "SCIP version 1 does not support the get time command.");
- }
- else if (_scipVersion == 2)
- {
- if (_verbose)
- cerr << "URGLaser::" << __func__ << "() Retrieving time from laser." << endl;
- SendCommand ("TM", "0", 1, NULL);
- SendCommand ("TM", "1", 1, NULL);
- char buffer[7];
- ReadLineWithCheck (buffer, 6);
- SendCommand ("TM", "2", 1, NULL);
- SkipLines (1);
- // We need to decode the time value that's in the buffer
- return Decode4ByteValue (buffer);
- }
- else
- throw URGError (URG_ERR_SCIPVERSION, "Unknown SCIP version.");
-
- return 0;
-}
-
-unsigned int URGLaser::GetRanges (URGData *data, int startStep, int endStep,
- unsigned int clusterCount)
-{
- if (data == NULL)
- throw URGError (URG_ERR_NODESTINATION, "No data destination provided.");
-
- char buffer[11];
- memset (buffer, 0, sizeof (char) * 11);
-
- if (startStep < 0)
- startStep = _firstStep;
- if (endStep < 0)
- endStep = _lastStep;
-
- unsigned int numSteps = (endStep - startStep + 1) / clusterCount;
- if (_verbose)
- {
- cerr << "URGLaser::" << __func__ << "() Reading " << numSteps << " ranges between " <<
- startStep << " and " << endStep << " with a cluster count of " << clusterCount << endl;
- }
-
- if (_scipVersion == 1)
- {
- // Send the command to ask for the most recent range data from startStep to endStep
- NumberToString (startStep, buffer, 3);
- NumberToString (endStep, &buffer[3], 3);
- NumberToString (clusterCount, &buffer[6], 2);
- data->_error = SendCommand ("G", buffer, 8, NULL);
- // In SCIP1 mode we're going to get back 2-byte data
- Read2ByteRangeData (data, numSteps);
- }
- else if (_scipVersion == 2)
- {
- // Send the command to ask for the most recent range data from startStep to endStep
- NumberToString (startStep, buffer, 4);
- NumberToString (endStep, &buffer[4], 4);
- NumberToString (clusterCount, &buffer[8], 2);
- data->_error = SendCommand ("GD", buffer, 10, NULL);
- // There will be a timestamp before the data (if there is data)
- // Normally we would send 6 for the expected length, but we may get no timestamp back if
- // there was no data.
- if (ReadLineWithCheck (buffer) == 0)
- throw URGError (URG_ERR_NODATA, "No data received. Check data error code.");
- data->_time = Decode4ByteValue (buffer);
- // In SCIP2 mode we're going to get back 3-byte data because we're sending the GD command
- Read3ByteRangeData (data, numSteps);
- }
- else
- throw URGError (URG_ERR_SCIPVERSION, "Unknown SCIP version.");
-
- return data->_length;
-}
-
-unsigned int URGLaser::GetRanges (URGData *data, double startAngle,
- double endAngle, unsigned int clusterCount)
-{
- if (data == NULL)
- throw URGError (URG_ERR_NODESTINATION, "No data destination provided.");
-
- // Calculate the given angles in steps, rounding towards _frontStep
- int startStep, endStep;
- startStep = AngleToStep (startAngle);
- endStep = AngleToStep (endAngle);
-// double startStepF, endStepF;
-// startStepF = _frontStep +
-// (static_cast<double> (startAngle) / static_cast<double> (_resolution));
-// // Round towards _frontStep so that the step values are always inside the angles given
-// if (startStepF < _frontStep)
-// startStep = static_cast<int> (ceil (startStepF));
-// else
-// startStep = static_cast<int> (floor (startStepF));
-// endStepF = _frontStep + (static_cast<double> (endAngle) / static_cast<double> (_resolution));
-// if (endStepF < _frontStep)
-// endStep = static_cast<int> (ceil (endStepF));
-// else
-// endStep = static_cast<int> (floor (endStepF));
-
- // Check the steps are within the allowable range
- if (startStep < _firstStep || startStep > _lastStep)
- throw URGError (URG_ERR_BADARG, "Start step is out of range.");
- if (endStep < _firstStep || endStep > _lastStep)
- throw URGError (URG_ERR_BADARG, "End step is out of range.");
-
- if (_verbose)
- {
- cerr << "URGLaser::" << __func__ << "() Start angle " << startAngle << " is step " <<
- startStep << ", end angle " << endAngle << " is step " << endStep << endl;
- }
-
- // Get the data
- return GetRanges (data, startStep, endStep, clusterCount);
-}
-
-unsigned int URGLaser::GetNewRanges (URGData *data, int startStep, int endStep,
- unsigned int clusterCount)
-{
- if (data == NULL)
- throw URGError (URG_ERR_NODESTINATION, "No data destination provided.");
-
- if (_scipVersion == 1)
- {
- throw URGError (URG_ERR_UNSUPPORTED,
- "SCIP version 1 does not support the get new ranges command.");
- }
- else if (_scipVersion == 2)
- {
- char buffer[14];
- memset (buffer, 0, sizeof (char) * 14);
-
- if (startStep < 0)
- startStep = _firstStep;
- if (endStep < 0)
- endStep = _lastStep;
-
- unsigned int numSteps = (endStep - startStep + 1) / clusterCount;
- if (_verbose)
- {
- cerr << "URGLaser::" << __func__ << "() Reading " << numSteps <<
- " new ranges between " << startStep << " and " << endStep <<
- " with a cluster count of " << clusterCount << endl;
- }
-
- // Send the command to ask for the most recent range data from startStep to endStep
- NumberToString (startStep, buffer, 4);
- NumberToString (endStep, &buffer[4], 4);
- NumberToString (clusterCount, &buffer[8], 2);
- NumberToString (1, &buffer[10], 1);
- NumberToString (1, &buffer[11], 2);
- data->_error = SendCommand ("MD", buffer, 13, NULL);
- // There will be a timestamp before the data (if there is data)
- // Normally we would send 6 for the expected length, but we may get no timestamp back if
- // there was no data.
- if (ReadLineWithCheck (buffer) == 0)
- throw URGError (URG_ERR_NODATA, "No data received. Check data error code.");
- data->_time = Decode4ByteValue (buffer);
- // In SCIP2 mode we're going to get back 3-byte data because we're sending the MD command
- Read3ByteRangeData (data, numSteps);
- }
- else
- throw URGError (URG_ERR_SCIPVERSION, "Unknown SCIP version.");
-
- return data->_length;
-}
-
-unsigned int URGLaser::GetNewRanges (URGData *data, double startAngle, double endAngle,
- unsigned int clusterCount)
-{
- if (data == NULL)
- throw URGError (URG_ERR_NODESTINATION, "No data destination provided.");
- if (_scipVersion == 1)
- {
- throw URGError (URG_ERR_UNSUPPORTED,
- "SCIP version 1 does not support the get new ranges command.");
- }
-
- // Calculate the given angles in steps, rounding towards _frontStep
- int startStep, endStep;
- startStep = AngleToStep (startAngle);
- endStep = AngleToStep (endAngle);
-// double startStepF, endStepF;
-// startStepF = _frontStep +
-// (static_cast<double> (startAngle) / static_cast<double> (_resolution));
-// // Round towards _frontStep so that the step values are always inside the angles given
-// if (startStepF < _frontStep)
-// startStep = static_cast<int> (ceil (startStepF));
-// else
-// startStep = static_cast<int> (floor (startStepF));
-// endStepF = _frontStep + (static_cast<double> (endAngle) / static_cast<double> (_resolution));
-// if (endStepF < _frontStep)
-// endStep = static_cast<int> (ceil (endStepF));
-// else
-// endStep = static_cast<int> (floor (endStepF));
-
- // Check the steps are within the allowable range
- if (startStep < _firstStep || startStep > _lastStep)
- throw URGError (URG_ERR_BADARG, "Start step is out of range.");
- if (endStep < _firstStep || endStep > _lastStep)
- throw URGError (URG_ERR_BADARG, "End step is out of range.");
-
- if (_verbose)
- {
- cerr << "URGLaser::" << __func__ << "() Start angle " << startAngle << " is step " <<
- startStep << ", end angle " << endAngle << " is step " << endStep << endl;
- }
-
- // Get the data
- return GetNewRanges (data, startStep, endStep, clusterCount);
-}
-
-double URGLaser::StepToAngle (unsigned int step)
-{
- return (static_cast<int> (step) - static_cast<int> (_frontStep)) * _resolution;
-}
-
-unsigned int URGLaser::AngleToStep (double angle)
-{
- unsigned int result;
- double resultF;
- resultF = _frontStep +
- (static_cast<double> (angle) / static_cast<double> (_resolution));
- // Round towards _frontStep so that the step values are always inside the angles given
- if (resultF < _frontStep)
- result = static_cast<int> (ceil (resultF));
- else
- result = static_cast<int> (floor (resultF));
-
- return result;
-}
-
-// Private functions
-////////////////////////////////////////////////////////////////////////////////////////////////////
-
-// If expectedLength is not -1, it should include the terminating line feed but not the NULL
-// (although the buffer still has to include this).
-// If expectedLenght is -1, this function expects buffer to be a certain length to allow up to the
-// maximum line length to be read. See SCIP1_LINE_LENGTH and SCIP2_LINE_LENGTH.
-// The line feed that terminates a line will be replaced with a NULL.
-// The return value is the number of bytes received, not including the NULL byte or the line feed.
-int URGLaser::ReadLine (char *buffer, int expectedLength)
-{
- int lineLength = 0;
-
- if (expectedLength == -1)
- {
- int maxLength = (_scipVersion == 1) ? SCIP1_LINE_LENGTH : SCIP2_LINE_LENGTH;
- if (_verbose)
- {
- cerr << "URGLaser::" << __func__ << "() Reading up to " << maxLength << " bytes." <<
- endl;
- }
- // We need to get at least 1 byte in a line: the line feed.
- if ((lineLength = _port->ReadLine (buffer, maxLength)) < 0)
- throw URGError (URG_ERR_READ, "Timed out trying to read a line.");
- else if (lineLength == 0)
- throw URGError (URG_ERR_READ, "No data received when trying to read a line.");
- // Replace the line feed with a NULL
- buffer[lineLength - 1] = '\0';
- }
- else
- {
- if (_verbose)
- {
- cerr << "URGLaser::" << __func__ << "() Reading exactly " << expectedLength <<
- " bytes." << endl;
- }
- if ((lineLength = _port->ReadLine (buffer, expectedLength + 1)) < 0) // +1 for the NULL
- throw URGError (URG_ERR_READ, "Timed out trying to read a line.");
- else if (lineLength == 0)
- throw URGError (URG_ERR_READ, "No data received when trying to read a line.");
- else if (lineLength < expectedLength)
- {
- stringstream ss;
- ss << "URGLaser::" << __func__ << "() Got an incorrect line length: " << lineLength <<
- " != " << expectedLength;
- throw URGError (URG_ERR_PROTOCOL, ss.str ());
- }
- // Replace the line feed with a NULL
- buffer[lineLength - 1] = '\0';
- }
-
- if (_verbose)
- {
- cerr << "URGLaser::" << __func__ << "() Read " << lineLength << " bytes." << endl;
- cerr << "URGLaser::" << __func__ << "() Line is " << buffer << endl;
- }
- return lineLength - 1; // Line feed not included
-}
-
-// This function will read a line and then calculate its checksum, comparing it with the checksum
-// at the end of the line. The checksum will be removed (along with the semi-colon, if present).
-// buffer and expectedLength args are as for ReadLine().
-// If hasSemicolon is true, the byte before the checksum is assumed to be the semi-colon separator
-// and so not a part of the checksum. If it's not a semi-colon, an exception is thrown.
-// Empty lines (i.e. a line that is just the line feed, as at the end of the message) will result in
-// a return value of zero and no checksum check will be performed. Otherwise the number of actual
-// data bytes (i.e. excluding the checksum and semicolon) will be returned.
-int URGLaser::ReadLineWithCheck (char *buffer, int expectedLength, bool hasSemicolon)
-{
- int lineLength = ReadLine (buffer, expectedLength);
- if (_scipVersion == 1)
- {
- // No checksums in SCIP version 1
- return lineLength;
- }
-
- // If the line is empty, assume it was a line-feed message terminator, in which case there is no
- // checksum to check.
- if (lineLength == 0)
- return 0;
-
- // Ignore the checksum itself, and possibly a semicolon (ReadLine has already chopped off the
- // line feed for us).
- int bytesToConsider = lineLength - 1 - (hasSemicolon ? 1 : 0);
- int checksumIndex = bytesToConsider + (hasSemicolon ? 1 : 0);
- if (_verbose)
- {
- cerr << "URGLaser::" << __func__ << "() Considering " << bytesToConsider <<
- " bytes for checksum from a line length of " << lineLength << " bytes." << endl;
- }
- if (bytesToConsider < 1)
- {
- stringstream ss;
- ss << "Not enough bytes to calculate checksum with: " << bytesToConsider <<
- " bytes (line length is " << lineLength << " bytes).";
- throw URGError (URG_ERR_PROTOCOL, ss.str ());
- }
-
- int checkSum = 0;
- // Start by adding the byte values
- for (int ii = 0; ii < bytesToConsider; ii++)
- checkSum += buffer[ii];
- // Take the lowest 6 bits
- checkSum &= 0x3F;
- // Add 0x30
- checkSum += 0x30;
-
- if (_verbose)
- {
- cerr << "URGLaser::" << __func__ << "() Calculated checksum = " << checkSum << " (" <<
- static_cast<char> (checkSum) << "), given checksum = " <<
- static_cast<int> (buffer[checksumIndex]) << " (" << buffer[checksumIndex] <<
- ")" << endl;
- }
- if (checkSum != static_cast<int> (buffer[checksumIndex]))
- {
- stringstream ss;
- ss << "Invalid checksum - given: " << static_cast<int> (buffer[checksumIndex]) <<
- ", calculated: " << checkSum;
- throw URGError (URG_ERR_PROTOCOL, ss.str ());
- }
-
- // Null out the semi-colon (if there) and checksum
- buffer[bytesToConsider] = '\0';
-
- return bytesToConsider;
-}
-
-// Reads lines until the number specified has passed.
-void URGLaser::SkipLines (int count)
-{
- if (_verbose)
- cerr << "URGLaser::" << __func__ << "() Skipping " << count << " lines." << endl;
- if (_port->SkipUntil (0x0A, count) < 0)
- throw URGError (URG_ERR_READ, "Timed out while skipping.");
-}
-
-// Sends a command with optional parameters and checks that the echo of the command and parameters
-// sent are correct, and that the returned status code is 0 or the first byte of extraOK (for
-// SCIP1), or 00, 99 or the first two bytes of extraOK (for SCIP2).
-// cmd must be a 1 byte string for SCIP1 and a 2-byte NULL-terminated string for SCIP2.
-// If paramLength is 0, no parameters will be sent or expected in the reply.
-// extraOK must be a 1-byte string for SCIP1 and a 2-byte string for SCIP2.
-// Return value is the status code returned for the command.
-int URGLaser::SendCommand (char *cmd, char *param, int paramLength, char *extraOK)
-{
- int statusCode = -1;
- char response[16];
- if (_scipVersion == 1)
- {
- if (_verbose)
- {
- cerr << "URGLaser::" << __func__ << "() Writing in SCIP1 mode. Command is " <<
- cmd[0] << ", parameters length is " << paramLength << endl;
- }
- // Write the command
- if (_port->Write (cmd, 1) < 1)
- throw URGError (URG_ERR_WRITE, "Failed to write command byte.");
- if (paramLength > 0)
- {
- if (_port->Write (param, paramLength) < paramLength)
- throw URGError (URG_ERR_WRITE, "Failed to write command parameters.");
- }
- if (_port->Write ("\n", 1) < 1)
- throw URGError (URG_ERR_WRITE, "Failed to write termination character.");
-
- // Read back the response (should get at least 4 bytes , possibly up to 16 including \n's
- // depending on the parameters): cmd[0] params \n status \n
- int statusIndex = 2 + paramLength;
- ReadLine (response, 2 + paramLength);
- ReadLine (&response[statusIndex], 2);
- // First make sure that the echoed command matches
- if (response[0] != cmd[0])
- {
- throw URGError (URG_ERR_PROTOCOL, string ("Incorrect command echo: ") + cmd[0] +
- string (" != ") + response[0]);
- }
- // Then compare the parameters
- if (paramLength > 0)
- {
- if (memcmp (&response[1], param, paramLength) != 0)
- throw URGError (URG_ERR_PROTOCOL, string ("Incorrect paramaters echo for command ")
- + cmd[0]);
- }
- // Next up, check the status byte
- if (_verbose)
- {
- cerr << "URGLaser::" << __func__ << "() Command response status: " <<
- response[statusIndex] << endl;
- }
- if (response[statusIndex] != '0')
- {
- if (extraOK != NULL)
- {
- if (response[statusIndex] != extraOK[0])
- {
- stringstream ss;
- ss << "Bad response to " << cmd[0] << " command: " << " " <<
- SCIP1ErrorToString (response[statusIndex], cmd[0]);
- throw URGError (URG_ERR_PROTOCOL, ss.str ());
- }
- }
- else
- {
- stringstream ss;
- ss << "Bad response to " << cmd[0] << " command: " << response[statusIndex] <<
- " " << SCIP1ErrorToString (response[statusIndex], cmd[0]);
- throw URGError (URG_ERR_PROTOCOL, ss.str ());
- }
- }
- statusCode = atoi (&response[statusIndex]);
- // All OK, data starts at beginning of port's buffer
- }
- else if (_scipVersion == 2)
- {
- if (_verbose)
- {
- cerr << "URGLaser::" << __func__ << "() Writing in SCIP2 mode. Command is " <<
- cmd << ", parameters length is " << paramLength << endl;
- }
- // Write the command
- if (_port->Write (cmd, 2) < 2)
- throw URGError (URG_ERR_WRITE, "Failed to write command byte.");
- if (paramLength > 0)
- {
- if (_port->Write (param, paramLength) < paramLength)
- throw URGError (URG_ERR_WRITE, "Failed to write command parameters.");
- }
- if (_port->Write ("\n", 1) < 1)
- throw URGError (URG_ERR_WRITE, "Failed to write termination character.");
-
- // Read back the command echo (minimum of 3 bytes, maximum of 16 bytes)
- ReadLine (response, 3 + paramLength);
- // Check the echo is correct
- if (response[0] != cmd[0] || response[1] != cmd[1])
- {
- stringstream ss;
- ss << "Incorrect command echo: " << cmd << " != " << response[0] << response[1];
- throw URGError (URG_ERR_PROTOCOL, ss.str ());
- }
- // Then compare the parameters
- if (paramLength > 0)
- {
- if (memcmp (&response[2], param, paramLength) != 0)
- throw URGError (URG_ERR_PROTOCOL, string ("Incorrect paramaters echo for command ")
- + cmd);
- }
-
- // The next line should be the status line
- ReadLineWithCheck (response, 4);
- if (_verbose)
- {
- cerr << "URGLaser::" << __func__ << "() Command ...
[truncated message content] |
|
From: <gb...@us...> - 2008-06-24 14:05:51
|
Revision: 209
http://gearbox.svn.sourceforge.net/gearbox/?rev=209&view=rev
Author: gbiggs
Date: 2008-06-24 07:03:59 -0700 (Tue, 24 Jun 2008)
Log Message:
-----------
Added some extra verbosity to logging of chunk reading
Modified Paths:
--------------
gearbox/trunk/submitted/flexiport/logfile.cpp
Modified: gearbox/trunk/submitted/flexiport/logfile.cpp
===================================================================
--- gearbox/trunk/submitted/flexiport/logfile.cpp 2008-06-24 12:25:19 UTC (rev 208)
+++ gearbox/trunk/submitted/flexiport/logfile.cpp 2008-06-24 14:03:59 UTC (rev 209)
@@ -1099,8 +1099,16 @@
{
if (_debug >= 3)
{
+ long pos;
+ if ((pos = ftell (file)) < 0)
+ {
+ stringstream ss;
+ ss << "LogFile::" << __func__ << "() ftell() error: (" << ErrNo () << ") " <<
+ StrError (ErrNo ());
+ throw PortException (ss.str ());
+ }
cerr << "LogFile::" << __func__ << "() Reading a single chunk from " <<
- ((file == _readFile) ? "read file." : "write file.") << endl;
+ ((file == _readFile) ? "read file" : "write file") << " at position " << pos << endl;
}
// Read the chunk info
@@ -1112,7 +1120,10 @@
ReadFromFile (file, &tempSize, sizeof (tempSize));
size = ntohl (tempSize);
if (_debug >= 3)
- cerr << "LogFile::" << __func__ << "() Chunk is " << size << " bytes." << endl;
+ {
+ cerr << "LogFile::" << __func__ << "() Chunk has time " << timeStamp.tv_sec << "s " <<
+ timeStamp.tv_usec << "us and is " << size << " bytes." << endl;
+ }
// Check if this chunk will fit in data
if (size > count)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <gb...@us...> - 2008-06-24 12:38:59
|
Revision: 208
http://gearbox.svn.sourceforge.net/gearbox/?rev=208&view=rev
Author: gbiggs
Date: 2008-06-24 05:25:19 -0700 (Tue, 24 Jun 2008)
Log Message:
-----------
Altered internal behaviour when reaching the end of the write file
Modified Paths:
--------------
gearbox/trunk/submitted/flexiport/logfile.cpp
Modified: gearbox/trunk/submitted/flexiport/logfile.cpp
===================================================================
--- gearbox/trunk/submitted/flexiport/logfile.cpp 2008-06-24 12:19:11 UTC (rev 207)
+++ gearbox/trunk/submitted/flexiport/logfile.cpp 2008-06-24 12:25:19 UTC (rev 208)
@@ -712,18 +712,26 @@
// One final thing to do: it's possible that the write file has reached its end and so been
// closed. This means that any attempts to read the response to the write will now fail. To
// prevent this, reopen the files and re-position the read file's offset to where it was. Any
- // writes after this would have choked anyway since the file is at an end so having the write
- // file at the beginning is unlikely to cause any problems.
+ // writes after this would have choked anyway since the file is at an end so moving the write
+ // file to 1 byte before the end is unlikely to cause any problems, while still registering as
+ // an open file.
if (!IsOpen ())
{
Open (_fileName, _read, _ignoreTimes);
if (fseek (_readFile, readFileOffset, SEEK_SET) < 0)
{
stringstream ss;
- ss << "LogFile::" << __func__ << "() fseek() error: (" << ErrNo () << ") " <<
+ ss << "LogFile::" << __func__ << "() fseek(_readFile) error: (" << ErrNo () << ") " <<
StrError (ErrNo ());
throw PortException (ss.str ());
}
+ if (fseek (_writeFile, -1, SEEK_END) < 0)
+ {
+ stringstream ss;
+ ss << "LogFile::" << __func__ << "() fseek(_writeFile) error: (" << ErrNo () << ") " <<
+ StrError (ErrNo ());
+ throw PortException (ss.str ());
+ }
}
delete[] fileData;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|