Skip to content

Commit

Permalink
Add support for disableImplicitNetworkWrapper and yarpDeviceName in g…
Browse files Browse the repository at this point in the history 
…azebo_yarp_maissensor
  • Loading branch information
@traversaro
traversaro committed Sep 18, 2024
1 parent 56fb8ea commit 796b8bb
Show file tree
Hide file tree
Showing 4 changed files with 133 additions and 80 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
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

0 comments on commit 796b8bb

Please sign in to comment.