Skip to content

Commit

Permalink
Merge pull request #688 from robotology/fixicubmodels234
Browse files Browse the repository at this point in the history 
gazebo_yarp_maissensor: Expose data via yarp::dev::IEncoderArrays and add support for  disableImplicitNetworkWrapper, yarpDeviceName and sensorName parameters
  • Loading branch information
@traversaro
traversaro authored Sep 19, 2024
2 parents f00ee8f + 4403b9f commit 50f2386
Show file tree
Hide file tree
Showing 6 changed files with 218 additions and 89 deletions.
2 changes: 1 addition & 1 deletion plugins/controlboard/include/gazebo/ControlBoard.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,7 +59,7 @@ private:
bool m_useVirtAnalogSensor = false;
#endif
yarp::dev::PolyDriver m_controlboardDriver;
bool m_deviceRegistered;
bool m_deviceRegistered{false};
std::string m_scopedDeviceName;
std::string m_yarpDeviceName;

Expand Down
5 changes: 5 additions & 0 deletions plugins/maissensor/include/gazebo/MaisSensor.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,11 @@ public:
void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);

private:
yarp::dev::PolyDriver m_maisEncodersDriver;
bool m_deviceRegistered{false};
std::string m_scopedDeviceName;
std::string m_yarpDeviceName;

yarp::dev::PolyDriver m_wrapper;
yarp::dev::IMultipleWrapper* m_iWrap;

Expand Down
15 changes: 12 additions & 3 deletions plugins/maissensor/include/yarp/dev/MaisSensorDriver.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
#include <yarp/dev/ControlBoardInterfacesImpl.h>
#include <yarp/dev/ControlBoardInterfaces.h>
#include <yarp/dev/IAnalogSensor.h>
#include <yarp/dev/MultipleAnalogSensorsInterfaces.h>
#include <yarp/sig/Vector.h>
#include <yarp/os/Time.h>
#include <yarp/os/Stamp.h>
Expand Down Expand Up @@ -61,7 +62,8 @@ namespace gazebo {
class yarp::dev::GazeboYarpMaisSensorDriver:
public DeviceDriver,
public DeviceResponder,
public IAnalogSensor
public IAnalogSensor,
public IEncoderArrays
{
public:

Expand All @@ -83,6 +85,12 @@ class yarp::dev::GazeboYarpMaisSensorDriver:
virtual bool open(yarp::os::Searchable& config);
virtual bool close();

// yarp::dev::IEncoderArrays
virtual size_t getNrOfEncoderArrays() const override;
virtual yarp::dev::MAS_status getEncoderArrayStatus(size_t sens_index) const override;
virtual bool getEncoderArrayName(size_t sens_index, std::string &name) const override;
virtual bool getEncoderArrayMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const override;
virtual size_t getEncoderArraySize(size_t sens_index) const override;

private:

Expand Down Expand Up @@ -114,14 +122,15 @@ class yarp::dev::GazeboYarpMaisSensorDriver:

yarp::os::Stamp m_lastTimestamp; /**< timestamp, updated with simulation time at each onUpdate call */

std::mutex m_mutex;
mutable std::mutex m_mutex;
yarp::sig::VectorOf<JointType> m_jointTypes;

int m_channels_num;
std::vector<std::string> m_jointNames;
std::vector<std::string> controlboard_joint_names;
std::vector<gazebo::physics::JointPtr> m_jointPointers; /* pointers for each joint, avoiding several calls to getJoint(joint_name) */
gazebo::transport::NodePtr m_gazeboNode;
std::string m_encoderArrayName{"gazebo_yarp_maisdriver_default_sensor_name"};


bool started;
Expand All @@ -142,7 +151,7 @@ class yarp::dev::GazeboYarpMaisSensorDriver:
virtual int calibrateSensor(const yarp::sig::Vector& value);
virtual int calibrateChannel(int ch);
virtual int calibrateChannel(int ch, double value);

/**
* \brief convert data read from Gazebo to user unit sistem,
* e.g. degrees for revolute joints and meters for prismatic joints
Expand Down
204 changes: 126 additions & 78 deletions plugins/maissensor/src/MaisSensor.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpMaisSensor)

GazeboYarpMaisSensor::GazeboYarpMaisSensor() : m_iWrap(0)
{

}

GazeboYarpMaisSensor::~GazeboYarpMaisSensor()
Expand All @@ -38,7 +38,17 @@ GazeboYarpMaisSensor::~GazeboYarpMaisSensor()
if (m_wrapper.isValid())
m_wrapper.close();

GazeboYarpPlugins::Handler::getHandler()->removeDevice(m_sensorName);
if (m_deviceRegistered)
{
if (m_parameters.check("disableImplicitNetworkWrapper"))
{
GazeboYarpPlugins::Handler::getHandler()->removeDevice(m_scopedDeviceName);
}
else
{
GazeboYarpPlugins::Handler::getHandler()->removeDevice(m_sensorName);
}
}
GazeboYarpPlugins::Handler::getHandler()->removeRobot(m_robotName);

yarp::os::Network::fini();
Expand Down Expand Up @@ -74,101 +84,139 @@ void GazeboYarpMaisSensor::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
return;
}

yarp::os::Bottle wrapper_group = m_parameters.findGroup("WRAPPER");
if(wrapper_group.isNull())
bool disable_wrapper = m_parameters.check("disableImplicitNetworkWrapper");

if (disable_wrapper && !m_parameters.check("yarpDeviceName"))
{
yError("GazeboYarpMaisSensor : [WRAPPER] group not found in config file\n");
yError() << "GazeboYarpMaisSensor : missing yarpDeviceName parameter for one device in robot " << m_robotName;
return;
}

if(m_parameters.check("ROS"))
if (!disable_wrapper)
{
std::string ROS;
ROS = std::string ("(") + m_parameters.findGroup("ROS").toString() + std::string (")");
wrapper_group.append(yarp::os::Bottle(ROS));
}

//Open the wrapper
if( m_wrapper.open(wrapper_group) )
{
}
else
{
yError()<<"GazeboYarpMaisSensor Plugin failed: error in opening yarp driver wrapper";
return;
}
yarp::os::Bottle wrapper_group = m_parameters.findGroup("WRAPPER");
if(wrapper_group.isNull())
{
yError("GazeboYarpMaisSensor : [WRAPPER] group not found in config file\n");
return;
}


yarp::os::Bottle *netList = wrapper_group.find("networks").asList();
if(m_parameters.check("ROS"))
{
std::string ROS;
ROS = std::string ("(") + m_parameters.findGroup("ROS").toString() + std::string (")");
wrapper_group.append(yarp::os::Bottle(ROS));
}

if (netList->isNull())
{
yError("GazeboYarpMaisSensor : net list to attach to was not found, load failed.");
m_wrapper.close();
return;
}

//---------------------------------------------
yarp::dev::PolyDriverDescriptor newPoly;

if (netList->size()!=1)
{
yError("GazeboYarpMaisSensor: size of 'networks' parameter cannot be != 1");
}

newPoly.key = netList->get(0).asString();
m_sensorName = m_robotName + "::" + newPoly.key.c_str();
newPoly.poly = GazeboYarpPlugins::Handler::getHandler()->getDevice(m_sensorName);
//Open the wrapper
if( m_wrapper.open(wrapper_group) )
{
}
else
{
yError()<<"GazeboYarpMaisSensor Plugin failed: error in opening yarp driver wrapper";
return;
}

yarp::os::Bottle driver_group;
if( newPoly.poly != NULL)
{
// device already exists, use it, setting it againg to increment the usage counter.
yError("mais %s already opened", newPoly.key.c_str());
}
else
{
driver_group = m_parameters.findGroup(newPoly.key.c_str());
if (driver_group.isNull())
yarp::os::Bottle *netList = wrapper_group.find("networks").asList();

if (netList->isNull())
{
yError("GazeboYarpMaisSensor::Load Error: [%s] group not found in config file. Closing wrapper for device [%s].", newPoly.key.c_str(), m_sensorName.c_str());
yError("GazeboYarpMaisSensor : net list to attach to was not found, load failed.");
m_wrapper.close();
return;
}

m_parameters.put("name", newPoly.key.c_str());
m_parameters.fromString(driver_group.toString(), false);
//---------------------------------------------
yarp::dev::PolyDriverDescriptor newPoly;

if (netList->size()!=1)
{
yError("GazeboYarpMaisSensor: size of 'networks' parameter cannot be != 1");
}

newPoly.key = netList->get(0).asString();
m_sensorName = m_robotName + "::" + newPoly.key.c_str();
newPoly.poly = GazeboYarpPlugins::Handler::getHandler()->getDevice(m_sensorName);

yarp::os::Bottle driver_group;
if( newPoly.poly != NULL)
{
// device already exists, use it, setting it againg to increment the usage counter.
yError("mais %s already opened", newPoly.key.c_str());
}
else
{
driver_group = m_parameters.findGroup(newPoly.key.c_str());
if (driver_group.isNull())
{
yError("GazeboYarpMaisSensor::Load Error: [%s] group not found in config file. Closing wrapper for device [%s].", newPoly.key.c_str(), m_sensorName.c_str());
return;
}

m_parameters.put("name", newPoly.key.c_str());
m_parameters.fromString(driver_group.toString(), false);
m_parameters.put("robotScopedName", m_robotName);
m_parameters.put("device","gazebo_maissensor");

newPoly.poly = new yarp::dev::PolyDriver;
if(! newPoly.poly->open(m_parameters) || ! newPoly.poly->isValid())
{
yError() << "mais <" << newPoly.key << "> did not open!!";
newPoly.poly->close();
return;
}
}
GazeboYarpPlugins::Handler::getHandler()->setDevice(m_sensorName, newPoly.poly);

//------------------
if (!m_wrapper.isValid())
{
yError("GazeboYarpMaisSensor: wrapper did not open");
}

if (!m_wrapper.view(m_iWrap))
{
yError("Wrapper interface not found");
}

//Attach the driver to the wrapper
yarp::dev::PolyDriverList driver_list;

m_deviceRegistered = true;
yInfo() << "GazeboYarpMaisSensor: Registered YARP device with instance name:" << m_sensorName;
driver_list.push(newPoly.poly,"dummy");

if( m_iWrap->attachAll(driver_list) ) {
} else {
yError() << "GazeboYarpMaisSensor : error in connecting wrapper and device ";
}
}
else
{
m_yarpDeviceName = m_parameters.find("yarpDeviceName").asString();
m_scopedDeviceName = m_robotName + "::" + m_yarpDeviceName;

m_parameters.put("name", m_scopedDeviceName);
m_parameters.put("robotScopedName", m_robotName);
m_parameters.put("device","gazebo_maissensor");

newPoly.poly = new yarp::dev::PolyDriver;
if(! newPoly.poly->open(m_parameters) || ! newPoly.poly->isValid())
if (!m_maisEncodersDriver.open(m_parameters) || ! m_maisEncodersDriver.isValid())
{
yError() << "mais <" << newPoly.key << "> did not open!!";
newPoly.poly->close();
yError() << "mais <" << m_yarpDeviceName.c_str() << "> did not open.";
m_maisEncodersDriver.close();
return;
}
}
GazeboYarpPlugins::Handler::getHandler()->setDevice(m_sensorName, newPoly.poly);

//------------------
if (!m_wrapper.isValid())
{
yError("GazeboYarpMaisSensor: wrapper did not open");
}

if (!m_wrapper.view(m_iWrap))
{
yError("Wrapper interface not found");
}

//Attach the driver to the wrapper
yarp::dev::PolyDriverList driver_list;

driver_list.push(newPoly.poly,"dummy");

if( m_iWrap->attachAll(driver_list) ) {
} else {
yError() << "GazeboYarpForceTorque : error in connecting wrapper and device ";
//Register the device with the given name
if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(m_scopedDeviceName, &m_maisEncodersDriver))
{
yError() << "GazeboYarpMaisSensor: failed setting scopedDeviceName(=" << m_scopedDeviceName << ")";
return;
}
m_deviceRegistered = true;
yInfo() << "GazeboYarpMaisSensor: Registered YARP device with instance name:" << m_scopedDeviceName;

}
}

Expand Down
2 changes: 1 addition & 1 deletion plugins/maissensor/src/MaisSensorDeviceDriver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,7 @@ bool GazeboYarpMaisSensorDriver::open(yarp::os::Searchable& config)

std::string robotName;
robotName = m_pluginParameters.find("robotScopedName").asString().c_str();

if (robotName == "")
{
yError() << "GazeboYarpMaisSensorDriver error: 'robotName' parameter not found'";
Expand Down
Loading

0 comments on commit 50f2386

Please sign in to comment.