Skip to content

Fix crash when running the optical tactile sensor world #2561

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 2 commits into from
Sep 3, 2024
Merged

Fix crash when running the optical tactile sensor world #2561

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
d78353a
fix crash
iche033 Aug 31, 2024
4d26eb0
Merge branch 'gz-sim9' into optical_tactile_plugin_crash
iche033 Sep 3, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
38 changes: 20 additions & 18 deletions src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -530,6 +530,7 @@ void OpticalTactilePluginPrivate::Load(const EntityComponentManager &_ecm)
int contactSensorCounter = 0;
sdf::Sensor depthCameraSdf;
components::Pose depthCameraPose = components::Pose();
std::string depthCameraTopic;
for (const Entity &sensor : sensorsInsideLink)
{
if (_ecm.EntityHasComponentType(sensor, components::DepthCamera::typeId))
Expand All @@ -538,6 +539,10 @@ void OpticalTactilePluginPrivate::Load(const EntityComponentManager &_ecm)
depthCameraSdf =
_ecm.Component<components::DepthCamera>(sensor)->Data();
depthCameraPose = *(_ecm.Component<components::Pose>(sensor));
auto depthCameraTopicComp =
_ecm.Component<components::SensorTopic>(sensor);
if (depthCameraTopicComp)
depthCameraTopic = depthCameraTopicComp->Data();
}

if (_ecm.EntityHasComponentType(sensor, components::ContactSensor::typeId))
Expand All @@ -558,16 +563,6 @@ void OpticalTactilePluginPrivate::Load(const EntityComponentManager &_ecm)
}

// Store depth camera update rate
if (!depthCameraSdf.Element()->HasElement("update_rate"))
{
if (!this->initErrorPrinted)
{
gzerr << "Depth camera should have an <update_rate> value "
<< "(only printed once)" << std::endl;
this->initErrorPrinted = true;
}
return;
}
this->cameraUpdateRate = depthCameraSdf.UpdateRate();

// Depth camera data is float, so convert Pose3d to Pose3f
Expand All @@ -581,20 +576,27 @@ void OpticalTactilePluginPrivate::Load(const EntityComponentManager &_ecm)
depthCameraPose.Data().Rot().Z());

// Configure subscriber for depth camera images
if (!depthCameraSdf.Element()->HasElement("topic"))
if (depthCameraTopic.empty())
{
gzwarn << "Depth camera publishing to __default__ topic. "
<< "It's possible that two depth cameras are publishing into the same "
<< "topic" << std::endl;
// get the topic from sdf if the one in sensor topic component is empty
depthCameraTopic = depthCameraSdf.Topic();
}
else

if (depthCameraTopic.empty())
{
gzdbg << "Depth camera publishing to "
<< depthCameraSdf.Topic() << " topic" << std::endl;
if (!this->initErrorPrinted)
{
gzerr << "Depth camera topic is empty. " << std::endl;
this->initErrorPrinted = true;
}
return;
}

gzdbg << "Depth camera publishing to "
<< depthCameraTopic << " topic" << std::endl;

std::string topic =
"/" + depthCameraSdf.Topic() + "/points";
"/" + depthCameraTopic + "/points";
if (!this->node.Subscribe(topic,
&OpticalTactilePluginPrivate::DepthCameraCallback, this))
{
Expand Down
Loading