From 2a9be7f1deb151c1002c4560a6ff3b8bf6d18374 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Tue, 18 Jun 2019 19:00:06 +0200 Subject: [PATCH 01/14] Open and connect centroid port --- .../StreamingDeviceController.cpp | 21 ++++++++++++++++++- .../StreamingDeviceController.hpp | 6 +++++- 2 files changed, 25 insertions(+), 2 deletions(-) diff --git a/programs/streamingDeviceController/StreamingDeviceController.cpp b/programs/streamingDeviceController/StreamingDeviceController.cpp index 672a3e542..d66cc03e2 100644 --- a/programs/streamingDeviceController/StreamingDeviceController.cpp +++ b/programs/streamingDeviceController/StreamingDeviceController.cpp @@ -105,7 +105,7 @@ bool StreamingDeviceController::configure(yarp::os::ResourceFinder &rf) if (!sensorsClientDevice.isValid()) { - CD_ERROR("sensors device not valid.\n"); + CD_ERROR("Sensors device not valid.\n"); return false; } @@ -119,6 +119,23 @@ bool StreamingDeviceController::configure(yarp::os::ResourceFinder &rf) } #endif // SDC_WITH_SENSORS + if (rf.check("remoteCentroid", "remote centroid port")) + { + std::string remoteCentroid = rf.check("remoteCentroid", yarp::os::Value::getNullValue()).asString(); + + if (!centroidPort.open("/sdc" + remoteCentroid + "/state:i")) + { + CD_ERROR("Unable to open local centroid port.\n"); + return false; + } + + if (!yarp::os::Network::connect(centroidPort.getName(), remoteCentroid, "udp")) + { + CD_ERROR("Unable to connect to %s.\n", remoteCentroid.c_str()); + return false; + } + } + isStopped = true; return true; @@ -201,6 +218,8 @@ bool StreamingDeviceController::close() ok &= sensorsClientDevice.close(); #endif // SDC_WITH_SENSORS + centroidPort.close(); + return ok; } diff --git a/programs/streamingDeviceController/StreamingDeviceController.hpp b/programs/streamingDeviceController/StreamingDeviceController.hpp index 085d2f7e6..5c4cb6b77 100644 --- a/programs/streamingDeviceController/StreamingDeviceController.hpp +++ b/programs/streamingDeviceController/StreamingDeviceController.hpp @@ -3,8 +3,10 @@ #include -#include +#include +#include #include +#include #include #include @@ -59,6 +61,8 @@ class StreamingDeviceController : public yarp::os::RFModule static const double SCALING_FACTOR_ON_ALERT; #endif // SDC_WITH_SENSORS + yarp::os::BufferedPort centroidPort; + double period; double scaling; From 3588bd3b0397613ad513df12c6c460470e18cd35 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Fri, 21 Jun 2019 14:20:32 +0200 Subject: [PATCH 02/14] Create CentroidTransform and register associations --- .../streamingDeviceController/CMakeLists.txt | 4 +- .../CentroidTransform.cpp | 12 ++++++ .../CentroidTransform.hpp | 38 +++++++++++++++++++ .../StreamingDevice.hpp | 4 ++ .../StreamingDeviceController.cpp | 21 +++++++++- .../StreamingDeviceController.hpp | 2 + 6 files changed, 79 insertions(+), 2 deletions(-) create mode 100644 programs/streamingDeviceController/CentroidTransform.cpp create mode 100644 programs/streamingDeviceController/CentroidTransform.hpp diff --git a/programs/streamingDeviceController/CMakeLists.txt b/programs/streamingDeviceController/CMakeLists.txt index d87229200..b950e7697 100644 --- a/programs/streamingDeviceController/CMakeLists.txt +++ b/programs/streamingDeviceController/CMakeLists.txt @@ -21,7 +21,9 @@ if(ENABLE_streamingDeviceController) WiimoteSensorDevice.hpp WiimoteSensorDevice.cpp StreamingDeviceController.hpp - StreamingDeviceController.cpp) + StreamingDeviceController.cpp + CentroidTransform.hpp + CentroidTransform.cpp) target_link_libraries(streamingDeviceController ${orocos_kdl_LIBRARIES} YARP::YARP_OS diff --git a/programs/streamingDeviceController/CentroidTransform.cpp b/programs/streamingDeviceController/CentroidTransform.cpp new file mode 100644 index 000000000..9bb8b9d96 --- /dev/null +++ b/programs/streamingDeviceController/CentroidTransform.cpp @@ -0,0 +1,12 @@ +#include "CentroidTransform.hpp" + +using namespace roboticslab; + +CentroidTransform::CentroidTransform() + : streamingDevice(NULL) +{} + +bool CentroidTransform::processBottle(const yarp::os::Bottle & b) +{ + return true; +} diff --git a/programs/streamingDeviceController/CentroidTransform.hpp b/programs/streamingDeviceController/CentroidTransform.hpp new file mode 100644 index 000000000..b0c280470 --- /dev/null +++ b/programs/streamingDeviceController/CentroidTransform.hpp @@ -0,0 +1,38 @@ +#ifndef __CENTROID_TRANSFORM_HPP__ +#define __CENTROID_TRANSFORM_HPP__ + +#include + +#include "StreamingDevice.hpp" + +namespace roboticslab +{ + +class StreamingDevice; + +/** + * @ingroup streamingDeviceController + * + * @brief ... + */ +class CentroidTransform +{ +public: + + //! Constructor + CentroidTransform(); + + //! Register handle to device + void registerStreamingDevice(StreamingDevice * streamingDevice) + { this->streamingDevice = streamingDevice; } + + bool processBottle(const yarp::os::Bottle & b); + +private: + + StreamingDevice * streamingDevice; +}; + +} // namespace roboticslab + +#endif // __CENTROID_TRANSFORM_HPP__ diff --git a/programs/streamingDeviceController/StreamingDevice.hpp b/programs/streamingDeviceController/StreamingDevice.hpp index 4743fa777..4800da292 100644 --- a/programs/streamingDeviceController/StreamingDevice.hpp +++ b/programs/streamingDeviceController/StreamingDevice.hpp @@ -9,11 +9,13 @@ #include #include "ICartesianControl.h" +#include "CentroidTransform.hpp" namespace roboticslab { class StreamingDevice; +class CentroidTransform; /** * @ingroup streamingDeviceController @@ -42,6 +44,8 @@ class StreamingDeviceFactory */ class StreamingDevice : protected yarp::dev::PolyDriver { + friend CentroidTransform; + public: using PolyDriver::isValid; diff --git a/programs/streamingDeviceController/StreamingDeviceController.cpp b/programs/streamingDeviceController/StreamingDeviceController.cpp index d66cc03e2..c918eb312 100644 --- a/programs/streamingDeviceController/StreamingDeviceController.cpp +++ b/programs/streamingDeviceController/StreamingDeviceController.cpp @@ -1,7 +1,8 @@ #include "StreamingDeviceController.hpp" -#include #include +#include +#include #include #include @@ -10,6 +11,8 @@ #include +#include "SpnavSensorDevice.hpp" // for typeid() check + using namespace roboticslab; #ifdef SDC_WITH_SENSORS @@ -121,6 +124,14 @@ bool StreamingDeviceController::configure(yarp::os::ResourceFinder &rf) if (rf.check("remoteCentroid", "remote centroid port")) { + const std::type_info & spnavType = typeid(SpnavSensorDevice); + + if (typeid(*streamingDevice) != spnavType) + { + CD_ERROR("Centroid transform extension only available with %s.\n", spnavType.name()); + return false; + } + std::string remoteCentroid = rf.check("remoteCentroid", yarp::os::Value::getNullValue()).asString(); if (!centroidPort.open("/sdc" + remoteCentroid + "/state:i")) @@ -134,6 +145,8 @@ bool StreamingDeviceController::configure(yarp::os::ResourceFinder &rf) CD_ERROR("Unable to connect to %s.\n", remoteCentroid.c_str()); return false; } + + centroidTransform.registerStreamingDevice(streamingDevice); } isStopped = true; @@ -184,6 +197,12 @@ bool StreamingDeviceController::updateModule() return true; } + if (!centroidPort.isClosed() && !centroidTransform.processBottle(*centroidPort.read(false))) + { + CD_ERROR("Failed to process incoming centroid bottle.\n"); + return true; + } + if (streamingDevice->hasValidMovementData()) { streamingDevice->sendMovementCommand(); diff --git a/programs/streamingDeviceController/StreamingDeviceController.hpp b/programs/streamingDeviceController/StreamingDeviceController.hpp index 5c4cb6b77..c84000602 100644 --- a/programs/streamingDeviceController/StreamingDeviceController.hpp +++ b/programs/streamingDeviceController/StreamingDeviceController.hpp @@ -12,6 +12,7 @@ #include #include "StreamingDevice.hpp" +#include "CentroidTransform.hpp" #include "ICartesianControl.h" @@ -62,6 +63,7 @@ class StreamingDeviceController : public yarp::os::RFModule #endif // SDC_WITH_SENSORS yarp::os::BufferedPort centroidPort; + CentroidTransform centroidTransform; double period; double scaling; From 6df2f6e530cb95fd4699a3c1e1218affafa2c033 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Fri, 21 Jun 2019 17:11:43 +0200 Subject: [PATCH 03/14] Project desired direction onto TCP's Z axis --- .../CentroidTransform.cpp | 45 ++++++++++++++++++- .../CentroidTransform.hpp | 1 + 2 files changed, 45 insertions(+), 1 deletion(-) diff --git a/programs/streamingDeviceController/CentroidTransform.cpp b/programs/streamingDeviceController/CentroidTransform.cpp index 9bb8b9d96..421d2dcae 100644 --- a/programs/streamingDeviceController/CentroidTransform.cpp +++ b/programs/streamingDeviceController/CentroidTransform.cpp @@ -1,12 +1,55 @@ #include "CentroidTransform.hpp" +#include + +#include +#include + using namespace roboticslab; CentroidTransform::CentroidTransform() : streamingDevice(NULL) {} -bool CentroidTransform::processBottle(const yarp::os::Bottle & b) +bool CentroidTransform::processBottle(const yarp::os::Bottle & b) { + if (!b.size() != 2) + { + CD_WARNING("Malformed input bottle, size %d (expected 2).\n", b.size()); + return false; + } + + // object centroids scaled to fit into [-1, 1] range + double cx = b.get(0).asFloat64(); // points right + double cy = b.get(1).asFloat64(); // points down + + std::vector x; + + if (!streamingDevice->iCartesianControl->stat(x)) + { + CD_WARNING("stat failed.\n"); + return false; + } + + KDL::Frame H_base_tcp = KdlVectorConverter::vectorToFrame(x); + + // express Z axis (TCP frame) in base frame + KDL::Vector v_base = H_base_tcp.M.Inverse() * KDL::Vector(0, 0, 1); + v_base.Normalize(); + + KDL::Frame H_d = KdlVectorConverter::vectorToFrame(streamingDevice->data); + + double norm = KDL::dot(H_d.p, v_base); + + if (norm <= 0.0) + { + return true; + } + + // project desired vector into TCP's Z axis, refer result to base frame + H_d.p = v_base * norm; + + streamingDevice->data = KdlVectorConverter::frameToVector(H_d); + return true; } diff --git a/programs/streamingDeviceController/CentroidTransform.hpp b/programs/streamingDeviceController/CentroidTransform.hpp index b0c280470..b75b273fd 100644 --- a/programs/streamingDeviceController/CentroidTransform.hpp +++ b/programs/streamingDeviceController/CentroidTransform.hpp @@ -26,6 +26,7 @@ class CentroidTransform void registerStreamingDevice(StreamingDevice * streamingDevice) { this->streamingDevice = streamingDevice; } + //! Process incoming bottle bool processBottle(const yarp::os::Bottle & b); private: From 82cd847f6a299bae225a19142e2cfd9c9bbea55c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Fri, 21 Jun 2019 17:40:15 +0200 Subject: [PATCH 04/14] Obtain rotation towards target --- .../CentroidTransform.cpp | 25 +++++++++++++------ 1 file changed, 17 insertions(+), 8 deletions(-) diff --git a/programs/streamingDeviceController/CentroidTransform.cpp b/programs/streamingDeviceController/CentroidTransform.cpp index 421d2dcae..da90d20a8 100644 --- a/programs/streamingDeviceController/CentroidTransform.cpp +++ b/programs/streamingDeviceController/CentroidTransform.cpp @@ -20,8 +20,8 @@ bool CentroidTransform::processBottle(const yarp::os::Bottle & b) } // object centroids scaled to fit into [-1, 1] range - double cx = b.get(0).asFloat64(); // points right - double cy = b.get(1).asFloat64(); // points down + double cx = b.get(0).asFloat64(); // points right (same as AMOR TCP's x axis) + double cy = b.get(1).asFloat64(); // points down (same as AMOR TCP's y axis) std::vector x; @@ -33,23 +33,32 @@ bool CentroidTransform::processBottle(const yarp::os::Bottle & b) KDL::Frame H_base_tcp = KdlVectorConverter::vectorToFrame(x); - // express Z axis (TCP frame) in base frame + // express TCP's z axis in base frame KDL::Vector v_base = H_base_tcp.M.Inverse() * KDL::Vector(0, 0, 1); v_base.Normalize(); - KDL::Frame H_d = KdlVectorConverter::vectorToFrame(streamingDevice->data); + KDL::Frame H_base_target = KdlVectorConverter::vectorToFrame(streamingDevice->data); - double norm = KDL::dot(H_d.p, v_base); + double norm = KDL::dot(H_base_target.p, v_base); if (norm <= 0.0) { + // no action if we move away from the target (negative TCP's z axis) return true; } - // project desired vector into TCP's Z axis, refer result to base frame - H_d.p = v_base * norm; + // project target vector into TCP's z axis, refer result to base frame + H_base_target.p = v_base * norm; - streamingDevice->data = KdlVectorConverter::frameToVector(H_d); + // find axis along which to rotate (in TCP frame) given pixel coords + KDL::Vector coords(cx, cy, 0); + KDL::Vector tcp_axis = KDL::Rotation::RotZ(KDL::PI / 2) * coords; + KDL::Rotation rot_tcp_target = KDL::Rotation::Rot(tcp_axis, coords.Norm()); + + // rotate towards the target in base frame + H_base_target.M = H_base_tcp.M * rot_tcp_target; + + streamingDevice->data = KdlVectorConverter::frameToVector(H_base_target); return true; } From f15337a941747e7baa3d12581848a810d7522788 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Fri, 21 Jun 2019 18:06:01 +0200 Subject: [PATCH 05/14] Disable transform handler if no bottles arrived --- .../streamingDeviceController/CentroidTransform.cpp | 2 +- .../StreamingDeviceController.cpp | 10 +++++++--- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/programs/streamingDeviceController/CentroidTransform.cpp b/programs/streamingDeviceController/CentroidTransform.cpp index da90d20a8..4a196afc3 100644 --- a/programs/streamingDeviceController/CentroidTransform.cpp +++ b/programs/streamingDeviceController/CentroidTransform.cpp @@ -44,7 +44,7 @@ bool CentroidTransform::processBottle(const yarp::os::Bottle & b) if (norm <= 0.0) { // no action if we move away from the target (negative TCP's z axis) - return true; + return false; } // project target vector into TCP's z axis, refer result to base frame diff --git a/programs/streamingDeviceController/StreamingDeviceController.cpp b/programs/streamingDeviceController/StreamingDeviceController.cpp index c918eb312..a8c00ac0d 100644 --- a/programs/streamingDeviceController/StreamingDeviceController.cpp +++ b/programs/streamingDeviceController/StreamingDeviceController.cpp @@ -197,10 +197,14 @@ bool StreamingDeviceController::updateModule() return true; } - if (!centroidPort.isClosed() && !centroidTransform.processBottle(*centroidPort.read(false))) + if (!centroidPort.isClosed()) { - CD_ERROR("Failed to process incoming centroid bottle.\n"); - return true; + yarp::os::Bottle * centroidBottle = centroidPort.read(false); + + if (centroidBottle && centroidTransform.processBottle(*centroidBottle)) + { + CD_WARNING("Centroid transform handler takes control.\n"); + } } if (streamingDevice->hasValidMovementData()) From 947c0f542e09cf25a5a569077e89abad01000277 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Sat, 29 Jun 2019 12:10:37 +0200 Subject: [PATCH 06/14] Fix port connections and bottle size check --- programs/streamingDeviceController/CentroidTransform.cpp | 2 +- .../streamingDeviceController/StreamingDeviceController.cpp | 6 ++++-- .../streamingDeviceController/StreamingDeviceController.hpp | 3 ++- 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/programs/streamingDeviceController/CentroidTransform.cpp b/programs/streamingDeviceController/CentroidTransform.cpp index 4a196afc3..b1dd31618 100644 --- a/programs/streamingDeviceController/CentroidTransform.cpp +++ b/programs/streamingDeviceController/CentroidTransform.cpp @@ -13,7 +13,7 @@ CentroidTransform::CentroidTransform() bool CentroidTransform::processBottle(const yarp::os::Bottle & b) { - if (!b.size() != 2) + if (b.size() != 2) { CD_WARNING("Malformed input bottle, size %d (expected 2).\n", b.size()); return false; diff --git a/programs/streamingDeviceController/StreamingDeviceController.cpp b/programs/streamingDeviceController/StreamingDeviceController.cpp index a8c00ac0d..eea7ae8bd 100644 --- a/programs/streamingDeviceController/StreamingDeviceController.cpp +++ b/programs/streamingDeviceController/StreamingDeviceController.cpp @@ -132,15 +132,17 @@ bool StreamingDeviceController::configure(yarp::os::ResourceFinder &rf) return false; } + std::string localCentroid = rf.check("localCentroid", yarp::os::Value(DEFAULT_CENTROID_LOCAL), + "local centroid port").asString(); std::string remoteCentroid = rf.check("remoteCentroid", yarp::os::Value::getNullValue()).asString(); - if (!centroidPort.open("/sdc" + remoteCentroid + "/state:i")) + if (!centroidPort.open(localCentroid + "/state:i")) { CD_ERROR("Unable to open local centroid port.\n"); return false; } - if (!yarp::os::Network::connect(centroidPort.getName(), remoteCentroid, "udp")) + if (!yarp::os::Network::connect(remoteCentroid, centroidPort.getName(), "udp")) { CD_ERROR("Unable to connect to %s.\n", remoteCentroid.c_str()); return false; diff --git a/programs/streamingDeviceController/StreamingDeviceController.hpp b/programs/streamingDeviceController/StreamingDeviceController.hpp index c84000602..46bbd1893 100644 --- a/programs/streamingDeviceController/StreamingDeviceController.hpp +++ b/programs/streamingDeviceController/StreamingDeviceController.hpp @@ -22,10 +22,11 @@ #define DEFAULT_DEVICE_NAME "SpaceNavigator" -#define DEFAULT_CARTESIAN_LOCAL "/StreamingDeviceCartesianControlClient" +#define DEFAULT_CARTESIAN_LOCAL "/streamingDevice/cartesianControlClient" #define DEFAULT_CARTESIAN_REMOTE "/CartesianControl" #define DEFAULT_PROXIMITY_SENSORS "/sensor_reader" +#define DEFAULT_CENTROID_LOCAL "/streamingDevice/centroid" #define DEFAULT_PERIOD 0.02 // [s] #define DEFAULT_SCALING 10.0 From 9aca64fe118fbbbebddced67ff478034a1910c7e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Sat, 29 Jun 2019 12:28:04 +0200 Subject: [PATCH 07/14] Allow incomplete size of incoming bottles --- programs/streamingDeviceController/SpnavSensorDevice.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/programs/streamingDeviceController/SpnavSensorDevice.cpp b/programs/streamingDeviceController/SpnavSensorDevice.cpp index 9d4d61e47..40b6c61cc 100644 --- a/programs/streamingDeviceController/SpnavSensorDevice.cpp +++ b/programs/streamingDeviceController/SpnavSensorDevice.cpp @@ -65,7 +65,7 @@ bool roboticslab::SpnavSensorDevice::acquireData() CD_DEBUG("%s\n", data.toString(4, 1).c_str()); - if (data.size() != 6) + if (data.size() < 6) { CD_WARNING("Invalid data size: %zu.\n", data.size()); return false; From 5b495cc28853edb681bbdb38a7d108691a7b19bb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Sat, 29 Jun 2019 19:53:33 +0200 Subject: [PATCH 08/14] Fix rotation, set vector elements individually --- .../streamingDeviceController/CentroidTransform.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/programs/streamingDeviceController/CentroidTransform.cpp b/programs/streamingDeviceController/CentroidTransform.cpp index b1dd31618..2e3690565 100644 --- a/programs/streamingDeviceController/CentroidTransform.cpp +++ b/programs/streamingDeviceController/CentroidTransform.cpp @@ -34,9 +34,7 @@ bool CentroidTransform::processBottle(const yarp::os::Bottle & b) KDL::Frame H_base_tcp = KdlVectorConverter::vectorToFrame(x); // express TCP's z axis in base frame - KDL::Vector v_base = H_base_tcp.M.Inverse() * KDL::Vector(0, 0, 1); - v_base.Normalize(); - + KDL::Vector v_base = H_base_tcp.M * KDL::Vector(0, 0, 1); KDL::Frame H_base_target = KdlVectorConverter::vectorToFrame(streamingDevice->data); double norm = KDL::dot(H_base_target.p, v_base); @@ -57,8 +55,12 @@ bool CentroidTransform::processBottle(const yarp::os::Bottle & b) // rotate towards the target in base frame H_base_target.M = H_base_tcp.M * rot_tcp_target; + std::vector temp = KdlVectorConverter::frameToVector(H_base_target); - streamingDevice->data = KdlVectorConverter::frameToVector(H_base_target); + for (int i = 0; i < temp.size(); i++) + { + streamingDevice->data[i] = temp[i]; + } return true; } From 06d64364b5c97133612451c0549b7259fce54cae Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Sun, 30 Jun 2019 12:42:14 +0200 Subject: [PATCH 09/14] Store last acquired bottle until timeout --- .../CentroidTransform.cpp | 35 ++++++++++++++----- .../CentroidTransform.hpp | 15 ++++++-- .../StreamingDeviceController.cpp | 14 +++++++- 3 files changed, 52 insertions(+), 12 deletions(-) diff --git a/programs/streamingDeviceController/CentroidTransform.cpp b/programs/streamingDeviceController/CentroidTransform.cpp index 2e3690565..6591b59f3 100644 --- a/programs/streamingDeviceController/CentroidTransform.cpp +++ b/programs/streamingDeviceController/CentroidTransform.cpp @@ -1,5 +1,7 @@ #include "CentroidTransform.hpp" +#include + #include #include @@ -8,20 +10,33 @@ using namespace roboticslab; CentroidTransform::CentroidTransform() - : streamingDevice(NULL) + : streamingDevice(NULL), + permanenceTime(0.0) {} -bool CentroidTransform::processBottle(const yarp::os::Bottle & b) +bool CentroidTransform::acceptBottle(yarp::os::Bottle * b) { - if (b.size() != 2) + if (b) { - CD_WARNING("Malformed input bottle, size %d (expected 2).\n", b.size()); - return false; + if (b->size() != 2) + { + CD_WARNING("Malformed input bottle, size %d (expected 2).\n", b->size()); + return false; + } + + lastBottle = *b; + lastAcquisition.update(); + return true; } + return yarp::os::Time::now() - lastAcquisition.getTime() <= permanenceTime; +} + +bool CentroidTransform::processStoredBottle() const +{ // object centroids scaled to fit into [-1, 1] range - double cx = b.get(0).asFloat64(); // points right (same as AMOR TCP's x axis) - double cy = b.get(1).asFloat64(); // points down (same as AMOR TCP's y axis) + double cx = lastBottle.get(0).asFloat64(); // points right (same as AMOR TCP's x axis) + double cy = lastBottle.get(1).asFloat64(); // points down (same as AMOR TCP's y axis) std::vector x; @@ -51,10 +66,12 @@ bool CentroidTransform::processBottle(const yarp::os::Bottle & b) // find axis along which to rotate (in TCP frame) given pixel coords KDL::Vector coords(cx, cy, 0); KDL::Vector tcp_axis = KDL::Rotation::RotZ(KDL::PI / 2) * coords; - KDL::Rotation rot_tcp_target = KDL::Rotation::Rot(tcp_axis, coords.Norm()); + KDL::Vector base_axis = H_base_tcp.M * tcp_axis; // rotate towards the target in base frame - H_base_target.M = H_base_tcp.M * rot_tcp_target; + H_base_target.M = KDL::Rotation::Rot(base_axis, coords.Norm() * 0.1); + + // apply changes to input transform std::vector temp = KdlVectorConverter::frameToVector(H_base_target); for (int i = 0; i < temp.size(); i++) diff --git a/programs/streamingDeviceController/CentroidTransform.hpp b/programs/streamingDeviceController/CentroidTransform.hpp index b75b273fd..6ba348418 100644 --- a/programs/streamingDeviceController/CentroidTransform.hpp +++ b/programs/streamingDeviceController/CentroidTransform.hpp @@ -2,6 +2,7 @@ #define __CENTROID_TRANSFORM_HPP__ #include +#include #include "StreamingDevice.hpp" @@ -26,12 +27,22 @@ class CentroidTransform void registerStreamingDevice(StreamingDevice * streamingDevice) { this->streamingDevice = streamingDevice; } - //! Process incoming bottle - bool processBottle(const yarp::os::Bottle & b); + //! Set new permanence time + void setPermanenceTime(double permanenceTime) + { this->permanenceTime = permanenceTime; } + + //! Register or dismiss incoming bottle + bool acceptBottle(yarp::os::Bottle * b); + + //! Process last stored bottle + bool processStoredBottle() const; private: StreamingDevice * streamingDevice; + double permanenceTime; + yarp::os::Bottle lastBottle; + yarp::os::Stamp lastAcquisition; }; } // namespace roboticslab diff --git a/programs/streamingDeviceController/StreamingDeviceController.cpp b/programs/streamingDeviceController/StreamingDeviceController.cpp index eea7ae8bd..b8f9abb24 100644 --- a/programs/streamingDeviceController/StreamingDeviceController.cpp +++ b/programs/streamingDeviceController/StreamingDeviceController.cpp @@ -136,6 +136,18 @@ bool StreamingDeviceController::configure(yarp::os::ResourceFinder &rf) "local centroid port").asString(); std::string remoteCentroid = rf.check("remoteCentroid", yarp::os::Value::getNullValue()).asString(); + double permanenceTime = rf.check("centroidPermTime", yarp::os::Value(0.0)).asFloat64(); + + if (permanenceTime < 0.0) + { + CD_ERROR("Illegal argument: --centroidPermTime cannot be less than zero: %f.\n", permanenceTime); + return false; + } + else + { + centroidTransform.setPermanenceTime(permanenceTime); + } + if (!centroidPort.open(localCentroid + "/state:i")) { CD_ERROR("Unable to open local centroid port.\n"); @@ -203,7 +215,7 @@ bool StreamingDeviceController::updateModule() { yarp::os::Bottle * centroidBottle = centroidPort.read(false); - if (centroidBottle && centroidTransform.processBottle(*centroidBottle)) + if (centroidTransform.acceptBottle(centroidBottle) && centroidTransform.processStoredBottle()) { CD_WARNING("Centroid transform handler takes control.\n"); } From 186ee1243ddc453d91a235ff5824e2e000b4e681 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Sun, 30 Jun 2019 12:43:53 +0200 Subject: [PATCH 10/14] Refactor rotation factor --- programs/streamingDeviceController/CentroidTransform.cpp | 2 +- programs/streamingDeviceController/CentroidTransform.hpp | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/programs/streamingDeviceController/CentroidTransform.cpp b/programs/streamingDeviceController/CentroidTransform.cpp index 6591b59f3..0c2c5a4c8 100644 --- a/programs/streamingDeviceController/CentroidTransform.cpp +++ b/programs/streamingDeviceController/CentroidTransform.cpp @@ -69,7 +69,7 @@ bool CentroidTransform::processStoredBottle() const KDL::Vector base_axis = H_base_tcp.M * tcp_axis; // rotate towards the target in base frame - H_base_target.M = KDL::Rotation::Rot(base_axis, coords.Norm() * 0.1); + H_base_target.M = KDL::Rotation::Rot(base_axis, coords.Norm() * ROT_FACTOR); // apply changes to input transform std::vector temp = KdlVectorConverter::frameToVector(H_base_target); diff --git a/programs/streamingDeviceController/CentroidTransform.hpp b/programs/streamingDeviceController/CentroidTransform.hpp index 6ba348418..f51ae96c8 100644 --- a/programs/streamingDeviceController/CentroidTransform.hpp +++ b/programs/streamingDeviceController/CentroidTransform.hpp @@ -6,6 +6,8 @@ #include "StreamingDevice.hpp" +#define ROT_FACTOR 0.1 + namespace roboticslab { From 0b2792a6c8fd0228428cd13fbd918e8d695ba87f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Tue, 2 Jul 2019 18:32:21 +0200 Subject: [PATCH 11/14] Upload app descriptor file --- .../streamingDeviceController/CMakeLists.txt | 4 + .../scripts/streamingSharedControl.xml | 79 +++++++++++++++++++ 2 files changed, 83 insertions(+) create mode 100644 share/streamingDeviceController/scripts/streamingSharedControl.xml diff --git a/share/streamingDeviceController/CMakeLists.txt b/share/streamingDeviceController/CMakeLists.txt index 91c0df503..9936b495c 100644 --- a/share/streamingDeviceController/CMakeLists.txt +++ b/share/streamingDeviceController/CMakeLists.txt @@ -1,2 +1,6 @@ yarp_install(FILES conf/streamingDeviceController.ini DESTINATION ${ROBOTICSLAB-KINEMATICS-DYNAMICS_CONTEXTS_INSTALL_DIR}/streamingDeviceController) + +yarp_install(FILES scripts/streamingSharedControl.xml + DESTINATION ${ROBOTICSLAB-KINEMATICS-DYNAMICS_APPLICATIONS_INSTALL_DIR}/streamingDeviceController) + \ No newline at end of file diff --git a/share/streamingDeviceController/scripts/streamingSharedControl.xml b/share/streamingDeviceController/scripts/streamingSharedControl.xml new file mode 100644 index 000000000..01881efc9 --- /dev/null +++ b/share/streamingDeviceController/scripts/streamingSharedControl.xml @@ -0,0 +1,79 @@ + + haarDetection2D shared control app + + + opencv_grabber + --name /frameGrabber2D --camera 1 + localhost + yarpdev + + + + haarDetection2D + --xmlCascade haarcascade_cocacola_can.xml + localhost + + + + haarDetection2D + --xmlCascade haarcascade_frontalface_alt.xml + localhost + + + + yarpview + --name /haarDetection2D/yarpview/img:i --compact + localhost + + + + amorSim + localhost + + + + AmorCartesianControl + --name /amor/CartesianControl --kinematics /usr/local/share/amor/contexts/kinematics/armKinematics.ini + localhost + yarpdev + + + + BasicCartesianControl + --name /amorSim/CartesianControl --from /usr/local/share/amor/contexts/kinematics/armKinematics.ini --local /BasicCartesianControl/amorSim --remote /amorRobot/amorManipulator + localhost + yarpdev + + + + SpaceNavigator + --period 5 --name /spacenavigator --channels 8 + localhost + yarpdev + + + + streamingDeviceController + --streamingDevice SpaceNavigator --remoteCartesian /amor/CartesianControl --SpaceNavigator::remote /spacenavigator + + + + streamingDeviceController + --streamingDevice SpaceNavigator --remoteCartesian /amorSim/CartesianControl --SpaceNavigator::remote /spacenavigator + + + + streamingDeviceController + --streamingDevice SpaceNavigator --remoteCartesian /amor/CartesianControl --SpaceNavigator::remote /spacenavigator --remoteCentroid /haarDetection2D/state:o --centroidPermTime 1.0 + + + + streamingDeviceController + --streamingDevice SpaceNavigator --remoteCartesian /amorSim/CartesianControl --SpaceNavigator::remote /spacenavigator --remoteCentroid /haarDetection2D/state:o --centroidPermTime 1.0 + + + + /haarDetection2D/img:o + /haarDetection2D/yarpview/img:i + + From fd55e83b0eefaffe02b22a99ec572b2a7209830b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Wed, 3 Jul 2019 18:51:29 +0200 Subject: [PATCH 12/14] Enforce fixed size acquisition data vectors --- .../streamingDeviceController/CentroidTransform.cpp | 7 +------ .../LeapMotionSensorDevice.cpp | 2 +- .../streamingDeviceController/SpnavSensorDevice.cpp | 10 +++++----- programs/streamingDeviceController/StreamingDevice.cpp | 4 ++-- 4 files changed, 9 insertions(+), 14 deletions(-) diff --git a/programs/streamingDeviceController/CentroidTransform.cpp b/programs/streamingDeviceController/CentroidTransform.cpp index 0c2c5a4c8..36daf1686 100644 --- a/programs/streamingDeviceController/CentroidTransform.cpp +++ b/programs/streamingDeviceController/CentroidTransform.cpp @@ -72,12 +72,7 @@ bool CentroidTransform::processStoredBottle() const H_base_target.M = KDL::Rotation::Rot(base_axis, coords.Norm() * ROT_FACTOR); // apply changes to input transform - std::vector temp = KdlVectorConverter::frameToVector(H_base_target); - - for (int i = 0; i < temp.size(); i++) - { - streamingDevice->data[i] = temp[i]; - } + streamingDevice->data = KdlVectorConverter::frameToVector(H_base_target); return true; } diff --git a/programs/streamingDeviceController/LeapMotionSensorDevice.cpp b/programs/streamingDeviceController/LeapMotionSensorDevice.cpp index f06d3de7a..f24028a10 100644 --- a/programs/streamingDeviceController/LeapMotionSensorDevice.cpp +++ b/programs/streamingDeviceController/LeapMotionSensorDevice.cpp @@ -117,7 +117,7 @@ bool roboticslab::LeapMotionSensorDevice::acquireData() CD_DEBUG("%s\n", data.toString(4, 1).c_str()); - if (data.size() < 6) + if (data.size() != 6 && data.size() != 8) { CD_WARNING("Invalid data size: %zu.\n", data.size()); return false; diff --git a/programs/streamingDeviceController/SpnavSensorDevice.cpp b/programs/streamingDeviceController/SpnavSensorDevice.cpp index 40b6c61cc..6e9e3fe6d 100644 --- a/programs/streamingDeviceController/SpnavSensorDevice.cpp +++ b/programs/streamingDeviceController/SpnavSensorDevice.cpp @@ -65,13 +65,13 @@ bool roboticslab::SpnavSensorDevice::acquireData() CD_DEBUG("%s\n", data.toString(4, 1).c_str()); - if (data.size() < 6) + if (data.size() != 6 && data.size() != 8) { CD_WARNING("Invalid data size: %zu.\n", data.size()); return false; } - for (int i = 0; i < data.size(); i++) + for (int i = 0; i < 6; i++) { this->data[i] = data[i]; } @@ -83,7 +83,7 @@ bool roboticslab::SpnavSensorDevice::transformData(double scaling) { if (usingMovi) { - for (int i = 0; i < data.size(); i++) + for (int i = 0; i < 6; i++) { if (!fixedAxes[i]) { @@ -107,7 +107,7 @@ bool roboticslab::SpnavSensorDevice::hasValidMovementData() const { if (usingMovi) { - for (int i = 0; i < data.size(); i++) + for (int i = 0; i < 6; i++) { if (!fixedAxes[i] && data[i] != currentX[i]) { @@ -129,7 +129,7 @@ void roboticslab::SpnavSensorDevice::sendMovementCommand() { iCartesianControl->movi(data); - for (int i = 0; i < data.size(); i++) + for (int i = 0; i < 6; i++) { currentX[i] = data[i]; } diff --git a/programs/streamingDeviceController/StreamingDevice.cpp b/programs/streamingDeviceController/StreamingDevice.cpp index 7c1759397..65faf7118 100644 --- a/programs/streamingDeviceController/StreamingDevice.cpp +++ b/programs/streamingDeviceController/StreamingDevice.cpp @@ -57,7 +57,7 @@ StreamingDevice::~StreamingDevice() bool StreamingDevice::transformData(double scaling) { - for (int i = 0; i < data.size(); i++) + for (int i = 0; i < 6; i++) { if (!fixedAxes[i]) { @@ -74,7 +74,7 @@ bool StreamingDevice::transformData(double scaling) bool StreamingDevice::hasValidMovementData() const { - for (int i = 0; i < data.size(); i++) + for (int i = 0; i < 6; i++) { if (data[i] != 0.0) { From 391e9c85991365833e29cc1a93e1dd854a6bbaf3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Wed, 3 Jul 2019 18:58:42 +0200 Subject: [PATCH 13/14] Conditionally handle button states --- .../SpnavSensorDevice.cpp | 17 +++++++++++------ .../SpnavSensorDevice.hpp | 2 ++ 2 files changed, 13 insertions(+), 6 deletions(-) diff --git a/programs/streamingDeviceController/SpnavSensorDevice.cpp b/programs/streamingDeviceController/SpnavSensorDevice.cpp index b4d588227..a3f1d8d12 100644 --- a/programs/streamingDeviceController/SpnavSensorDevice.cpp +++ b/programs/streamingDeviceController/SpnavSensorDevice.cpp @@ -8,7 +8,9 @@ roboticslab::SpnavSensorDevice::SpnavSensorDevice(yarp::os::Searchable & config, : StreamingDevice(config), iAnalogSensor(NULL), usingMovi(usingMovi), - gain(gain) + gain(gain), + buttonClose(false), + buttonOpen(false) {} bool roboticslab::SpnavSensorDevice::acquireInterfaces() @@ -76,6 +78,12 @@ bool roboticslab::SpnavSensorDevice::acquireData() this->data[i] = data[i]; } + if (data.size() == 8) + { + buttonClose = data[6] == 1; + buttonOpen = data[7] == 1; + } + return true; } @@ -105,14 +113,11 @@ bool roboticslab::SpnavSensorDevice::transformData(double scaling) int roboticslab::SpnavSensorDevice::getActuatorState() { - int button1 = data[6]; - int button2 = data[7]; - - if (button1 == 1) + if (buttonClose) { actuatorState = VOCAB_CC_ACTUATOR_CLOSE_GRIPPER; } - else if (button2 == 1) + else if (buttonOpen) { actuatorState = VOCAB_CC_ACTUATOR_OPEN_GRIPPER; } diff --git a/programs/streamingDeviceController/SpnavSensorDevice.hpp b/programs/streamingDeviceController/SpnavSensorDevice.hpp index 4a1d435f6..46f761bd6 100644 --- a/programs/streamingDeviceController/SpnavSensorDevice.hpp +++ b/programs/streamingDeviceController/SpnavSensorDevice.hpp @@ -45,6 +45,8 @@ class SpnavSensorDevice : public StreamingDevice bool usingMovi; double gain; + bool buttonClose; + bool buttonOpen; }; } // namespace roboticslab From b5f8a8bb317365cb8308b3212ed15d1b7766091c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Wed, 3 Jul 2019 23:20:25 +0200 Subject: [PATCH 14/14] Generalize tcp2camera transform for generic robots --- .../CentroidTransform.cpp | 31 ++++++++++++++++--- .../CentroidTransform.hpp | 6 ++++ .../StreamingDeviceController.cpp | 8 +++++ 3 files changed, 40 insertions(+), 5 deletions(-) diff --git a/programs/streamingDeviceController/CentroidTransform.cpp b/programs/streamingDeviceController/CentroidTransform.cpp index 36daf1686..0494d37a3 100644 --- a/programs/streamingDeviceController/CentroidTransform.cpp +++ b/programs/streamingDeviceController/CentroidTransform.cpp @@ -1,5 +1,7 @@ #include "CentroidTransform.hpp" +#include + #include #include @@ -14,6 +16,25 @@ CentroidTransform::CentroidTransform() permanenceTime(0.0) {} +bool CentroidTransform::setTcpToCameraRotation(yarp::os::Bottle * b) +{ + if (b->size() != 3) + { + CD_WARNING("Bottle size must equal 3, was: %d.\n", b->size()); + return false; + } + + double roll = b->get(0).asFloat64() * M_PI / 180.0; + double pitch = b->get(1).asFloat64() * M_PI / 180.0; + double yaw = b->get(2).asFloat64() * M_PI / 180.0; + + CD_INFO("centroidFrameRPY [rad]: %f %f %f\n", roll, pitch, yaw); + + rot_tcp_camera = KDL::Rotation::RPY(roll, pitch, yaw); + + return true; +} + bool CentroidTransform::acceptBottle(yarp::os::Bottle * b) { if (b) @@ -35,8 +56,8 @@ bool CentroidTransform::acceptBottle(yarp::os::Bottle * b) bool CentroidTransform::processStoredBottle() const { // object centroids scaled to fit into [-1, 1] range - double cx = lastBottle.get(0).asFloat64(); // points right (same as AMOR TCP's x axis) - double cy = lastBottle.get(1).asFloat64(); // points down (same as AMOR TCP's y axis) + double cx = lastBottle.get(0).asFloat64(); // points right + double cy = lastBottle.get(1).asFloat64(); // points down std::vector x; @@ -48,8 +69,8 @@ bool CentroidTransform::processStoredBottle() const KDL::Frame H_base_tcp = KdlVectorConverter::vectorToFrame(x); - // express TCP's z axis in base frame - KDL::Vector v_base = H_base_tcp.M * KDL::Vector(0, 0, 1); + // express camera's z axis (points "forward") in base frame + KDL::Vector v_base = H_base_tcp.M * rot_tcp_camera * KDL::Vector(0, 0, 1); KDL::Frame H_base_target = KdlVectorConverter::vectorToFrame(streamingDevice->data); double norm = KDL::dot(H_base_target.p, v_base); @@ -66,7 +87,7 @@ bool CentroidTransform::processStoredBottle() const // find axis along which to rotate (in TCP frame) given pixel coords KDL::Vector coords(cx, cy, 0); KDL::Vector tcp_axis = KDL::Rotation::RotZ(KDL::PI / 2) * coords; - KDL::Vector base_axis = H_base_tcp.M * tcp_axis; + KDL::Vector base_axis = H_base_tcp.M * rot_tcp_camera * tcp_axis; // rotate towards the target in base frame H_base_target.M = KDL::Rotation::Rot(base_axis, coords.Norm() * ROT_FACTOR); diff --git a/programs/streamingDeviceController/CentroidTransform.hpp b/programs/streamingDeviceController/CentroidTransform.hpp index f51ae96c8..64316dcbd 100644 --- a/programs/streamingDeviceController/CentroidTransform.hpp +++ b/programs/streamingDeviceController/CentroidTransform.hpp @@ -4,6 +4,8 @@ #include #include +#include + #include "StreamingDevice.hpp" #define ROT_FACTOR 0.1 @@ -29,6 +31,9 @@ class CentroidTransform void registerStreamingDevice(StreamingDevice * streamingDevice) { this->streamingDevice = streamingDevice; } + //! Set TCP to camera frame + bool setTcpToCameraRotation(yarp::os::Bottle * b); + //! Set new permanence time void setPermanenceTime(double permanenceTime) { this->permanenceTime = permanenceTime; } @@ -45,6 +50,7 @@ class CentroidTransform double permanenceTime; yarp::os::Bottle lastBottle; yarp::os::Stamp lastAcquisition; + KDL::Rotation rot_tcp_camera; }; } // namespace roboticslab diff --git a/programs/streamingDeviceController/StreamingDeviceController.cpp b/programs/streamingDeviceController/StreamingDeviceController.cpp index b8f9abb24..e28e63883 100644 --- a/programs/streamingDeviceController/StreamingDeviceController.cpp +++ b/programs/streamingDeviceController/StreamingDeviceController.cpp @@ -136,8 +136,16 @@ bool StreamingDeviceController::configure(yarp::os::ResourceFinder &rf) "local centroid port").asString(); std::string remoteCentroid = rf.check("remoteCentroid", yarp::os::Value::getNullValue()).asString(); + yarp::os::Value vCentroidRPY = rf.check("centroidRPY", yarp::os::Value::getNullValue()); + double permanenceTime = rf.check("centroidPermTime", yarp::os::Value(0.0)).asFloat64(); + if (!vCentroidRPY.isNull() && vCentroidRPY.isList() && !centroidTransform.setTcpToCameraRotation(vCentroidRPY.asList())) + { + CD_ERROR("Illegal argument: malformed bottle --centroidRPY: %s.\n", vCentroidRPY.toString().c_str()); + return false; + } + if (permanenceTime < 0.0) { CD_ERROR("Illegal argument: --centroidPermTime cannot be less than zero: %f.\n", permanenceTime);