diff --git a/source/RobotAPI/applications/WeissHapticSensor/CMakeLists.txt b/source/RobotAPI/applications/WeissHapticSensor/CMakeLists.txt
index e08a3f6d4ec46d0b3417ef9746b60dda744a1300..378475a1c7b43d082822a9fc398709dfdaff1482 100644
--- a/source/RobotAPI/applications/WeissHapticSensor/CMakeLists.txt
+++ b/source/RobotAPI/applications/WeissHapticSensor/CMakeLists.txt
@@ -8,7 +8,7 @@ if (Eigen3_FOUND)
${Eigen3_INCLUDE_DIR})
endif()
-set(COMPONENT_LIBS ArmarXInterfaces ArmarXCore ArmarXCoreObservers ArmarXCoreEigen3Variants WeissHapticSensorListener RobotAPIUnits)
+set(COMPONENT_LIBS ArmarXInterfaces ArmarXCore ArmarXCoreObservers ArmarXCoreEigen3Variants WeissHapticSensorListener RobotAPIUnits RobotAPIInterfaces)
set(EXE_SOURCE main.cpp WeissHapticSensorApp.h)
diff --git a/source/RobotAPI/applications/WeissHapticSensorsUnit/CMakeLists.txt b/source/RobotAPI/applications/WeissHapticSensorsUnit/CMakeLists.txt
index e49b359070b79deda52d02399d2ded44faa38dea..3f7af3cd59fbdac4735e98129332367b788ef8d1 100644
--- a/source/RobotAPI/applications/WeissHapticSensorsUnit/CMakeLists.txt
+++ b/source/RobotAPI/applications/WeissHapticSensorsUnit/CMakeLists.txt
@@ -8,7 +8,7 @@ if (Eigen3_FOUND)
${Eigen3_INCLUDE_DIR})
endif()
-set(COMPONENT_LIBS ArmarXInterfaces ArmarXCore ArmarXCoreObservers ArmarXCoreEigen3Variants WeissHapticSensor RobotAPIUnits)
+set(COMPONENT_LIBS ArmarXInterfaces ArmarXCore ArmarXCoreObservers ArmarXCoreEigen3Variants WeissHapticSensor RobotAPIUnits RobotAPIInterfaces)
set(EXE_SOURCE main.cpp WeissHapticSensorsUnitApp.h)
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/AbstractInterface.cpp b/source/RobotAPI/drivers/WeissHapticSensor/AbstractInterface.cpp
index d8cfc06a6c68b2be86d3c51cfabc9a0bad3487e5..b450fb668f81c4bc9bc73986211bb46d92ef2733 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/AbstractInterface.cpp
+++ b/source/RobotAPI/drivers/WeissHapticSensor/AbstractInterface.cpp
@@ -10,16 +10,13 @@
AbstractInterface::AbstractInterface()
- : connected(false), log(NULL)
+ : connected(false)
{
}
-AbstractInterface::~AbstractInterface() {
- if(log != NULL)
- {
- delete log;
- }
+AbstractInterface::~AbstractInterface()
+{
}
int AbstractInterface::read(unsigned char *buf, unsigned int len)
@@ -47,20 +44,6 @@ int AbstractInterface::write(unsigned char *buf, unsigned int len)
Response AbstractInterface::submitCmd( unsigned char id, unsigned char *payload, unsigned int len, bool pending )
{
- /*int res;
-
- // Check if we're connected
- if ( !connected )
- {
- throw TransmissionException("Interface not connected");
- }
-
- // Send command
- res = send( id, len, payload );
- if ( res < 0 )
- {
- throw TransmissionException("Message send failed");
- }*/
fireAndForgetCmd(id, payload, len, pending);
return receive(pending, id);
}
@@ -86,7 +69,7 @@ void AbstractInterface::fireAndForgetCmd( unsigned char id, unsigned char *paylo
void AbstractInterface::startLogging(std::string file)
{
- log = new BinaryLogger(file);
+ log.reset(new BinaryLogger(file));
}
void AbstractInterface::logText(std::string message)
@@ -197,8 +180,12 @@ int AbstractInterface::receive( msg_t *msg )
msg->len = make_short( header[1], header[2] );
// Allocate space for payload and checksum
- msg->data.resize( msg->len + 2u );
+ //msg->data.resize( msg->len + 2u );
//if ( !msg->data ) return -1;
+ //boost::shared_ptr<unsigned char[]> data(new unsigned char[ msg->len + 2u ]);
+
+ unsigned char* data = new unsigned char[ msg->len + 2u ];
+
// Read payload and checksum
/*int maxReads = 10;
@@ -206,7 +193,7 @@ int AbstractInterface::receive( msg_t *msg )
int dataOffset = 0;
while ( maxReads > 0 && remaining > 0)
{
- int read = this->read( msg->data.data() + dataOffset, remaining );
+ int read = this->read( data + dataOffset, remaining );
dataOffset += read;
remaining -= read;
maxReads--;
@@ -223,9 +210,10 @@ int AbstractInterface::receive( msg_t *msg )
// Read payload and checksum:
// payload: msg->len
// checksum: 2 byte
- int read = this->read( msg->data.data(), msg->len + 2 );
- if (read != msg->len + 2)
+ int read = this->read( data, msg->len + 2 );
+ if (read != (int)msg->len + 2)
{
+ delete[] data;
throw TransmissionException(str(boost::format("Not enough data (%d, expected %d), Command = %02X") % res % (msg->len + 2) % msg->id));
}
@@ -239,13 +227,16 @@ int AbstractInterface::receive( msg_t *msg )
}*/
// Check checksum
- checksum = Checksum::Update_crc16( msg->data.data(), msg->len + 2, checksum );
+ checksum = Checksum::Update_crc16( data, msg->len + 2, checksum );
if ( checksum != 0 )
{
+ delete[] data;
logText("CHECKSUM ERROR");
throw ChecksumErrorException("Checksum error");
}
+ msg->data = std::vector<unsigned char>(data, data + msg->len + 2);
+ delete[] data;
logText("receive done.");
return msg->len + 8;
}
@@ -281,6 +272,7 @@ int AbstractInterface::send(unsigned char id, unsigned int len, unsigned char *d
res = write( buf, 6 + len + 2 );
if ( res < 6 + (int)len + 2 )
{
+ delete[] buf;
close();
throw TransmissionException("Failed to submit message checksum");
//cerr << "Failed to submit message checksum" << endl;
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/AbstractInterface.h b/source/RobotAPI/drivers/WeissHapticSensor/AbstractInterface.h
index f8d6c1b9a4dae3a5723d9701fe5624e3bb25e618..19e45a9cc1821573cd222cd4f1c53b2894c640af 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/AbstractInterface.h
+++ b/source/RobotAPI/drivers/WeissHapticSensor/AbstractInterface.h
@@ -4,6 +4,7 @@
#include <string>
#include "Types.h"
#include "BinaryLogger.h"
+#include <boost/shared_ptr.hpp>
#define MSG_PREAMBLE_BYTE 0xaa
@@ -55,7 +56,7 @@ protected:
private:
friend std::ostream& operator<<(std::ostream&, const AbstractInterface&);
- BinaryLogger* log;
+ boost::shared_ptr<BinaryLogger> log;
};
std::ostream& operator<<(std::ostream &strm, const AbstractInterface &a);
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/SerialInterface.cpp b/source/RobotAPI/drivers/WeissHapticSensor/SerialInterface.cpp
index ee19bf6cf6ace8182ecd9bff9c8710087c8013a2..387d2f5236177c7f4583460325209259bad95993 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/SerialInterface.cpp
+++ b/source/RobotAPI/drivers/WeissHapticSensor/SerialInterface.cpp
@@ -38,6 +38,7 @@ SerialInterface::SerialInterface(const char *device, unsigned int bitrate)
{
this->device = device;
this->bitrate = bitrate;
+ this->fd = -1;
}
SerialInterface::~SerialInterface() {
@@ -134,19 +135,23 @@ int SerialInterface::readInternal( unsigned char *buf, unsigned int len)
int SerialInterface::blockingReadAll(unsigned char *buf, unsigned int len)
{
- int readData = 0;
+ int dataToRead = len;
while (1)
{
- int res = ::read( fd, buf, len );
+ int res = ::read( fd, buf, dataToRead );
if (res < 0)
{
return res;
}
- readData += res;
+ dataToRead -= res;
buf += res;
- if (readData >= len)
+ if (dataToRead == 0)
{
- return readData;
+ return len;
+ }
+ if (dataToRead < 0)
+ {
+ throw new std::runtime_error("Internal error: dataToRead < 0");
}
usleep(1);
}
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.cpp b/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.cpp
index 82beabb08d12a19d06a04cdb410787842345b45f..f71bf414b5f4cac1bd917e57440aa2d7ab70db64 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.cpp
+++ b/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.cpp
@@ -6,7 +6,7 @@
using namespace armarx;
WeissHapticSensor::WeissHapticSensor(std::string device)
- : device(device)
+ : device(device), connected(false)
{
sensorTask = new RunningTask<WeissHapticSensor>(this, &WeissHapticSensor::frameAcquisitionTaskLoop);
boost::smatch match;
diff --git a/source/RobotAPI/units/CMakeLists.txt b/source/RobotAPI/units/CMakeLists.txt
index 494f973851ce21d59ccc39d36db1be6221a9d611..0bc0993563f552e2dd27def2a175066064346bfe 100644
--- a/source/RobotAPI/units/CMakeLists.txt
+++ b/source/RobotAPI/units/CMakeLists.txt
@@ -13,6 +13,8 @@ if (Eigen3_FOUND AND Simox_FOUND)
${Simox_INCLUDE_DIRS})
endif()
+message(status ${Eigen3_INCLUDE_DIR})
+
set(LIB_NAME RobotAPIUnits)
set(LIB_VERSION 0.1.0)
set(LIB_SOVERSION 0)