Merge branch 'master' of https://i61wiki.itec.uka.de/git/robot-api

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)
 

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)
 

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;
 }

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);

source/RobotAPI/drivers/WeissHapticSensor/SerialInterface.cpp

@@ -135,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);
     }

source/RobotAPI/motioncontrol/MotionControl.cpp

@@ -698,16 +698,86 @@ void MotionControl::StopRobot::onEnter()
     sendEvent<EvSuccess>();
 }
 
+void DoPreshapeSet::defineParameters()
+{
+    addToInput("useLeftHand", VariantType::Bool, false);
+    addToInput("preshapes", VariantType::List(VariantType::String), false);
+    addToLocal("counterRef", VariantType::ChannelRef);
+}
 
 
 
-void CloseHand::defineParameters()
+void DoPreshapeSet::defineSubstates()
+{
+    StatePtr preshape = addState<SelectAndDoPreshape>("SelectAndDoPreshape");
+    setInitState(preshape, PM::createMapping()->mapFromParent("*"));
+    StatePtr success = addState<SuccessState>("SuccessState");
+    StatePtr counter = addState<CounterStateTemplate<EvReachedIntermediatePreshape, EvReachedFinalPreshape> >("EvReachedIntermediatePreshape");
+
+    addTransition<EvSuccess>(preshape, counter, PM::createMapping()->mapFromParent("*"));
+    addTransition<EvReachedIntermediatePreshape>(counter, preshape, PM::createMapping()->mapFromParent("*"));
+    addTransition<EvReachedFinalPreshape>(counter, success, PM::createMapping()->mapFromParent("*"));
+
+}
+
+
+
+void DoPreshapeSet::onEnter()
+{
+
+    ChannelRefPtr counterRef = ChannelRefPtr::dynamicCast(getContext()->systemObserverPrx->startCounter(0,"preshapeCounter"));
+    setLocal("counterRef", counterRef);
+}
+
+void DoPreshapeSet::onExit()
+{
+    getContext()->systemObserverPrx->removeCounter(getLocal<ChannelRef>("counterRef"));
+}
+
+
+
+
+
+
+
+void SelectAndDoPreshape::defineParameters()
 {
     addToInput("useLeftHand", VariantType::Bool, false);
+    addToInput("preshapes", VariantType::List(VariantType::String), false);
+    addToInput("counterRef", VariantType::ChannelRef, false);
+
+    addToLocal("selectedPreshapeName", VariantType::String);
+
 }
 
+void SelectAndDoPreshape::defineSubstates()
+{
+    StatePtr doPreshape = addState<DoPreshape>("DoPreshape");
+    PMPtr mapping = PM::createMapping()->
+            mapFromParent("*")
+            ->mapFromParent("selectedPreshapeName", "preshapeName");
+    setInitState(doPreshape, mapping);
+    StatePtr success = addState<SuccessState >("SuccessState");
+    addTransition<EvSuccess>(doPreshape, success);
 
-void CloseHand::onEnter()
+}
+
+
+
+void SelectAndDoPreshape::onEnter()
+{
+    int index = getInput<ChannelRef>("counterRef")->getDataField("value")->getInt();
+    std::string preshapeName = getInput<SingleTypeVariantList>("preshapes")->getVariant(index)->getString();
+    setLocal("selectedPreshapeName", preshapeName);
+}
+
+void DoPreshape::defineParameters()
+{
+    addToInput("useLeftHand", VariantType::Bool, false);
+    addToInput("preshapeName", VariantType::String, false);
+}
+
+void DoPreshape::onEnter()
 {
     RobotStatechartContext* context = getContext<RobotStatechartContext>();
     bool useLeftHand = getInput<bool>("useLeftHand");
@@ -724,30 +794,54 @@ void CloseHand::onEnter()
     std::map<std::string, HandUnitInterfacePrx>::iterator it = context->handUnits.find(handUnitName);
     if(it != context->handUnits.end())
     {
-        it->second->close();
+//        NameValueMap jointValues =it->second->getPreshapeJointValues(getInput<std::string>("PreshapeName"));
+        it->second->preshape(getInput<std::string>("preshapeName"));
+        setTimeoutEvent(2000, createEvent<EvSuccess>());
+
     }
     else
     {
         ARMARX_ERROR  << "No Proxy for HandUnit with Name "  << handUnitName << " known";
     }
 
-
-
-    sendEvent<EvSuccess>();
 }
 
+SingleTypeVariantList DoPreshape::GetPreshapeSet(const SingleTypeVariantListPtr preshapes, const std::string &preshapePrefix)
+{
+    SingleTypeVariantList result(VariantType::String);
+    for(int i=0; i < preshapes->getSize(); i++)
+    {
+        const std::string & preshapeName = preshapes->getVariant(i)->getString();
+        if(preshapeName.find(preshapePrefix) != std::string::npos)
+        {
+            result.addVariant(preshapeName);
+        }
+    }
+    return result;
+}
 
 
+void DoPrefixPreshapeSet::defineSubstates()
+{
+    StatePtr doPreshape = addState<DoPreshapeSet>("DoPreshapeSet");
+    PMPtr mapping = PM::createMapping()->
+            mapFromParent("*");
+    setInitState(doPreshape, mapping);
+    StatePtr success = addState<SuccessState>("SuccessState");
+    addTransition<EvSuccess>(doPreshape, success);
+}
 
-void OpenHand::defineParameters()
+void DoPrefixPreshapeSet::defineParameters()
 {
     addToInput("useLeftHand", VariantType::Bool, false);
+    addToInput("preshapePrefix", VariantType::String, false);
 }
 
 
-void OpenHand::onEnter()
+void DoPrefixPreshapeSet::onEnter()
 {
     RobotStatechartContext* context = getContext<RobotStatechartContext>();
+
     bool useLeftHand = getInput<bool>("useLeftHand");
     std::string handUnitName;
     if (useLeftHand)
@@ -762,19 +856,61 @@ void OpenHand::onEnter()
     std::map<std::string, HandUnitInterfacePrx>::iterator it = context->handUnits.find(handUnitName);
     if(it != context->handUnits.end())
     {
-        it->second->open();
+        setLocal("preshapes", DoPreshape::GetPreshapeSet(SingleTypeVariantListPtr::dynamicCast(it->second->getPreshapeNames()), getInput<std::string>("preshapePrefix")));
     }
     else
     {
         ARMARX_ERROR  << "No Proxy for HandUnit with Name "  << handUnitName << " known";
     }
+}
 
-    sendEvent<EvSuccess>();
+
+
+
+
+void OpenHand::defineSubstates()
+{
+    StatePtr doPreshape = addState<DoPrefixPreshapeSet>("DoPreshapeSet");
+    PMPtr mapping = PM::createMapping()->
+            mapFromParent("*");
+    setInitState(doPreshape, mapping);
+    StatePtr success = addState<SuccessState>("SuccessState");
+    addTransition<EvSuccess>(doPreshape, success);
 }
 
+void OpenHand::defineParameters()
+{
+    addToInput("useLeftHand", VariantType::Bool, false);
+    addToLocal("preshapePrefix", VariantType::String);
+}
+
+void OpenHand::onEnter()
+{
+    setLocal("preshapePrefix", "Open");
+
+}
+
+void CloseHand::defineSubstates()
+{
+    StatePtr doPreshape = addState<DoPrefixPreshapeSet>("DoPreshapeSet");
+    PMPtr mapping = PM::createMapping()->
+            mapFromParent("*");
+    setInitState(doPreshape, mapping);
+    StatePtr success = addState<SuccessState>("SuccessState");
+    addTransition<EvSuccess>(doPreshape, success);
+}
 
+void CloseHand::defineParameters()
+{
+    addToInput("useLeftHand", VariantType::Bool, false);
+    addToLocal("preshapePrefix", VariantType::String);
+}
 
+void CloseHand::onEnter()
+{
+    setLocal("preshapePrefix", "Close");
 
+}
 
 void HeadLookAtTarget::defineParameters()
 {
@@ -884,3 +1020,6 @@ void CalculateHeadIK::run()
 }
 
 
+
+
+

source/RobotAPI/motioncontrol/MotionControl.h

@@ -224,15 +224,43 @@ namespace MotionControl
     };
 
 
-
-
     /**
     *  \ingroup MotionControl
     * Closes the given hand of the robot
     * \param useLeftHand True if left hand should be closed, false for the right hand
     */
-    struct CloseHand : StateTemplate<CloseHand>
+    struct DoPreshape : StateTemplate<DoPreshape>
+    {
+        void defineParameters();
+        void onEnter();
+        static SingleTypeVariantList GetPreshapeSet(const SingleTypeVariantListPtr preshapes, const std::string& preshapePrefix);
+    };
+
+    DEFINEEVENT(EvReachedIntermediatePreshape)
+    DEFINEEVENT(EvReachedFinalPreshape)
+    struct SelectAndDoPreshape : StateTemplate<SelectAndDoPreshape>
+    {
+        void defineSubstates();
+        void defineParameters();
+        void onEnter();
+    };
+
+    struct DoPreshapeSet : StateTemplate<DoPreshapeSet>
+    {
+        void defineSubstates();
+        void defineParameters();
+        void onEnter();
+        void onExit();
+
+    };
+
+
+
+
+
+    struct DoPrefixPreshapeSet : StateTemplate<DoPrefixPreshapeSet>
     {
+        void defineSubstates();
         void defineParameters();
         void onEnter();
     };
@@ -240,6 +268,7 @@ namespace MotionControl
 
 
 
+
     /**
     *  \ingroup MotionControl
     * Opens the given hand of the robot
@@ -247,6 +276,20 @@ namespace MotionControl
     */
     struct OpenHand : StateTemplate<OpenHand>
     {
+        void defineSubstates();
+        void defineParameters();
+        void onEnter();
+    };
+
+
+    /**
+    *  \ingroup MotionControl
+    * Closes the given hand of the robot
+    * \param useLeftHand True if left hand should be closed, false for the right hand
+    */
+    struct CloseHand : StateTemplate<CloseHand>
+    {
+        void defineSubstates();
         void defineParameters();
         void onEnter();
     };

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)