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 authored Sep 18, 2024
1 parent 56fb8ea commit d823de3
Show file tree
Hide file tree
Showing 3 changed files with 128 additions and 73 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
194 changes: 122 additions & 72 deletions plugins/maissensor/src/MaisSensor.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,141 @@ 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));
}
yarp::os::Bottle wrapper_group = m_parameters.findGroup("WRAPPER");
if(wrapper_group.isNull())
{
yError("GazeboYarpMaisSensor : [WRAPPER] group not found in config file\n");
return;
}

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));
}

//Open the wrapper
if( m_wrapper.open(wrapper_group) )
{
}
else
{
yError()<<"GazeboYarpMaisSensor Plugin failed: error in opening yarp driver wrapper";
return;
}
//Open the wrapper
if( m_wrapper.open(wrapper_group) )
{
}
else
{
yError()<<"GazeboYarpMaisSensor Plugin failed: error in opening yarp driver wrapper";
return;
}


yarp::os::Bottle *netList = wrapper_group.find("networks").asList();
yarp::os::Bottle *netList = wrapper_group.find("networks").asList();

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

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

if (netList->size()!=1)
{
yError("GazeboYarpMaisSensor: size of 'networks' parameter cannot be != 1");
}
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);
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())
yarp::os::Bottle driver_group;
if( newPoly.poly != NULL)
{
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;
// 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;
yCInfo(GAZEBOCONTROLBOARD) << "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", newPoly.key.c_str());
m_parameters.put("name", m_scopedDeviceName);
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())
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))
{
yCError() << "GazeboYarpMaisSensor: failed setting scopedDeviceName(=" << m_scopedDeviceName << ")";
return;
}
m_deviceRegistered = true;
yCInfo(GAZEBOCONTROLBOARD) << "Registered YARP device with instance name:" << m_scopedDeviceName;

}
}

Expand Down

0 comments on commit d823de3

Please sign in to comment.