Skip to content

Commit

Permalink
Merge pull request #735 from stereolabs/fix_zed2i_support
Browse files Browse the repository at this point in the history
Fix ZED2i support
  • Loading branch information
Myzhar authored Jun 17, 2021
2 parents 44f9d93 + b83c672 commit a666d67
Show file tree
Hide file tree
Showing 2 changed files with 28 additions and 8 deletions.
4 changes: 4 additions & 0 deletions latest_changes.md
Original file line number Diff line number Diff line change
@@ -1,6 +1,10 @@
LATEST CHANGES
==============

ZED2i support fix (2021-06-17)
------------------------------
- Fix sensors topics pubblication for ZED2i. The support for the new camera was not complete

OpenNI mode fix (2021-06-15)
----------------------------
- Fix sensor_msgs type for depth image in OpenNI mode, from `sensor_msgs::image_encodings::mono16` to `sensor_msgs::image_encodings::TYPE_16UC1`. Depth image in OpenNI mode is now compatible with the nodelet `depthimage_to_laserscan`
Expand Down
32 changes: 24 additions & 8 deletions zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -323,6 +323,22 @@ void ZEDWrapperNodelet::onInit()
"the value of the parameter 'camera_model' to 'zed2'");
}

#if ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION < 1
mSlCamImuTransf = mZed.getCameraInformation().camera_imu_transform;
#else
mSlCamImuTransf = mZed.getCameraInformation().sensors_configuration.camera_imu_transform;
#endif

NODELET_INFO("Camera-IMU Transform: \n %s", mSlCamImuTransf.getInfos().c_str());
}
else if (mZedRealCamModel == sl::MODEL::ZED2i)
{
if (mZedUserCamModel != mZedRealCamModel)
{
NODELET_WARN("Camera model does not match user parameter. Please modify "
"the value of the parameter 'camera_model' to 'zed2i'");
}

#if ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION < 1
mSlCamImuTransf = mZed.getCameraInformation().camera_imu_transform;
#else
Expand Down Expand Up @@ -526,7 +542,7 @@ void ZEDWrapperNodelet::onInit()
mPubImuMag = mNhNs.advertise<sensor_msgs::MagneticField>(imu_mag_topic, 1 /*MAG_FREQ*/);
NODELET_INFO_STREAM("Advertised on topic " << mPubImuMag.getTopic());

if (mZedRealCamModel == sl::MODEL::ZED2)
if (mZedRealCamModel == sl::MODEL::ZED2 || mZedRealCamModel == sl::MODEL::ZED2i)
{
// IMU temperature sensor
mPubImuTemp = mNhNs.advertise<sensor_msgs::Temperature>(imu_temp_topic, 1 /*static_cast<int>(mSensPubRate)*/);
Expand Down Expand Up @@ -1478,9 +1494,9 @@ void ZEDWrapperNodelet::stop_3d_mapping()

bool ZEDWrapperNodelet::start_obj_detect()
{
if (mZedRealCamModel != sl::MODEL::ZED2)
if (mZedRealCamModel != sl::MODEL::ZED2 && mZedRealCamModel != sl::MODEL::ZED2i)
{
NODELET_ERROR_STREAM("Object detection not started. OD is available only using a ZED2 camera model");
NODELET_ERROR_STREAM("Object detection not started. OD is available only using a ZED2/ZED2i camera model");
return false;
}

Expand Down Expand Up @@ -2963,7 +2979,7 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t)
uint32_t tempLeftSubNumber = 0;
uint32_t tempRightSubNumber = 0;

if (mZedRealCamModel == sl::MODEL::ZED2)
if (mZedRealCamModel == sl::MODEL::ZED2 || mZedRealCamModel == sl::MODEL::ZED2i)
{
imu_TempSubNumber = mPubImuTemp.getNumSubscribers();
imu_MagSubNumber = mPubImuMag.getNumSubscribers();
Expand Down Expand Up @@ -3055,7 +3071,7 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t)
}
// <---- Publish odometry tf only if enabled

if (mZedRealCamModel == sl::MODEL::ZED2)
if (mZedRealCamModel == sl::MODEL::ZED2 || mZedRealCamModel == sl::MODEL::ZED2i)
{
// Update temperatures for Diagnostic
sens_data.temperature.get(sl::SensorsData::TemperatureData::SENSOR_LOCATION::ONBOARD_LEFT, mTempLeft);
Expand Down Expand Up @@ -4240,7 +4256,7 @@ void ZEDWrapperNodelet::callback_updateDiagnostic(diagnostic_updater::Diagnostic
stat.add("IMU", "Topics not subscribed");
}

if (mZedRealCamModel == sl::MODEL::ZED2)
if (mZedRealCamModel == sl::MODEL::ZED2 || mZedRealCamModel == sl::MODEL::ZED2i)
{
stat.addf("Left CMOS Temp.", "%.1f °C", mTempLeft);
stat.addf("Right CMOS Temp.", "%.1f °C", mTempRight);
Expand Down Expand Up @@ -4538,12 +4554,12 @@ bool ZEDWrapperNodelet::on_start_object_detection(zed_interfaces::start_object_d
{
NODELET_INFO("Called 'start_object_detection' service");

if (mZedRealCamModel != sl::MODEL::ZED2)
if (mZedRealCamModel != sl::MODEL::ZED2 && mZedRealCamModel != sl::MODEL::ZED2i)
{
mObjDetEnabled = false;
mObjDetRunning = false;

NODELET_ERROR_STREAM("Object detection not started. OD is available only using a ZED2 camera model");
NODELET_ERROR_STREAM("Object detection not started. OD is available only using a ZED2/ZED2i camera model");
res.done = false;
return res.done;
}
Expand Down

0 comments on commit a666d67

Please sign in to comment.