Skip to content

Commit

Permalink
Register the device even if "yarpDeviceName" is not set
Browse files Browse the repository at this point in the history
  • Loading branch information
@drdanz
drdanz committed Jun 30, 2021
1 parent 02d6117 commit 63f960c
Show file tree
Hide file tree
Showing 5 changed files with 74 additions and 94 deletions.
30 changes: 14 additions & 16 deletions plugins/controlboard/src/ControlBoard.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -159,7 +159,15 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpControlBoard)
virt_group.append(yarp::os::Bottle(net));
}

std::string scopedDeviceName = m_robotName + "::" + newPoly.key.c_str();
std::string scopedDeviceName;
if(!m_parameters.check("yarpDeviceName"))
{
scopedDeviceName = m_robotName + "::" + newPoly.key.c_str();
}
else
{
scopedDeviceName = m_robotName + "::" + m_parameters.find("yarpDeviceName").asString();
}
newPoly.poly = GazeboYarpPlugins::Handler::getHandler()->getDevice(scopedDeviceName);
if( newPoly.poly != NULL)
{
Expand Down Expand Up @@ -200,23 +208,13 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpControlBoard)
}

//Register the device with the given name
if(!m_parameters.check("yarpDeviceName"))
if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(scopedDeviceName, newPoly.poly))
{
yError()<<"GazeboYarpControlBoard: cannot find yarpDeviceName parameter in ini file.";
yError() << "GazeboYarpControlBoard: failed setting scopedDeviceName(=" << scopedDeviceName << ")";
return;
}
else
{
std::string robotName = _parent->GetScopedName();
std::string deviceId = m_parameters.find("yarpDeviceName").asString();
std::string scopedDeviceName = robotName + "::" + deviceId;
yInfo() << "GazeboYarpControlBoard: Registered YARP device with instance name:" << scopedDeviceName;

if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(scopedDeviceName, newPoly.poly))
{
yError()<<"GazeboYarpControlBoard: failed setting scopedDeviceName(=" << scopedDeviceName << ")";
return;
}
yInfo() << "GazeboYarpControlBoard: Registered YARP device with instance name:" << scopedDeviceName;
}
m_controlBoards.push(newPoly);
}

Expand Down Expand Up @@ -259,4 +257,4 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpControlBoard)
return;
}
}
}
} // namespace gazebo
25 changes: 12 additions & 13 deletions plugins/depthCamera/src/DepthCamera.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -114,31 +114,30 @@ void GazeboYarpDepthCamera::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sd
return;
}

driver_list.push(&m_cameraDriver,"dummy");
driver_list.push(&m_cameraDriver, "depthcamera");

if(!m_iWrap->attachAll(driver_list) )
{
yError() << "GazeboYarpDepthCamera : error in connecting wrapper and device ";
}

//Register the device with the given name
std::string scopedDeviceName;
if(!m_driverParameters.check("yarpDeviceName"))
{
yError()<<"GazeboYarpDepthCamera: cannot find yarpDeviceName parameter in ini file.";
scopedDeviceName = m_sensorName + "::" + driver_list[0]->key;
}
else
{
std::string sensorName = _sensor->ScopedName();
std::string deviceId = m_driverParameters.find("yarpDeviceName").asString();
std::string scopedDeviceName = sensorName + "::" + deviceId;

if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(scopedDeviceName, &m_cameraDriver))
{
yError()<<"GazeboYarpDepthCamera: failed setting scopedDeviceName(=" << scopedDeviceName << ")";
return;
}
yInfo() << "Registered YARP device with instance name:" << scopedDeviceName;
scopedDeviceName = m_sensorName + "::" + m_driverParameters.find("yarpDeviceName").asString();
}
}

if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(scopedDeviceName, &m_cameraDriver))
{
yError()<<"GazeboYarpDepthCamera: failed setting scopedDeviceName(=" << scopedDeviceName << ")";
return;
}
yInfo() << "Registered YARP device with instance name:" << scopedDeviceName;
}

} // namespace gazebo
27 changes: 13 additions & 14 deletions plugins/doublelaser/src/DoubleLaser.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -249,6 +249,7 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpDoubleLaser)
yarp::dev::PolyDriverList listofdoubellaser; //it will contain only double laser
yarp::dev::PolyDriverDescriptor doublelaser_desc;
doublelaser_desc.poly = &m_driver_doublelaser;
doublelaser_desc.key = "doublelaser";

listofdoubellaser.push(doublelaser_desc);

Expand All @@ -263,24 +264,22 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpDoubleLaser)
GazeboYarpPlugins::Handler::getHandler()->setRobot(get_pointer(_parent));

// 9) Register the device with the given name
std::string robotName = _parent->GetScopedName();
std::string scopedDeviceName;
if(!m_parameters.check("yarpDeviceName"))
{
yError()<<"GazeboYarpDoubleLaser: cannot find yarpDeviceName parameter in ini file.";
//return;
scopedDeviceName = robotName + "::" + doublelaser_desc.key;
}
else
{
std::string robotName = _parent->GetScopedName();
std::string deviceId = m_parameters.find("yarpDeviceName").asString();
std::string scopedDeviceName = robotName + "::" + deviceId;

if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(scopedDeviceName, &m_driver_doublelaser))
{
yError()<<"GazeboYarpDoubleLaser: failed setting scopedDeviceName(=" << scopedDeviceName << ")";
return;
}
yInfo() << "Registered YARP device with instance name:" << scopedDeviceName;
scopedDeviceName = robotName + "::" + m_parameters.find("yarpDeviceName").asString();
}
}

}
if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(scopedDeviceName, &m_driver_doublelaser))
{
yError()<<"GazeboYarpDoubleLaser: failed setting scopedDeviceName(=" << scopedDeviceName << ")";
return;
}
yInfo() << "Registered YARP device with instance name:" << scopedDeviceName;
}
} // namespace gazebo
65 changes: 25 additions & 40 deletions plugins/lasersensor/src/LaserSensor.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -98,44 +98,6 @@ void GazeboYarpLaserSensor::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sd
return;
}

//Register the device with the given name
//#if 0
//this block will be soon deprecated
if(!driver_properties.check("deviceId"))
{
yError()<<"GazeboYarpLaserSensor Plugin failed: cannot find deviceId parameter in ini file.";
}
else
{
yError() << "GazeboYarpLaserSensor: deviceId parameter has been deprecated. Please use yarpDeviceName instead";
std::string deviceId = driver_properties.find("deviceId").asString();
if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(deviceId, &m_laserDriver))
{
yError()<<"GazeboYarpLaserSensor: failed setting deviceId(=" << deviceId << ")";
return;
}
}
//#else
if(!driver_properties.check("yarpDeviceName"))
{
yError()<<"GazeboYarpLaserSensor: cannot find yarpDeviceName parameter in ini file.";
//return;
}
else
{
std::string sensorName = _sensor->ScopedName();
std::string deviceId = driver_properties.find("yarpDeviceName").asString();
std::string scopedDeviceName = sensorName + "::" + deviceId;

if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(scopedDeviceName, &m_laserDriver))
{
yError()<<"GazeboYarpLaserSensor: failed setting scopedDeviceName(=" << scopedDeviceName << ")";
return;
}
yInfo() << "Registered YARP device with instance name:" << scopedDeviceName;
}
//#endif

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

Expand All @@ -145,15 +107,38 @@ void GazeboYarpLaserSensor::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sd
return;
}

driver_list.push(&m_laserDriver,"dummy");
driver_list.push(&m_laserDriver, "lasersensor");

if( m_iWrap->attachAll(driver_list) ) {
} else
{
yError() << "GazeboYarpLaserSensor : error in connecting wrapper and device " ;
}

}
//Register the device with the given name
std::string sensorName = _sensor->ScopedName();
std::string scopedDeviceName;
if(driver_properties.check("deviceId"))
{
yWarning() << "GazeboYarpLaserSensor: deviceId parameter has been deprecated. Please use yarpDeviceName instead";
scopedDeviceName = sensorName + "::" + driver_properties.find("deviceId").asString();
}
else if(!driver_properties.check("yarpDeviceName"))
{
scopedDeviceName = sensorName + "::" + driver_list[0]->key;
}
else
{
scopedDeviceName = sensorName + "::" + driver_properties.find("yarpDeviceName").asString();
}


if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(scopedDeviceName, &m_laserDriver))
{
yError()<<"GazeboYarpLaserSensor: failed setting scopedDeviceName(=" << scopedDeviceName << ")";
return;
}
yInfo() << "Registered YARP device with instance name:" << scopedDeviceName;
}

} // namespace gazebo
21 changes: 10 additions & 11 deletions plugins/multicamera/src/MultiCamera.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -84,23 +84,22 @@ void GazeboYarpMultiCamera::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sd


//Register the device with the given name
std::string scopedDeviceName;
if(!m_parameters.check("yarpDeviceName"))
{
yError()<<"GazeboYarpMultiCamera: cannot find yarpDeviceName parameter in ini file.";
scopedDeviceName = m_sensorName + "::" "multicamera";
}
else
{
std::string sensorName = _sensor->ScopedName();
std::string deviceId = m_parameters.find("yarpDeviceName").asString();
std::string scopedDeviceName = sensorName + "::" + deviceId;

if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(scopedDeviceName, &m_cameraDriver))
{
yError()<<"GazeboYarpMultiCamera: failed setting scopedDeviceName(=" << scopedDeviceName << ")";
return;
}
yInfo() << "GazeboYarpMultiCamera: Register YARP device with instance name:" << scopedDeviceName;
scopedDeviceName = m_sensorName + "::" + m_parameters.find("yarpDeviceName").asString();
}

if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(scopedDeviceName, &m_cameraDriver))
{
yError()<<"GazeboYarpMultiCamera: failed setting scopedDeviceName(=" << scopedDeviceName << ")";
return;
}
yInfo() << "GazeboYarpMultiCamera: Register YARP device with instance name:" << scopedDeviceName;
}

} // namespace gazebo

0 comments on commit 63f960c

Please sign in to comment.