i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-09-16 08:10:19 +08:00
Code Issues Packages Projects Releases Wiki Activity

ROS2 various QOL updates (preparing new binary release) (#1225)

Browse Source 
* Updating examples and README

* updated main readme

* odom: Added always_check_imu_tf parameter. sync: increased default sync_queue_size from 2 to 5 (to be more flexible to different hardware), added input and output diagnostics. rtabmap_viz: fixed node with same name redeclared warning.

* update readme

* Added husky demo

* converted demo_robot_mapping.launch to ros2

* Converted stereo_outdoor demo from ros1 to ros2

* Converted multi-session demo to ros2

* Converted demo_find_object launch to ros2

* renamed files

* uniformized default qos, increased sync_queue_size default to 10

* Added vlp16 + imu example

* Added husky 3D lidar demos

* moved demos in subdir

* Added vlp16+zed example

* rtabmap_viz: ignore odom update if tf not ready when not using topic (to avoid showing red screen). Added champ VSLAM demo.

* forwarded camera_model arg for zed examples, removed qos for turtlebot4 demo

* pointcloud_to_depthimage: added more error logs, removed output camera_info published twice and match ros1 namespaces

* Created two base general examples for 3D lidar usage, then create hardware specific examples on top of them.

* Added isaac sim demo

* Final update of all demos. Fixed MapCloud rviz plugin crashing when floor/ceiling filtering is used and resulting cloud is empty.

* Fixed 2d scan deskewing output frame, moved nav2 params under "params" folder, added number of topics processed/dropped for odometry, added icp_odometry option for turtlebot3 scan-only demo.

* rtabmap_demos: added README with examples

* Added TOC

* bump version to 0.21.9
This commit is contained in:
matlabbe
2024-11-30 17:28:16 -08:00
committed by GitHub
parent e9aa8ed082
commit 3f6adad463
116 changed files with 6102 additions and 575 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_viz/src/GuiWrapper.cpp
+64 -25
View File
@@ -75,10 +75,6 @@ GuiWrapper::GuiWrapper(const rclcpp::NodeOptions & options) :
maxOdomUpdateRate_(10)
{
tfBuffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
//auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
// this->get_node_base_interface(),
// this->get_node_timers_interface());
//tfBuffer_->setCreateTimerInterface(timer_interface);
tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
QString configFile = QDir::homePath()+"/.ros/rtabmapGUI.ini";
@@ -548,8 +544,10 @@ void GuiWrapper::commonMultiCameraCallback(
std_msgs::msg::Header odomHeader;
std::string frameId = frameId_;
Transform odomT;
if(odomMsg.get())
{
odomT = rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose);
odomHeader = odomMsg->header;
if(!odomMsg->child_frame_id.empty())
{
@@ -583,9 +581,18 @@ void GuiWrapper::commonMultiCameraCallback(
odomHeader = imageMsgs[0]->header;
}
odomHeader.frame_id = odomFrameId_;
odomT = rtabmap_conversions::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, *tfBuffer_, waitForTransform_);
if(odomT.isNull())
{
RCLCPP_WARN(this->get_logger(), "Could not get odometry pose from "
"TF for stamp %f, aborting! To show red screen in rtabmap_viz "
"when this happens (indicating potentially lost), set subscribe_odom "
"to true.", rclcpp::Time(odomHeader.stamp).seconds());
return;
}
}
Transform odomT = rtabmap_conversions::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, *tfBuffer_, waitForTransform_);
cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
if(odomMsg.get())
{
@@ -726,7 +733,7 @@ void GuiWrapper::commonMultiCameraCallback(
cameraModels,
0,
rtabmap_conversions::timestampFromROS(odomHeader.stamp)),
odomMsg.get()?rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose):odomT,
odomT,
info);
QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent), Q_ARG(bool, ignoreData));
@@ -749,8 +756,10 @@ void GuiWrapper::commonStereoCallback(
{
std_msgs::msg::Header odomHeader;
std::string frameId = frameId_;
Transform odomT;
if(odomMsg.get())
{
odomT = rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose);
odomHeader = odomMsg->header;
if(!odomMsg->child_frame_id.empty())
{
@@ -776,9 +785,18 @@ void GuiWrapper::commonStereoCallback(
odomHeader = leftCamInfoMsg.header;
}
odomHeader.frame_id = odomFrameId_;
odomT = rtabmap_conversions::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, *tfBuffer_, waitForTransform_);
if(odomT.isNull())
{
RCLCPP_WARN(this->get_logger(), "Could not get odometry pose from "
"TF for stamp %f, aborting! To show red screen in rtabmap_viz "
"when this happens (indicating potentially lost), set subscribe_odom "
"to true.", rclcpp::Time(odomHeader.stamp).seconds());
return;
}
}
Transform odomT = rtabmap_conversions::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, *tfBuffer_, waitForTransform_);
cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
if(odomMsg.get())
{
@@ -906,7 +924,7 @@ void GuiWrapper::commonStereoCallback(
stereoModel,
0,
rtabmap_conversions::timestampFromROS(odomHeader.stamp)),
odomMsg.get()?rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose):odomT,
odomT,
info);
QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent), Q_ARG(bool, ignoreData));
@@ -922,8 +940,10 @@ void GuiWrapper::commonLaserScanCallback(
{
std_msgs::msg::Header odomHeader;
std::string frameId = frameId_;
Transform odomT;
if(odomMsg.get())
{
odomT = rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose);
odomHeader = odomMsg->header;
if(!odomMsg->child_frame_id.empty())
{
@@ -949,9 +969,18 @@ void GuiWrapper::commonLaserScanCallback(
return;
}
odomHeader.frame_id = odomFrameId_;
odomT = rtabmap_conversions::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, *tfBuffer_, waitForTransform_);
if(odomT.isNull())
{
RCLCPP_WARN(this->get_logger(), "Could not get odometry pose from "
"TF for stamp %f, aborting! To show red screen in rtabmap_viz "
"when this happens (indicating potentially lost), set subscribe_odom "
"to true.", rclcpp::Time(odomHeader.stamp).seconds());
return;
}
}
Transform odomT = rtabmap_conversions::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, *tfBuffer_, waitForTransform_);
cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
if(odomMsg.get())
{
@@ -1053,7 +1082,7 @@ void GuiWrapper::commonLaserScanCallback(
rtabmap::CameraModel(),
0,
rtabmap_conversions::timestampFromROS(odomHeader.stamp)),
odomMsg.get()?rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose):odomT,
odomT,
info);
QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent), Q_ARG(bool, ignoreData));
@@ -1068,21 +1097,20 @@ void GuiWrapper::commonOdomCallback(
std_msgs::msg::Header odomHeader = odomMsg->header;
Transform odomT = rtabmap_conversions::getTransform(odomHeader.frame_id, odomMsg->child_frame_id, odomHeader.stamp, *tfBuffer_, waitForTransform_);
Transform odomT = rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose);
cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
if(odomMsg.get())
UASSERT(odomMsg->twist.covariance.size() == 36);
if(odomMsg->twist.covariance[0] != 0 &&
odomMsg->twist.covariance[7] != 0 &&
odomMsg->twist.covariance[14] != 0 &&
odomMsg->twist.covariance[21] != 0 &&
odomMsg->twist.covariance[28] != 0 &&
odomMsg->twist.covariance[35] != 0)
{
UASSERT(odomMsg->twist.covariance.size() == 36);
if(odomMsg->twist.covariance[0] != 0 &&
odomMsg->twist.covariance[7] != 0 &&
odomMsg->twist.covariance[14] != 0 &&
odomMsg->twist.covariance[21] != 0 &&
odomMsg->twist.covariance[28] != 0 &&
odomMsg->twist.covariance[35] != 0)
{
covariance = cv::Mat(6,6,CV_64FC1,(void*)odomMsg->twist.covariance.data()).clone();
}
covariance = cv::Mat(6,6,CV_64FC1,(void*)odomMsg->twist.covariance.data()).clone();
}
if(odomHeader.frame_id.empty())
{
RCLCPP_ERROR(this->get_logger(), "Odometry frame not set!?");
@@ -1125,7 +1153,7 @@ void GuiWrapper::commonOdomCallback(
rtabmap::CameraModel(),
0,
rtabmap_conversions::timestampFromROS(odomHeader.stamp)),
odomMsg.get()?rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose):odomT,
odomT,
info);
QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent), Q_ARG(bool, ignoreData));
@@ -1139,8 +1167,10 @@ void GuiWrapper::commonSensorDataCallback(
UASSERT(sensorDataMsg.get());
std_msgs::msg::Header odomHeader;
std::string frameId = frameId_;
Transform odomT;
if(odomMsg.get())
{
odomT = rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose);
odomHeader = odomMsg->header;
if(!odomMsg->child_frame_id.empty())
{
@@ -1155,9 +1185,18 @@ void GuiWrapper::commonSensorDataCallback(
{
odomHeader = sensorDataMsg->header;
odomHeader.frame_id = odomFrameId_;
odomT = rtabmap_conversions::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, *tfBuffer_, waitForTransform_);
if(odomT.isNull())
{
RCLCPP_WARN(this->get_logger(), "Could not get odometry pose from "
"TF for stamp %f, aborting! To show red screen in rtabmap_viz "
"when this happens (indicating potentially lost), set subscribe_odom "
"to true.", rclcpp::Time(odomHeader.stamp).seconds());
return;
}
}
Transform odomT = rtabmap_conversions::getTransform(odomHeader.frame_id, frameId, odomHeader.stamp, *tfBuffer_, waitForTransform_);
cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
if(odomMsg.get())
{
@@ -1228,7 +1267,7 @@ void GuiWrapper::commonSensorDataCallback(
info.reg.covariance = covariance;
rtabmap::OdometryEvent odomEvent(
data,
odomMsg.get()?rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose):odomT,
odomT,
info);
QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent), Q_ARG(bool, ignoreData));
rtabmap_viz/src/PreferencesDialogROS.cpp
+12 -8
View File
@@ -85,8 +85,7 @@ QString PreferencesDialogROS::getParamMessage()
bool PreferencesDialogROS::hasAllParameters()
{
auto node = std::make_shared<rclcpp::Node>("rtabmap_viz");
auto client = std::make_shared<rclcpp::AsyncParametersClient>(node, rtabmapNodeName_);
auto client = std::make_shared<rclcpp::AsyncParametersClient>(node_, rtabmapNodeName_);
return client->service_is_ready();
}
@@ -98,8 +97,13 @@ bool PreferencesDialogROS::readCoreSettings(const QString & filePath)
path = filePath;
}
auto node = std::make_shared<rclcpp::Node>("rtabmap_viz");
RCLCPP_INFO(node->get_logger(), "%s", this->getParamMessage().toStdString().c_str());
char nodeName[42];
snprintf(
nodeName, sizeof(nodeName), "rtabmap_viz_param_client_%zx",
reinterpret_cast<size_t>(this)
);
auto node = std::make_shared<rclcpp::Node>(nodeName);
RCLCPP_INFO(node_->get_logger(), "%s", this->getParamMessage().toStdString().c_str());
rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
// remove Odom parameters
for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end();)
@@ -145,7 +149,7 @@ bool PreferencesDialogROS::readCoreSettings(const QString & filePath)
auto client = std::make_shared<rclcpp::AsyncParametersClient>(node, rtabmapNodeName_);
if (!client->wait_for_service(std::chrono::seconds(5))) {
RCLCPP_ERROR(node->get_logger(), "Can't call rtabmap parameters service, is the node running?");
RCLCPP_ERROR(node_->get_logger(), "Can't call rtabmap parameters service, is the node running?");
}
int readCount = 0;
if(client->service_is_ready())
@@ -163,18 +167,18 @@ bool PreferencesDialogROS::readCoreSettings(const QString & filePath)
}
}
RCLCPP_INFO(node->get_logger(), "Parameters read = %d", readCount);
RCLCPP_INFO(node_->get_logger(), "Parameters read = %d", readCount);
if(readCount>0)
{
RCLCPP_INFO(node->get_logger(), "Parameters successfully read.");
RCLCPP_INFO(node_->get_logger(), "Parameters successfully read.");
}
else
{
if(this->isVisible())
{
QString warning = tr("Failed to get RTAB-Map parameters from ROS server, the rtabmap node may be not started or some parameters won't work...");
RCLCPP_WARN(node->get_logger(), "%s", warning.toStdString().c_str());
RCLCPP_WARN(node_->get_logger(), "%s", warning.toStdString().c_str());
QMessageBox::warning(this, tr("Can't read parameters from ROS server."), warning);
}
return false;