diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp
index ac6fd314ce63e86096080f282ed6d9d673b74363..17d1fc9ec7aeaab4126c967431b491c1dfccf37d 100644
--- a/nav2_amcl/src/amcl_node.cpp
+++ b/nav2_amcl/src/amcl_node.cpp
@@ -591,9 +591,9 @@ bool AmclNode::addNewScanner(
tf_buffer_->transform(ident, laser_pose, base_frame_id_);
} catch (tf2::TransformException & e) {
RCLCPP_ERROR(get_logger(), "Couldn't transform from %s to %s, "
- "even though the message notifier is in use",
+ "even though the message notifier is in use: (%s)",
laser_scan->header.frame_id.c_str(),
- base_frame_id_.c_str());
+ base_frame_id_.c_str(), e.what());
return false;
}
@@ -833,8 +833,8 @@ AmclNode::calculateMaptoOdomTransform(
tf2::toMsg(tmp_tf.inverse(), tmp_tf_stamped.pose);
tf_buffer_->transform(tmp_tf_stamped, odom_to_map, odom_frame_id_);
- } catch (tf2::TransformException &) {
- RCLCPP_DEBUG(get_logger(), "Failed to subtract base to odom transform");
+ } catch (tf2::TransformException & e) {
+ RCLCPP_DEBUG(get_logger(), "Failed to subtract base to odom transform: (%s)", e.what());
return;
}
diff --git a/nav2_map_server/src/occ_grid_loader.cpp b/nav2_map_server/src/occ_grid_loader.cpp
index 924e0e70b2c57f05ed9f3e10c924e7ec9b25ca66..2d6b176137eda02c48cbf707e004d2a39df8a4c1 100644
--- a/nav2_map_server/src/occ_grid_loader.cpp
+++ b/nav2_map_server/src/occ_grid_loader.cpp
@@ -90,8 +90,9 @@ OccGridLoader::on_configure(const rclcpp_lifecycle::State & /*state*/)
try {
loadParameters.resolution = doc["resolution"].as<double>();
- } catch (YAML::Exception &) {
- throw std::runtime_error("The map does not contain a resolution tag or it is invalid");
+ } catch (YAML::Exception & e) {
+ std::string err("The map does not contain a resolution tag or it is invalid: %s", e.what());
+ throw std::runtime_error(err);
}
try {
@@ -99,19 +100,23 @@ OccGridLoader::on_configure(const rclcpp_lifecycle::State & /*state*/)
loadParameters.origin[1] = doc["origin"][1].as<double>();
loadParameters.origin[2] = doc["origin"][2].as<double>();
} catch (YAML::Exception &) {
- throw std::runtime_error("The map does not contain an origin tag or it is invalid");
+ std::string err("The map does not contain an origin tag or it is invalid: %s", e.what());
+ throw std::runtime_error(err);
}
try {
loadParameters.free_thresh = doc["free_thresh"].as<double>();
- } catch (YAML::Exception &) {
- throw std::runtime_error("The map does not contain a free_thresh tag or it is invalid");
+ } catch (YAML::Exception & e) {
+ std::string err("The map does not contain a free_thresh tag or it is invalid: %s", e.what());
+ throw std::runtime_error(err);
}
try {
loadParameters.occupied_thresh = doc["occupied_thresh"].as<double>();
- } catch (YAML::Exception &) {
- throw std::runtime_error("The map does not contain an occupied_thresh tag or it is invalid");
+ } catch (YAML::Exception & e) {
+ std::string err("The map does not contain an "
+ "occupied_thresh tag or it is invalid: %s", e.what());
+ throw std::runtime_error(err);
}
std::string mode_str;
@@ -131,15 +136,18 @@ OccGridLoader::on_configure(const rclcpp_lifecycle::State & /*state*/)
mode_str.c_str());
loadParameters.mode = TRINARY;
}
- } catch (YAML::Exception &) {
- RCLCPP_WARN(node_->get_logger(), "Mode parameter not set, using default value (trinary)");
+ } catch (YAML::Exception & e) {
+ RCLCPP_WARN(node_->get_logger(),
+ "Mode parameter not set, using default value (trinary): %s", e.what());
loadParameters.mode = TRINARY;
}
try {
loadParameters.negate = doc["negate"].as<int>();
- } catch (YAML::Exception &) {
- throw std::runtime_error("The map does not contain a negate tag or it is invalid");
+ } catch (YAML::Exception & e) {
+ std::string err("The map does not contain a negate tag or "
+ "it is invalid: %s", e.what());
+ throw std::runtime_error(err);
}
RCLCPP_DEBUG(node_->get_logger(), "resolution: %f", loadParameters.resolution);