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..0494d37a3 --- /dev/null +++ b/programs/streamingDeviceController/CentroidTransform.cpp @@ -0,0 +1,99 @@ +#include "CentroidTransform.hpp" + +#include + +#include + +#include + +#include +#include + +using namespace roboticslab; + +CentroidTransform::CentroidTransform() + : streamingDevice(NULL), + 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) + { + 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 = lastBottle.get(0).asFloat64(); // points right + double cy = lastBottle.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 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); + + if (norm <= 0.0) + { + // no action if we move away from the target (negative TCP's z axis) + return false; + } + + // project target vector into TCP's z axis, refer result to base frame + H_base_target.p = v_base * norm; + + // 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 * 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); + + // apply changes to input transform + streamingDevice->data = KdlVectorConverter::frameToVector(H_base_target); + + return true; +} diff --git a/programs/streamingDeviceController/CentroidTransform.hpp b/programs/streamingDeviceController/CentroidTransform.hpp new file mode 100644 index 000000000..64316dcbd --- /dev/null +++ b/programs/streamingDeviceController/CentroidTransform.hpp @@ -0,0 +1,58 @@ +#ifndef __CENTROID_TRANSFORM_HPP__ +#define __CENTROID_TRANSFORM_HPP__ + +#include +#include + +#include + +#include "StreamingDevice.hpp" + +#define ROT_FACTOR 0.1 + +namespace roboticslab +{ + +class StreamingDevice; + +/** + * @ingroup streamingDeviceController + * + * @brief ... + */ +class CentroidTransform +{ +public: + + //! Constructor + CentroidTransform(); + + //! Register handle to device + 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; } + + //! 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; + KDL::Rotation rot_tcp_camera; +}; + +} // namespace roboticslab + +#endif // __CENTROID_TRANSFORM_HPP__ diff --git a/programs/streamingDeviceController/LeapMotionSensorDevice.cpp b/programs/streamingDeviceController/LeapMotionSensorDevice.cpp index f32b1e334..fb5f2a9b7 100644 --- a/programs/streamingDeviceController/LeapMotionSensorDevice.cpp +++ b/programs/streamingDeviceController/LeapMotionSensorDevice.cpp @@ -119,7 +119,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 5a908e732..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() @@ -65,17 +67,23 @@ bool roboticslab::SpnavSensorDevice::acquireData() CD_DEBUG("%s\n", data.toString(4, 1).c_str()); - if (data.size() != 8) + 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]; } + if (data.size() == 8) + { + buttonClose = data[6] == 1; + buttonOpen = data[7] == 1; + } + return true; } @@ -83,7 +91,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]) { @@ -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; } @@ -139,7 +144,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]) { @@ -161,7 +166,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/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 diff --git a/programs/streamingDeviceController/StreamingDevice.cpp b/programs/streamingDeviceController/StreamingDevice.cpp index 1037827f9..c9c772ebc 100644 --- a/programs/streamingDeviceController/StreamingDevice.cpp +++ b/programs/streamingDeviceController/StreamingDevice.cpp @@ -58,7 +58,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]) { @@ -80,7 +80,7 @@ bool StreamingDevice::hasValidMovementData() const return false; } - for (int i = 0; i < data.size(); i++) + for (int i = 0; i < 6; i++) { if (data[i] != 0.0) { diff --git a/programs/streamingDeviceController/StreamingDevice.hpp b/programs/streamingDeviceController/StreamingDevice.hpp index 48c5ca089..6ebbf32fe 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 0f0f36b99..f217acbcc 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 @@ -105,7 +108,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 +122,55 @@ bool StreamingDeviceController::configure(yarp::os::ResourceFinder &rf) } #endif // SDC_WITH_SENSORS + 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 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(); + + 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); + return false; + } + else + { + centroidTransform.setPermanenceTime(permanenceTime); + } + + if (!centroidPort.open(localCentroid + "/state:i")) + { + CD_ERROR("Unable to open local centroid port.\n"); + return false; + } + + if (!yarp::os::Network::connect(remoteCentroid, centroidPort.getName(), "udp")) + { + CD_ERROR("Unable to connect to %s.\n", remoteCentroid.c_str()); + return false; + } + + centroidTransform.registerStreamingDevice(streamingDevice); + } + isStopped = true; return true; @@ -161,17 +213,27 @@ bool StreamingDeviceController::updateModule() } #endif // SDC_WITH_SENSORS + int actuatorState = streamingDevice->getActuatorState(); + + if (actuatorState != VOCAB_CC_ACTUATOR_NONE) + { + iCartesianControl->act(actuatorState); + } + if (!streamingDevice->transformData(localScaling)) { CD_ERROR("Failed to transform acquired data from streaming device.\n"); return true; } - int actuatorState = streamingDevice->getActuatorState(); - - if (actuatorState != VOCAB_CC_ACTUATOR_NONE) + if (!centroidPort.isClosed()) { - iCartesianControl->act(actuatorState); + yarp::os::Bottle * centroidBottle = centroidPort.read(false); + + if (centroidTransform.acceptBottle(centroidBottle) && centroidTransform.processStoredBottle()) + { + CD_WARNING("Centroid transform handler takes control.\n"); + } } if (streamingDevice->hasValidMovementData()) @@ -208,6 +270,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 7e1740e06..a9c94cb03 100644 --- a/programs/streamingDeviceController/StreamingDeviceController.hpp +++ b/programs/streamingDeviceController/StreamingDeviceController.hpp @@ -3,11 +3,15 @@ #include -#include +#include +#include #include +#include + #include #include "StreamingDevice.hpp" +#include "CentroidTransform.hpp" #include "ICartesianControl.h" @@ -17,10 +21,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 @@ -57,6 +62,9 @@ class StreamingDeviceController : public yarp::os::RFModule static const double SCALING_FACTOR_ON_ALERT; #endif // SDC_WITH_SENSORS + yarp::os::BufferedPort centroidPort; + CentroidTransform centroidTransform; + double period; double scaling; 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 + +