diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..3c62bf6 --- /dev/null +++ b/.clang-format @@ -0,0 +1,105 @@ +--- +Language: Cpp +# BasedOnStyle: WebKit +AccessModifierOffset: -4 +AlignAfterOpenBracket: Align +AlignConsecutiveAssignments: false +AlignConsecutiveDeclarations: false +AlignEscapedNewlines: Left +AlignOperands: true +AlignTrailingComments: false +AllowAllParametersOfDeclarationOnNextLine: true +AllowShortBlocksOnASingleLine: false +AllowShortCaseLabelsOnASingleLine: false +AllowShortFunctionsOnASingleLine: None +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakAfterReturnType: None +AlwaysBreakBeforeMultilineStrings: false +AlwaysBreakTemplateDeclarations: false +BinPackArguments: false +BinPackParameters: false +BraceWrapping: + AfterClass: true + AfterControlStatement: true + AfterEnum: true + AfterFunction: true + AfterNamespace: true + AfterObjCDeclaration: false + AfterStruct: true + AfterUnion: true + BeforeCatch: false + BeforeElse: false + IndentBraces: false + SplitEmptyFunction: true + SplitEmptyRecord: true + SplitEmptyNamespace: true +BreakBeforeBinaryOperators: All +BreakBeforeBraces: Custom +BreakBeforeInheritanceComma: false +BreakBeforeTernaryOperators: true +BreakConstructorInitializersBeforeComma: true +BreakConstructorInitializers: BeforeComma +BreakAfterJavaFieldAnnotations: false +BreakStringLiterals: true +ColumnLimit: 100 +CommentPragmas: '^ IWYU pragma:' +CompactNamespaces: false +ConstructorInitializerAllOnOneLineOrOnePerLine: false +ConstructorInitializerIndentWidth: 4 +ContinuationIndentWidth: 4 +Cpp11BracedListStyle: true +DerivePointerAlignment: false +DisableFormat: false +ExperimentalAutoDetectBinPacking: false +FixNamespaceComments: true +ForEachMacros: + - foreach + - Q_FOREACH + - BOOST_FOREACH +IncludeCategories: + - Regex: '^"(llvm|llvm-c|clang|clang-c)/' + Priority: 2 + - Regex: '^(<|"(gtest|gmock|isl|json)/)' + Priority: 3 + - Regex: '.*' + Priority: 1 +IncludeIsMainRegex: '(Test)?$' +IndentCaseLabels: false +IndentWidth: 4 +IndentWrappedFunctionNames: false +JavaScriptQuotes: Leave +JavaScriptWrapImports: true +KeepEmptyLinesAtTheStartOfBlocks: true +MacroBlockBegin: '' +MacroBlockEnd: '' +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCBlockIndentWidth: 4 +ObjCSpaceAfterProperty: true +ObjCSpaceBeforeProtocolList: true +PenaltyBreakAssignment: 10 +PenaltyBreakBeforeFirstCallParameter: 1000 +PenaltyBreakComment: 10 +PenaltyBreakString: 10 +PenaltyExcessCharacter: 100 +PenaltyReturnTypeOnItsOwnLine: 5 +PointerAlignment: Left +ReflowComments: true +SortIncludes: true +SortUsingDeclarations: true +SpaceAfterCStyleCast: false +SpaceAfterTemplateKeyword: true +SpaceBeforeAssignmentOperators: true +SpaceBeforeParens: ControlStatements +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 1 +SpacesInAngles: false +SpacesInContainerLiterals: true +SpacesInCStyleCastParentheses: false +SpacesInParentheses: false +SpacesInSquareBrackets: false +Standard: Cpp11 +TabWidth: 4 +UseTab: Never diff --git a/.git-blame-ignore-revs b/.git-blame-ignore-revs new file mode 100644 index 0000000..26ffc12 --- /dev/null +++ b/.git-blame-ignore-revs @@ -0,0 +1,3 @@ +# .git-blame-ignore-revs +# Refactored all source code files with clang-format +c053fb04719221a796132bfc73b3d09bb4e5bab2 \ No newline at end of file diff --git a/libraries/singleton-devices/Handler.cc b/libraries/singleton-devices/Handler.cc index 79e5a82..5f6b826 100644 --- a/libraries/singleton-devices/Handler.cc +++ b/libraries/singleton-devices/Handler.cc @@ -2,25 +2,26 @@ #include #include -namespace gzyarp +namespace gzyarp { Handler* Handler::getHandler() { std::lock_guard lock(mutex()); - if (!s_handle) + if (!s_handle) { s_handle = new Handler(); if (!s_handle) yError() << "Error while calling gzyarp::Handler constructor"; } - + return s_handle; } - -bool Handler::getDevicesAsPolyDriverList(const std::string& modelScopedName, yarp::dev::PolyDriverList& list, - std::vector& deviceScopedNames/*, const std::string& worldName*/) +bool Handler::getDevicesAsPolyDriverList( + const std::string& modelScopedName, + yarp::dev::PolyDriverList& list, + std::vector& deviceScopedNames /*, const std::string& worldName*/) { deviceScopedNames.resize(0); @@ -30,34 +31,35 @@ bool Handler::getDevicesAsPolyDriverList(const std::string& modelScopedName, yar // to the returned yarp::dev::PolyDriverList std::unordered_map inserted_yarpDeviceName2deviceDatabaseKey; - for (auto&& devicesMapElem: m_devicesMap) + for (auto&& devicesMapElem : m_devicesMap) { std::string deviceDatabaseKey = devicesMapElem.first; std::string yarpDeviceName; - - // If the deviceDatabaseKey starts with the modelScopedName (device spawned by model plugins), - // then it is eligible for insertion in the returned list - if ((deviceDatabaseKey.rfind(modelScopedName, 0) == 0) /*|| (deviceDatabaseKey.rfind(worldName + "/" + modelScopedName, 0) == 0)*/) + + // If the deviceDatabaseKey starts with the modelScopedName (device spawned by model + // plugins), then it is eligible for insertion in the returned list + if ((deviceDatabaseKey.rfind(modelScopedName, 0) + == 0) /*|| (deviceDatabaseKey.rfind(worldName + "/" + modelScopedName, 0) == 0)*/) { // Extract yarpDeviceName from deviceDatabaseKey - yarpDeviceName = deviceDatabaseKey.substr(deviceDatabaseKey.find_last_of("/")+1); + yarpDeviceName = deviceDatabaseKey.substr(deviceDatabaseKey.find_last_of("/") + 1); // Check if a device with the same yarpDeviceName was already inserted auto got = inserted_yarpDeviceName2deviceDatabaseKey.find(yarpDeviceName); // If not found, insert and continue - if (got == inserted_yarpDeviceName2deviceDatabaseKey.end()) + if (got == inserted_yarpDeviceName2deviceDatabaseKey.end()) { // If no name collision is found, insert and continue - inserted_yarpDeviceName2deviceDatabaseKey.insert({yarpDeviceName, deviceDatabaseKey}); + inserted_yarpDeviceName2deviceDatabaseKey.insert( + {yarpDeviceName, deviceDatabaseKey}); list.push(devicesMapElem.second, yarpDeviceName.c_str()); deviceScopedNames.push_back(deviceDatabaseKey); - } - else + } else { // If a name collision is found, print a clear error and return yError() << "gzyarp::Handler robotinterface getDevicesAsPolyDriverList error: "; - yError() << "two YARP devices with yarpDeviceName " << yarpDeviceName + yError() << "two YARP devices with yarpDeviceName " << yarpDeviceName << " found in model " << modelScopedName; yError() << "First instance: " << got->second; yError() << "Second instance: " << deviceDatabaseKey; @@ -75,26 +77,29 @@ bool Handler::setDevice(std::string deviceDatabaseKey, yarp::dev::PolyDriver* de { bool ret = false; DevicesMap::iterator device = m_devicesMap.find(deviceDatabaseKey); - if (device != m_devicesMap.end()) + if (device != m_devicesMap.end()) { - if(device->second == device2add) + if (device->second == device2add) ret = true; else { yError() << " Error in gzyarp::Handler while inserting a new yarp device pointer!"; - yError() << " The name of the device is already present but the pointer does not match with the one already registered!"; - yError() << " This should not happen, check the names are correct in your config file. Fatal error."; + yError() << " The name of the device is already present but the pointer does not match " + "with the one already registered!"; + yError() << " This should not happen, check the names are correct in your config file. " + "Fatal error."; } - } - else + } else { - //device does not exists. Add to map - if (!m_devicesMap.insert(std::pair(deviceDatabaseKey, device2add)).second) + // device does not exists. Add to map + if (!m_devicesMap + .insert( + std::pair(deviceDatabaseKey, device2add)) + .second) { yError() << " Error in gzyarp::Handler while inserting a new device pointer!"; ret = false; - } - else + } else ret = true; } return ret; @@ -105,43 +110,40 @@ yarp::dev::PolyDriver* Handler::getDevice(const std::string& deviceDatabaseKey) yarp::dev::PolyDriver* tmp = NULL; DevicesMap::const_iterator device = m_devicesMap.find(deviceDatabaseKey); - if (device != m_devicesMap.end()) + if (device != m_devicesMap.end()) tmp = device->second; - else + else tmp = NULL; - + return tmp; } -void Handler::removeDevice(const std::string &deviceDatabaseKey) +void Handler::removeDevice(const std::string& deviceDatabaseKey) { DevicesMap::iterator device = m_devicesMap.find(deviceDatabaseKey); - if (device != m_devicesMap.end()) - { + if (device != m_devicesMap.end()) + { device->second->close(); m_devicesMap.erase(device); - } - else + } else { yError() << "Could not remove device " << deviceDatabaseKey << ". Device was not found"; } return; } -Handler::Handler() : m_devicesMap() +Handler::Handler() + : m_devicesMap() { m_devicesMap.clear(); } Handler* Handler::s_handle = NULL; - std::mutex& Handler::mutex() { static std::mutex s_mutex; return s_mutex; } -} - - +} // namespace gzyarp diff --git a/libraries/singleton-devices/Handler.hh b/libraries/singleton-devices/Handler.hh index 8fdc503..b683fab 100644 --- a/libraries/singleton-devices/Handler.hh +++ b/libraries/singleton-devices/Handler.hh @@ -1,34 +1,33 @@ +#include #include #include #include -#include -namespace gzyarp +namespace gzyarp { - + class Handler -{ - public: - static Handler* getHandler(); - - bool getDevicesAsPolyDriverList(const std::string& modelScopedName, yarp::dev::PolyDriverList& list, - std::vector& deviceScopedNames/*, const std::string& worldName*/); - - bool setDevice(std::string deviceDatabaseKey, yarp::dev::PolyDriver* device2add); - - yarp::dev::PolyDriver* getDevice(const std::string& deviceDatabaseKey) const; - - void removeDevice(const std::string& deviceDatabaseKey); - - private: - Handler(); - static Handler* s_handle; - static std::mutex& mutex(); - typedef std::map DevicesMap; - DevicesMap m_devicesMap; // map of known yarp devices +{ +public: + static Handler* getHandler(); -}; + bool getDevicesAsPolyDriverList( + const std::string& modelScopedName, + yarp::dev::PolyDriverList& list, + std::vector& deviceScopedNames /*, const std::string& worldName*/); + + bool setDevice(std::string deviceDatabaseKey, yarp::dev::PolyDriver* device2add); -} + yarp::dev::PolyDriver* getDevice(const std::string& deviceDatabaseKey) const; + void removeDevice(const std::string& deviceDatabaseKey); + +private: + Handler(); + static Handler* s_handle; + static std::mutex& mutex(); + typedef std::map DevicesMap; + DevicesMap m_devicesMap; // map of known yarp devices +}; +} // namespace gzyarp diff --git a/plugins/camera/Camera.cc b/plugins/camera/Camera.cc index d082e98..5fa29ad 100644 --- a/plugins/camera/Camera.cc +++ b/plugins/camera/Camera.cc @@ -1,50 +1,48 @@ -#include -#include -#include +#include "CameraDriver.cpp" #include -#include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include #include -#include -#include #include -#include "CameraDriver.cpp" - +#include +#include +#include +#include +#include +#include +#include using namespace gz; using namespace sim; using namespace systems; -namespace gzyarp +namespace gzyarp { -class Camera - : public System, - public ISystemConfigure, - public ISystemPreUpdate, - public ISystemPostUpdate - +class Camera : public System, + public ISystemConfigure, + public ISystemPreUpdate, + public ISystemPostUpdate + { - public: - - Camera() : m_deviceRegistered(false) +public: + Camera() + : m_deviceRegistered(false) { } - + virtual ~Camera() { - if (m_deviceRegistered) + if (m_deviceRegistered) { Handler::getHandler()->removeDevice(m_deviceScopedName); m_deviceRegistered = false; } - - if(m_cameraDriver.isValid()) + + if (m_cameraDriver.isValid()) { m_cameraDriver.close(); } @@ -52,11 +50,11 @@ class Camera yarp::os::Network::fini(); } - virtual void Configure(const Entity &_entity, - const std::shared_ptr &_sdf, - EntityComponentManager &_ecm, - EventManager &/*_eventMgr*/) override - { + virtual void Configure(const Entity& _entity, + const std::shared_ptr& _sdf, + EntityComponentManager& _ecm, + EventManager& /*_eventMgr*/) override + { yarp::os::Network::init(); if (!yarp::os::Network::checkNetwork()) { @@ -64,8 +62,10 @@ class Camera return; } - ::yarp::dev::Drivers::factory().add(new ::yarp::dev::DriverCreatorOf< ::yarp::dev::gzyarp::CameraDriver> - ("gazebo_camera", "grabber", "CameraDriver")); + ::yarp::dev::Drivers::factory().add( + new ::yarp::dev::DriverCreatorOf<::yarp::dev::gzyarp::CameraDriver>("gazebo_camera", + "grabber", + "CameraDriver")); ::yarp::os::Property driver_properties; bool wipe = false; @@ -83,30 +83,29 @@ class Camera yError() << "gz-sim-yarp-camera-system : missing parentLinkName parameter"; return; } - yInfo() << "gz-sim-yarp-camera-system: configuration of sensor " << driver_properties.find("sensorName").asString() + yInfo() << "gz-sim-yarp-camera-system: configuration of sensor " + << driver_properties.find("sensorName").asString() << " loaded from yarpConfigurationString : " << configuration_string << "\n"; - } - else + } else { yError() << "gz-sim-yarp-camera-system : missing yarpConfigurationString element"; - return; + return; } std::string sensorName = driver_properties.find("sensorName").asString(); std::string parentLinkName = driver_properties.find("parentLinkName").asString(); - + auto model = Model(_entity); auto parentLink = model.LinkByName(_ecm, parentLinkName); - this->sensor = _ecm.EntityByComponents( - components::ParentEntity(parentLink), - components::Name(sensorName), - components::Sensor()); + this->sensor = _ecm.EntityByComponents(components::ParentEntity(parentLink), + components::Name(sensorName), + components::Sensor()); auto sdfSensor = _ecm.ComponentData(sensor).value().Element(); auto sdfImage = sdfSensor.get()->GetElement("camera").get()->GetElement("image").get(); cameraData.m_height = sdfImage->Get("height"); cameraData.m_width = sdfImage->Get("width"); - cameraData.m_bufferSize = 3*cameraData.m_width*cameraData.m_height; + cameraData.m_bufferSize = 3 * cameraData.m_width * cameraData.m_height; sensorScopedName = scopedName(this->sensor, _ecm); this->cameraData.sensorScopedName = sensorScopedName; @@ -114,47 +113,50 @@ class Camera driver_properties.put(YarpCameraScopedName.c_str(), sensorScopedName.c_str()); if (!driver_properties.check("yarpDeviceName")) { - yError() << "gz-sim-yarp-camera-system : missing yarpDeviceName parameter for device" << sensorScopedName; + yError() << "gz-sim-yarp-camera-system : missing yarpDeviceName parameter for device" + << sensorScopedName; return; } - //Insert the pointer in the singleton handler for retriving it in the yarp driver + // Insert the pointer in the singleton handler for retriving it in the yarp driver HandlerCamera::getHandler()->setSensor(&(this->cameraData)); - driver_properties.put("device","gazebo_camera"); + driver_properties.put("device", "gazebo_camera"); driver_properties.put("sensor_name", sensorName); - //Open the driver - if(!m_cameraDriver.open(driver_properties)) + // Open the driver + if (!m_cameraDriver.open(driver_properties)) { - yError()<<"gz-sim-yarp-camera-system Plugin failed: error in opening yarp driver"; + yError() << "gz-sim-yarp-camera-system Plugin failed: error in opening yarp driver"; return; } - + m_cameraDriver.view(iFrameGrabberImage); if (iFrameGrabberImage == NULL) { - yError()<< "Unable to get the iFrameGrabberImage interface from the device"; - return; + yError() << "Unable to get the iFrameGrabberImage interface from the device"; + return; } - m_deviceScopedName = sensorScopedName + "/" + driver_properties.find("yarpDeviceName").asString(); + m_deviceScopedName + = sensorScopedName + "/" + driver_properties.find("yarpDeviceName").asString(); - if(!Handler::getHandler()->setDevice(m_deviceScopedName, &m_cameraDriver)) + if (!Handler::getHandler()->setDevice(m_deviceScopedName, &m_cameraDriver)) { - yError()<<"gz-sim-yarp-camera-system: failed setting scopedDeviceName(=" << m_deviceScopedName << ")"; + yError() << "gz-sim-yarp-camera-system: failed setting scopedDeviceName(=" + << m_deviceScopedName << ")"; return; } this->m_deviceRegistered = true; this->cameraInitialized = false; - yInfo() << "gz-sim-yarp-camera-system: Registered YARP device with instance name:" << m_deviceScopedName; - + yInfo() << "gz-sim-yarp-camera-system: Registered YARP device with instance name:" + << m_deviceScopedName; } - virtual void PreUpdate(const UpdateInfo &_info, - EntityComponentManager &_ecm) override + virtual void PreUpdate(const UpdateInfo& _info, EntityComponentManager& _ecm) override { - if(!this->cameraInitialized && _ecm.ComponentData(sensor).has_value()) + if (!this->cameraInitialized + && _ecm.ComponentData(sensor).has_value()) { this->cameraInitialized = true; auto CameraTopicName = _ecm.ComponentData(sensor).value(); @@ -162,8 +164,7 @@ class Camera } } - virtual void PostUpdate(const UpdateInfo &_info, - const EntityComponentManager &_ecm) override + virtual void PostUpdate(const UpdateInfo& _info, const EntityComponentManager& _ecm) override { gz::msgs::Image cameraMsg; { @@ -171,21 +172,21 @@ class Camera cameraMsg = this->cameraMsg; } - if(this->cameraInitialized) + if (this->cameraInitialized) { std::lock_guard lock(cameraData.m_mutex); memcpy(cameraData.m_imageBuffer, cameraMsg.data().c_str(), cameraData.m_bufferSize); - cameraData.simTime = _info.simTime.count()/1e9; + cameraData.simTime = _info.simTime.count() / 1e9; } } - void CameraCb(const gz::msgs::Image &_msg) + void CameraCb(const gz::msgs::Image& _msg) { std::lock_guard lock(this->cameraMsgMutex); cameraMsg = _msg; } - - private: + +private: Entity sensor; yarp::dev::PolyDriver m_cameraDriver; std::string m_deviceScopedName; @@ -199,8 +200,7 @@ class Camera yarp::dev::IFrameGrabberImage* iFrameGrabberImage; }; -} - +} // namespace gzyarp // Register plugin GZ_ADD_PLUGIN(gzyarp::Camera, diff --git a/plugins/camera/CameraDriver.cpp b/plugins/camera/CameraDriver.cpp index dfe19a0..1ca255a 100644 --- a/plugins/camera/CameraDriver.cpp +++ b/plugins/camera/CameraDriver.cpp @@ -1,289 +1,285 @@ -#include -#include +#include "../../libraries/singleton-devices/Handler.hh" +#include "singleton-camera/Handler.hh" +#include #include #include -#include -#include "singleton-camera/Handler.hh" -#include "../../libraries/singleton-devices/Handler.hh" - +#include +#include -namespace yarp { - namespace dev { - namespace gzyarp { - class CameraDriver; - } - } +namespace yarp +{ +namespace dev +{ +namespace gzyarp +{ +class CameraDriver; } +} // namespace dev +} // namespace yarp const std::string YarpCameraScopedName = "sensorScopedName"; -class yarp::dev::gzyarp::CameraDriver: - public yarp::dev::DeviceDriver, - public yarp::dev::IFrameGrabberImage +class yarp::dev::gzyarp::CameraDriver : public yarp::dev::DeviceDriver, + public yarp::dev::IFrameGrabberImage { - public: +public: + CameraDriver() + { + m_vertical_flip = false; + m_horizontal_flip = false; + m_display_time_box = false; + m_display_timestamp = false; + counter = 0; + sprintf(num[0].data, "**** ** ** ****"); + sprintf(num[1].data, " * * * * * "); + sprintf(num[2].data, "*** ***** ***"); + sprintf(num[3].data, "*** **** ****"); + sprintf(num[4].data, "* ** **** * *"); + sprintf(num[5].data, "**** *** ****"); + sprintf(num[6].data, "**** **** ****"); + sprintf(num[7].data, "*** * * * *"); + sprintf(num[8].data, "**** ***** ****"); + sprintf(num[9].data, "**** **** ****"); + sprintf(num[10].data, " "); + sprintf(num[11].data, " ** **"); + } + + virtual ~CameraDriver() + { + } + + // DEVICE DRIVER + virtual bool open(yarp::os::Searchable& config) + { + std::string sensorScopedName(config.find(YarpCameraScopedName.c_str()).asString().c_str()); + m_sensorData = ::gzyarp::HandlerCamera::getHandler()->getSensor(sensorScopedName); - CameraDriver() + if (!m_sensorData) { - m_vertical_flip = false; - m_horizontal_flip = false; - m_display_time_box = false; - m_display_timestamp = false; - counter=0; - sprintf(num[0].data, "**** ** ** ****"); - sprintf(num[1].data, " * * * * * "); - sprintf(num[2].data, "*** ***** ***"); - sprintf(num[3].data, "*** **** ****"); - sprintf(num[4].data, "* ** **** * *"); - sprintf(num[5].data, "**** *** ****"); - sprintf(num[6].data, "**** **** ****"); - sprintf(num[7].data, "*** * * * *"); - sprintf(num[8].data, "**** ***** ****"); - sprintf(num[9].data, "**** **** ****"); - sprintf(num[10].data," "); - sprintf(num[11].data," ** **"); + yError() << "Error, Camera sensor was not found"; + return false; } - virtual ~CameraDriver() { + std::lock_guard lock(m_sensorData->m_mutex); + m_sensorData->m_imageBuffer = new unsigned char[getRawBufferSize()]; + memset(m_sensorData->m_imageBuffer, 0x00, getRawBufferSize()); } + if (config.check("vertical_flip")) + m_vertical_flip = true; + if (config.check("horizontal_flip")) + m_horizontal_flip = true; + if (config.check("display_timestamp")) + m_display_timestamp = true; + if (config.check("display_time_box")) + m_display_time_box = true; - //DEVICE DRIVER - virtual bool open(yarp::os::Searchable& config) - { - std::string sensorScopedName(config.find(YarpCameraScopedName.c_str()).asString().c_str()); - m_sensorData = ::gzyarp::HandlerCamera::getHandler()->getSensor(sensorScopedName); - - if (!m_sensorData) - { - yError() << "Error, Camera sensor was not found"; - return false; - } - - { - std::lock_guard lock(m_sensorData->m_mutex); - m_sensorData->m_imageBuffer = new unsigned char[getRawBufferSize()]; - memset(m_sensorData->m_imageBuffer, 0x00, getRawBufferSize()); - } - - if (config.check("vertical_flip")) - m_vertical_flip =true; - if (config.check("horizontal_flip")) - m_horizontal_flip =true; - if (config.check("display_timestamp")) - m_display_timestamp =true; - if (config.check("display_time_box")) - m_display_time_box =true; + return true; + } - return true; - } + virtual bool close() + { + delete[] m_sensorData->m_imageBuffer; + m_sensorData->m_imageBuffer = 0; + return true; + } - virtual bool close() - { - delete[] m_sensorData->m_imageBuffer; - m_sensorData->m_imageBuffer = 0; - return true; - } + // IFRAMEGRABBER IMAGE + virtual bool getImage(yarp::sig::ImageOf& _image) + { + std::lock_guard lock(m_sensorData->m_mutex); + _image.resize(width(), height()); + unsigned char* pBuffer = _image.getRawImage(); - // IFRAMEGRABBER IMAGE - virtual bool getImage(yarp::sig::ImageOf& _image) + if (m_vertical_flip == true && m_horizontal_flip == false) { - std::lock_guard lock(m_sensorData->m_mutex); - _image.resize(width(), height()); - - unsigned char *pBuffer = _image.getRawImage(); - - if (m_vertical_flip==true && m_horizontal_flip==false) + int r = 0; + int c = 0; + for (int c = 0; c < width(); c++) { - int r=0; - int c=0; - for (int c=0; cm_imageBuffer+r*width()*3+c*3+0); - pixel[1] = *(m_sensorData->m_imageBuffer+r*width()*3+c*3+1); - pixel[2] = *(m_sensorData->m_imageBuffer+r*width()*3+c*3+2); - } + unsigned char* pixel = _image.getPixelAddress(c, height() - r - 1); + pixel[0] = *(m_sensorData->m_imageBuffer + r * width() * 3 + c * 3 + 0); + pixel[1] = *(m_sensorData->m_imageBuffer + r * width() * 3 + c * 3 + 1); + pixel[2] = *(m_sensorData->m_imageBuffer + r * width() * 3 + c * 3 + 2); } } - else if (m_vertical_flip==false && m_horizontal_flip==true) + } else if (m_vertical_flip == false && m_horizontal_flip == true) + { + int r = 0; + int c = 0; + for (int c = 0; c < width(); c++) { - int r=0; - int c=0; - for (int c=0; cm_imageBuffer+r*width()*3+c*3+0); - pixel[1] = *(m_sensorData->m_imageBuffer+r*width()*3+c*3+1); - pixel[2] = *(m_sensorData->m_imageBuffer+r*width()*3+c*3+2); - } + unsigned char* pixel = _image.getPixelAddress(width() - c - 1, r); + pixel[0] = *(m_sensorData->m_imageBuffer + r * width() * 3 + c * 3 + 0); + pixel[1] = *(m_sensorData->m_imageBuffer + r * width() * 3 + c * 3 + 1); + pixel[2] = *(m_sensorData->m_imageBuffer + r * width() * 3 + c * 3 + 2); } } - else if (m_vertical_flip==true && m_horizontal_flip==true) + } else if (m_vertical_flip == true && m_horizontal_flip == true) + { + int r = 0; + int c = 0; + for (int c = 0; c < width(); c++) { - int r=0; - int c=0; - for (int c=0; cm_imageBuffer+r*width()*3+c*3+0); - pixel[1] = *(m_sensorData->m_imageBuffer+r*width()*3+c*3+1); - pixel[2] = *(m_sensorData->m_imageBuffer+r*width()*3+c*3+2); - } + unsigned char* pixel + = _image.getPixelAddress(width() - c - 1, height() - r - 1); + pixel[0] = *(m_sensorData->m_imageBuffer + r * width() * 3 + c * 3 + 0); + pixel[1] = *(m_sensorData->m_imageBuffer + r * width() * 3 + c * 3 + 1); + pixel[2] = *(m_sensorData->m_imageBuffer + r * width() * 3 + c * 3 + 2); } } - else - { - memcpy(pBuffer, m_sensorData->m_imageBuffer, getRawBufferSize()); - } + } else + { + memcpy(pBuffer, m_sensorData->m_imageBuffer, getRawBufferSize()); + } + + if (m_display_time_box) + { + counter++; + if (counter == 10) + counter = 0; - if (m_display_time_box) + for (int c = 0 + counter * 30; c < 30 + counter * 30; c++) { - counter++; - if (counter == 10) - counter = 0; - - for (int c=0+counter*30; c<30+counter*30; c++) + for (int r = 0; r < 30; r++) { - for (int r=0; r<30; r++) + if (counter % 2 == 0) { - if (counter % 2 ==0) - { - unsigned char *pixel = _image.getPixelAddress(width()-c-1, height()-r-1); - pixel[0] = 255; - pixel[1] = 0; - pixel[2] = 0; - } - else - { - unsigned char *pixel = _image.getPixelAddress(width()-c-1, height()-r-1); - pixel[0] = 0; - pixel[1] = 255; - pixel[2] = 0; - } + unsigned char* pixel + = _image.getPixelAddress(width() - c - 1, height() - r - 1); + pixel[0] = 255; + pixel[1] = 0; + pixel[2] = 0; + } else + { + unsigned char* pixel + = _image.getPixelAddress(width() - c - 1, height() - r - 1); + pixel[0] = 0; + pixel[1] = 255; + pixel[2] = 0; } } - } - - if (m_display_timestamp) - { - char txtbuf[1000]; - sprintf(txtbuf, "%.3f", m_sensorData->simTime); - int len = strlen(txtbuf); - if (len<20) - print(pBuffer, width(), height(), 0, 0, txtbuf, len); } - - return true; } - virtual int height() const + if (m_display_timestamp) { - return m_sensorData->m_height; + char txtbuf[1000]; + sprintf(txtbuf, "%.3f", m_sensorData->simTime); + int len = strlen(txtbuf); + if (len < 20) + print(pBuffer, width(), height(), 0, 0, txtbuf, len); } - virtual int width() const - { - return m_sensorData->m_width; - } + return true; + } - virtual int getRawBufferSize() - { - return m_sensorData->m_bufferSize; - } + virtual int height() const + { + return m_sensorData->m_height; + } + + virtual int width() const + { + return m_sensorData->m_width; + } + virtual int getRawBufferSize() + { + return m_sensorData->m_bufferSize; + } - void print(unsigned char* pixbuf, int pixbuf_w, int pixbuf_h, int x, int y, char* s, int size) + void print(unsigned char* pixbuf, int pixbuf_w, int pixbuf_h, int x, int y, char* s, int size) + { + int pixelsize = 5; + for (int i = 0; i < size; i++) { - int pixelsize = 5; - for (int i=0; i #include -namespace gzyarp +namespace gzyarp { HandlerCamera* HandlerCamera::getHandler() { std::lock_guard lock(mutex()); - if (!s_handle) + if (!s_handle) { s_handle = new HandlerCamera(); if (!s_handle) yError() << "Error while calling gzyarp::HandlerCamera constructor"; } - + return s_handle; } @@ -24,19 +24,22 @@ bool HandlerCamera::setSensor(CameraData* _sensorDataPtr) std::string sensorScopedName = _sensorDataPtr->sensorScopedName; SensorsMap::iterator sensor = m_sensorsMap.find(sensorScopedName); - if (sensor != m_sensorsMap.end()) + if (sensor != m_sensorsMap.end()) ret = true; - else + else { - //sensor does not exists. Add to map - if (!m_sensorsMap.insert(std::pair(sensorScopedName, _sensorDataPtr)).second) + // sensor does not exists. Add to map + if (!m_sensorsMap + .insert(std::pair(sensorScopedName, _sensorDataPtr)) + .second) { yError() << "Error in gzyarp::HandlerCamera while inserting a new sensor pointer!"; - yError() << " The name of the sensor is already present but the pointer does not match with the one already registered!"; - yError() << " This should not happen, as the scoped name should be unique in Gazebo. Fatal error."; + yError() << " The name of the sensor is already present but the pointer does not match " + "with the one already registered!"; + yError() << " This should not happen, as the scoped name should be unique in Gazebo. " + "Fatal error."; ret = false; - } - else + } else ret = true; } return ret; @@ -47,11 +50,10 @@ CameraData* HandlerCamera::getSensor(const std::string& sensorScopedName) const CameraData* tmp; SensorsMap::const_iterator sensor = m_sensorsMap.find(sensorScopedName); - if (sensor != m_sensorsMap.end()) + if (sensor != m_sensorsMap.end()) { tmp = sensor->second; - } - else + } else { yError() << "Sensor was not found: " << sensorScopedName; tmp = NULL; @@ -59,34 +61,30 @@ CameraData* HandlerCamera::getSensor(const std::string& sensorScopedName) const return tmp; } -void HandlerCamera::removeSensor(const std::string &sensorName) +void HandlerCamera::removeSensor(const std::string& sensorName) { SensorsMap::iterator sensor = m_sensorsMap.find(sensorName); - if (sensor != m_sensorsMap.end()) + if (sensor != m_sensorsMap.end()) { m_sensorsMap.erase(sensor); - } - else + } else { yError() << "Could not remove sensor " << sensorName << ". Sensor was not found"; } } -HandlerCamera::HandlerCamera() : m_sensorsMap() +HandlerCamera::HandlerCamera() + : m_sensorsMap() { m_sensorsMap.clear(); } HandlerCamera* HandlerCamera::s_handle = NULL; - std::mutex& HandlerCamera::mutex() { static std::mutex s_mutex; return s_mutex; } -} - - - +} // namespace gzyarp diff --git a/plugins/camera/singleton-camera/Handler.hh b/plugins/camera/singleton-camera/Handler.hh index 2d09424..bafdb01 100644 --- a/plugins/camera/singleton-camera/Handler.hh +++ b/plugins/camera/singleton-camera/Handler.hh @@ -1,43 +1,40 @@ +#include #include #include #include #include -#include struct CameraData { - std::mutex m_mutex; - int m_width; - int m_height; - int m_bufferSize; - unsigned char *m_imageBuffer; - std::string sensorScopedName; - double simTime; + std::mutex m_mutex; + int m_width; + int m_height; + int m_bufferSize; + unsigned char* m_imageBuffer; + std::string sensorScopedName; + double simTime; }; -namespace gzyarp +namespace gzyarp { class HandlerCamera -{ - public: - static HandlerCamera* getHandler(); +{ +public: + static HandlerCamera* getHandler(); - bool setSensor(CameraData* _sensorDataPtr); + bool setSensor(CameraData* _sensorDataPtr); - CameraData* getSensor(const std::string& sensorScopedName) const; + CameraData* getSensor(const std::string& sensorScopedName) const; - void removeSensor(const std::string& sensorName); - - private: - HandlerCamera(); - static HandlerCamera* s_handle; - static std::mutex& mutex(); - typedef std::map SensorsMap; - SensorsMap m_sensorsMap; + void removeSensor(const std::string& sensorName); +private: + HandlerCamera(); + static HandlerCamera* s_handle; + static std::mutex& mutex(); + typedef std::map SensorsMap; + SensorsMap m_sensorsMap; }; -} - - +} // namespace gzyarp diff --git a/plugins/forcetorque/ForceTorque.cc b/plugins/forcetorque/ForceTorque.cc index fc9c130..35dc84b 100644 --- a/plugins/forcetorque/ForceTorque.cc +++ b/plugins/forcetorque/ForceTorque.cc @@ -1,56 +1,56 @@ -#include -#include -#include +#include "ForceTorqueDriver.cpp" #include #include +#include #include -#include -#include +#include +#include +#include +#include #include +#include #include #include -#include "ForceTorqueDriver.cpp" -#include -#include -#include - +#include +#include using namespace gz; using namespace sim; using namespace systems; -namespace gzyarp +namespace gzyarp { -class ForceTorque - : public System, - public ISystemConfigure, - public ISystemPreUpdate, - public ISystemPostUpdate +class ForceTorque : public System, + public ISystemConfigure, + public ISystemPreUpdate, + public ISystemPostUpdate { - public: - ForceTorque() : m_deviceRegistered(false) +public: + ForceTorque() + : m_deviceRegistered(false) { } - + virtual ~ForceTorque() { - if (m_deviceRegistered) - { - Handler::getHandler()->removeDevice(m_deviceScopedName); - m_deviceRegistered = false; - } - - if( m_forceTorqueDriver.isValid() ) m_forceTorqueDriver.close(); - HandlerForceTorque::getHandler()->removeSensor(sensorScopedName); - yarp::os::Network::fini(); + if (m_deviceRegistered) + { + Handler::getHandler()->removeDevice(m_deviceScopedName); + m_deviceRegistered = false; + } + + if (m_forceTorqueDriver.isValid()) + m_forceTorqueDriver.close(); + HandlerForceTorque::getHandler()->removeSensor(sensorScopedName); + yarp::os::Network::fini(); } - - virtual void Configure(const Entity &_entity, - const std::shared_ptr &_sdf, - EntityComponentManager &_ecm, - EventManager &/*_eventMgr*/) override - { + + virtual void Configure(const Entity& _entity, + const std::shared_ptr& _sdf, + EntityComponentManager& _ecm, + EventManager& /*_eventMgr*/) override + { yarp::os::Network::init(); if (!yarp::os::Network::checkNetwork()) { @@ -59,9 +59,14 @@ class ForceTorque } std::string netWrapper = "analogServer"; - ::yarp::dev::Drivers::factory().add(new ::yarp::dev::DriverCreatorOf< ::yarp::dev::gzyarp::ForceTorqueDriver> - ("gazebo_forcetorque", netWrapper.c_str(), "ForceTorqueDriver")); - + ::yarp::dev::Drivers::factory().add( + new ::yarp::dev::DriverCreatorOf<::yarp::dev::gzyarp::ForceTorqueDriver>("gazebo_" + "forcetorque", + netWrapper + .c_str(), + "ForceTorqueDr" + "iver")); + ::yarp::os::Property driver_properties; bool wipe = false; @@ -79,15 +84,15 @@ class ForceTorque yError() << "gz-sim-yarp-forcetorque-system : missing jointName parameter"; return; } - yInfo() << "gz-sim-yarp-forcetorque-system: configuration of sensor " << driver_properties.find("sensorName").asString() + yInfo() << "gz-sim-yarp-forcetorque-system: configuration of sensor " + << driver_properties.find("sensorName").asString() << " loaded from yarpConfigurationString : " << configuration_string << "\n"; - } - else + } else { yError() << "gz-sim-yarp-forcetorque-system : missing yarpConfigurationString element"; - return; + return; } - + std::string sensorName = driver_properties.find("sensorName").asString(); std::string jointName = driver_properties.find("jointName").asString(); auto model = Model(_entity); @@ -96,55 +101,57 @@ class ForceTorque sensorScopedName = scopedName(this->sensor, _ecm); this->forceTorqueData.sensorScopedName = sensorScopedName; - + driver_properties.put(YarpForceTorqueScopedName.c_str(), sensorScopedName.c_str()); if (!driver_properties.check("yarpDeviceName")) { - yError() << "gz-sim-yarp-forcetorque-system : missing yarpDeviceName parameter for device" << sensorScopedName; + yError() << "gz-sim-yarp-forcetorque-system : missing yarpDeviceName parameter for " + "device" + << sensorScopedName; return; } - //Insert the pointer in the singleton handler for retriving it in the yarp driver + // Insert the pointer in the singleton handler for retriving it in the yarp driver HandlerForceTorque::getHandler()->setSensor(&(this->forceTorqueData)); - driver_properties.put("device","gazebo_forcetorque"); + driver_properties.put("device", "gazebo_forcetorque"); driver_properties.put("sensor_name", sensorName); - if( !m_forceTorqueDriver.open(driver_properties) ) + if (!m_forceTorqueDriver.open(driver_properties)) { - yError()<<"gz-sim-yarp-forcetorque-system Plugin failed: error in opening yarp driver"; + yError() << "gz-sim-yarp-forcetorque-system Plugin failed: error in opening yarp " + "driver"; return; } - m_deviceScopedName = sensorScopedName + "/" + driver_properties.find("yarpDeviceName").asString(); + m_deviceScopedName + = sensorScopedName + "/" + driver_properties.find("yarpDeviceName").asString(); - if(!Handler::getHandler()->setDevice(m_deviceScopedName, &m_forceTorqueDriver)) + if (!Handler::getHandler()->setDevice(m_deviceScopedName, &m_forceTorqueDriver)) { - yError()<<"gz-sim-yarp-forcetorque-system: failed setting scopedDeviceName(=" << m_deviceScopedName << ")"; + yError() << "gz-sim-yarp-forcetorque-system: failed setting scopedDeviceName(=" + << m_deviceScopedName << ")"; return; } m_deviceRegistered = true; yInfo() << "Registered YARP device with instance name:" << m_deviceScopedName; } - virtual void PreUpdate(const UpdateInfo &_info, - EntityComponentManager &_ecm) override + virtual void PreUpdate(const UpdateInfo& _info, EntityComponentManager& _ecm) override { - if(!this->ftInitialized && _ecm.ComponentData(sensor).has_value()) + if (!this->ftInitialized && _ecm.ComponentData(sensor).has_value()) { this->ftInitialized = true; auto ftTopicName = _ecm.ComponentData(sensor).value(); this->node.Subscribe(ftTopicName, &ForceTorque::ftCb, this); } } - - virtual void PostUpdate(const UpdateInfo &_info, - const EntityComponentManager &_ecm) override + virtual void PostUpdate(const UpdateInfo& _info, const EntityComponentManager& _ecm) override { gz::msgs::Wrench ftMsg; { - std::lock_guard lock(this->ftMsgMutex); - ftMsg = this->ftMsg; + std::lock_guard lock(this->ftMsgMutex); + ftMsg = this->ftMsg; } std::lock_guard lock(forceTorqueData.m_mutex); @@ -154,16 +161,16 @@ class ForceTorque forceTorqueData.m_data[3] = (ftMsg.torque().x() != 0) ? ftMsg.torque().x() : 0; forceTorqueData.m_data[4] = (ftMsg.torque().y() != 0) ? ftMsg.torque().y() : 0; forceTorqueData.m_data[5] = (ftMsg.torque().z() != 0) ? ftMsg.torque().z() : 0; - forceTorqueData.simTime = _info.simTime.count()/1e9; + forceTorqueData.simTime = _info.simTime.count() / 1e9; } - void ftCb(const gz::msgs::Wrench &_msg) + void ftCb(const gz::msgs::Wrench& _msg) { std::lock_guard lock(this->ftMsgMutex); ftMsg = _msg; } - - private: + +private: Entity sensor; yarp::dev::PolyDriver m_forceTorqueDriver; std::string m_deviceScopedName; @@ -174,17 +181,13 @@ class ForceTorque gz::transport::Node node; gz::msgs::Wrench ftMsg; std::mutex ftMsgMutex; - }; -} +} // namespace gzyarp - // Register plugin GZ_ADD_PLUGIN(gzyarp::ForceTorque, gz::sim::System, gzyarp::ForceTorque::ISystemConfigure, gzyarp::ForceTorque::ISystemPreUpdate, gzyarp::ForceTorque::ISystemPostUpdate) - - diff --git a/plugins/forcetorque/ForceTorqueDriver.cpp b/plugins/forcetorque/ForceTorqueDriver.cpp index 9aeb703..988fdb8 100644 --- a/plugins/forcetorque/ForceTorqueDriver.cpp +++ b/plugins/forcetorque/ForceTorqueDriver.cpp @@ -1,129 +1,137 @@ -#include -#include +#include "../../libraries/singleton-devices/Handler.hh" +#include "singleton-forcetorque/Handler.hh" +#include #include #include #include -#include -#include "singleton-forcetorque/Handler.hh" -#include "../../libraries/singleton-devices/Handler.hh" - +#include +#include -namespace yarp { - namespace dev { - namespace gzyarp { - class ForceTorqueDriver; - } - } +namespace yarp +{ +namespace dev +{ +namespace gzyarp +{ +class ForceTorqueDriver; } +} // namespace dev +} // namespace yarp -const unsigned YarpForceTorqueChannelsNumber = 6; //The ForceTorque sensor has 6 fixed channels +const unsigned YarpForceTorqueChannelsNumber = 6; // The ForceTorque sensor has 6 fixed channels const std::string YarpForceTorqueScopedName = "sensorScopedName"; -class yarp::dev::gzyarp::ForceTorqueDriver: - public yarp::dev::DeviceDriver, - public yarp::dev::ISixAxisForceTorqueSensors +class yarp::dev::gzyarp::ForceTorqueDriver : public yarp::dev::DeviceDriver, + public yarp::dev::ISixAxisForceTorqueSensors { - public: - ForceTorqueDriver(){} - virtual ~ForceTorqueDriver(){} +public: + ForceTorqueDriver() + { + } + virtual ~ForceTorqueDriver() + { + } - //DEVICE DRIVER - virtual bool open(yarp::os::Searchable& config) - { + // DEVICE DRIVER + virtual bool open(yarp::os::Searchable& config) + { - std::string sensorScopedName(config.find(YarpForceTorqueScopedName.c_str()).asString().c_str()); - - std::string separator = "/"; - auto pos = config.find("sensor_name").asString().find_last_of(separator); - if (pos == std::string::npos) - { - m_sensorName = config.find("sensor_name").asString(); - } - else - { - m_sensorName = config.find("sensor_name").asString().substr(pos + separator.size() - 1); - } - - m_frameName = m_sensorName; - m_sensorData = ::gzyarp::HandlerForceTorque::getHandler()->getSensor(sensorScopedName); - - if (!m_sensorData) - { - yError() << "Error, ForceTorque sensor was not found"; - return false; - } - - return true; - } + std::string sensorScopedName( + config.find(YarpForceTorqueScopedName.c_str()).asString().c_str()); - virtual bool close() + std::string separator = "/"; + auto pos = config.find("sensor_name").asString().find_last_of(separator); + if (pos == std::string::npos) { - return true; + m_sensorName = config.find("sensor_name").asString(); + } else + { + m_sensorName = config.find("sensor_name").asString().substr(pos + separator.size() - 1); } - // SIX AXIS FORCE TORQUE SENSORS - virtual size_t getNrOfSixAxisForceTorqueSensors() const + m_frameName = m_sensorName; + m_sensorData = ::gzyarp::HandlerForceTorque::getHandler()->getSensor(sensorScopedName); + + if (!m_sensorData) { - return 1; + yError() << "Error, ForceTorque sensor was not found"; + return false; } - virtual yarp::dev::MAS_status getSixAxisForceTorqueSensorStatus(size_t sens_index) const - { - if (sens_index >= 1) - { - return MAS_UNKNOWN; - } - return MAS_OK; - } - virtual bool getSixAxisForceTorqueSensorName(size_t sens_index, std::string &name) const + return true; + } + + virtual bool close() + { + return true; + } + + // SIX AXIS FORCE TORQUE SENSORS + virtual size_t getNrOfSixAxisForceTorqueSensors() const + { + return 1; + } + virtual yarp::dev::MAS_status getSixAxisForceTorqueSensorStatus(size_t sens_index) const + { + if (sens_index >= 1) { - if (sens_index >= 1) - { - return false; - } + return MAS_UNKNOWN; + } - name = m_sensorName; - return true; + return MAS_OK; + } + virtual bool getSixAxisForceTorqueSensorName(size_t sens_index, std::string& name) const + { + if (sens_index >= 1) + { + return false; } - virtual bool getSixAxisForceTorqueSensorFrameName(size_t sens_index, std::string &frameName) const + + name = m_sensorName; + return true; + } + virtual bool + getSixAxisForceTorqueSensorFrameName(size_t sens_index, std::string& frameName) const + { + if (sens_index >= 1) { - if (sens_index >= 1) - { - return false; - } + return false; + } - frameName = m_frameName; - return true; + frameName = m_frameName; + return true; + } + virtual bool getSixAxisForceTorqueSensorMeasure(size_t sens_index, + yarp::sig::Vector& out, + double& timestamp) const + { + if (sens_index >= 1) + { + return false; } - virtual bool getSixAxisForceTorqueSensorMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const + + if (out.size() != YarpForceTorqueChannelsNumber) { - if (sens_index >= 1) - { - return false; - } - - if (out.size() != YarpForceTorqueChannelsNumber) - { - out.resize(YarpForceTorqueChannelsNumber); - } - std::lock_guard lock(m_sensorData->m_mutex); - yarp::sig::Vector m_forceTorqueData; - - m_forceTorqueData.resize(YarpForceTorqueChannelsNumber, 0.0); - m_forceTorqueData[0] = m_sensorData->m_data[0]; - m_forceTorqueData[1] = m_sensorData->m_data[1]; - m_forceTorqueData[2] = m_sensorData->m_data[2]; - m_forceTorqueData[3] = m_sensorData->m_data[3]; - m_forceTorqueData[4] = m_sensorData->m_data[4]; - m_forceTorqueData[5] = m_sensorData->m_data[5]; - out = m_forceTorqueData; - - timestamp = m_sensorData->simTime; - return true; + out.resize(YarpForceTorqueChannelsNumber); } + std::lock_guard lock(m_sensorData->m_mutex); + yarp::sig::Vector m_forceTorqueData; + + m_forceTorqueData.resize(YarpForceTorqueChannelsNumber, 0.0); + m_forceTorqueData[0] = m_sensorData->m_data[0]; + m_forceTorqueData[1] = m_sensorData->m_data[1]; + m_forceTorqueData[2] = m_sensorData->m_data[2]; + m_forceTorqueData[3] = m_sensorData->m_data[3]; + m_forceTorqueData[4] = m_sensorData->m_data[4]; + m_forceTorqueData[5] = m_sensorData->m_data[5]; + out = m_forceTorqueData; + + timestamp = m_sensorData->simTime; + return true; + } - private: - ForceTorqueData* m_sensorData; - std::string m_sensorName; - std::string m_frameName; +private: + ForceTorqueData* m_sensorData; + std::string m_sensorName; + std::string m_frameName; }; diff --git a/plugins/forcetorque/singleton-forcetorque/Handler.cc b/plugins/forcetorque/singleton-forcetorque/Handler.cc index 8fde39f..832f61a 100644 --- a/plugins/forcetorque/singleton-forcetorque/Handler.cc +++ b/plugins/forcetorque/singleton-forcetorque/Handler.cc @@ -2,19 +2,19 @@ #include #include -namespace gzyarp +namespace gzyarp { HandlerForceTorque* HandlerForceTorque::getHandler() { std::lock_guard lock(mutex()); - if (!s_handle) + if (!s_handle) { s_handle = new HandlerForceTorque(); if (!s_handle) yError() << "Error while calling gzyarp::HandlerForceTorque constructor"; } - + return s_handle; } @@ -24,35 +24,36 @@ bool HandlerForceTorque::setSensor(ForceTorqueData* _sensorDataPtr) std::string sensorScopedName = _sensorDataPtr->sensorScopedName; SensorsMap::iterator sensor = m_sensorsMap.find(sensorScopedName); - if (sensor != m_sensorsMap.end()) + if (sensor != m_sensorsMap.end()) ret = true; - else + else { - //sensor does not exists. Add to map - if (!m_sensorsMap.insert(std::pair(sensorScopedName, _sensorDataPtr)).second) + // sensor does not exists. Add to map + if (!m_sensorsMap + .insert(std::pair(sensorScopedName, _sensorDataPtr)) + .second) { yError() << "Error in gzyarp::HandlerForceTorque while inserting a new sensor pointer!"; - yError() << " The name of the sensor is already present but the pointer does not match with the one already registered!"; - yError() << " This should not happen, as the scoped name should be unique in Gazebo. Fatal error."; + yError() << " The name of the sensor is already present but the pointer does not match " + "with the one already registered!"; + yError() << " This should not happen, as the scoped name should be unique in Gazebo. " + "Fatal error."; ret = false; - } - else + } else ret = true; } return ret; } - ForceTorqueData* HandlerForceTorque::getSensor(const std::string& sensorScopedName) const { ForceTorqueData* tmp; SensorsMap::const_iterator sensor = m_sensorsMap.find(sensorScopedName); - if (sensor != m_sensorsMap.end()) + if (sensor != m_sensorsMap.end()) { tmp = sensor->second; - } - else + } else { yError() << "Sensor was not found: " << sensorScopedName; tmp = NULL; @@ -60,37 +61,30 @@ ForceTorqueData* HandlerForceTorque::getSensor(const std::string& sensorScopedNa return tmp; } -void HandlerForceTorque::removeSensor(const std::string &sensorName) +void HandlerForceTorque::removeSensor(const std::string& sensorName) { SensorsMap::iterator sensor = m_sensorsMap.find(sensorName); - if (sensor != m_sensorsMap.end()) + if (sensor != m_sensorsMap.end()) { m_sensorsMap.erase(sensor); - } - else + } else { yError() << "Could not remove sensor " << sensorName << ". Sensor was not found"; } } - - -HandlerForceTorque::HandlerForceTorque() : m_sensorsMap() +HandlerForceTorque::HandlerForceTorque() + : m_sensorsMap() { m_sensorsMap.clear(); } HandlerForceTorque* HandlerForceTorque::s_handle = NULL; - std::mutex& HandlerForceTorque::mutex() { static std::mutex s_mutex; return s_mutex; } -} - - - - +} // namespace gzyarp diff --git a/plugins/forcetorque/singleton-forcetorque/Handler.hh b/plugins/forcetorque/singleton-forcetorque/Handler.hh index 0c51ed9..d172577 100644 --- a/plugins/forcetorque/singleton-forcetorque/Handler.hh +++ b/plugins/forcetorque/singleton-forcetorque/Handler.hh @@ -1,41 +1,38 @@ #include +#include #include #include #include #include -#include struct ForceTorqueData { - std::mutex m_mutex; - std::array m_data; - std::string sensorScopedName; - double simTime; + std::mutex m_mutex; + std::array m_data; + std::string sensorScopedName; + double simTime; }; -namespace gzyarp +namespace gzyarp { class HandlerForceTorque -{ - public: - static HandlerForceTorque* getHandler(); +{ +public: + static HandlerForceTorque* getHandler(); - bool setSensor(ForceTorqueData* _sensorDataPtr); + bool setSensor(ForceTorqueData* _sensorDataPtr); - ForceTorqueData* getSensor(const std::string& sensorScopedName) const; + ForceTorqueData* getSensor(const std::string& sensorScopedName) const; - void removeSensor(const std::string& sensorName); - - private: - HandlerForceTorque(); - static HandlerForceTorque* s_handle; - static std::mutex& mutex(); - typedef std::map SensorsMap; - SensorsMap m_sensorsMap; // map of known sensors + void removeSensor(const std::string& sensorName); +private: + HandlerForceTorque(); + static HandlerForceTorque* s_handle; + static std::mutex& mutex(); + typedef std::map SensorsMap; + SensorsMap m_sensorsMap; // map of known sensors }; -} - - +} // namespace gzyarp diff --git a/plugins/imu/IMU.cc b/plugins/imu/IMU.cc index 280d252..4fdf395 100644 --- a/plugins/imu/IMU.cc +++ b/plugins/imu/IMU.cc @@ -1,60 +1,59 @@ -#include -#include -#include +#include "IMUDriver.cpp" #include -#include #include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include #include -#include -#include -#include -#include -#include -#include "IMUDriver.cpp" - +#include +#include using namespace gz; using namespace sim; using namespace systems; -namespace gzyarp +namespace gzyarp { -class IMU - : public System, - public ISystemConfigure, - public ISystemPreUpdate, - public ISystemPostUpdate - +class IMU : public System, + public ISystemConfigure, + public ISystemPreUpdate, + public ISystemPostUpdate + { - public: - - IMU() : m_deviceRegistered(false) +public: + IMU() + : m_deviceRegistered(false) { } - + virtual ~IMU() { - if (m_deviceRegistered) - { - Handler::getHandler()->removeDevice(m_deviceScopedName); - m_deviceRegistered = false; - } - - if( m_imuDriver.isValid() ) m_imuDriver.close(); - HandlerIMU::getHandler()->removeSensor(sensorScopedName); - yarp::os::Network::fini(); + if (m_deviceRegistered) + { + Handler::getHandler()->removeDevice(m_deviceScopedName); + m_deviceRegistered = false; + } + + if (m_imuDriver.isValid()) + m_imuDriver.close(); + HandlerIMU::getHandler()->removeSensor(sensorScopedName); + yarp::os::Network::fini(); } - - virtual void Configure(const Entity &_entity, - const std::shared_ptr &_sdf, - EntityComponentManager &_ecm, - EventManager &/*_eventMgr*/) override - { + + virtual void Configure(const Entity& _entity, + const std::shared_ptr& _sdf, + EntityComponentManager& _ecm, + EventManager& /*_eventMgr*/) override + { yarp::os::Network::init(); if (!yarp::os::Network::checkNetwork()) { @@ -63,9 +62,11 @@ class IMU } std::string netWrapper = "inertial"; - ::yarp::dev::Drivers::factory().add(new ::yarp::dev::DriverCreatorOf< ::yarp::dev::gzyarp::IMUDriver> - ("gazebo_imu", netWrapper.c_str(), "IMUDriver")); - + ::yarp::dev::Drivers::factory().add( + new ::yarp::dev::DriverCreatorOf<::yarp::dev::gzyarp::IMUDriver>("gazebo_imu", + netWrapper.c_str(), + "IMUDriver")); + ::yarp::os::Property driver_properties; bool wipe = false; @@ -83,59 +84,60 @@ class IMU yError() << "gz-sim-yarp-imu-system : missing parentLinkName parameter"; return; } - yInfo() << "gz-sim-yarp-imu-system: configuration of sensor " << driver_properties.find("sensorName").asString() + yInfo() << "gz-sim-yarp-imu-system: configuration of sensor " + << driver_properties.find("sensorName").asString() << " loaded from yarpConfigurationString : " << configuration_string << "\n"; - } - else + } else { yError() << "gz-sim-yarp-imu-system : missing yarpConfigurationString element"; - return; + return; } std::string sensorName = driver_properties.find("sensorName").asString(); std::string parentLinkName = driver_properties.find("parentLinkName").asString(); auto model = Model(_entity); auto parentLink = model.LinkByName(_ecm, parentLinkName); - this->sensor = _ecm.EntityByComponents( - components::ParentEntity(parentLink), - components::Name(sensorName), - components::Sensor()); - + this->sensor = _ecm.EntityByComponents(components::ParentEntity(parentLink), + components::Name(sensorName), + components::Sensor()); + sensorScopedName = scopedName(this->sensor, _ecm); this->imuData.sensorScopedName = sensorScopedName; driver_properties.put(YarpIMUScopedName.c_str(), sensorScopedName.c_str()); if (!driver_properties.check("yarpDeviceName")) { - yError() << "gz-sim-yarp-imu-system : missing yarpDeviceName parameter for device" << sensorScopedName; + yError() << "gz-sim-yarp-imu-system : missing yarpDeviceName parameter for device" + << sensorScopedName; return; } - //Insert the pointer in the singleton handler for retriving it in the yarp driver + // Insert the pointer in the singleton handler for retriving it in the yarp driver HandlerIMU::getHandler()->setSensor(&(this->imuData)); - driver_properties.put("device","gazebo_imu"); + driver_properties.put("device", "gazebo_imu"); driver_properties.put("sensor_name", sensorName); - if( !m_imuDriver.open(driver_properties) ) + if (!m_imuDriver.open(driver_properties)) { - yError()<<"gz-sim-yarp-imu-system Plugin failed: error in opening yarp driver"; + yError() << "gz-sim-yarp-imu-system Plugin failed: error in opening yarp driver"; return; } - m_deviceScopedName = sensorScopedName + "/" + driver_properties.find("yarpDeviceName").asString(); + m_deviceScopedName + = sensorScopedName + "/" + driver_properties.find("yarpDeviceName").asString(); - if(!Handler::getHandler()->setDevice(m_deviceScopedName, &m_imuDriver)) + if (!Handler::getHandler()->setDevice(m_deviceScopedName, &m_imuDriver)) { - yError()<<"gz-sim-yarp-imu-system: failed setting scopedDeviceName(=" << m_deviceScopedName << ")"; + yError() << "gz-sim-yarp-imu-system: failed setting scopedDeviceName(=" + << m_deviceScopedName << ")"; return; } m_deviceRegistered = true; yInfo() << "Registered YARP device with instance name:" << m_deviceScopedName; - } - virtual void PreUpdate(const UpdateInfo &_info, - EntityComponentManager &_ecm) override + virtual void PreUpdate(const UpdateInfo& _info, EntityComponentManager& _ecm) override { - if(!this->imuInitialized && _ecm.ComponentData(sensor).has_value()) + if (!this->imuInitialized + && _ecm.ComponentData(sensor).has_value()) { this->imuInitialized = true; auto imuTopicName = _ecm.ComponentData(sensor).value(); @@ -143,8 +145,7 @@ class IMU } } - virtual void PostUpdate(const UpdateInfo &_info, - const EntityComponentManager &_ecm) override + virtual void PostUpdate(const UpdateInfo& _info, const EntityComponentManager& _ecm) override { gz::msgs::IMU imuMsg; { @@ -155,22 +156,28 @@ class IMU imuData.m_data[0] = (imuMsg.orientation().x() != 0) ? imuMsg.orientation().x() : 0; imuData.m_data[1] = (imuMsg.orientation().y() != 0) ? imuMsg.orientation().y() : 0; imuData.m_data[2] = (imuMsg.orientation().w() != 0) ? imuMsg.orientation().w() : 0; - imuData.m_data[3] = (imuMsg.linear_acceleration().x() != 0) ? imuMsg.linear_acceleration().x() : 0; - imuData.m_data[4] = (imuMsg.linear_acceleration().y() != 0) ? imuMsg.linear_acceleration().y() : 0; - imuData.m_data[5] = (imuMsg.linear_acceleration().z() != 0) ? imuMsg.linear_acceleration().z() : 0; - imuData.m_data[6] = (imuMsg.angular_velocity().x() != 0) ? imuMsg.angular_velocity().x() : 0; - imuData.m_data[7] = (imuMsg.angular_velocity().y() != 0) ? imuMsg.angular_velocity().y() : 0; - imuData.m_data[8] = (imuMsg.angular_velocity().z() != 0) ? imuMsg.angular_velocity().z() : 0; - imuData.simTime = _info.simTime.count()/1e9; + imuData.m_data[3] + = (imuMsg.linear_acceleration().x() != 0) ? imuMsg.linear_acceleration().x() : 0; + imuData.m_data[4] + = (imuMsg.linear_acceleration().y() != 0) ? imuMsg.linear_acceleration().y() : 0; + imuData.m_data[5] + = (imuMsg.linear_acceleration().z() != 0) ? imuMsg.linear_acceleration().z() : 0; + imuData.m_data[6] = (imuMsg.angular_velocity().x() != 0) ? imuMsg.angular_velocity().x() + : 0; + imuData.m_data[7] = (imuMsg.angular_velocity().y() != 0) ? imuMsg.angular_velocity().y() + : 0; + imuData.m_data[8] = (imuMsg.angular_velocity().z() != 0) ? imuMsg.angular_velocity().z() + : 0; + imuData.simTime = _info.simTime.count() / 1e9; } - void imuCb(const gz::msgs::IMU &_msg) + void imuCb(const gz::msgs::IMU& _msg) { std::lock_guard lock(this->imuMsgMutex); imuMsg = _msg; } - - private: + +private: Entity sensor; yarp::dev::PolyDriver m_imuDriver; std::string m_deviceScopedName; @@ -183,7 +190,7 @@ class IMU std::mutex imuMsgMutex; }; -} +} // namespace gzyarp // Register plugin GZ_ADD_PLUGIN(gzyarp::IMU, diff --git a/plugins/imu/IMUDriver.cpp b/plugins/imu/IMUDriver.cpp index e145c5c..c302b4d 100644 --- a/plugins/imu/IMUDriver.cpp +++ b/plugins/imu/IMUDriver.cpp @@ -1,209 +1,227 @@ -#include -#include +#include "../../libraries/singleton-devices/Handler.hh" +#include "singleton-imu/Handler.hh" +#include #include #include -#include -#include "singleton-imu/Handler.hh" -#include "../../libraries/singleton-devices/Handler.hh" - +#include +#include -namespace yarp { - namespace dev { - namespace gzyarp { - class IMUDriver; - } - } +namespace yarp +{ +namespace dev +{ +namespace gzyarp +{ +class IMUDriver; } +} // namespace dev +} // namespace yarp -const unsigned YarpIMUChannelsNumber = 9; -constexpr size_t rpyStartIdx = 0; +const unsigned YarpIMUChannelsNumber = 9; +constexpr size_t rpyStartIdx = 0; constexpr size_t accelStartIdx = 3; -constexpr size_t gyroStartIdx = 6; +constexpr size_t gyroStartIdx = 6; const std::string YarpIMUScopedName = "sensorScopedName"; /* * * 0 1 2 = Euler orientation data (Kalman filter processed) * 3 4 5 = Calibrated 3-axis (X, Y, Z) acceleration data * 6 7 8 = Calibrated 3-axis (X, Y, Z) gyroscope data - * + * */ -class yarp::dev::gzyarp::IMUDriver: - public yarp::dev::DeviceDriver, - public yarp::dev::IThreeAxisGyroscopes, - public yarp::dev::IThreeAxisLinearAccelerometers, - public yarp::dev::IOrientationSensors +class yarp::dev::gzyarp::IMUDriver : public yarp::dev::DeviceDriver, + public yarp::dev::IThreeAxisGyroscopes, + public yarp::dev::IThreeAxisLinearAccelerometers, + public yarp::dev::IOrientationSensors { - public: - IMUDriver(){} - virtual ~IMUDriver(){} +public: + IMUDriver() + { + } + virtual ~IMUDriver() + { + } - //DEVICE DRIVER - virtual bool open(yarp::os::Searchable& config) - { + // DEVICE DRIVER + virtual bool open(yarp::os::Searchable& config) + { - std::string sensorScopedName(config.find(YarpIMUScopedName.c_str()).asString().c_str()); - - std::string separator = "/"; - auto pos = config.find("sensor_name").asString().find_last_of(separator); - if (pos == std::string::npos) - { - m_sensorName = config.find("sensor_name").asString(); - } - else - { - m_sensorName = config.find("sensor_name").asString().substr(pos + separator.size() - 1); - } - - m_frameName = m_sensorName; - m_sensorData = ::gzyarp::HandlerIMU::getHandler()->getSensor(sensorScopedName); - - if (!m_sensorData) - { - yError() << "Error, IMU sensor was not found"; - return false; - } - - return true; - } + std::string sensorScopedName(config.find(YarpIMUScopedName.c_str()).asString().c_str()); - virtual bool close() + std::string separator = "/"; + auto pos = config.find("sensor_name").asString().find_last_of(separator); + if (pos == std::string::npos) { - return true; - } - - // THREE AXIS GYROSCOPES - virtual size_t getNrOfThreeAxisGyroscopes() const + m_sensorName = config.find("sensor_name").asString(); + } else { - return 1; + m_sensorName = config.find("sensor_name").asString().substr(pos + separator.size() - 1); } - virtual yarp::dev::MAS_status getThreeAxisGyroscopeStatus(size_t sens_index) const - { - return genericGetStatus(sens_index); - } + m_frameName = m_sensorName; + m_sensorData = ::gzyarp::HandlerIMU::getHandler()->getSensor(sensorScopedName); - virtual bool getThreeAxisGyroscopeName(size_t sens_index, std::string& name) const + if (!m_sensorData) { - return genericGetSensorName(sens_index, name); + yError() << "Error, IMU sensor was not found"; + return false; } - virtual bool getThreeAxisGyroscopeFrameName(size_t sens_index, std::string& frameName) const - { - return genericGetFrameName(sens_index, frameName); - } + return true; + } - virtual bool getThreeAxisGyroscopeMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const - { - return genericGetMeasure(sens_index, out, timestamp, gyroStartIdx); - } + virtual bool close() + { + return true; + } - // THREE AXIS LINEAR ACCELEROMETERS - virtual size_t getNrOfThreeAxisLinearAccelerometers() const - { - return 1; - } + // THREE AXIS GYROSCOPES + virtual size_t getNrOfThreeAxisGyroscopes() const + { + return 1; + } - virtual yarp::dev::MAS_status getThreeAxisLinearAccelerometerStatus(size_t sens_index) const - { - return genericGetStatus(sens_index); - } + virtual yarp::dev::MAS_status getThreeAxisGyroscopeStatus(size_t sens_index) const + { + return genericGetStatus(sens_index); + } - virtual bool getThreeAxisLinearAccelerometerName(size_t sens_index, std::string& name) const - { - return genericGetSensorName(sens_index, name); - } + virtual bool getThreeAxisGyroscopeName(size_t sens_index, std::string& name) const + { + return genericGetSensorName(sens_index, name); + } - virtual bool getThreeAxisLinearAccelerometerFrameName(size_t sens_index, std::string& frameName) const - { - return genericGetFrameName(sens_index, frameName); - } + virtual bool getThreeAxisGyroscopeFrameName(size_t sens_index, std::string& frameName) const + { + return genericGetFrameName(sens_index, frameName); + } - virtual bool getThreeAxisLinearAccelerometerMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const - { - return genericGetMeasure(sens_index, out, timestamp, accelStartIdx); - } + virtual bool + getThreeAxisGyroscopeMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const + { + return genericGetMeasure(sens_index, out, timestamp, gyroStartIdx); + } - // ORIENTATION SENSORS - virtual size_t getNrOfOrientationSensors() const - { - return 1; - } + // THREE AXIS LINEAR ACCELEROMETERS + virtual size_t getNrOfThreeAxisLinearAccelerometers() const + { + return 1; + } - virtual yarp::dev::MAS_status getOrientationSensorStatus(size_t sens_index) const - { - return genericGetStatus(sens_index); - } + virtual yarp::dev::MAS_status getThreeAxisLinearAccelerometerStatus(size_t sens_index) const + { + return genericGetStatus(sens_index); + } - virtual bool getOrientationSensorName(size_t sens_index, std::string& name) const - { - return genericGetSensorName(sens_index, name); - } + virtual bool getThreeAxisLinearAccelerometerName(size_t sens_index, std::string& name) const + { + return genericGetSensorName(sens_index, name); + } - virtual bool getOrientationSensorFrameName(size_t sens_index, std::string& frameName) const - { - return genericGetFrameName(sens_index, frameName); - } + virtual bool + getThreeAxisLinearAccelerometerFrameName(size_t sens_index, std::string& frameName) const + { + return genericGetFrameName(sens_index, frameName); + } - virtual bool getOrientationSensorMeasureAsRollPitchYaw(size_t sens_index, yarp::sig::Vector& rpy, double& timestamp) const - { - return genericGetMeasure(sens_index, rpy, timestamp, rpyStartIdx); - } + virtual bool getThreeAxisLinearAccelerometerMeasure(size_t sens_index, + yarp::sig::Vector& out, + double& timestamp) const + { + return genericGetMeasure(sens_index, out, timestamp, accelStartIdx); + } - private: - IMUData* m_sensorData; - std::string m_sensorName; - std::string m_frameName; + // ORIENTATION SENSORS + virtual size_t getNrOfOrientationSensors() const + { + return 1; + } - yarp::dev::MAS_status genericGetStatus(size_t sens_index) const - { - if (sens_index != 0) - { - yError() << "IMUDriver: sens_index must be equal to 0, since there is only one sensor in consideration"; - return yarp::dev::MAS_status::MAS_ERROR; - } + virtual yarp::dev::MAS_status getOrientationSensorStatus(size_t sens_index) const + { + return genericGetStatus(sens_index); + } - return yarp::dev::MAS_status::MAS_OK; - } + virtual bool getOrientationSensorName(size_t sens_index, std::string& name) const + { + return genericGetSensorName(sens_index, name); + } + + virtual bool getOrientationSensorFrameName(size_t sens_index, std::string& frameName) const + { + return genericGetFrameName(sens_index, frameName); + } + + virtual bool getOrientationSensorMeasureAsRollPitchYaw(size_t sens_index, + yarp::sig::Vector& rpy, + double& timestamp) const + { + return genericGetMeasure(sens_index, rpy, timestamp, rpyStartIdx); + } - bool genericGetSensorName(size_t sens_index, std::string& name) const +private: + IMUData* m_sensorData; + std::string m_sensorName; + std::string m_frameName; + + yarp::dev::MAS_status genericGetStatus(size_t sens_index) const + { + if (sens_index != 0) { - if (sens_index != 0) - { - yError() << "IMUDriver: sens_index must be equal to 0, since there is only one sensor in consideration"; - return false; - } - - name = m_sensorName; - return true; + yError() << "IMUDriver: sens_index must be equal to 0, since there is only one sensor " + "in consideration"; + return yarp::dev::MAS_status::MAS_ERROR; } - bool genericGetFrameName(size_t sens_index, std::string& frameName) const + return yarp::dev::MAS_status::MAS_OK; + } + + bool genericGetSensorName(size_t sens_index, std::string& name) const + { + if (sens_index != 0) { - if (sens_index != 0) - { - yError() << "IMUDriver: sens_index must be equal to 0, since there is only one sensor in consideration"; - return false; - } - - frameName = m_frameName; - return true; + yError() << "IMUDriver: sens_index must be equal to 0, since there is only one sensor " + "in consideration"; + return false; } - bool genericGetMeasure(size_t sens_index, yarp::sig::Vector &out, double ×tamp, size_t startIdx) const { + name = m_sensorName; + return true; + } + + bool genericGetFrameName(size_t sens_index, std::string& frameName) const + { + if (sens_index != 0) + { + yError() << "IMUDriver: sens_index must be equal to 0, since there is only one sensor " + "in consideration"; + return false; + } - if (sens_index != 0) - { - yError() << "IMUDriver: sens_index must be equal to 0, since there is only one sensor in consideration"; - return false; - } + frameName = m_frameName; + return true; + } - out.resize(3); - std::lock_guard lock(m_sensorData->m_mutex); - out[0] = m_sensorData->m_data[startIdx]; - out[1] = m_sensorData->m_data[startIdx + 1]; - out[2] = m_sensorData->m_data[startIdx + 2]; + bool genericGetMeasure(size_t sens_index, + yarp::sig::Vector& out, + double& timestamp, + size_t startIdx) const + { - timestamp = m_sensorData->simTime; - return true; + if (sens_index != 0) + { + yError() << "IMUDriver: sens_index must be equal to 0, since there is only one sensor " + "in consideration"; + return false; } + + out.resize(3); + std::lock_guard lock(m_sensorData->m_mutex); + out[0] = m_sensorData->m_data[startIdx]; + out[1] = m_sensorData->m_data[startIdx + 1]; + out[2] = m_sensorData->m_data[startIdx + 2]; + + timestamp = m_sensorData->simTime; + return true; + } }; diff --git a/plugins/imu/singleton-imu/Handler.cc b/plugins/imu/singleton-imu/Handler.cc index 3f842e7..977a5da 100644 --- a/plugins/imu/singleton-imu/Handler.cc +++ b/plugins/imu/singleton-imu/Handler.cc @@ -2,19 +2,19 @@ #include #include -namespace gzyarp +namespace gzyarp { HandlerIMU* HandlerIMU::getHandler() { std::lock_guard lock(mutex()); - if (!s_handle) + if (!s_handle) { s_handle = new HandlerIMU(); if (!s_handle) yError() << "Error while calling gzyarp::HandlerIMU constructor"; } - + return s_handle; } @@ -24,19 +24,21 @@ bool HandlerIMU::setSensor(IMUData* _sensorDataPtr) std::string sensorScopedName = _sensorDataPtr->sensorScopedName; SensorsMap::iterator sensor = m_sensorsMap.find(sensorScopedName); - if (sensor != m_sensorsMap.end()) + if (sensor != m_sensorsMap.end()) ret = true; - else + else { - //sensor does not exists. Add to map - if (!m_sensorsMap.insert(std::pair(sensorScopedName, _sensorDataPtr)).second) + // sensor does not exists. Add to map + if (!m_sensorsMap.insert(std::pair(sensorScopedName, _sensorDataPtr)) + .second) { yError() << "Error in gzyarp::HandlerIMU while inserting a new sensor pointer!"; - yError() << " The name of the sensor is already present but the pointer does not match with the one already registered!"; - yError() << " This should not happen, as the scoped name should be unique in Gazebo. Fatal error."; + yError() << " The name of the sensor is already present but the pointer does not match " + "with the one already registered!"; + yError() << " This should not happen, as the scoped name should be unique in Gazebo. " + "Fatal error."; ret = false; - } - else + } else ret = true; } return ret; @@ -47,11 +49,10 @@ IMUData* HandlerIMU::getSensor(const std::string& sensorScopedName) const IMUData* tmp; SensorsMap::const_iterator sensor = m_sensorsMap.find(sensorScopedName); - if (sensor != m_sensorsMap.end()) + if (sensor != m_sensorsMap.end()) { tmp = sensor->second; - } - else + } else { yError() << "Sensor was not found: " << sensorScopedName; tmp = NULL; @@ -59,35 +60,30 @@ IMUData* HandlerIMU::getSensor(const std::string& sensorScopedName) const return tmp; } -void HandlerIMU::removeSensor(const std::string &sensorName) +void HandlerIMU::removeSensor(const std::string& sensorName) { SensorsMap::iterator sensor = m_sensorsMap.find(sensorName); - if (sensor != m_sensorsMap.end()) + if (sensor != m_sensorsMap.end()) { m_sensorsMap.erase(sensor); - } - else + } else { yError() << "Could not remove sensor " << sensorName << ". Sensor was not found"; } } -HandlerIMU::HandlerIMU() : m_sensorsMap() +HandlerIMU::HandlerIMU() + : m_sensorsMap() { m_sensorsMap.clear(); } HandlerIMU* HandlerIMU::s_handle = NULL; - std::mutex& HandlerIMU::mutex() { static std::mutex s_mutex; return s_mutex; } -} - - - - +} // namespace gzyarp diff --git a/plugins/imu/singleton-imu/Handler.hh b/plugins/imu/singleton-imu/Handler.hh index 6137f8f..3346970 100644 --- a/plugins/imu/singleton-imu/Handler.hh +++ b/plugins/imu/singleton-imu/Handler.hh @@ -1,41 +1,38 @@ #include +#include #include #include #include #include -#include struct IMUData { - std::mutex m_mutex; - std::array m_data; - std::string sensorScopedName; - double simTime; + std::mutex m_mutex; + std::array m_data; + std::string sensorScopedName; + double simTime; }; -namespace gzyarp +namespace gzyarp { class HandlerIMU -{ - public: - static HandlerIMU* getHandler(); +{ +public: + static HandlerIMU* getHandler(); - bool setSensor(IMUData* _sensorDataPtr); + bool setSensor(IMUData* _sensorDataPtr); - IMUData* getSensor(const std::string& sensorScopedName) const; + IMUData* getSensor(const std::string& sensorScopedName) const; - void removeSensor(const std::string& sensorName); - - private: - HandlerIMU(); - static HandlerIMU* s_handle; - static std::mutex& mutex(); - typedef std::map SensorsMap; - SensorsMap m_sensorsMap; // map of known sensors + void removeSensor(const std::string& sensorName); +private: + HandlerIMU(); + static HandlerIMU* s_handle; + static std::mutex& mutex(); + typedef std::map SensorsMap; + SensorsMap m_sensorsMap; // map of known sensors }; -} - - +} // namespace gzyarp diff --git a/plugins/laser/Laser.cc b/plugins/laser/Laser.cc index 08c9b08..9b30d0e 100644 --- a/plugins/laser/Laser.cc +++ b/plugins/laser/Laser.cc @@ -1,60 +1,59 @@ -#include -#include -#include +#include "LaserDriver.cpp" #include -#include #include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include #include -#include -#include -#include -#include -#include -#include "LaserDriver.cpp" - +#include +#include using namespace gz; using namespace sim; using namespace systems; -namespace gzyarp +namespace gzyarp { -class Laser - : public System, - public ISystemConfigure, - public ISystemPreUpdate, - public ISystemPostUpdate - +class Laser : public System, + public ISystemConfigure, + public ISystemPreUpdate, + public ISystemPostUpdate + { - public: - - Laser() : m_deviceRegistered(false) +public: + Laser() + : m_deviceRegistered(false) { } - + virtual ~Laser() { - if (m_deviceRegistered) - { - Handler::getHandler()->removeDevice(m_deviceScopedName); - m_deviceRegistered = false; - } - - if( m_laserDriver.isValid() ) m_laserDriver.close(); - HandlerLaser::getHandler()->removeSensor(sensorScopedName); - yarp::os::Network::fini(); + if (m_deviceRegistered) + { + Handler::getHandler()->removeDevice(m_deviceScopedName); + m_deviceRegistered = false; + } + + if (m_laserDriver.isValid()) + m_laserDriver.close(); + HandlerLaser::getHandler()->removeSensor(sensorScopedName); + yarp::os::Network::fini(); } - - virtual void Configure(const Entity &_entity, - const std::shared_ptr &_sdf, - EntityComponentManager &_ecm, - EventManager &/*_eventMgr*/) override - { + + virtual void Configure(const Entity& _entity, + const std::shared_ptr& _sdf, + EntityComponentManager& _ecm, + EventManager& /*_eventMgr*/) override + { yarp::os::Network::init(); if (!yarp::os::Network::checkNetwork()) { @@ -62,15 +61,17 @@ class Laser return; } - ::yarp::dev::Drivers::factory().add(new ::yarp::dev::DriverCreatorOf< ::yarp::dev::gzyarp::LaserDriver> - ("gazebo_laser", "", "LaserDriver")); + ::yarp::dev::Drivers::factory().add( + new ::yarp::dev::DriverCreatorOf<::yarp::dev::gzyarp::LaserDriver>("gazebo_laser", + "", + "LaserDriver")); ::yarp::os::Property driver_properties; bool wipe = false; - if (_sdf->HasElement("yarpConfigurationFile")) + if (_sdf->HasElement("yarpConfigurationFile")) { - std::string ini_file_path = _sdf->Get("yarpConfigurationFile"); - driver_properties.fromConfigFile(ini_file_path.c_str(),wipe); + std::string ini_file_path = _sdf->Get("yarpConfigurationFile"); + driver_properties.fromConfigFile(ini_file_path.c_str(), wipe); if (!driver_properties.check("sensorName")) { yError() << "gz-sim-yarp-laser-system : missing sensorName parameter"; @@ -81,58 +82,60 @@ class Laser yError() << "gz-sim-yarp-laser-system : missing parentLinkName parameter"; return; } - yInfo() << "gz-sim-yarp-laser-system: configuration of sensor " << driver_properties.find("sensorName").asString() + yInfo() << "gz-sim-yarp-laser-system: configuration of sensor " + << driver_properties.find("sensorName").asString() << " loaded from yarpConfigurationFile : " << ini_file_path << "\n"; - } - else + } else { yError() << "gz-sim-yarp-laser-system : missing yarpConfigurationFile element"; - return; + return; } std::string sensorName = driver_properties.find("sensorName").asString(); std::string parentLinkName = driver_properties.find("parentLinkName").asString(); auto model = Model(_entity); auto parentLink = model.LinkByName(_ecm, parentLinkName); - this->sensor = _ecm.EntityByComponents( - components::ParentEntity(parentLink), - components::Name(sensorName), - components::Sensor()); - + this->sensor = _ecm.EntityByComponents(components::ParentEntity(parentLink), + components::Name(sensorName), + components::Sensor()); + sensorScopedName = scopedName(this->sensor, _ecm); this->laserData.sensorScopedName = sensorScopedName; driver_properties.put(YarpLaserScopedName.c_str(), sensorScopedName.c_str()); if (!driver_properties.check("yarpDeviceName")) { - yError() << "gz-sim-yarp-laser-system : missing yarpDeviceName parameter for device" << sensorScopedName; + yError() << "gz-sim-yarp-laser-system : missing yarpDeviceName parameter for device" + << sensorScopedName; return; } - //Insert the pointer in the singleton handler for retriving it in the yarp driver + // Insert the pointer in the singleton handler for retriving it in the yarp driver HandlerLaser::getHandler()->setSensor(&(this->laserData)); - driver_properties.put("device","gazebo_laser"); + driver_properties.put("device", "gazebo_laser"); driver_properties.put("sensor_name", sensorName); - if( !m_laserDriver.open(driver_properties) ) + if (!m_laserDriver.open(driver_properties)) { - yError()<<"gz-sim-yarp-laser-system Plugin failed: error in opening yarp driver"; + yError() << "gz-sim-yarp-laser-system Plugin failed: error in opening yarp driver"; return; } - m_deviceScopedName = sensorScopedName + "/" + driver_properties.find("yarpDeviceName").asString(); + m_deviceScopedName + = sensorScopedName + "/" + driver_properties.find("yarpDeviceName").asString(); - if(!Handler::getHandler()->setDevice(m_deviceScopedName, &m_laserDriver)) + if (!Handler::getHandler()->setDevice(m_deviceScopedName, &m_laserDriver)) { - yError()<<"gz-sim-yarp-laser-system: failed setting scopedDeviceName(=" << m_deviceScopedName << ")"; + yError() << "gz-sim-yarp-laser-system: failed setting scopedDeviceName(=" + << m_deviceScopedName << ")"; return; } m_deviceRegistered = true; yInfo() << "Registered YARP device with instance name:" << m_deviceScopedName; } - virtual void PreUpdate(const UpdateInfo &_info, - EntityComponentManager &_ecm) override + virtual void PreUpdate(const UpdateInfo& _info, EntityComponentManager& _ecm) override { - if(!this->laserInitialized && _ecm.ComponentData(sensor).has_value()) + if (!this->laserInitialized + && _ecm.ComponentData(sensor).has_value()) { this->laserInitialized = true; auto laserTopicName = _ecm.ComponentData(sensor).value(); @@ -140,8 +143,7 @@ class Laser } } - virtual void PostUpdate(const UpdateInfo &_info, - const EntityComponentManager &_ecm) override + virtual void PostUpdate(const UpdateInfo& _info, const EntityComponentManager& _ecm) override { gz::msgs::LaserScan laserMsg; { @@ -152,22 +154,21 @@ class Laser std::lock_guard lock(laserData.m_mutex); laserData.m_data.resize(laserMsg.ranges().size()); - for (size_t i=0; i< laserMsg.ranges().size(); i++) + for (size_t i = 0; i < laserMsg.ranges().size(); i++) { - laserData.m_data[i] = laserMsg.ranges(i); + laserData.m_data[i] = laserMsg.ranges(i); } - - laserData.simTime = _info.simTime.count()/1e9; - + + laserData.simTime = _info.simTime.count() / 1e9; } - void laserCb(const gz::msgs::LaserScan &_msg) + void laserCb(const gz::msgs::LaserScan& _msg) { std::lock_guard lock(this->laserMsgMutex); laserMsg = _msg; } - - private: + +private: Entity sensor; yarp::dev::PolyDriver m_laserDriver; std::string m_deviceScopedName; @@ -180,7 +181,7 @@ class Laser std::mutex laserMsgMutex; }; -} +} // namespace gzyarp // Register plugin GZ_ADD_PLUGIN(gzyarp::Laser, @@ -188,4 +189,3 @@ GZ_ADD_PLUGIN(gzyarp::Laser, gzyarp::Laser::ISystemConfigure, gzyarp::Laser::ISystemPreUpdate, gzyarp::Laser::ISystemPostUpdate) - diff --git a/plugins/laser/LaserDriver.cpp b/plugins/laser/LaserDriver.cpp index 59fa8fa..2b05d98 100644 --- a/plugins/laser/LaserDriver.cpp +++ b/plugins/laser/LaserDriver.cpp @@ -1,127 +1,126 @@ -#include -#include +#include "../../libraries/singleton-devices/Handler.hh" +#include "singleton-laser/Handler.hh" +#include +#include #include #include #include #include -#include -#include -#include "singleton-laser/Handler.hh" -#include "../../libraries/singleton-devices/Handler.hh" - +#include +#include -namespace yarp { - namespace dev { - namespace gzyarp { - class LaserDriver; - } - } +namespace yarp +{ +namespace dev +{ +namespace gzyarp +{ +class LaserDriver; } +} // namespace dev +} // namespace yarp const std::string YarpLaserScopedName = "sensorScopedName"; -class yarp::dev::gzyarp::LaserDriver: - public yarp::dev::Lidar2DDeviceBase, - public yarp::dev::DeviceDriver +class yarp::dev::gzyarp::LaserDriver : public yarp::dev::Lidar2DDeviceBase, + public yarp::dev::DeviceDriver { - public: - LaserDriver(){} - virtual ~LaserDriver(){} +public: + LaserDriver() + { + } + virtual ~LaserDriver() + { + } + + // DEVICE DRIVER + virtual bool open(yarp::os::Searchable& config) + { + std::string sensorScopedName(config.find(YarpLaserScopedName.c_str()).asString().c_str()); - //DEVICE DRIVER - virtual bool open(yarp::os::Searchable& config) + std::string separator = "/"; + auto pos = config.find("sensor_name").asString().find_last_of(separator); + if (pos == std::string::npos) { - std::string sensorScopedName(config.find(YarpLaserScopedName.c_str()).asString().c_str()); - - std::string separator = "/"; - auto pos = config.find("sensor_name").asString().find_last_of(separator); - if (pos == std::string::npos) - { - m_sensorName = config.find("sensor_name").asString(); - } - else - { - m_sensorName = config.find("sensor_name").asString().substr(pos + separator.size() - 1); - } - m_frameName = m_sensorName; - m_sensorData = ::gzyarp::HandlerLaser::getHandler()->getSensor(sensorScopedName); - - if (!m_sensorData) - { - yError() << "Error, Laser sensor was not found"; - return false; - } - if (this->parseConfiguration(config) == false) - { - yError() << "error parsing parameters"; - return false; - } - m_device_status = yarp::dev::IRangefinder2D::Device_status::DEVICE_OK_IN_USE; - - return true; + m_sensorName = config.find("sensor_name").asString(); + } else + { + m_sensorName = config.find("sensor_name").asString().substr(pos + separator.size() - 1); } + m_frameName = m_sensorName; + m_sensorData = ::gzyarp::HandlerLaser::getHandler()->getSensor(sensorScopedName); - virtual bool close() + if (!m_sensorData) { - return true; + yError() << "Error, Laser sensor was not found"; + return false; } - - //Lidar2DDeviceBase - bool acquireDataFromHW() + if (this->parseConfiguration(config) == false) { - return true; + yError() << "error parsing parameters"; + return false; } + m_device_status = yarp::dev::IRangefinder2D::Device_status::DEVICE_OK_IN_USE; + return true; + } - bool getRawData (yarp::sig::Vector& ranges, double *timestamp) - { - std::lock_guard lock(m_sensorData->m_mutex); - - ranges.resize(m_sensorData->m_data.size(), 0.0); - for (size_t i=0; i< m_sensorData->m_data.size(); i++) - { - ranges[i] = m_sensorData->m_data[i]; - } - *timestamp = m_sensorData->simTime; - - return true; + virtual bool close() + { + return true; + } - } + // Lidar2DDeviceBase + bool acquireDataFromHW() + { + return true; + } + bool getRawData(yarp::sig::Vector& ranges, double* timestamp) + { + std::lock_guard lock(m_sensorData->m_mutex); - //IRangefinder2D - virtual bool setDistanceRange (double min, double max) + ranges.resize(m_sensorData->m_data.size(), 0.0); + for (size_t i = 0; i < m_sensorData->m_data.size(); i++) { - std::lock_guard guard(m_mutex); - yError() << "setDistanceRange() Not yet implemented"; - return false; + ranges[i] = m_sensorData->m_data[i]; } + *timestamp = m_sensorData->simTime; - virtual bool setScanLimits (double min, double max) - { - std::lock_guard guard(m_mutex); - yError() << "setScanLimits() Not yet implemented"; - return false; - } + return true; + } - virtual bool setHorizontalResolution (double step) - { - std::lock_guard guard(m_mutex); - yError() << "setHorizontalResolution() Not yet implemented"; - return false; - } + // IRangefinder2D + virtual bool setDistanceRange(double min, double max) + { + std::lock_guard guard(m_mutex); + yError() << "setDistanceRange() Not yet implemented"; + return false; + } - virtual bool setScanRate (double rate) - { - std::lock_guard guard(m_mutex); - yError() << "setScanRate() Not yet implemented"; - return false; - } + virtual bool setScanLimits(double min, double max) + { + std::lock_guard guard(m_mutex); + yError() << "setScanLimits() Not yet implemented"; + return false; + } + virtual bool setHorizontalResolution(double step) + { + std::lock_guard guard(m_mutex); + yError() << "setHorizontalResolution() Not yet implemented"; + return false; + } - private: - LaserData* m_sensorData; - std::string m_sensorName; - std::string m_frameName; + virtual bool setScanRate(double rate) + { + std::lock_guard guard(m_mutex); + yError() << "setScanRate() Not yet implemented"; + return false; + } +private: + LaserData* m_sensorData; + std::string m_sensorName; + std::string m_frameName; }; diff --git a/plugins/laser/singleton-laser/Handler.cc b/plugins/laser/singleton-laser/Handler.cc index e2d16b7..605a95e 100644 --- a/plugins/laser/singleton-laser/Handler.cc +++ b/plugins/laser/singleton-laser/Handler.cc @@ -2,19 +2,19 @@ #include #include -namespace gzyarp +namespace gzyarp { HandlerLaser* HandlerLaser::getHandler() { std::lock_guard lock(mutex()); - if (!s_handle) + if (!s_handle) { s_handle = new HandlerLaser(); if (!s_handle) yError() << "Error while calling gzyarp::HandlerLaser constructor"; } - + return s_handle; } @@ -24,19 +24,22 @@ bool HandlerLaser::setSensor(LaserData* _sensorDataPtr) std::string sensorScopedName = _sensorDataPtr->sensorScopedName; SensorsMap::iterator sensor = m_sensorsMap.find(sensorScopedName); - if (sensor != m_sensorsMap.end()) + if (sensor != m_sensorsMap.end()) ret = true; - else + else { - //sensor does not exists. Add to map - if (!m_sensorsMap.insert(std::pair(sensorScopedName, _sensorDataPtr)).second) + // sensor does not exists. Add to map + if (!m_sensorsMap + .insert(std::pair(sensorScopedName, _sensorDataPtr)) + .second) { yError() << "Error in gzyarp::HandlerLaser while inserting a new sensor pointer!"; - yError() << " The name of the sensor is already present but the pointer does not match with the one already registered!"; - yError() << " This should not happen, as the scoped name should be unique in Gazebo. Fatal error."; + yError() << " The name of the sensor is already present but the pointer does not match " + "with the one already registered!"; + yError() << " This should not happen, as the scoped name should be unique in Gazebo. " + "Fatal error."; ret = false; - } - else + } else ret = true; } return ret; @@ -47,11 +50,10 @@ LaserData* HandlerLaser::getSensor(const std::string& sensorScopedName) const LaserData* tmp; SensorsMap::const_iterator sensor = m_sensorsMap.find(sensorScopedName); - if (sensor != m_sensorsMap.end()) + if (sensor != m_sensorsMap.end()) { tmp = sensor->second; - } - else + } else { yError() << "Sensor was not found: " << sensorScopedName; tmp = NULL; @@ -59,35 +61,30 @@ LaserData* HandlerLaser::getSensor(const std::string& sensorScopedName) const return tmp; } -void HandlerLaser::removeSensor(const std::string &sensorName) +void HandlerLaser::removeSensor(const std::string& sensorName) { SensorsMap::iterator sensor = m_sensorsMap.find(sensorName); - if (sensor != m_sensorsMap.end()) + if (sensor != m_sensorsMap.end()) { m_sensorsMap.erase(sensor); - } - else + } else { yError() << "Could not remove sensor " << sensorName << ". Sensor was not found"; } } -HandlerLaser::HandlerLaser() : m_sensorsMap() +HandlerLaser::HandlerLaser() + : m_sensorsMap() { m_sensorsMap.clear(); } HandlerLaser* HandlerLaser::s_handle = NULL; - std::mutex& HandlerLaser::mutex() { static std::mutex s_mutex; return s_mutex; } -} - - - - +} // namespace gzyarp diff --git a/plugins/laser/singleton-laser/Handler.hh b/plugins/laser/singleton-laser/Handler.hh index 8d7f37b..f17e05c 100644 --- a/plugins/laser/singleton-laser/Handler.hh +++ b/plugins/laser/singleton-laser/Handler.hh @@ -1,40 +1,38 @@ +#include #include #include #include #include #include -#include struct LaserData { - std::mutex m_mutex; - std::vector m_data; - std::string sensorScopedName; - double simTime; + std::mutex m_mutex; + std::vector m_data; + std::string sensorScopedName; + double simTime; }; -namespace gzyarp +namespace gzyarp { class HandlerLaser -{ - public: - static HandlerLaser* getHandler(); +{ +public: + static HandlerLaser* getHandler(); - bool setSensor(LaserData* _sensorDataPtr); + bool setSensor(LaserData* _sensorDataPtr); - LaserData* getSensor(const std::string& sensorScopedName) const; + LaserData* getSensor(const std::string& sensorScopedName) const; - void removeSensor(const std::string& sensorName); - - private: - HandlerLaser(); - static HandlerLaser* s_handle; - static std::mutex& mutex(); - typedef std::map SensorsMap; - SensorsMap m_sensorsMap; // map of known sensors + void removeSensor(const std::string& sensorName); +private: + HandlerLaser(); + static HandlerLaser* s_handle; + static std::mutex& mutex(); + typedef std::map SensorsMap; + SensorsMap m_sensorsMap; // map of known sensors }; -} - +} // namespace gzyarp diff --git a/plugins/robotinterface/RobotInterface.cc b/plugins/robotinterface/RobotInterface.cc index 8ea93cf..27a5661 100644 --- a/plugins/robotinterface/RobotInterface.cc +++ b/plugins/robotinterface/RobotInterface.cc @@ -1,137 +1,146 @@ +#include "../../libraries/singleton-devices/Handler.hh" +#include #include -#include #include -#include -#include -#include +#include #include #include #include -#include "../../libraries/singleton-devices/Handler.hh" - +#include +#include using namespace gz; using namespace sim; using namespace systems; -namespace gzyarp +namespace gzyarp { - -class RobotInterface - : public System, - public ISystemConfigure + +class RobotInterface : public System, public ISystemConfigure { - public: - RobotInterface(): m_robotInterfaceCorrectlyStarted(false) - { - } - - virtual ~RobotInterface() - { - CloseRobotInterface(); - yarp::os::Network::fini(); - } +public: + RobotInterface() + : m_robotInterfaceCorrectlyStarted(false) + { + } - void CloseRobotInterface() + virtual ~RobotInterface() + { + CloseRobotInterface(); + yarp::os::Network::fini(); + } + + void CloseRobotInterface() + { + if (m_robotInterfaceCorrectlyStarted) { - if(m_robotInterfaceCorrectlyStarted) + // Close robotinterface + bool ok = m_xmlRobotInterfaceResult.robot.enterPhase( + yarp::robotinterface::ActionPhaseInterrupt1); + if (!ok) { - // Close robotinterface - bool ok = m_xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::ActionPhaseInterrupt1); - if (!ok) - { - yError() << "gz-sim-yarp-robotinterface-system: impossible to run phase ActionPhaseInterrupt1 robotinterface"; - } - ok = m_xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::ActionPhaseShutdown); - if (!ok) - { - yError() << "gz-sim-yarp-robotinterface-system: impossible to run phase ActionPhaseShutdown in robotinterface"; - } - m_robotInterfaceCorrectlyStarted = false; + yError() << "gz-sim-yarp-robotinterface-system: impossible to run phase " + "ActionPhaseInterrupt1 robotinterface"; } + ok = m_xmlRobotInterfaceResult.robot.enterPhase( + yarp::robotinterface::ActionPhaseShutdown); + if (!ok) + { + yError() << "gz-sim-yarp-robotinterface-system: impossible to run phase " + "ActionPhaseShutdown in robotinterface"; + } + m_robotInterfaceCorrectlyStarted = false; } + } - virtual void Configure(const Entity &_entity, - const std::shared_ptr &_sdf, - EntityComponentManager &_ecm, - EventManager &/*_eventMgr*/) override + virtual void Configure(const Entity& _entity, + const std::shared_ptr& _sdf, + EntityComponentManager& _ecm, + EventManager& /*_eventMgr*/) override + { + yarp::os::Network::init(); + if (!yarp::os::Network::checkNetwork()) { - yarp::os::Network::init(); - if (!yarp::os::Network::checkNetwork()) - { - yError() << "gz-sim-yarp-robotinterface-system : yarp network does not seem to be available, is the yarpserver running?"; - return; - } - auto model = Model(_entity); + yError() << "gz-sim-yarp-robotinterface-system : yarp network does not seem to be " + "available, is the yarpserver running?"; + return; + } + auto model = Model(_entity); - bool loaded_configuration = false; - if (_sdf->HasElement("yarpRobotInterfaceConfigurationFile")) + bool loaded_configuration = false; + if (_sdf->HasElement("yarpRobotInterfaceConfigurationFile")) + { + robotinterface_file_name = _sdf->Get("yarpRobotInterfaceConfigurationFil" + "e"); + if (robotinterface_file_name == "") { - robotinterface_file_name = _sdf->Get("yarpRobotInterfaceConfigurationFile"); - if (robotinterface_file_name == "") + yError() << "gz-sim-yarp-robotinterface-system error: failure in finding " + "robotinterface configuration for model" + << model.Name(_ecm) << "\n" + << "gz-sim-yarp-robotinterface-system error: " + "yarpRobotInterfaceConfigurationFile : " + << robotinterface_file_name; + loaded_configuration = false; + } else + { + m_xmlRobotInterfaceResult + = m_xmlRobotInterfaceReader.getRobotFromFile(robotinterface_file_name); + if (m_xmlRobotInterfaceResult.parsingIsSuccessful) { - yError() << "gz-sim-yarp-robotinterface-system error: failure in finding robotinterface configuration for model" << model.Name(_ecm) << "\n" - << "gz-sim-yarp-robotinterface-system error: yarpRobotInterfaceConfigurationFile : " << robotinterface_file_name; - loaded_configuration = false; - } - else + loaded_configuration = true; + } else { - m_xmlRobotInterfaceResult = m_xmlRobotInterfaceReader.getRobotFromFile(robotinterface_file_name); - if (m_xmlRobotInterfaceResult.parsingIsSuccessful) - { - loaded_configuration = true; - } - else - { - yError() << "gz-sim-yarp-robotinterface-system error: failure in parsing robotinterface configuration for model" << model.Name(_ecm) << "\n" - << "gz-sim-yarp-robotinterface-system error: yarpRobotInterfaceConfigurationFile : " << robotinterface_file_name; - loaded_configuration = false; - } + yError() << "gz-sim-yarp-robotinterface-system error: failure in parsing " + "robotinterface configuration for model" + << model.Name(_ecm) << "\n" + << "gz-sim-yarp-robotinterface-system error: " + "yarpRobotInterfaceConfigurationFile : " + << robotinterface_file_name; + loaded_configuration = false; } } - if (!loaded_configuration) - { - yError() << "gz-sim-yarp-robotinterface-system : xml file specified in yarpRobotInterfaceConfigurationFile not found or not loaded."; - return; - } + } + if (!loaded_configuration) + { + yError() << "gz-sim-yarp-robotinterface-system : xml file specified in " + "yarpRobotInterfaceConfigurationFile not found or not loaded."; + return; + } - yarp::dev::PolyDriverList externalDriverList; + yarp::dev::PolyDriverList externalDriverList; - Handler::getHandler()->getDevicesAsPolyDriverList(scopedName(model.Entity(), _ecm), externalDriverList, m_deviceScopedNames); + Handler::getHandler()->getDevicesAsPolyDriverList(scopedName(model.Entity(), _ecm), + externalDriverList, + m_deviceScopedNames); - bool ok = m_xmlRobotInterfaceResult.robot.setExternalDevices(externalDriverList); - if (!ok) - { - yError() << "gz-sim-yarp-robotinterface-system : impossible to set external devices"; - return; - } - - // Start robotinterface - ok = m_xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::ActionPhaseStartup); - if (!ok) - { - yError() << "gz-sim-yarp-robotinterface-system : impossible to start robotinterface"; - m_xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::ActionPhaseInterrupt1); - m_xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::ActionPhaseShutdown); - return; - } - m_robotInterfaceCorrectlyStarted = true; - + bool ok = m_xmlRobotInterfaceResult.robot.setExternalDevices(externalDriverList); + if (!ok) + { + yError() << "gz-sim-yarp-robotinterface-system : impossible to set external devices"; + return; } - - private: - yarp::robotinterface::XMLReader m_xmlRobotInterfaceReader; - yarp::robotinterface::XMLReaderResult m_xmlRobotInterfaceResult; - std::string robotinterface_file_name; - std::vector m_deviceScopedNames; - bool m_robotInterfaceCorrectlyStarted; + // Start robotinterface + ok = m_xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::ActionPhaseStartup); + if (!ok) + { + yError() << "gz-sim-yarp-robotinterface-system : impossible to start robotinterface"; + m_xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::ActionPhaseInterrupt1); + m_xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::ActionPhaseShutdown); + return; + } + m_robotInterfaceCorrectlyStarted = true; + } +private: + yarp::robotinterface::XMLReader m_xmlRobotInterfaceReader; + yarp::robotinterface::XMLReaderResult m_xmlRobotInterfaceResult; + std::string robotinterface_file_name; + std::vector m_deviceScopedNames; + bool m_robotInterfaceCorrectlyStarted; }; -} +} // namespace gzyarp // Register plugin -GZ_ADD_PLUGIN(gzyarp::RobotInterface, - gz::sim::System, - gzyarp::RobotInterface::ISystemConfigure) \ No newline at end of file +GZ_ADD_PLUGIN(gzyarp::RobotInterface, gz::sim::System, gzyarp::RobotInterface::ISystemConfigure) \ No newline at end of file diff --git a/tests/camera/CameraTest.cc b/tests/camera/CameraTest.cc index 356dccc..002aa62 100644 --- a/tests/camera/CameraTest.cc +++ b/tests/camera/CameraTest.cc @@ -1,278 +1,274 @@ #include -#include +#include #include +#include #include -#include TEST(CameraTest, PluginTest) { - yarp::os::NetworkBase::setLocalMode(true); - - // Maximum verbosity helps with debugging - gz::common::Console::SetVerbosity(4); - - // Instantiate test fixture - gz::sim::TestFixture fixture("../../../tests/camera/model.sdf"); - - int iterations = 1000; - - fixture.Server()->Run(/*_blocking=*/true, iterations, /*_paused=*/false); - - yarp::os::Property option; - option.put("device","frameGrabber_nwc_yarp"); - option.put("remote","/camera"); - option.put("local", "/CameraTest"); - yarp::dev::PolyDriver driver; - - ASSERT_TRUE(driver.open(option)); - - yarp::dev::IFrameGrabberImage* igrabimg = nullptr; - - ASSERT_TRUE(driver.view(igrabimg)); - fixture.Server()->Run(/*_blocking=*/true, iterations, /*_paused=*/false); - - yarp::sig::ImageOf image; - - size_t maxNrOfReadingAttempts = 20; - bool readSuccessful = false; - for (size_t i=0; (i < maxNrOfReadingAttempts) && !readSuccessful ; i++) - { - readSuccessful = igrabimg->getImage(image); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } - ASSERT_TRUE(readSuccessful); - - int blue[] = {0, 0, 255}; - int dark_blue[] = {0, 0, 100}; - int grey[] = {218, 218, 218}; - int dark_grey[] = {162, 162, 162}; - - // background - grey - unsigned char *pixel = image.getPixelAddress(0,40); - EXPECT_EQ(int(pixel[0]), grey[0]); - EXPECT_EQ(int(pixel[1]), grey[1]); - EXPECT_EQ(int(pixel[2]), grey[2]); - - // timestamp - blue - pixel = image.getPixelAddress(35,22); - EXPECT_EQ(int(pixel[0]), blue[0]); - EXPECT_EQ(int(pixel[1]), blue[1]); - EXPECT_EQ(int(pixel[2]), blue[2]); - - // sphere - dark blue - pixel = image.getPixelAddress(218,221); - EXPECT_EQ(int(pixel[0]), dark_blue[0]); - EXPECT_EQ(int(pixel[1]), dark_blue[1]); - EXPECT_EQ(int(pixel[2]), dark_blue[2]); - - // ground plane - dark grey - pixel = image.getPixelAddress(289,246); - EXPECT_EQ(int(pixel[0]), dark_grey[0]); - EXPECT_EQ(int(pixel[1]), dark_grey[1]); - EXPECT_EQ(int(pixel[2]), dark_grey[2]); + yarp::os::NetworkBase::setLocalMode(true); + + // Maximum verbosity helps with debugging + gz::common::Console::SetVerbosity(4); + + // Instantiate test fixture + gz::sim::TestFixture fixture("../../../tests/camera/model.sdf"); + + int iterations = 1000; + + fixture.Server()->Run(/*_blocking=*/true, iterations, /*_paused=*/false); + + yarp::os::Property option; + option.put("device", "frameGrabber_nwc_yarp"); + option.put("remote", "/camera"); + option.put("local", "/CameraTest"); + yarp::dev::PolyDriver driver; + + ASSERT_TRUE(driver.open(option)); + + yarp::dev::IFrameGrabberImage* igrabimg = nullptr; + + ASSERT_TRUE(driver.view(igrabimg)); + fixture.Server()->Run(/*_blocking=*/true, iterations, /*_paused=*/false); + + yarp::sig::ImageOf image; + + size_t maxNrOfReadingAttempts = 20; + bool readSuccessful = false; + for (size_t i = 0; (i < maxNrOfReadingAttempts) && !readSuccessful; i++) + { + readSuccessful = igrabimg->getImage(image); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + ASSERT_TRUE(readSuccessful); + + int blue[] = {0, 0, 255}; + int dark_blue[] = {0, 0, 100}; + int grey[] = {218, 218, 218}; + int dark_grey[] = {162, 162, 162}; + + // background - grey + unsigned char* pixel = image.getPixelAddress(0, 40); + EXPECT_EQ(int(pixel[0]), grey[0]); + EXPECT_EQ(int(pixel[1]), grey[1]); + EXPECT_EQ(int(pixel[2]), grey[2]); + + // timestamp - blue + pixel = image.getPixelAddress(35, 22); + EXPECT_EQ(int(pixel[0]), blue[0]); + EXPECT_EQ(int(pixel[1]), blue[1]); + EXPECT_EQ(int(pixel[2]), blue[2]); + + // sphere - dark blue + pixel = image.getPixelAddress(218, 221); + EXPECT_EQ(int(pixel[0]), dark_blue[0]); + EXPECT_EQ(int(pixel[1]), dark_blue[1]); + EXPECT_EQ(int(pixel[2]), dark_blue[2]); + + // ground plane - dark grey + pixel = image.getPixelAddress(289, 246); + EXPECT_EQ(int(pixel[0]), dark_grey[0]); + EXPECT_EQ(int(pixel[1]), dark_grey[1]); + EXPECT_EQ(int(pixel[2]), dark_grey[2]); } - TEST(CameraTest, HorizontalFlip) { - yarp::os::NetworkBase::setLocalMode(true); - - // Maximum verbosity helps with debugging - gz::common::Console::SetVerbosity(4); - - // Instantiate test fixture - gz::sim::TestFixture fixture("../../../tests/camera/model_hor_flip.sdf"); - - int iterations = 1000; - fixture.Server()->Run(/*_blocking=*/true, iterations, /*_paused=*/false); - - yarp::os::Property option; - option.put("device","frameGrabber_nwc_yarp"); - option.put("remote","/camera"); - option.put("local", "/CameraTest"); - yarp::dev::PolyDriver driver; - - ASSERT_TRUE(driver.open(option)); - - yarp::dev::IFrameGrabberImage* igrabimg = nullptr; - - ASSERT_TRUE(driver.view(igrabimg)); - fixture.Server()->Run(/*_blocking=*/true, iterations, /*_paused=*/false); - - yarp::sig::ImageOf image; - - size_t maxNrOfReadingAttempts = 20; - bool readSuccessful = false; - for (size_t i=0; (i < maxNrOfReadingAttempts) && !readSuccessful ; i++) - { - readSuccessful = igrabimg->getImage(image); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } - ASSERT_TRUE(readSuccessful); - - int blue[] = {0, 0, 255}; - int dark_blue[] = {0, 0, 100}; - int grey[] = {218, 218, 218}; - int dark_grey[] = {162, 162, 162}; - - // background - grey - unsigned char *pixel = image.getPixelAddress(0,40); - EXPECT_EQ(int(pixel[0]), grey[0]); - EXPECT_EQ(int(pixel[1]), grey[1]); - EXPECT_EQ(int(pixel[2]), grey[2]); - - // timestamp - blue - pixel = image.getPixelAddress(35,22); - EXPECT_EQ(int(pixel[0]), blue[0]); - EXPECT_EQ(int(pixel[1]), blue[1]); - EXPECT_EQ(int(pixel[2]), blue[2]); - - // sphere - dark blue - pixel = image.getPixelAddress(420,220); - EXPECT_EQ(int(pixel[0]), dark_blue[0]); - EXPECT_EQ(int(pixel[1]), dark_blue[1]); - EXPECT_EQ(int(pixel[2]), dark_blue[2]); - - // ground plane - dark grey - pixel = image.getPixelAddress(304,247); - EXPECT_EQ(int(pixel[0]), dark_grey[0]); - EXPECT_EQ(int(pixel[1]), dark_grey[1]); - EXPECT_EQ(int(pixel[2]), dark_grey[2]); + yarp::os::NetworkBase::setLocalMode(true); + + // Maximum verbosity helps with debugging + gz::common::Console::SetVerbosity(4); + + // Instantiate test fixture + gz::sim::TestFixture fixture("../../../tests/camera/model_hor_flip.sdf"); + + int iterations = 1000; + fixture.Server()->Run(/*_blocking=*/true, iterations, /*_paused=*/false); + + yarp::os::Property option; + option.put("device", "frameGrabber_nwc_yarp"); + option.put("remote", "/camera"); + option.put("local", "/CameraTest"); + yarp::dev::PolyDriver driver; + + ASSERT_TRUE(driver.open(option)); + + yarp::dev::IFrameGrabberImage* igrabimg = nullptr; + + ASSERT_TRUE(driver.view(igrabimg)); + fixture.Server()->Run(/*_blocking=*/true, iterations, /*_paused=*/false); + + yarp::sig::ImageOf image; + + size_t maxNrOfReadingAttempts = 20; + bool readSuccessful = false; + for (size_t i = 0; (i < maxNrOfReadingAttempts) && !readSuccessful; i++) + { + readSuccessful = igrabimg->getImage(image); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + ASSERT_TRUE(readSuccessful); + + int blue[] = {0, 0, 255}; + int dark_blue[] = {0, 0, 100}; + int grey[] = {218, 218, 218}; + int dark_grey[] = {162, 162, 162}; + + // background - grey + unsigned char* pixel = image.getPixelAddress(0, 40); + EXPECT_EQ(int(pixel[0]), grey[0]); + EXPECT_EQ(int(pixel[1]), grey[1]); + EXPECT_EQ(int(pixel[2]), grey[2]); + + // timestamp - blue + pixel = image.getPixelAddress(35, 22); + EXPECT_EQ(int(pixel[0]), blue[0]); + EXPECT_EQ(int(pixel[1]), blue[1]); + EXPECT_EQ(int(pixel[2]), blue[2]); + + // sphere - dark blue + pixel = image.getPixelAddress(420, 220); + EXPECT_EQ(int(pixel[0]), dark_blue[0]); + EXPECT_EQ(int(pixel[1]), dark_blue[1]); + EXPECT_EQ(int(pixel[2]), dark_blue[2]); + + // ground plane - dark grey + pixel = image.getPixelAddress(304, 247); + EXPECT_EQ(int(pixel[0]), dark_grey[0]); + EXPECT_EQ(int(pixel[1]), dark_grey[1]); + EXPECT_EQ(int(pixel[2]), dark_grey[2]); } TEST(CameraTest, VerticalFlip) { - yarp::os::NetworkBase::setLocalMode(true); - - // Maximum verbosity helps with debugging - gz::common::Console::SetVerbosity(4); - - // Instantiate test fixture - gz::sim::TestFixture fixture("../../../tests/camera/model_ver_flip.sdf"); - - int iterations = 1000; - fixture.Server()->Run(/*_blocking=*/true, iterations, /*_paused=*/false); - - yarp::os::Property option; - option.put("device","frameGrabber_nwc_yarp"); - option.put("remote","/camera"); - option.put("local", "/CameraTest"); - yarp::dev::PolyDriver driver; - - ASSERT_TRUE(driver.open(option)); - - yarp::dev::IFrameGrabberImage* igrabimg = nullptr; - - ASSERT_TRUE(driver.view(igrabimg)); - fixture.Server()->Run(/*_blocking=*/true, iterations, /*_paused=*/false); - - yarp::sig::ImageOf image; - - size_t maxNrOfReadingAttempts = 20; - bool readSuccessful = false; - for (size_t i=0; (i < maxNrOfReadingAttempts) && !readSuccessful ; i++) - { - readSuccessful = igrabimg->getImage(image); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } - ASSERT_TRUE(readSuccessful); - - int blue[] = {0, 0, 255}; - int dark_blue[] = {0, 0, 100}; - int grey[] = {218, 218, 218}; - int dark_grey[] = {162, 162, 162}; - - // background - grey - unsigned char *pixel = image.getPixelAddress(270,400); - EXPECT_EQ(int(pixel[0]), grey[0]); - EXPECT_EQ(int(pixel[1]), grey[1]); - EXPECT_EQ(int(pixel[2]), grey[2]); - - // timestamp - blue - pixel = image.getPixelAddress(35,22); - EXPECT_EQ(int(pixel[0]), blue[0]); - EXPECT_EQ(int(pixel[1]), blue[1]); - EXPECT_EQ(int(pixel[2]), blue[2]); - - // sphere - dark blue - pixel = image.getPixelAddress(219,254); - EXPECT_EQ(int(pixel[0]), dark_blue[0]); - EXPECT_EQ(int(pixel[1]), dark_blue[1]); - EXPECT_EQ(int(pixel[2]), dark_blue[2]); - - // ground plane - dark grey - pixel = image.getPixelAddress(380,232); - EXPECT_EQ(int(pixel[0]), dark_grey[0]); - EXPECT_EQ(int(pixel[1]), dark_grey[1]); - EXPECT_EQ(int(pixel[2]), dark_grey[2]); + yarp::os::NetworkBase::setLocalMode(true); + + // Maximum verbosity helps with debugging + gz::common::Console::SetVerbosity(4); + + // Instantiate test fixture + gz::sim::TestFixture fixture("../../../tests/camera/model_ver_flip.sdf"); + + int iterations = 1000; + fixture.Server()->Run(/*_blocking=*/true, iterations, /*_paused=*/false); + + yarp::os::Property option; + option.put("device", "frameGrabber_nwc_yarp"); + option.put("remote", "/camera"); + option.put("local", "/CameraTest"); + yarp::dev::PolyDriver driver; + + ASSERT_TRUE(driver.open(option)); + + yarp::dev::IFrameGrabberImage* igrabimg = nullptr; + + ASSERT_TRUE(driver.view(igrabimg)); + fixture.Server()->Run(/*_blocking=*/true, iterations, /*_paused=*/false); + + yarp::sig::ImageOf image; + + size_t maxNrOfReadingAttempts = 20; + bool readSuccessful = false; + for (size_t i = 0; (i < maxNrOfReadingAttempts) && !readSuccessful; i++) + { + readSuccessful = igrabimg->getImage(image); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + ASSERT_TRUE(readSuccessful); + + int blue[] = {0, 0, 255}; + int dark_blue[] = {0, 0, 100}; + int grey[] = {218, 218, 218}; + int dark_grey[] = {162, 162, 162}; + + // background - grey + unsigned char* pixel = image.getPixelAddress(270, 400); + EXPECT_EQ(int(pixel[0]), grey[0]); + EXPECT_EQ(int(pixel[1]), grey[1]); + EXPECT_EQ(int(pixel[2]), grey[2]); + + // timestamp - blue + pixel = image.getPixelAddress(35, 22); + EXPECT_EQ(int(pixel[0]), blue[0]); + EXPECT_EQ(int(pixel[1]), blue[1]); + EXPECT_EQ(int(pixel[2]), blue[2]); + + // sphere - dark blue + pixel = image.getPixelAddress(219, 254); + EXPECT_EQ(int(pixel[0]), dark_blue[0]); + EXPECT_EQ(int(pixel[1]), dark_blue[1]); + EXPECT_EQ(int(pixel[2]), dark_blue[2]); + + // ground plane - dark grey + pixel = image.getPixelAddress(380, 232); + EXPECT_EQ(int(pixel[0]), dark_grey[0]); + EXPECT_EQ(int(pixel[1]), dark_grey[1]); + EXPECT_EQ(int(pixel[2]), dark_grey[2]); } - TEST(CameraTest, HorizontalVerticalFlip) { - yarp::os::NetworkBase::setLocalMode(true); - - // Maximum verbosity helps with debugging - gz::common::Console::SetVerbosity(4); - - // Instantiate test fixture - gz::sim::TestFixture fixture("../../../tests/camera/model_hor_ver_flip.sdf"); - - int iterations = 1000; - fixture.Server()->Run(/*_blocking=*/true, iterations, /*_paused=*/false); - - yarp::os::Property option; - option.put("device","frameGrabber_nwc_yarp"); - option.put("remote","/camera"); - option.put("local", "/CameraTest"); - yarp::dev::PolyDriver driver; - - ASSERT_TRUE(driver.open(option)); - - yarp::dev::IFrameGrabberImage* igrabimg = nullptr; - - ASSERT_TRUE(driver.view(igrabimg)); - fixture.Server()->Run(/*_blocking=*/true, iterations, /*_paused=*/false); - - yarp::sig::ImageOf image; - - size_t maxNrOfReadingAttempts = 20; - bool readSuccessful = false; - for (size_t i=0; (i < maxNrOfReadingAttempts) && !readSuccessful ; i++) - { - readSuccessful = igrabimg->getImage(image); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } - ASSERT_TRUE(readSuccessful); - - int blue[] = {0, 0, 255}; - int dark_blue[] = {0, 0, 100}; - int grey[] = {218, 218, 218}; - int dark_grey[] = {162, 162, 162}; - - // background - grey - unsigned char *pixel = image.getPixelAddress(301,389); - EXPECT_EQ(int(pixel[0]), grey[0]); - EXPECT_EQ(int(pixel[1]), grey[1]); - EXPECT_EQ(int(pixel[2]), grey[2]); - - // timestamp - blue - pixel = image.getPixelAddress(35,22); - EXPECT_EQ(int(pixel[0]), blue[0]); - EXPECT_EQ(int(pixel[1]), blue[1]); - EXPECT_EQ(int(pixel[2]), blue[2]); - - // sphere - dark blue - pixel = image.getPixelAddress(416,259); - EXPECT_EQ(int(pixel[0]), dark_blue[0]); - EXPECT_EQ(int(pixel[1]), dark_blue[1]); - EXPECT_EQ(int(pixel[2]), dark_blue[2]); - - // ground plane - dark grey - pixel = image.getPixelAddress(334,233); - EXPECT_EQ(int(pixel[0]), dark_grey[0]); - EXPECT_EQ(int(pixel[1]), dark_grey[1]); - EXPECT_EQ(int(pixel[2]), dark_grey[2]); + yarp::os::NetworkBase::setLocalMode(true); + + // Maximum verbosity helps with debugging + gz::common::Console::SetVerbosity(4); + + // Instantiate test fixture + gz::sim::TestFixture fixture("../../../tests/camera/model_hor_ver_flip.sdf"); + + int iterations = 1000; + fixture.Server()->Run(/*_blocking=*/true, iterations, /*_paused=*/false); + + yarp::os::Property option; + option.put("device", "frameGrabber_nwc_yarp"); + option.put("remote", "/camera"); + option.put("local", "/CameraTest"); + yarp::dev::PolyDriver driver; + + ASSERT_TRUE(driver.open(option)); + + yarp::dev::IFrameGrabberImage* igrabimg = nullptr; + + ASSERT_TRUE(driver.view(igrabimg)); + fixture.Server()->Run(/*_blocking=*/true, iterations, /*_paused=*/false); + + yarp::sig::ImageOf image; + + size_t maxNrOfReadingAttempts = 20; + bool readSuccessful = false; + for (size_t i = 0; (i < maxNrOfReadingAttempts) && !readSuccessful; i++) + { + readSuccessful = igrabimg->getImage(image); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + ASSERT_TRUE(readSuccessful); + + int blue[] = {0, 0, 255}; + int dark_blue[] = {0, 0, 100}; + int grey[] = {218, 218, 218}; + int dark_grey[] = {162, 162, 162}; + + // background - grey + unsigned char* pixel = image.getPixelAddress(301, 389); + EXPECT_EQ(int(pixel[0]), grey[0]); + EXPECT_EQ(int(pixel[1]), grey[1]); + EXPECT_EQ(int(pixel[2]), grey[2]); + + // timestamp - blue + pixel = image.getPixelAddress(35, 22); + EXPECT_EQ(int(pixel[0]), blue[0]); + EXPECT_EQ(int(pixel[1]), blue[1]); + EXPECT_EQ(int(pixel[2]), blue[2]); + + // sphere - dark blue + pixel = image.getPixelAddress(416, 259); + EXPECT_EQ(int(pixel[0]), dark_blue[0]); + EXPECT_EQ(int(pixel[1]), dark_blue[1]); + EXPECT_EQ(int(pixel[2]), dark_blue[2]); + + // ground plane - dark grey + pixel = image.getPixelAddress(334, 233); + EXPECT_EQ(int(pixel[0]), dark_grey[0]); + EXPECT_EQ(int(pixel[1]), dark_grey[1]); + EXPECT_EQ(int(pixel[2]), dark_grey[2]); } - - diff --git a/tests/forcetorque/ForceTorqueTest.cc b/tests/forcetorque/ForceTorqueTest.cc index 83e12b7..a9d4fcc 100644 --- a/tests/forcetorque/ForceTorqueTest.cc +++ b/tests/forcetorque/ForceTorqueTest.cc @@ -1,59 +1,59 @@ #include -#include +#include #include +#include #include -#include TEST(ForceTorqueTest, PluginTest) { - yarp::os::NetworkBase::setLocalMode(true); - - // Maximum verbosity helps with debugging - gz::common::Console::SetVerbosity(4); - - // Instantiate test fixture - gz::sim::TestFixture fixture("../../../tests/forcetorque/model.sdf"); - - int iterations = 1000; - fixture.Server()->Run(/*_blocking=*/true, iterations, /*_paused=*/false); - - yarp::os::Property option; - option.put("device","multipleanalogsensorsclient"); - option.put("remote","/forcetorque"); - option.put("timeout",1.0); - option.put("local", "/ForceTorqueTest"); - yarp::dev::PolyDriver driver; - - ASSERT_TRUE(driver.open(option)); - - yarp::dev::ISixAxisForceTorqueSensors* isixaxis = nullptr; - - ASSERT_TRUE(driver.view(isixaxis)); - - fixture.Server()->Run(/*_blocking=*/true, iterations, /*_paused=*/false); - yarp::sig::Vector measure(6); - std::string sensorName; - double timestamp; - - isixaxis->getSixAxisForceTorqueSensorName(0, sensorName); - size_t maxNrOfReadingAttempts = 20; - bool readSuccessful = false; - for (size_t i=0; (i < maxNrOfReadingAttempts) && !readSuccessful ; i++) - { - readSuccessful = isixaxis->getSixAxisForceTorqueSensorMeasure(0, measure, timestamp); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } - ASSERT_TRUE(readSuccessful); - - yarp::dev::MAS_status status; - status = isixaxis->getSixAxisForceTorqueSensorStatus(0); - ASSERT_EQ(status, yarp::dev::MAS_OK); - - EXPECT_NEAR(measure(0), 0.0, 1e-2); - EXPECT_NEAR(measure(1), 0.0, 1e-2); - EXPECT_NEAR(measure(2), -9.8*10, 1e-2); - EXPECT_NEAR(measure(3), 0.0, 1e-2); - EXPECT_NEAR(measure(4), 0.0, 1e-2); - EXPECT_NEAR(measure(5), 0.0, 1e-2); - EXPECT_GT(timestamp, 0.0); + yarp::os::NetworkBase::setLocalMode(true); + + // Maximum verbosity helps with debugging + gz::common::Console::SetVerbosity(4); + + // Instantiate test fixture + gz::sim::TestFixture fixture("../../../tests/forcetorque/model.sdf"); + + int iterations = 1000; + fixture.Server()->Run(/*_blocking=*/true, iterations, /*_paused=*/false); + + yarp::os::Property option; + option.put("device", "multipleanalogsensorsclient"); + option.put("remote", "/forcetorque"); + option.put("timeout", 1.0); + option.put("local", "/ForceTorqueTest"); + yarp::dev::PolyDriver driver; + + ASSERT_TRUE(driver.open(option)); + + yarp::dev::ISixAxisForceTorqueSensors* isixaxis = nullptr; + + ASSERT_TRUE(driver.view(isixaxis)); + + fixture.Server()->Run(/*_blocking=*/true, iterations, /*_paused=*/false); + yarp::sig::Vector measure(6); + std::string sensorName; + double timestamp; + + isixaxis->getSixAxisForceTorqueSensorName(0, sensorName); + size_t maxNrOfReadingAttempts = 20; + bool readSuccessful = false; + for (size_t i = 0; (i < maxNrOfReadingAttempts) && !readSuccessful; i++) + { + readSuccessful = isixaxis->getSixAxisForceTorqueSensorMeasure(0, measure, timestamp); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + ASSERT_TRUE(readSuccessful); + + yarp::dev::MAS_status status; + status = isixaxis->getSixAxisForceTorqueSensorStatus(0); + ASSERT_EQ(status, yarp::dev::MAS_OK); + + EXPECT_NEAR(measure(0), 0.0, 1e-2); + EXPECT_NEAR(measure(1), 0.0, 1e-2); + EXPECT_NEAR(measure(2), -9.8 * 10, 1e-2); + EXPECT_NEAR(measure(3), 0.0, 1e-2); + EXPECT_NEAR(measure(4), 0.0, 1e-2); + EXPECT_NEAR(measure(5), 0.0, 1e-2); + EXPECT_GT(timestamp, 0.0); } diff --git a/tests/imu/ImuTest.cc b/tests/imu/ImuTest.cc index 5e93bc2..62bf62f 100644 --- a/tests/imu/ImuTest.cc +++ b/tests/imu/ImuTest.cc @@ -1,84 +1,88 @@ #include -#include +#include #include +#include #include -#include TEST(ImuTest, PluginTest) { - yarp::os::NetworkBase::setLocalMode(true); - - // Maximum verbosity helps with debugging - gz::common::Console::SetVerbosity(4); - - // Instantiate test fixture - gz::sim::TestFixture fixture("../../../tests/imu/model.sdf"); - - int iterations = 1000; - fixture.Server()->Run(/*_blocking=*/true, iterations, /*_paused=*/false); - - yarp::os::Property option; - option.put("device","multipleanalogsensorsclient"); - option.put("remote","/IMU"); - option.put("timeout",1.0); - option.put("local", "/ImuTest"); - yarp::dev::PolyDriver driver; - - ASSERT_TRUE(driver.open(option)); - - yarp::dev::IThreeAxisGyroscopes* igyroscope = nullptr; - yarp::dev::IOrientationSensors* iorientation = nullptr; - yarp::dev::IThreeAxisLinearAccelerometers* iaccelerometer = nullptr; - - ASSERT_TRUE(driver.view(igyroscope)); - ASSERT_TRUE(driver.view(iorientation)); - ASSERT_TRUE(driver.view(iaccelerometer)); - - - fixture.Server()->Run(/*_blocking=*/true, iterations, /*_paused=*/false); - - yarp::sig::Vector measureGyroscope(3); - yarp::sig::Vector measureOrientation(3); - yarp::sig::Vector measureAccelerometer(3); - double timestampGyroscope; - double timestampOrientation; - double timestampAccelerometer; - yarp::dev::MAS_status statusGyroscope; - yarp::dev::MAS_status statusOrientation; - yarp::dev::MAS_status statusAccelerometer; - - size_t maxNrOfReadingAttempts = 20; - bool readSuccessful = false; - - for (size_t i=0; (i < maxNrOfReadingAttempts) && !readSuccessful ; i++) - { - readSuccessful = igyroscope->getThreeAxisGyroscopeMeasure(0, measureGyroscope, timestampGyroscope) && - iorientation->getOrientationSensorMeasureAsRollPitchYaw(0, measureOrientation, timestampOrientation) && - iaccelerometer->getThreeAxisLinearAccelerometerMeasure(0, measureAccelerometer, timestampAccelerometer); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } - - ASSERT_TRUE(readSuccessful); - - statusGyroscope = igyroscope->getThreeAxisGyroscopeStatus(0); - ASSERT_EQ(statusGyroscope, yarp::dev::MAS_OK); - statusOrientation = iorientation->getOrientationSensorStatus(0); - ASSERT_EQ(statusOrientation, yarp::dev::MAS_OK); - statusAccelerometer = iaccelerometer->getThreeAxisLinearAccelerometerStatus(0); - ASSERT_EQ(statusAccelerometer, yarp::dev::MAS_OK); - - EXPECT_NEAR(measureGyroscope(0), 0.0, 1e-2); - EXPECT_NEAR(measureGyroscope(1), 0.0, 1e-2); - EXPECT_NEAR(measureGyroscope(2), 0.0, 1e-2); - EXPECT_GT(timestampGyroscope, 0.0); - - EXPECT_NEAR(measureOrientation(0), 0.0, 1e-2); - EXPECT_NEAR(measureOrientation(1), 0.0, 1e-2); - EXPECT_NEAR(measureOrientation(2), 1.0, 1e-2); - EXPECT_GT(timestampOrientation, 0.0); - - EXPECT_NEAR(measureAccelerometer(0), 0.0, 1e-2); - EXPECT_NEAR(measureAccelerometer(1), 0.0, 1e-2); - EXPECT_NEAR(measureAccelerometer(2), 9.8, 1e-2); - EXPECT_GT(timestampAccelerometer, 0.0); + yarp::os::NetworkBase::setLocalMode(true); + + // Maximum verbosity helps with debugging + gz::common::Console::SetVerbosity(4); + + // Instantiate test fixture + gz::sim::TestFixture fixture("../../../tests/imu/model.sdf"); + + int iterations = 1000; + fixture.Server()->Run(/*_blocking=*/true, iterations, /*_paused=*/false); + + yarp::os::Property option; + option.put("device", "multipleanalogsensorsclient"); + option.put("remote", "/IMU"); + option.put("timeout", 1.0); + option.put("local", "/ImuTest"); + yarp::dev::PolyDriver driver; + + ASSERT_TRUE(driver.open(option)); + + yarp::dev::IThreeAxisGyroscopes* igyroscope = nullptr; + yarp::dev::IOrientationSensors* iorientation = nullptr; + yarp::dev::IThreeAxisLinearAccelerometers* iaccelerometer = nullptr; + + ASSERT_TRUE(driver.view(igyroscope)); + ASSERT_TRUE(driver.view(iorientation)); + ASSERT_TRUE(driver.view(iaccelerometer)); + + fixture.Server()->Run(/*_blocking=*/true, iterations, /*_paused=*/false); + + yarp::sig::Vector measureGyroscope(3); + yarp::sig::Vector measureOrientation(3); + yarp::sig::Vector measureAccelerometer(3); + double timestampGyroscope; + double timestampOrientation; + double timestampAccelerometer; + yarp::dev::MAS_status statusGyroscope; + yarp::dev::MAS_status statusOrientation; + yarp::dev::MAS_status statusAccelerometer; + + size_t maxNrOfReadingAttempts = 20; + bool readSuccessful = false; + + for (size_t i = 0; (i < maxNrOfReadingAttempts) && !readSuccessful; i++) + { + readSuccessful + = igyroscope->getThreeAxisGyroscopeMeasure(0, measureGyroscope, timestampGyroscope) + && iorientation->getOrientationSensorMeasureAsRollPitchYaw(0, + measureOrientation, + timestampOrientation) + && iaccelerometer->getThreeAxisLinearAccelerometerMeasure(0, + measureAccelerometer, + timestampAccelerometer); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + ASSERT_TRUE(readSuccessful); + + statusGyroscope = igyroscope->getThreeAxisGyroscopeStatus(0); + ASSERT_EQ(statusGyroscope, yarp::dev::MAS_OK); + statusOrientation = iorientation->getOrientationSensorStatus(0); + ASSERT_EQ(statusOrientation, yarp::dev::MAS_OK); + statusAccelerometer = iaccelerometer->getThreeAxisLinearAccelerometerStatus(0); + ASSERT_EQ(statusAccelerometer, yarp::dev::MAS_OK); + + EXPECT_NEAR(measureGyroscope(0), 0.0, 1e-2); + EXPECT_NEAR(measureGyroscope(1), 0.0, 1e-2); + EXPECT_NEAR(measureGyroscope(2), 0.0, 1e-2); + EXPECT_GT(timestampGyroscope, 0.0); + + EXPECT_NEAR(measureOrientation(0), 0.0, 1e-2); + EXPECT_NEAR(measureOrientation(1), 0.0, 1e-2); + EXPECT_NEAR(measureOrientation(2), 1.0, 1e-2); + EXPECT_GT(timestampOrientation, 0.0); + + EXPECT_NEAR(measureAccelerometer(0), 0.0, 1e-2); + EXPECT_NEAR(measureAccelerometer(1), 0.0, 1e-2); + EXPECT_NEAR(measureAccelerometer(2), 9.8, 1e-2); + EXPECT_GT(timestampAccelerometer, 0.0); } diff --git a/tests/laser/LaserTest.cc b/tests/laser/LaserTest.cc index b72e201..e3aad28 100644 --- a/tests/laser/LaserTest.cc +++ b/tests/laser/LaserTest.cc @@ -1,54 +1,54 @@ #include -#include +#include #include +#include #include -#include TEST(LaserTest, PluginTest) { - yarp::os::NetworkBase::setLocalMode(true); - - // Maximum verbosity helps with debugging - gz::common::Console::SetVerbosity(4); - - // Instantiate test fixture - gz::sim::TestFixture fixture("../../../tests/laser/model.sdf"); - - int iterations = 1000; - fixture.Server()->Run(/*_blocking=*/true, iterations, /*_paused=*/false); - - yarp::os::Property option; - option.put("device","Rangefinder2DClient"); - option.put("remote","/laser"); - option.put("local", "/LaserTest"); - yarp::dev::PolyDriver driver; - - ASSERT_TRUE(driver.open(option)); - yarp::dev::IRangefinder2D* irange = nullptr; - ASSERT_TRUE(driver.view(irange)); - - fixture.Server()->Run(/*_blocking=*/true, iterations, /*_paused=*/false); - - yarp::sig::Vector measure(360); - double timestamp; - - size_t maxNrOfReadingAttempts = 20; - bool readSuccessful = false; - for (size_t i=0; (i < maxNrOfReadingAttempts) && !readSuccessful ; i++) - { - readSuccessful = irange->getRawData(measure, ×tamp); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } - ASSERT_TRUE(readSuccessful); - - double distance = 4.0; - double size_box = 1.0; - EXPECT_NEAR(measure(0), distance-size_box/2, 1e-1); - EXPECT_NEAR(measure(1), distance-size_box/2, 1e-1); - EXPECT_NEAR(measure(2), distance-size_box/2, 1e-1); - EXPECT_EQ(measure(100), INFINITY); - EXPECT_EQ(measure(200), INFINITY); - EXPECT_EQ(measure(300), INFINITY); - EXPECT_EQ(measure(359), INFINITY); - EXPECT_GT(timestamp, 0.0); + yarp::os::NetworkBase::setLocalMode(true); + + // Maximum verbosity helps with debugging + gz::common::Console::SetVerbosity(4); + + // Instantiate test fixture + gz::sim::TestFixture fixture("../../../tests/laser/model.sdf"); + + int iterations = 1000; + fixture.Server()->Run(/*_blocking=*/true, iterations, /*_paused=*/false); + + yarp::os::Property option; + option.put("device", "Rangefinder2DClient"); + option.put("remote", "/laser"); + option.put("local", "/LaserTest"); + yarp::dev::PolyDriver driver; + + ASSERT_TRUE(driver.open(option)); + yarp::dev::IRangefinder2D* irange = nullptr; + ASSERT_TRUE(driver.view(irange)); + + fixture.Server()->Run(/*_blocking=*/true, iterations, /*_paused=*/false); + + yarp::sig::Vector measure(360); + double timestamp; + + size_t maxNrOfReadingAttempts = 20; + bool readSuccessful = false; + for (size_t i = 0; (i < maxNrOfReadingAttempts) && !readSuccessful; i++) + { + readSuccessful = irange->getRawData(measure, ×tamp); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + ASSERT_TRUE(readSuccessful); + + double distance = 4.0; + double size_box = 1.0; + EXPECT_NEAR(measure(0), distance - size_box / 2, 1e-1); + EXPECT_NEAR(measure(1), distance - size_box / 2, 1e-1); + EXPECT_NEAR(measure(2), distance - size_box / 2, 1e-1); + EXPECT_EQ(measure(100), INFINITY); + EXPECT_EQ(measure(200), INFINITY); + EXPECT_EQ(measure(300), INFINITY); + EXPECT_EQ(measure(359), INFINITY); + EXPECT_GT(timestamp, 0.0); }