From 14e2ff3602108ec45ee89c832c9512358c3e0e58 Mon Sep 17 00:00:00 2001 From: castor639 <316019788+castor639@users.noreply.github.com> Date: Sat, 29 Aug 2026 17:43:31 +0000 Subject: [PATCH] [parsing] Use DiagnosticPolicy for model directives errors Wire model directives loading and parsing to DiagnosticPolicy so errors follow the same non-fatal policy path as URDF and SDFormat. IsValid() schema checks report via the policy, and parser throws are replaced with diagnostic.Error() plus continue/return. Add tests that a non-fatal error policy does not crash the parser. Fixes #18052 --- multibody/parsing/BUILD.bazel | 1 + multibody/parsing/detail_dmd_parser.cc | 51 ++++++---- multibody/parsing/detail_dmd_parser.h | 9 +- multibody/parsing/model_directives.cc | 94 ++++++++++++++----- multibody/parsing/model_directives.h | 9 ++ multibody/parsing/process_model_directives.cc | 18 +++- .../parsing/test/detail_dmd_parser_test.cc | 64 +++++++++++++ multibody/parsing/test/parser_test.cc | 7 +- .../test/process_model_directives_test.cc | 8 +- 9 files changed, 210 insertions(+), 51 deletions(-) diff --git a/multibody/parsing/BUILD.bazel b/multibody/parsing/BUILD.bazel index fa7404629312..89295954e4f6 100644 --- a/multibody/parsing/BUILD.bazel +++ b/multibody/parsing/BUILD.bazel @@ -402,6 +402,7 @@ drake_cc_library( hdrs = ["model_directives.h"], visibility = ["//visibility:public"], deps = [ + "//common:diagnostic_policy", "//common:essential", "//common:name_value", "//common/schema:transform", diff --git a/multibody/parsing/detail_dmd_parser.cc b/multibody/parsing/detail_dmd_parser.cc index d7bbeb6984f0..1ccfaa9c8b10 100644 --- a/multibody/parsing/detail_dmd_parser.cc +++ b/multibody/parsing/detail_dmd_parser.cc @@ -16,6 +16,7 @@ namespace drake { namespace multibody { namespace internal { +using drake::internal::DiagnosticPolicy; using parsing::GetScopedFrameByName; using parsing::ModelDirectives; using parsing::ModelInstanceInfo; @@ -29,7 +30,8 @@ void AddWeld(const Frame& parent_frame, const Frame& child_frame, const math::RigidTransform& X_PC, MultibodyPlant* plant, - std::vector* added_models) { + std::vector* added_models, + const DiagnosticPolicy& diagnostic) { plant->WeldFrames(parent_frame, child_frame, X_PC); if (added_models) { // Record weld info into crappy ModelInstanceInfo struct. @@ -43,7 +45,13 @@ void AddWeld(const Frame& parent_frame, info.X_PC = X_PC; } } - DRAKE_THROW_UNLESS(found); + if (!found) { + diagnostic.Error(fmt::format( + "add_weld: child frame '{}' (model instance '{}') does not match " + "any model added by these directives", + child_frame.name(), + plant->GetModelInstanceName(child_frame.model_instance()))); + } } } @@ -201,9 +209,10 @@ void ParseModelDirectivesImpl(const ModelDirectives& directives, auto& frame = *directive.add_frame; drake::log()->debug(" add_frame: {}", frame.name); if (!frame.X_PF.base_frame) { - // This would be caught elsewhere, but it is clearer to throw here. - throw std::logic_error( + // This would be caught elsewhere, but it is clearer to diagnose here. + diagnostic.Error( "add_frame directive with empty base frame is ambiguous"); + continue; } // Only override instance if scope is explicitly specified. std::optional instance; @@ -234,7 +243,7 @@ void ParseModelDirectivesImpl(const ModelDirectives& directives, } AddWeld(get_scoped_frame(directive.add_weld->parent), get_scoped_frame(directive.add_weld->child), X_PC, plant, - added_models); + added_models, diagnostic); } else if (directive.add_collision_filter_group) { // If there's no geometry registered, there's nothing to be done with @@ -277,9 +286,10 @@ void ParseModelDirectivesImpl(const ModelDirectives& directives, drake::log()->debug(" new_model_namespace: {}", new_model_namespace); if (!new_model_namespace.empty() && !plant->HasModelInstanceNamed(new_model_namespace)) { - throw std::runtime_error( + diagnostic.Error( fmt::format("Namespace '{}' does not exist as model instance", new_model_namespace)); + continue; } const ResolveUriResult resolved = ResolveUri(diagnostic, sub.file, package_map, {}); @@ -288,9 +298,12 @@ void ParseModelDirectivesImpl(const ModelDirectives& directives, continue; } const std::string filename = resolved.full_path.string(); - auto sub_directives = - LoadModelDirectives({DataSource::kFilename, &filename}); - ParseModelDirectivesImpl(sub_directives, new_model_namespace, workspace, + std::optional sub_directives = + LoadModelDirectives({DataSource::kFilename, &filename}, diagnostic); + if (!sub_directives.has_value()) { + continue; + } + ParseModelDirectivesImpl(*sub_directives, new_model_namespace, workspace, added_models); } } @@ -304,7 +317,8 @@ ScopedName DmdScopedNameJoin(const std::string& namespace_name, return ScopedName::Join(namespace_name, element_name); } -ModelDirectives LoadModelDirectives(const DataSource& data_source) { +std::optional LoadModelDirectives( + const DataSource& data_source, const DiagnosticPolicy& diagnostic) { // Even though the 'defaults' we use to start parsing here are empty, by // providing any defaults at all, the effect during parsing will be that any // of the users' ModelDirective structs and sub-structs will _also_ start @@ -318,8 +332,9 @@ ModelDirectives LoadModelDirectives(const DataSource& data_source) { drake::log()->debug("LoadModelDirectives: {}", filename); if (!std::filesystem::exists({filename})) { - throw std::runtime_error( + diagnostic.Error( fmt::format("No such file {} during LoadModelDirectives", filename)); + return std::nullopt; } directives = yaml::LoadYamlFile( @@ -329,7 +344,9 @@ ModelDirectives LoadModelDirectives(const DataSource& data_source) { directives = yaml::LoadYamlString( data_source.contents(), std::nullopt /* child_name */, defaults); } - DRAKE_THROW_UNLESS(directives.IsValid()); + if (!directives.IsValid(diagnostic)) { + return std::nullopt; + } return directives; } @@ -365,11 +382,13 @@ std::vector DmdParserWrapper::AddAllModels( const DataSource& data_source, const std::optional& parent_model_name, const ParsingWorkspace& workspace) { - // TODO(#18052): diagnostic policy? - ModelDirectives directives = LoadModelDirectives(data_source); - // TODO(#18052): diagnostic policy? + std::optional directives = + LoadModelDirectives(data_source, workspace.diagnostic); + if (!directives.has_value()) { + return {}; + } const std::vector infos = - ParseModelDirectives(directives, parent_model_name, workspace); + ParseModelDirectives(*directives, parent_model_name, workspace); std::vector results; results.reserve(infos.size()); for (const auto& info : infos) { diff --git a/multibody/parsing/detail_dmd_parser.h b/multibody/parsing/detail_dmd_parser.h index deedeeb87026..5c9d685fe92b 100644 --- a/multibody/parsing/detail_dmd_parser.h +++ b/multibody/parsing/detail_dmd_parser.h @@ -4,6 +4,7 @@ #include #include +#include "drake/common/diagnostic_policy.h" #include "drake/multibody/parsing/detail_common.h" #include "drake/multibody/parsing/detail_parsing_workspace.h" #include "drake/multibody/parsing/model_directives.h" @@ -20,8 +21,12 @@ namespace internal { ScopedName DmdScopedNameJoin(const std::string& namespace_name, const std::string& element_name); -// TODO(#18052): diagnostic policy? -parsing::ModelDirectives LoadModelDirectives(const DataSource& data_source); +// Loads model directives from the given data source, reporting errors via +// @p diagnostic. Returns std::nullopt if an error was reported (and the +// diagnostic policy did not throw). +std::optional LoadModelDirectives( + const DataSource& data_source, + const drake::internal::DiagnosticPolicy& diagnostic); std::vector ParseModelDirectives( const parsing::ModelDirectives& directives, diff --git a/multibody/parsing/model_directives.cc b/multibody/parsing/model_directives.cc index 1d9629404f60..98ec3ee1d770 100644 --- a/multibody/parsing/model_directives.cc +++ b/multibody/parsing/model_directives.cc @@ -6,23 +6,44 @@ namespace drake { namespace multibody { namespace parsing { +using drake::internal::DiagnosticDetail; +using drake::internal::DiagnosticPolicy; + +namespace { + +/* Preserves the historical IsValid() behavior: log errors and return false +without throwing. */ +DiagnosticPolicy MakeLoggingPolicy() { + DiagnosticPolicy policy; + policy.SetActionForErrors([](const DiagnosticDetail& detail) { + drake::log()->error(detail.message); + }); + return policy; +} + +} // namespace + bool AddWeld::IsValid() const { + return IsValid(MakeLoggingPolicy()); +} + +bool AddWeld::IsValid(const DiagnosticPolicy& diagnostic) const { if (parent.empty()) { - drake::log()->error("add_weld: `parent` must be non-empty"); + diagnostic.Error("add_weld: `parent` must be non-empty"); return false; } else if (child.empty()) { - drake::log()->error("add_weld: `child` must be non-empty"); + diagnostic.Error("add_weld: `child` must be non-empty"); return false; } if (X_PC) { if (X_PC->base_frame) { - drake::log()->error( + diagnostic.Error( "add_weld: `X_PC` must not specify a `base_frame`; the pose is " "always in the parent frame."); return false; } if (!X_PC->IsDeterministic()) { - drake::log()->error( + diagnostic.Error( "add_weld: `X_PC` must specify a deterministic transform, not a " "distribution."); return false; @@ -32,16 +53,20 @@ bool AddWeld::IsValid() const { } bool AddModel::IsValid() const { + return IsValid(MakeLoggingPolicy()); +} + +bool AddModel::IsValid(const DiagnosticPolicy& diagnostic) const { if (file.empty()) { - drake::log()->error("add_model: `file` must be non-empty"); + diagnostic.Error("add_model: `file` must be non-empty"); return false; } else if (name.empty()) { - drake::log()->error("add_model: `name` must be non-empty"); + diagnostic.Error("add_model: `name` must be non-empty"); return false; } for (const auto& [body_name, pose] : default_free_body_pose) { if (!pose.IsDeterministic()) { - drake::log()->error( + diagnostic.Error( "add_model: `default_free_body_pose` must specify a " "deterministic transform, not a distribution."); return false; @@ -51,22 +76,30 @@ bool AddModel::IsValid() const { } bool AddModelInstance::IsValid() const { + return IsValid(MakeLoggingPolicy()); +} + +bool AddModelInstance::IsValid(const DiagnosticPolicy& diagnostic) const { if (name.empty()) { - drake::log()->error("add_model_instance: `name` must be non-empty"); + diagnostic.Error("add_model_instance: `name` must be non-empty"); return false; } return true; } bool AddFrame::IsValid() const { + return IsValid(MakeLoggingPolicy()); +} + +bool AddFrame::IsValid(const DiagnosticPolicy& diagnostic) const { if (name.empty()) { - drake::log()->error("add_frame: `name` must be non-empty"); + diagnostic.Error("add_frame: `name` must be non-empty"); return false; } else if (!X_PF.base_frame || X_PF.base_frame->empty()) { - drake::log()->error("add_frame: `X_PF.base_frame` must be defined"); + diagnostic.Error("add_frame: `X_PF.base_frame` must be defined"); return false; } else if (!X_PF.IsDeterministic()) { - drake::log()->error( + diagnostic.Error( "add_frame: `X_PF` must specify a deterministic transform, not a " "distribution."); return false; @@ -75,11 +108,16 @@ bool AddFrame::IsValid() const { } bool AddCollisionFilterGroup::IsValid() const { + return IsValid(MakeLoggingPolicy()); +} + +bool AddCollisionFilterGroup::IsValid( + const DiagnosticPolicy& diagnostic) const { if (name.empty()) { - drake::log()->error("add_collision_filter_group: `name` must be non-empty"); + diagnostic.Error("add_collision_filter_group: `name` must be non-empty"); return false; } else if (members.empty() && member_groups.empty()) { - drake::log()->error( + diagnostic.Error( "add_collision_filter_group:" " at least one of `members` or `member_groups` must be non-empty"); return false; @@ -88,41 +126,53 @@ bool AddCollisionFilterGroup::IsValid() const { } bool AddDirectives::IsValid() const { + return IsValid(MakeLoggingPolicy()); +} + +bool AddDirectives::IsValid(const DiagnosticPolicy& diagnostic) const { if (file.empty()) { - drake::log()->error("add_directives: `file` must be non-empty"); + diagnostic.Error("add_directives: `file` must be non-empty"); return false; } return true; } bool ModelDirective::IsValid() const { + return IsValid(MakeLoggingPolicy()); +} + +bool ModelDirective::IsValid(const DiagnosticPolicy& diagnostic) const { const bool unique = (add_model.has_value() + add_model_instance.has_value() + add_frame.has_value() + add_weld.has_value() + add_collision_filter_group.has_value() + add_directives.has_value()) == 1; if (!unique) { - drake::log()->error( + diagnostic.Error( "directive: Specify one of `add_model`, `add_model_instance`, " "`add_frame`, `add_collision_filter_group`, or `add_directives`"); return false; } else if (add_model) { - return add_model->IsValid(); + return add_model->IsValid(diagnostic); } else if (add_model_instance) { - return add_model_instance->IsValid(); + return add_model_instance->IsValid(diagnostic); } else if (add_frame) { - return add_frame->IsValid(); + return add_frame->IsValid(diagnostic); } else if (add_weld) { - return add_weld->IsValid(); + return add_weld->IsValid(diagnostic); } else if (add_collision_filter_group) { - return add_collision_filter_group->IsValid(); + return add_collision_filter_group->IsValid(diagnostic); } else { - return add_directives->IsValid(); + return add_directives->IsValid(diagnostic); } } bool ModelDirectives::IsValid() const { + return IsValid(MakeLoggingPolicy()); +} + +bool ModelDirectives::IsValid(const DiagnosticPolicy& diagnostic) const { for (auto& directive : directives) { - if (!directive.IsValid()) return false; + if (!directive.IsValid(diagnostic)) return false; } return true; } diff --git a/multibody/parsing/model_directives.h b/multibody/parsing/model_directives.h index df61736f13c0..d60154ea55c0 100644 --- a/multibody/parsing/model_directives.h +++ b/multibody/parsing/model_directives.h @@ -15,6 +15,7 @@ #include +#include "drake/common/diagnostic_policy.h" #include "drake/common/eigen_types.h" #include "drake/common/name_value.h" #include "drake/common/schema/transform.h" @@ -28,6 +29,7 @@ namespace parsing { /// Directive to add a weld between two named frames, a parent and a child. struct AddWeld { bool IsValid() const; + bool IsValid(const drake::internal::DiagnosticPolicy& diagnostic) const; template void Serialize(Archive* a) { @@ -49,6 +51,7 @@ struct AddWeld { /// given name for the added instance. struct AddModel { bool IsValid() const; + bool IsValid(const drake::internal::DiagnosticPolicy& diagnostic) const; template void Serialize(Archive* a) { @@ -113,6 +116,7 @@ struct AddModel { /// Directive to add an empty, named model instance to a scene. struct AddModelInstance { bool IsValid() const; + bool IsValid(const drake::internal::DiagnosticPolicy& diagnostic) const; template void Serialize(Archive* a) { @@ -127,6 +131,7 @@ struct AddModelInstance { /// and a transform with a base frame and offset. struct AddFrame { bool IsValid() const; + bool IsValid(const drake::internal::DiagnosticPolicy& diagnostic) const; template void Serialize(Archive* a) { @@ -146,6 +151,7 @@ struct AddFrame { /// @ref tag_drake_collision_filter_group in XML model formats. struct AddCollisionFilterGroup { bool IsValid() const; + bool IsValid(const drake::internal::DiagnosticPolicy& diagnostic) const; template void Serialize(Archive* a) { @@ -184,6 +190,7 @@ struct AddCollisionFilterGroup { /// its elements prefixed with a namespace. struct AddDirectives { bool IsValid() const; + bool IsValid(const drake::internal::DiagnosticPolicy& diagnostic) const; template void Serialize(Archive* a) { @@ -217,6 +224,7 @@ struct AddDirectives { /// intended type for the directive. struct ModelDirective { bool IsValid() const; + bool IsValid(const drake::internal::DiagnosticPolicy& diagnostic) const; template void Serialize(Archive* a) { @@ -239,6 +247,7 @@ struct ModelDirective { /// Top-level structure for a model directives yaml file schema. struct ModelDirectives { bool IsValid() const; + bool IsValid(const drake::internal::DiagnosticPolicy& diagnostic) const; template void Serialize(Archive* a) { diff --git a/multibody/parsing/process_model_directives.cc b/multibody/parsing/process_model_directives.cc index 459a61e02776..19f94ca291a2 100644 --- a/multibody/parsing/process_model_directives.cc +++ b/multibody/parsing/process_model_directives.cc @@ -193,14 +193,24 @@ std::vector ProcessModelDirectives( ModelDirectives LoadModelDirectives(const std::filesystem::path& filename) { const std::string filename_str = filename.string(); - return multibody::internal::LoadModelDirectives( - {DataSource::kFilename, &filename_str}); + ::drake::internal::DiagnosticPolicy policy; + std::optional result = + multibody::internal::LoadModelDirectives( + {DataSource::kFilename, &filename_str}, policy); + // Default policy throws on error, so a missing value is unexpected. + DRAKE_DEMAND(result.has_value()); + return *result; } ModelDirectives LoadModelDirectivesFromString( const std::string& model_directives) { - return multibody::internal::LoadModelDirectives( - {DataSource::kContents, &model_directives}); + ::drake::internal::DiagnosticPolicy policy; + std::optional result = + multibody::internal::LoadModelDirectives( + {DataSource::kContents, &model_directives}, policy); + // Default policy throws on error, so a missing value is unexpected. + DRAKE_DEMAND(result.has_value()); + return *result; } void FlattenModelDirectives(const ModelDirectives& directives, diff --git a/multibody/parsing/test/detail_dmd_parser_test.cc b/multibody/parsing/test/detail_dmd_parser_test.cc index ac1dca1474ab..72333fffea7f 100644 --- a/multibody/parsing/test/detail_dmd_parser_test.cc +++ b/multibody/parsing/test/detail_dmd_parser_test.cc @@ -26,6 +26,9 @@ using drake::internal::DiagnosticPolicy; using Eigen::Vector3d; using geometry::SceneGraph; using math::RigidTransformd; +using parsing::AddDirectives; +using parsing::AddFrame; +using parsing::ModelDirective; using parsing::ModelDirectives; using parsing::ModelInstanceInfo; using schema::Transform; @@ -539,6 +542,67 @@ TEST_F(DmdParserTest, DefaultJointPositions) { DRAKE_EXPECT_THROWS_MESSAGE(ParseModelDirectives(directives), ".*no Joint.*"); } +/* With a non-fatal error policy, schema-invalid directives must not crash. */ +TEST_F(DmdParserTest, NonFatalInvalidDirectives) { + const std::string contents = R"( +directives: +- add_model_instance: + name: "" +)"; + auto maybe = LoadModelDirectives({DataSource::kContents, &contents}, + diagnostic_policy_); + EXPECT_FALSE(maybe.has_value()); + EXPECT_THAT( + TakeError(), + testing::MatchesRegex(".*add_model_instance.*name.*must be non-empty.*")); +} + +/* With a non-fatal error policy, a missing directives file must not crash. */ +TEST_F(DmdParserTest, NonFatalMissingFile) { + const std::string missing = "/no/such/directives.dmd.yaml"; + auto maybe = LoadModelDirectives({DataSource::kFilename, &missing}, + diagnostic_policy_); + EXPECT_FALSE(maybe.has_value()); + EXPECT_THAT(TakeError(), + testing::MatchesRegex(".*No such file.*directives.dmd.yaml.*")); +} + +/* With a non-fatal error policy, add_frame with an empty base_frame and + add_directives with a missing model instance must not crash. */ +TEST_F(DmdParserTest, NonFatalParseErrors) { + // Build directive structs directly to bypass schema IsValid() checks that + // LoadModelDirectives would perform first. + { + ModelDirectives directives; + ModelDirective directive; + AddFrame add_frame; + add_frame.name = "f"; + // Leave base_frame unset. + directive.add_frame = add_frame; + directives.directives.push_back(directive); + + EXPECT_NO_THROW(ParseModelDirectives(directives)); + EXPECT_THAT(TakeError(), + testing::MatchesRegex(".*empty base frame is ambiguous.*")); + } + + { + ModelDirectives directives; + ModelDirective directive; + AddDirectives add_directives; + add_directives.file = "package://drake/does_not_matter.dmd.yaml"; + add_directives.model_namespace = "missing_instance"; + directive.add_directives = add_directives; + directives.directives.push_back(directive); + + EXPECT_NO_THROW(ParseModelDirectives(directives)); + EXPECT_THAT(TakeError(), + testing::MatchesRegex( + ".*Namespace 'missing_instance' does not exist as model " + "instance.*")); + } +} + } // namespace } // namespace internal } // namespace multibody diff --git a/multibody/parsing/test/parser_test.cc b/multibody/parsing/test/parser_test.cc index fda4d8540a62..b9e884747fb8 100644 --- a/multibody/parsing/test/parser_test.cc +++ b/multibody/parsing/test/parser_test.cc @@ -320,8 +320,6 @@ GTEST_TEST(FileParserTest, BadStringTest) { ".*Failed to parse XML string: XML_ERROR_PARSING_TEXT"); // Malformed DMD string is an error. - // TODO(#18052): Until the underlying parser supports diagnostic policy, the - // input needs to crafted to avoid reachable fatal assertions. DRAKE_EXPECT_THROWS_MESSAGE( Parser(&plant).AddModelsFromString("bad:", "dmd.yaml"), ".*YAML.*bad.*"); @@ -330,10 +328,11 @@ GTEST_TEST(FileParserTest, BadStringTest) { // N.B. This directive is missing the required `name` attribute. constexpr char yaml[] = "directives:\n" - "- add_model_instance:\n"; + "- add_model_instance:\n" + " name: \"\"\n"; DRAKE_EXPECT_THROWS_MESSAGE( Parser(&plant).AddModelsFromString(yaml, "dmd.yaml"), - ".*IsValid.*failed.*"); + ".*add_model_instance.*name.*must be non-empty.*"); } // Unknown extension is an error. diff --git a/multibody/parsing/test/process_model_directives_test.cc b/multibody/parsing/test/process_model_directives_test.cc index ac6a4b2428c2..f3e722bdf7ad 100644 --- a/multibody/parsing/test/process_model_directives_test.cc +++ b/multibody/parsing/test/process_model_directives_test.cc @@ -233,7 +233,7 @@ GTEST_TEST(ProcessModelDirectivesTest, AddStochasticFrameBad) { // Setting a stochastic transform yields a failure. yaml += " rotation: !Uniform {}\n"; DRAKE_EXPECT_THROWS_MESSAGE(LoadModelDirectivesFromString(yaml), - ".*IsValid.*"); + ".*deterministic transform.*"); } // Test backreference behavior in ModelDirectives. @@ -519,7 +519,8 @@ GTEST_TEST(ProcessModelDirectivesTest, DeepNestedChildWelds) { DRAKE_EXPECT_THROWS_MESSAGE( ProcessModelDirectives(directives, &plant, nullptr, make_parser(&plant).get()), - R"(.*Failure at .* in AddWeld\(\): condition 'found' failed.*)"); + ".*add_weld: child frame.*does not match any model added by these " + "directives.*"); } // Test model directives failure to load welds with a child to a @@ -532,7 +533,8 @@ GTEST_TEST(ProcessModelDirectivesTest, DeepNestedChildFrameWelds) { DRAKE_EXPECT_THROWS_MESSAGE( ProcessModelDirectives(directives, &plant, nullptr, make_parser(&plant).get()), - R"(.*Failure at .* in AddWeld\(\): condition 'found' failed.*)"); + ".*add_weld: child frame.*does not match any model added by these " + "directives.*"); } // Test that flattening is idempotent and semantically a no-op.