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

merged master->ros2

Browse Source
This commit is contained in:
matlabbe
2022-12-04 18:11:37 -08:00
parent a9db92e47f f327b3f289
commit 16875b4ad5
7 changed files with 302 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+8 -1
View File
@@ -126,7 +126,8 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
mappingAltitudeDelta_(Parameters::defaultGridGlobalAltitudeDelta()),
alreadyRectifiedImages_(Parameters::defaultRtabmapImagesAlreadyRectified()),
twoDMapping_(Parameters::defaultRegForce3DoF()),
previousStamp_(0)
previousStamp_(0),
ulogToRosout_(this)
{
char * rosHomePath = getenv("ROS_HOME");
std::string workingDir = rosHomePath?rosHomePath:UDirectory::homeDir()+"/.ros";
@@ -172,6 +173,11 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
}
}
int eventLevel = ULogger::kFatal;
eventLevel = this->declare_parameter("log_to_rosout_level", eventLevel);
UASSERT(eventLevel >= ULogger::kDebug && eventLevel <= ULogger::kFatal);
ULogger::setEventLevel((ULogger::Level)eventLevel);
publishTf = this->declare_parameter("publish_tf", publishTf);
tfDelay = this->declare_parameter("tf_delay", tfDelay);
tfTolerance = this->declare_parameter("tf_tolerance", tfTolerance);
@@ -211,6 +217,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
groundTruthBaseFrameId_.c_str());
}
RCLCPP_INFO(this->get_logger(), "rtabmap: map_frame_id = %s", mapFrameId_.c_str());
RCLCPP_INFO(this->get_logger(), "rtabmap: log_to_rosout_level = %d", eventLevel);
RCLCPP_INFO(this->get_logger(), "rtabmap: initial_pose = %s", initialPoseStr.c_str());
RCLCPP_INFO(this->get_logger(), "rtabmap: use_action_for_goal = %s", useActionForGoal_?"true":"false");
RCLCPP_INFO(this->get_logger(), "rtabmap: tf_delay = %f", tfDelay);
src/OdometryROS.cpp
+8 -1
View File
@@ -87,7 +87,8 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
waitIMUToinit_(false),
imuProcessed_(false),
configPath_(),
initialPose_(Transform::getIdentity())
initialPose_(Transform::getIdentity()),
ulogToRosout_(this)
{
int qos = this->declare_parameter("qos", (int)qos_);
qos_ = (rmw_qos_reliability_policy_t)qos;
@@ -131,6 +132,11 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
waitIMUToinit_ = this->declare_parameter("wait_imu_to_init", waitIMUToinit_);
int eventLevel = ULogger::kFatal;
eventLevel = this->declare_parameter("log_to_rosout_level", eventLevel);
UASSERT(eventLevel >= ULogger::kDebug && eventLevel <= ULogger::kFatal);
ULogger::setEventLevel((ULogger::Level)eventLevel);
if(publishTf_ && !guessFrameId_.empty() && guessFrameId_.compare(odomFrameId_) == 0)
{
RCLCPP_WARN(this->get_logger(), "\"publish_tf\" and \"guess_frame_id\" cannot be used "
@@ -142,6 +148,7 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
RCLCPP_INFO(this->get_logger(), "Odometry: odom_frame_id = %s", odomFrameId_.c_str());
RCLCPP_INFO(this->get_logger(), "Odometry: publish_tf = %s", publishTf_?"true":"false");
RCLCPP_INFO(this->get_logger(), "Odometry: wait_for_transform = %f", waitForTransform_);
RCLCPP_INFO(this->get_logger(), "Odometry: log_to_rosout_level = %d", eventLevel);
RCLCPP_INFO(this->get_logger(), "Odometry: initial_pose = %s", initialPose_.prettyPrint().c_str());
RCLCPP_INFO(this->get_logger(), "Odometry: ground_truth_frame_id = %s", groundTruthFrameId_.c_str());
RCLCPP_INFO(this->get_logger(), "Odometry: ground_truth_base_frame_id = %s", groundTruthBaseFrameId_.c_str());
src/nodelets/stereo_odometry.cpp
-1
View File
@@ -351,7 +351,6 @@ void StereoOdometry::commonCallback(
}
}
int quality = -1;
if(!leftImages[i]->image.empty() && !rightImages[i]->image.empty())
{
bool alreadyRectified = true;