Add actuator control vocabs, fill missing documentation

#115
roboticslab-uc3m · Sep 9, 2017 · 7bf9927 · 7bf9927
1 parent 35c9f5b
commit 7bf9927
Show file tree

Hide file tree

Showing 10 changed files with 64 additions and 42 deletions.
diff --git a/libraries/TeoYarp/AmorCartesianControl/AmorCartesianControl.hpp b/libraries/TeoYarp/AmorCartesianControl/AmorCartesianControl.hpp
@@ -63,7 +63,7 @@ class AmorCartesianControl : public yarp::dev::DeviceDriver, public ICartesianCo
 
     virtual bool tool(const std::vector<double> &x);
 
-    virtual bool act(int commandCode);
+    virtual bool act(int command);
 
     virtual void fwd(const std::vector<double> &rot, double step);
 

diff --git a/libraries/TeoYarp/AmorCartesianControl/ICartesianControlImpl.cpp b/libraries/TeoYarp/AmorCartesianControl/ICartesianControlImpl.cpp
@@ -4,6 +4,8 @@
 
 #include <cmath>
 
+#include <yarp/os/Vocab.h>
+
 #include "KinematicRepresentation.hpp"
 
 // ------------------- ICartesianControl Related ------------------------------------
@@ -219,35 +221,29 @@ bool roboticslab::AmorCartesianControl::tool(const std::vector<double> &x)
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::AmorCartesianControl::act(int commandCode)
+bool roboticslab::AmorCartesianControl::act(int command)
 {
-    if (commandCode == 1)
-    {
-        if (amor_close_hand(handle) != AMOR_SUCCESS)
-        {
-            CD_ERROR("%s\n", amor_error());
-            return false;
-        }
-    }
-    else if (commandCode == 2)
-    {
-        if (amor_open_hand(handle) != AMOR_SUCCESS)
-        {
-            CD_ERROR("%s\n", amor_error());
-            return false;
-        }
-    }
-    else if (commandCode == 3)
-    {
-        if (amor_stop_hand(handle) != AMOR_SUCCESS)
-        {
-            CD_ERROR("%s\n", amor_error());
-            return false;
-        }
+    AMOR_RESULT (*amor_command)(AMOR_HANDLE);
+
+    switch (command)
+    {
+    case VOCAB_CC_ACTUATOR_CLOSE_GRIPPER:
+        amor_command = amor_close_hand;
+        break;
+    case VOCAB_CC_ACTUATOR_OPEN_GRIPPER:
+        amor_command = amor_open_hand;
+        break;
+    case VOCAB_CC_ACTUATOR_STOP_GRIPPER:
+        amor_command = amor_stop_hand;
+        break;
+    default:
+        CD_ERROR("Unrecognized command with code %d (%s).\n", command, yarp::os::Vocab::decode(command));
+        return false;
     }
-    else
+
+    if (amor_command(handle) != AMOR_SUCCESS)
     {
-        CD_ERROR("Unrecognized command code: %d.\n", commandCode);
+        CD_ERROR("%s\n", amor_error());
         return false;
     }
 

diff --git a/libraries/TeoYarp/BasicCartesianControl/BasicCartesianControl.hpp b/libraries/TeoYarp/BasicCartesianControl/BasicCartesianControl.hpp
@@ -136,7 +136,7 @@ class BasicCartesianControl : public yarp::dev::DeviceDriver, public ICartesianC
 
         virtual bool tool(const std::vector<double> &x);
 
-        virtual bool act(int commandCode);
+        virtual bool act(int command);
 
         virtual void fwd(const std::vector<double> &rot, double step);
 

diff --git a/libraries/TeoYarp/BasicCartesianControl/ICartesianControlImpl.cpp b/libraries/TeoYarp/BasicCartesianControl/ICartesianControlImpl.cpp
@@ -273,7 +273,7 @@ bool roboticslab::BasicCartesianControl::tool(const std::vector<double> &x)
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::BasicCartesianControl::act(int commandCode)
+bool roboticslab::BasicCartesianControl::act(int command)
 {
     CD_ERROR("Not implemented.\n");
     return false;

diff --git a/libraries/TeoYarp/CartesianControlClient/CartesianControlClient.hpp b/libraries/TeoYarp/CartesianControlClient/CartesianControlClient.hpp
@@ -61,7 +61,7 @@ class CartesianControlClient : public yarp::dev::DeviceDriver, public ICartesian
 
         virtual bool tool(const std::vector<double> &x);
 
-        virtual bool act(int commandCode);
+        virtual bool act(int command);
 
         virtual void fwd(const std::vector<double> &rot, double step);
 

diff --git a/libraries/TeoYarp/CartesianControlClient/ICartesianControlImpl.cpp b/libraries/TeoYarp/CartesianControlClient/ICartesianControlImpl.cpp
@@ -221,12 +221,12 @@ bool roboticslab::CartesianControlClient::tool(const std::vector<double> &x)
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::CartesianControlClient::act(int commandCode)
+bool roboticslab::CartesianControlClient::act(int command)
 {
     yarp::os::Bottle cmd, response;
 
     cmd.addVocab(VOCAB_CC_ACT);
-    cmd.addInt(commandCode);
+    cmd.addInt(command);
 
     rpcClient.write(cmd,response);
 

diff --git a/libraries/TeoYarp/ICartesianControl.h b/libraries/TeoYarp/ICartesianControl.h
@@ -33,7 +33,7 @@
 #define VOCAB_CC_FORC VOCAB4('f','o','r','c') ///< Force control
 #define VOCAB_CC_STOP VOCAB4('s','t','o','p') ///< Stop control
 #define VOCAB_CC_TOOL VOCAB4('t','o','o','l') ///< Change tool
-#define VOCAB_CC_ACT VOCAB3('a','c','t')
+#define VOCAB_CC_ACT VOCAB3('a','c','t')      ///< Actuate tool
 
 /** @} */
 
@@ -71,6 +71,24 @@
 
 /** @} */
 
+/**
+ * @anchor ICartesianControl_actuator_vocabs
+ * @name Actuator control vocabs
+ *
+ * Used by roboticslab::ICartesianControl::act to control the actuator.
+ *
+ * @{
+ */
+
+// Actuator control
+#define VOCAB_CC_ACTUATOR_NONE VOCAB3('a','c','n')              ///< No actuator or no action
+#define VOCAB_CC_ACTUATOR_CLOSE_GRIPPER VOCAB4('a','c','c','g') ///< Close gripper
+#define VOCAB_CC_ACTUATOR_OPEN_GRIPPER VOCAB4('a','c','o','g')  ///< Open gripper
+#define VOCAB_CC_ACTUATOR_STOP_GRIPPER VOCAB4('a','c','s','g')  ///< Stop gripper
+#define VOCAB_CC_ACTUATOR_GENERIC VOCAB3('a','c','g')           ///< Generic actuator
+
+/** @} */
+
 namespace roboticslab
 {
 
@@ -221,8 +239,16 @@ class ICartesianControl
          */
         virtual bool tool(const std::vector<double> &x) = 0;
 
-        /** actuate tool */
-        virtual bool act(int commandCode) = 0;
+        /**
+         * @brief Actuate tool
+         *
+         * Send control command to actuate the robot's tool, if available.
+         *
+         * @param command One of available @ref ICartesianControl_actuator_vocabs "actuator vocabs".
+         *
+         * @return true on success, false otherwise
+         */
+        virtual bool act(int command) = 0;
 
         /** @} */
 

diff --git a/programs/streamingDeviceController/SpnavSensorDevice.cpp b/programs/streamingDeviceController/SpnavSensorDevice.cpp
@@ -45,20 +45,20 @@ int roboticslab::SpnavSensorDevice::getActuatorState()
 
     if (button1 == 1)
     {
-        actuatorState = 1;
+        actuatorState = VOCAB_CC_ACTUATOR_CLOSE_GRIPPER;
     }
     else if (button2 == 1)
     {
-        actuatorState = 2;
+        actuatorState = VOCAB_CC_ACTUATOR_OPEN_GRIPPER;
     }
     else
     {
-        if (actuatorState != 0)
+        if (actuatorState != VOCAB_CC_ACTUATOR_NONE)
         {
-            actuatorState = 3;
+            actuatorState = VOCAB_CC_ACTUATOR_STOP_GRIPPER;
         }
 
-        actuatorState = 0;
+        actuatorState = VOCAB_CC_ACTUATOR_NONE;
     }
 
     return actuatorState;

diff --git a/programs/streamingDeviceController/StreamingDevice.cpp b/programs/streamingDeviceController/StreamingDevice.cpp
@@ -46,7 +46,7 @@ StreamingDevice * StreamingDeviceFactory::makeDevice(const std::string & deviceN
 
 StreamingDevice::StreamingDevice(yarp::os::Searchable & config)
     : iCartesianControl(NULL),
-      actuatorState(0)
+      actuatorState(VOCAB_CC_ACTUATOR_NONE)
 {
     data.resize(6, 0.0);
     fixedAxes.resize(6, false);

diff --git a/programs/streamingDeviceController/StreamingDeviceController.cpp b/programs/streamingDeviceController/StreamingDeviceController.cpp
@@ -131,7 +131,7 @@ bool StreamingDeviceController::updateModule()
 
     int actuatorState = streamingDevice->getActuatorState();
 
-    if (actuatorState != 0)
+    if (actuatorState != VOCAB_CC_ACTUATOR_NONE)
     {
         iCartesianControl->act(actuatorState);
     }