Skip to content
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

Jump to bottom

Added sync diagnostic #1026

Merged
merged 2 commits into from
Aug 27, 2023
Merged

Added sync diagnostic #1026

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
cd61b7b
Added sync diagnotic
matlabbe Aug 26, 2023
4bb227d
Removed composite task to avoid empty msg, added odom status task
matlabbe Aug 27, 2023
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
2 changes: 2 additions & 0 deletions rtabmap_odom/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@ find_package(catkin REQUIRED COMPONENTS
cv_bridge image_geometry laser_geometry message_filters
nav_msgs nodelet pcl_conversions pcl_ros pluginlib roscpp
sensor_msgs rtabmap_conversions rtabmap_msgs rtabmap_util
rtabmap_sync
)

catkin_package(
Expand All @@ -13,6 +14,7 @@ catkin_package(
CATKIN_DEPENDS cv_bridge image_geometry laser_geometry message_filters
nav_msgs nodelet pcl_conversions pcl_ros pluginlib roscpp
sensor_msgs rtabmap_conversions rtabmap_msgs rtabmap_util
rtabmap_sync
)

###########
Expand Down
22 changes: 16 additions & 6 deletions rtabmap_odom/include/rtabmap_odom/OdometryROS.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,21 +38,24 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <std_msgs/Header.h>
#include <sensor_msgs/Imu.h>

#include <diagnostic_updater/diagnostic_updater.h>

#include <rtabmap_msgs/ResetPose.h>
#include <rtabmap/core/SensorData.h>
#include <rtabmap/core/Parameters.h>

#include <boost/thread.hpp>

#include "rtabmap_util/ULogToRosout.h"
#include "rtabmap_sync/SyncDiagnostic.h"

namespace rtabmap {
class Odometry;
}

namespace rtabmap_odom {

class OdometryROS : public nodelet::Nodelet
class OdometryROS : public nodelet::Nodelet, public rtabmap_sync::SyncDiagnostic
{

public:
Expand All @@ -77,8 +80,7 @@ class OdometryROS : public nodelet::Nodelet
bool isPaused() const {return paused_;}

protected:
void startWarningThread(const std::string & subscribedTopicsMsg, bool approxSync);
void callbackCalled() {callbackCalled_ = true;}
void initDiagnosticMsg(const std::string & subscribedTopicsMsg, bool approxSync, const std::string & subscribedTopic = "");

virtual void flushCallbacks() = 0;
tf::TransformListener & tfListener() {return tfListener_;}
Expand All @@ -88,7 +90,6 @@ class OdometryROS : public nodelet::Nodelet
virtual void postProcessData(const rtabmap::SensorData & data, const std_msgs::Header & header) const {}

private:
void warningLoop(const std::string & subscribedTopicsMsg, bool approxSync);
virtual void onInit();
virtual void onOdomInit() = 0;
virtual void updateParameters(rtabmap::ParametersMap & parameters) {}
Expand All @@ -98,8 +99,6 @@ class OdometryROS : public nodelet::Nodelet

private:
rtabmap::Odometry * odometry_;
boost::thread * warningThread_;
bool callbackCalled_;

// parameters
std::string frameId_;
Expand Down Expand Up @@ -154,6 +153,17 @@ class OdometryROS : public nodelet::Nodelet
std::pair<rtabmap::SensorData, std_msgs::Header > bufferedData_;

rtabmap_util::ULogToRosout ulogToRosout_;

class OdomStatusTask : public diagnostic_updater::DiagnosticTask
{
public:
OdomStatusTask();
void setStatus(bool isLost);
void run(diagnostic_updater::DiagnosticStatusWrapper &stat);
private:
bool lost_;
};
OdomStatusTask statusDiagnostic_;
};

}
Expand Down
1 change: 1 addition & 0 deletions rtabmap_odom/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@
<depend>rtabmap_conversions</depend>
<depend>rtabmap_msgs</depend>
<depend>rtabmap_util</depend>
<depend>rtabmap_sync</depend>

<export>
<nodelet plugin="${prefix}/nodelet_plugins.xml" />
Expand Down
65 changes: 40 additions & 25 deletions rtabmap_odom/src/OdometryROS.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,9 +57,8 @@ using namespace rtabmap;
namespace rtabmap_odom {

OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) :
rtabmap_sync::SyncDiagnostic(0.5),
odometry_(0),
warningThread_(0),
callbackCalled_(false),
frameId_("base_link"),
odomFrameId_("odom"),
groundTruthFrameId_(""),
Expand Down Expand Up @@ -91,13 +90,6 @@ OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) :

OdometryROS::~OdometryROS()
{
if(warningThread_)
{
callbackCalled();
warningThread_->join();
delete warningThread_;
}

delete odometry_;
}

Expand Down Expand Up @@ -378,29 +370,20 @@ void OdometryROS::onInit()
onOdomInit();
}

void OdometryROS::startWarningThread(const std::string & subscribedTopicsMsg, bool approxSync)
void OdometryROS::initDiagnosticMsg(const std::string & subscribedTopicsMsg, bool approxSync, const std::string & subscribedTopic)
{
warningThread_ = new boost::thread(boost::bind(&OdometryROS::warningLoop, this, subscribedTopicsMsg, approxSync));
NODELET_INFO("%s", subscribedTopicsMsg.c_str());
}

void OdometryROS::warningLoop(const std::string & subscribedTopicsMsg, bool approxSync)
{
ros::Duration r(5.0);
while(!callbackCalled_)
{
r.sleep();
if(!callbackCalled_)
{
ROS_WARN("%s: Did not receive data since 5 seconds! Make sure the input topics are "
std::vector<diagnostic_updater::DiagnosticTask*> tasks;
tasks.push_back(&statusDiagnostic_);
initDiagnostic(subscribedTopic,
uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are "
"published (\"$ rostopic hz my_topic\") and the timestamps in their "
"header are set. %s%s",
getName().c_str(),
approxSync?"":"Parameter \"approx_sync\" is false, which means that input "
"topics should have all the exact timestamp for the callback to be called.",
subscribedTopicsMsg.c_str());
}
}
subscribedTopicsMsg.c_str()),
tasks);
}

rtabmap::Transform OdometryROS::velocityGuess() const
Expand Down Expand Up @@ -953,6 +936,17 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header
{
NODELET_INFO( "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (ros::WallTime::now()-time).toSec());
}

statusDiagnostic_.setStatus(pose.isNull());
if(!pose.isNull())
{
double curentRate = 1.0/(ros::WallTime::now()-time).toSec();
tick(header.stamp,
maxUpdateRate_>0 && maxUpdateRate_ < curentRate ? maxUpdateRate_:
expectedUpdateRate_>0 && expectedUpdateRate_ < curentRate ? expectedUpdateRate_:
previousStamp_ == 0.0 || header.stamp.toSec() - previousStamp_ > 1.0/curentRate?0:curentRate);
}

previousStamp_ = header.stamp.toSec();
}
}
Expand Down Expand Up @@ -1038,5 +1032,26 @@ bool OdometryROS::setLogError(std_srvs::Empty::Request&, std_srvs::Empty::Respon
return true;
}

OdometryROS::OdomStatusTask::OdomStatusTask() :
diagnostic_updater::DiagnosticTask("Odom status"),
lost_(false)
{}

void OdometryROS::OdomStatusTask::setStatus(bool isLost)
{
lost_ = isLost;
}

void OdometryROS::OdomStatusTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat)
{
if(lost_)
{
stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Lost!");
}
else
{
stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Tracking.");
}
}

}
7 changes: 7 additions & 0 deletions rtabmap_odom/src/nodelets/icp_odometry.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -157,6 +157,11 @@ class ICPOdometry : public OdometryROS
cloud_sub_ = nh.subscribe("scan_cloud", queueSize, &ICPOdometry::callbackCloud, this);

filtered_scan_pub_ = nh.advertise<sensor_msgs::PointCloud2>("odom_filtered_input_scan", 1);

initDiagnosticMsg(uFormat("\n%s subscribed to %s and %s (make sure only one of this topic is published, otherwise remap one to a dummy topic name).",
getName().c_str(),
scan_sub_.getTopic().c_str(),
cloud_sub_.getTopic().c_str()), true);
}

virtual void updateParameters(ParametersMap & parameters)
Expand Down Expand Up @@ -329,6 +334,7 @@ class ICPOdometry : public OdometryROS
scan_sub_.shutdown();
return;
}

scanReceived_ = true;
if(this->isPaused())
{
Expand Down Expand Up @@ -570,6 +576,7 @@ class ICPOdometry : public OdometryROS
cloud_sub_.shutdown();
return;
}

cloudReceived_ = true;
if(this->isPaused())
{
Expand Down
11 changes: 3 additions & 8 deletions rtabmap_odom/src/nodelets/rgbd_odometry.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -161,6 +161,7 @@ class RGBDOdometry : public OdometryROS
NODELET_INFO("RGBDOdometry: rgbd_cameras = %d", rgbdCameras);
NODELET_INFO("RGBDOdometry: keep_color = %s", keepColor_?"true":"false");

std::string subscribedTopic;
std::string subscribedTopicsMsg;
if(subscribeRGBD)
{
Expand Down Expand Up @@ -356,6 +357,7 @@ class RGBDOdometry : public OdometryROS
exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
}

subscribedTopic = rgb_nh.resolveName("image");
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s",
getName().c_str(),
approxSync?"approx":"exact",
Expand All @@ -364,7 +366,7 @@ class RGBDOdometry : public OdometryROS
image_depth_sub_.getTopic().c_str(),
info_sub_.getTopic().c_str());
}
this->startWarningThread(subscribedTopicsMsg, approxSync);
initDiagnosticMsg(subscribedTopicsMsg, approxSync, subscribedTopic);
}

virtual void updateParameters(ParametersMap & parameters)
Expand Down Expand Up @@ -546,7 +548,6 @@ class RGBDOdometry : public OdometryROS
const sensor_msgs::ImageConstPtr& depth,
const sensor_msgs::CameraInfoConstPtr& cameraInfo)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(1);
Expand Down Expand Up @@ -575,7 +576,6 @@ class RGBDOdometry : public OdometryROS
void callbackRGBD(
const rtabmap_msgs::RGBDImageConstPtr& image)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(1);
Expand All @@ -591,7 +591,6 @@ class RGBDOdometry : public OdometryROS
void callbackRGBDX(
const rtabmap_msgs::RGBDImagesConstPtr& images)
{
callbackCalled();
if(!this->isPaused())
{
if(images->rgbd_images.empty())
Expand All @@ -616,7 +615,6 @@ class RGBDOdometry : public OdometryROS
const rtabmap_msgs::RGBDImageConstPtr& image,
const rtabmap_msgs::RGBDImageConstPtr& image2)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(2);
Expand All @@ -636,7 +634,6 @@ class RGBDOdometry : public OdometryROS
const rtabmap_msgs::RGBDImageConstPtr& image2,
const rtabmap_msgs::RGBDImageConstPtr& image3)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(3);
Expand All @@ -659,7 +656,6 @@ class RGBDOdometry : public OdometryROS
const rtabmap_msgs::RGBDImageConstPtr& image3,
const rtabmap_msgs::RGBDImageConstPtr& image4)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(4);
Expand All @@ -685,7 +681,6 @@ class RGBDOdometry : public OdometryROS
const rtabmap_msgs::RGBDImageConstPtr& image4,
const rtabmap_msgs::RGBDImageConstPtr& image5)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(5);
Expand Down
3 changes: 1 addition & 2 deletions rtabmap_odom/src/nodelets/rgbdicp_odometry.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -202,7 +202,7 @@ class RGBDICPOdometry : public OdometryROS
info_sub_.getTopic().c_str(),
scan_sub_.getTopic().c_str());
}
this->startWarningThread(subscribedTopicsMsg, approxSync);
initDiagnosticMsg(subscribedTopicsMsg, approxSync);
}

virtual void updateParameters(ParametersMap & parameters)
Expand Down Expand Up @@ -243,7 +243,6 @@ class RGBDICPOdometry : public OdometryROS
const sensor_msgs::LaserScanConstPtr& scanMsg,
const sensor_msgs::PointCloud2ConstPtr& cloudMsg)
{
callbackCalled();
if(!this->isPaused())
{
if(!(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
Expand Down
Loading
Loading