diff --git a/libraries/TeoYarp/AmorCartesianControl/AmorCartesianControl.hpp b/libraries/TeoYarp/AmorCartesianControl/AmorCartesianControl.hpp index 09d9fe12e..f51d6d11a 100644 --- 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 &x); - virtual bool act(int commandCode); + virtual bool act(int command); virtual void fwd(const std::vector &rot, double step); diff --git a/libraries/TeoYarp/AmorCartesianControl/ICartesianControlImpl.cpp b/libraries/TeoYarp/AmorCartesianControl/ICartesianControlImpl.cpp index 21cbe48dd..7df630bfe 100644 --- a/libraries/TeoYarp/AmorCartesianControl/ICartesianControlImpl.cpp +++ b/libraries/TeoYarp/AmorCartesianControl/ICartesianControlImpl.cpp @@ -4,6 +4,8 @@ #include +#include + #include "KinematicRepresentation.hpp" // ------------------- ICartesianControl Related ------------------------------------ @@ -219,35 +221,29 @@ bool roboticslab::AmorCartesianControl::tool(const std::vector &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 index 781b53f08..1c4700618 100644 --- 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 &x); - virtual bool act(int commandCode); + virtual bool act(int command); virtual void fwd(const std::vector &rot, double step); diff --git a/libraries/TeoYarp/BasicCartesianControl/ICartesianControlImpl.cpp b/libraries/TeoYarp/BasicCartesianControl/ICartesianControlImpl.cpp index 52c0d8163..4156f26bf 100644 --- a/libraries/TeoYarp/BasicCartesianControl/ICartesianControlImpl.cpp +++ b/libraries/TeoYarp/BasicCartesianControl/ICartesianControlImpl.cpp @@ -273,7 +273,7 @@ bool roboticslab::BasicCartesianControl::tool(const std::vector &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 index dff461ce2..daab2baef 100644 --- 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 &x); - virtual bool act(int commandCode); + virtual bool act(int command); virtual void fwd(const std::vector &rot, double step); diff --git a/libraries/TeoYarp/CartesianControlClient/ICartesianControlImpl.cpp b/libraries/TeoYarp/CartesianControlClient/ICartesianControlImpl.cpp index ee2f18268..35d22a045 100644 --- a/libraries/TeoYarp/CartesianControlClient/ICartesianControlImpl.cpp +++ b/libraries/TeoYarp/CartesianControlClient/ICartesianControlImpl.cpp @@ -221,12 +221,12 @@ bool roboticslab::CartesianControlClient::tool(const std::vector &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 index c4e8fd813..aa85e80d0 100644 --- 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 &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 index 80cd5baef..5588b4551 100644 --- 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 index 58da66d5b..57a5fadb7 100644 --- 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 index 906675a9a..7dbcbabe3 100644 --- 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); }