Skip to content

Commit e816e2c

Browse files
lucaswlucasw
committed
Undo ROS_DEBUG_STREAM changes, only create clock publisher when needed
1 parent 5c059c3 commit e816e2c

File tree

1 file changed

+49
-50
lines changed

1 file changed

+49
-50
lines changed

‎zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp‎

Lines changed: 49 additions & 50 deletions
Original file line numberDiff line numberDiff line change
@@ -427,97 +427,99 @@ void ZEDWrapperNodelet::onInit()
427427
image_transport::ImageTransport it_zed(mNhNs);
428428

429429
mPubRgb = it_zed.advertiseCamera(rgb_topic, 1); // rgb
430-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubRgb.getTopic());
431-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubRgb.getInfoTopic());
430+
NODELET_INFO_STREAM("Advertised on topic " << mPubRgb.getTopic());
431+
NODELET_INFO_STREAM("Advertised on topic " << mPubRgb.getInfoTopic());
432432
mPubRawRgb = it_zed.advertiseCamera(rgb_raw_topic, 1); // rgb raw
433-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubRawRgb.getTopic());
434-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubRawRgb.getInfoTopic());
433+
NODELET_INFO_STREAM("Advertised on topic " << mPubRawRgb.getTopic());
434+
NODELET_INFO_STREAM("Advertised on topic " << mPubRawRgb.getInfoTopic());
435435
mPubLeft = it_zed.advertiseCamera(left_topic, 1); // left
436-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubLeft.getTopic());
437-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubLeft.getInfoTopic());
436+
NODELET_INFO_STREAM("Advertised on topic " << mPubLeft.getTopic());
437+
NODELET_INFO_STREAM("Advertised on topic " << mPubLeft.getInfoTopic());
438438
mPubRawLeft = it_zed.advertiseCamera(left_raw_topic, 1); // left raw
439-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubRawLeft.getTopic());
440-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubRawLeft.getInfoTopic());
439+
NODELET_INFO_STREAM("Advertised on topic " << mPubRawLeft.getTopic());
440+
NODELET_INFO_STREAM("Advertised on topic " << mPubRawLeft.getInfoTopic());
441441
mPubRight = it_zed.advertiseCamera(right_topic, 1); // right
442-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubRight.getTopic());
443-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubRight.getInfoTopic());
442+
NODELET_INFO_STREAM("Advertised on topic " << mPubRight.getTopic());
443+
NODELET_INFO_STREAM("Advertised on topic " << mPubRight.getInfoTopic());
444444
mPubRawRight = it_zed.advertiseCamera(right_raw_topic, 1); // right raw
445-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubRawRight.getTopic());
446-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubRawRight.getInfoTopic());
445+
NODELET_INFO_STREAM("Advertised on topic " << mPubRawRight.getTopic());
446+
NODELET_INFO_STREAM("Advertised on topic " << mPubRawRight.getInfoTopic());
447447

448448
mPubRgbGray = it_zed.advertiseCamera(rgb_gray_topic, 1); // rgb
449-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubRgbGray.getTopic());
450-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubRgbGray.getInfoTopic());
449+
NODELET_INFO_STREAM("Advertised on topic " << mPubRgbGray.getTopic());
450+
NODELET_INFO_STREAM("Advertised on topic " << mPubRgbGray.getInfoTopic());
451451
mPubRawRgbGray = it_zed.advertiseCamera(rgb_raw_gray_topic, 1); // rgb raw
452-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubRawRgbGray.getTopic());
453-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubRawRgbGray.getInfoTopic());
452+
NODELET_INFO_STREAM("Advertised on topic " << mPubRawRgbGray.getTopic());
453+
NODELET_INFO_STREAM("Advertised on topic " << mPubRawRgbGray.getInfoTopic());
454454
mPubLeftGray = it_zed.advertiseCamera(left_gray_topic, 1); // left
455-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubLeftGray.getTopic());
456-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubLeftGray.getInfoTopic());
455+
NODELET_INFO_STREAM("Advertised on topic " << mPubLeftGray.getTopic());
456+
NODELET_INFO_STREAM("Advertised on topic " << mPubLeftGray.getInfoTopic());
457457
mPubRawLeftGray = it_zed.advertiseCamera(left_raw_gray_topic, 1); // left raw
458-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubRawLeftGray.getTopic());
459-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubRawLeftGray.getInfoTopic());
458+
NODELET_INFO_STREAM("Advertised on topic " << mPubRawLeftGray.getTopic());
459+
NODELET_INFO_STREAM("Advertised on topic " << mPubRawLeftGray.getInfoTopic());
460460
mPubRightGray = it_zed.advertiseCamera(right_gray_topic, 1); // right
461-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubRightGray.getTopic());
462-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubRightGray.getInfoTopic());
461+
NODELET_INFO_STREAM("Advertised on topic " << mPubRightGray.getTopic());
462+
NODELET_INFO_STREAM("Advertised on topic " << mPubRightGray.getInfoTopic());
463463
mPubRawRightGray = it_zed.advertiseCamera(right_raw_gray_topic, 1); // right raw
464-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubRawRightGray.getTopic());
465-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubRawRightGray.getInfoTopic());
464+
NODELET_INFO_STREAM("Advertised on topic " << mPubRawRightGray.getTopic());
465+
NODELET_INFO_STREAM("Advertised on topic " << mPubRawRightGray.getInfoTopic());
466466

467467
mPubDepth = it_zed.advertiseCamera(depth_topic_root, 1); // depth
468-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubDepth.getTopic());
469-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubDepth.getInfoTopic());
468+
NODELET_INFO_STREAM("Advertised on topic " << mPubDepth.getTopic());
469+
NODELET_INFO_STREAM("Advertised on topic " << mPubDepth.getInfoTopic());
470470

471471
mPubStereo = it_zed.advertise(stereo_topic, 1);
472-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubStereo.getTopic());
472+
NODELET_INFO_STREAM("Advertised on topic " << mPubStereo.getTopic());
473473
mPubRawStereo = it_zed.advertise(stereo_raw_topic, 1);
474-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubRawStereo.getTopic());
474+
NODELET_INFO_STREAM("Advertised on topic " << mPubRawStereo.getTopic());
475475

476-
// TODO(lucasw) only use if use_sim_time param is true
477-
mPubSimClock = mNhNs.advertise<rosgraph_msgs::Clock>("/clock", 2);
476+
if (mUseSimTime)
477+
{
478+
mPubSimClock = mNhNs.advertise<rosgraph_msgs::Clock>("/clock", 2);
479+
}
478480

479481
// Confidence Map publisher
480482
mPubConfMap = mNhNs.advertise<sensor_msgs::Image>(conf_map_topic, 1); // confidence map
481-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubConfMap.getTopic());
483+
NODELET_INFO_STREAM("Advertised on topic " << mPubConfMap.getTopic());
482484

483485
// Disparity publisher
484486
mPubDisparity = mNhNs.advertise<stereo_msgs::DisparityImage>(disparityTopic, static_cast<int>(mVideoDepthFreq));
485-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubDisparity.getTopic());
487+
NODELET_INFO_STREAM("Advertised on topic " << mPubDisparity.getTopic());
486488

487489
// PointCloud publishers
488490
mPubCloud = mNhNs.advertise<sensor_msgs::PointCloud2>(pointcloud_topic, 1);
489-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubCloud.getTopic());
491+
NODELET_INFO_STREAM("Advertised on topic " << mPubCloud.getTopic());
490492

491493
if (mMappingEnabled)
492494
{
493495
mPubFusedCloud = mNhNs.advertise<sensor_msgs::PointCloud2>(pointcloud_fused_topic, 1);
494-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubFusedCloud.getTopic() << " @ " << mFusedPcPubFreq << " Hz");
496+
NODELET_INFO_STREAM("Advertised on topic " << mPubFusedCloud.getTopic() << " @ " << mFusedPcPubFreq << " Hz");
495497
}
496498

497499
// Object detection publishers
498500
if (mObjDetEnabled)
499501
{
500502
mPubObjDet = mNhNs.advertise<zed_interfaces::ObjectsStamped>(object_det_topic, 1);
501-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubObjDet.getTopic());
503+
NODELET_INFO_STREAM("Advertised on topic " << mPubObjDet.getTopic());
502504
}
503505

504506
// Odometry and Pose publisher
505507
mPubPose = mNhNs.advertise<geometry_msgs::PoseStamped>(poseTopic, 1);
506-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubPose.getTopic());
508+
NODELET_INFO_STREAM("Advertised on topic " << mPubPose.getTopic());
507509

508510
mPubPoseCov = mNhNs.advertise<geometry_msgs::PoseWithCovarianceStamped>(pose_cov_topic, 1);
509-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubPoseCov.getTopic());
511+
NODELET_INFO_STREAM("Advertised on topic " << mPubPoseCov.getTopic());
510512

511513
mPubOdom = mNhNs.advertise<nav_msgs::Odometry>(odometryTopic, 1);
512-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubOdom.getTopic());
514+
NODELET_INFO_STREAM("Advertised on topic " << mPubOdom.getTopic());
513515

514516
// Camera Path
515517
if (mPathPubRate > 0)
516518
{
517519
mPubOdomPath = mNhNs.advertise<nav_msgs::Path>(odom_path_topic, 1, true);
518-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubOdomPath.getTopic());
520+
NODELET_INFO_STREAM("Advertised on topic " << mPubOdomPath.getTopic());
519521
mPubMapPath = mNhNs.advertise<nav_msgs::Path>(map_path_topic, 1, true);
520-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubMapPath.getTopic());
522+
NODELET_INFO_STREAM("Advertised on topic " << mPubMapPath.getTopic());
521523

522524
mPathTimer = mNhNs.createTimer(ros::Duration(1.0 / mPathPubRate), &ZEDWrapperNodelet::callback_pubPath, this);
523525

@@ -540,27 +542,27 @@ void ZEDWrapperNodelet::onInit()
540542
{
541543
// IMU Publishers
542544
mPubImu = mNhNs.advertise<sensor_msgs::Imu>(imu_topic, 1 /*static_cast<int>(mSensPubRate)*/);
543-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubImu.getTopic());
545+
NODELET_INFO_STREAM("Advertised on topic " << mPubImu.getTopic());
544546
mPubImuRaw = mNhNs.advertise<sensor_msgs::Imu>(imu_topic_raw, 1 /*static_cast<int>(mSensPubRate)*/);
545-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubImuRaw.getTopic());
547+
NODELET_INFO_STREAM("Advertised on topic " << mPubImuRaw.getTopic());
546548
mPubImuMag = mNhNs.advertise<sensor_msgs::MagneticField>(imu_mag_topic, 1 /*MAG_FREQ*/);
547-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubImuMag.getTopic());
549+
NODELET_INFO_STREAM("Advertised on topic " << mPubImuMag.getTopic());
548550

549551
if (mZedRealCamModel == sl::MODEL::ZED2 || mZedRealCamModel == sl::MODEL::ZED2i)
550552
{
551553
// IMU temperature sensor
552554
mPubImuTemp = mNhNs.advertise<sensor_msgs::Temperature>(imu_temp_topic, 1 /*static_cast<int>(mSensPubRate)*/);
553-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubImuTemp.getTopic());
555+
NODELET_INFO_STREAM("Advertised on topic " << mPubImuTemp.getTopic());
554556

555557
// Atmospheric pressure
556558
mPubPressure = mNhNs.advertise<sensor_msgs::FluidPressure>(pressure_topic, 1 /*static_cast<int>(BARO_FREQ)*/);
557-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubPressure.getTopic());
559+
NODELET_INFO_STREAM("Advertised on topic " << mPubPressure.getTopic());
558560

559561
// CMOS sensor temperatures
560562
mPubTempL = mNhNs.advertise<sensor_msgs::Temperature>(temp_topic_left, 1 /*static_cast<int>(BARO_FREQ)*/);
561-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubTempL.getTopic());
563+
NODELET_INFO_STREAM("Advertised on topic " << mPubTempL.getTopic());
562564
mPubTempR = mNhNs.advertise<sensor_msgs::Temperature>(temp_topic_right, 1 /*static_cast<int>(BARO_FREQ)*/);
563-
NODELET_DEBUG_STREAM("Advertised on topic " << mPubTempR.getTopic());
565+
NODELET_INFO_STREAM("Advertised on topic " << mPubTempR.getTopic());
564566
}
565567

566568
// Publish camera imu transform in a latched topic
@@ -1162,7 +1164,6 @@ bool ZEDWrapperNodelet::getCamera2BaseTransform()
11621164
}
11631165
catch (tf2::TransformException& ex)
11641166
{
1165-
NODELET_WARN("failed looked tranform");
11661167
if (!first_error)
11671168
{
11681169
NODELET_DEBUG_THROTTLE(1.0, "Transform error: %s", ex.what());
@@ -1214,7 +1215,6 @@ bool ZEDWrapperNodelet::getSens2CameraTransform()
12141215
}
12151216
catch (tf2::TransformException& ex)
12161217
{
1217-
NODELET_INFO("look up transform failed");
12181218
if (!first_error)
12191219
{
12201220
NODELET_DEBUG_THROTTLE(1.0, "Transform error: %s", ex.what());
@@ -1613,7 +1613,6 @@ void ZEDWrapperNodelet::start_pos_tracking()
16131613

16141614
do
16151615
{
1616-
NODELET_INFO_STREAM("waiting for transforms");
16171616
transformOk = set_pose(mInitialBasePose[0], mInitialBasePose[1], mInitialBasePose[2], mInitialBasePose[3],
16181617
mInitialBasePose[4], mInitialBasePose[5]);
16191618

0 commit comments

Comments
 (0)