(roseus_bt) Raise runtime errors when blackboard entry is not found

jsk-ros-pkg · Oct 26, 2022 · 327c0b6 · 327c0b6
1 parent 26648e5
commit 327c0b6
Show file tree

Hide file tree

Showing 2 changed files with 32 additions and 8 deletions.
diff --git a/roseus_bt/include/roseus_bt/gen_template.h b/roseus_bt/include/roseus_bt/gen_template.h
@@ -132,6 +132,14 @@ std::string GenTemplate::action_class_template(std::string package_name, std::st
                                                std::vector<std::string> provided_ports,
                                                std::vector<std::string> get_inputs,
                                                std::vector<std::string> set_outputs) {
+  auto format_send_goal = [](const std::string body) {
+    std::string decl = 1 + R"(
+    BT::Result res;
+)";
+    if (!body.empty()) return fmt::format("{}{}", decl, body);
+    return body;
+  };
+
   std::string fmt_string = 1 + R"(
 class %2%: public EusActionNode<%1%::%2%Action>
 {
@@ -176,7 +184,7 @@ EusActionNode<%1%::%2%Action>(handle, name, conf) {}
     package_name %
     nodeID %
     boost::algorithm::join(provided_ports, ",\n") %
-    boost::algorithm::join(get_inputs, "\n") %
+    format_send_goal(boost::algorithm::join(get_inputs, "\n")) %
     boost::algorithm::join(set_outputs, "\n");
 
   return bfmt.str();
@@ -193,6 +201,7 @@ std::string GenTemplate::remote_action_class_template(
     std::string decl = 1 + R"(
     rapidjson::CopyDocument document;
     GoalType ros_msg;
+    BT::Result res;
 )";
     if (!body.empty()) return fmt::format("{}{}", decl, body);
     return body;
@@ -251,6 +260,14 @@ EusRemoteActionNode("%1%/%2%Action", name, conf) {}
 std::string GenTemplate::condition_class_template(std::string package_name, std::string nodeID,
                                                  std::vector<std::string> provided_ports,
                                                  std::vector<std::string> get_inputs) {
+  auto format_send_request = [](const std::string body) {
+    std::string decl = 1 + R"(
+    BT::Result res;
+)";
+    if (!body.empty()) return fmt::format("{}{}", decl, body);
+    return body;
+  };
+
   std::string fmt_string = 1 + R"(
 class %2%: public EusConditionNode<%1%::%2%>
 {
@@ -289,7 +306,7 @@ class %2%: public EusConditionNode<%1%::%2%>
     package_name %
     nodeID %
     boost::algorithm::join(provided_ports, ",\n") %
-    boost::algorithm::join(get_inputs, "\n");
+    format_send_request(boost::algorithm::join(get_inputs, "\n"));
 
   return bfmt.str();
 }
@@ -303,6 +320,7 @@ std::string GenTemplate::remote_condition_class_template(std::string package_nam
     std::string json;
     rapidjson::Document document;
     RequestType ros_msg;
+    BT::Result res;
 )";
     if (!body.empty()) return fmt::format("{}{}", decl, body);
     return body;

diff --git a/roseus_bt/include/roseus_bt/xml_parser.h b/roseus_bt/include/roseus_bt/xml_parser.h
@@ -632,13 +632,15 @@ std::string XMLParser::format_get_remote_input(const XMLElement* node, const std
   std::string msg_type = node->Attribute("type");
   if (msg_type.find('/') != std::string::npos)
     return fmt::format(R"(
-    getInput("{0}", document);
+    res = getInput("{0}", document);
+    if (!res) throw BT::RuntimeError(res.error());
     rapidjson::Value {0}(document, document.GetAllocator());
     {1}->AddMember("{0}", {0}, {1}->GetAllocator());)",
     node->Attribute("name"),
     name);
   return fmt::format(R"(
-    getInput("{0}", ros_msg.{0});
+    res = getInput("{0}", ros_msg.{0});
+    if (!res) throw BT::RuntimeError(res.error());
     rapidjson::Value {0};
     {0}.{2};
     {1}->AddMember("{0}", {0}, {1}->GetAllocator());)",
@@ -818,8 +820,10 @@ std::string XMLParser::generate_action_class(const XMLElement* node, const std::
                        node->Attribute("name"));
   };
   auto format_get_input = [](const XMLElement* node) {
-    return fmt::format("    getInput(\"{0}\", goal.{0});",
-                       node->Attribute("name"));
+    return fmt::format(1 + R"(
+    res = getInput("{0}", goal.{0});
+    if (!res) throw BT::RuntimeError(res.error());)",
+        node->Attribute("name"));
   };
   auto format_set_output = [](const XMLElement* node) {
     return fmt::format(
@@ -926,8 +930,10 @@ std::string XMLParser::generate_condition_class(const XMLElement* node, const st
                        node->Attribute("name"));
   };
   auto format_get_input = [](const XMLElement* node) {
-    return fmt::format("    getInput(\"{0}\", request.{0});",
-                       node->Attribute("name"));
+    return fmt::format(1 + R"(
+    res = getInput("{0}", request.{0});
+    if (!res) throw BT::RuntimeError(res.error());)",
+        node->Attribute("name"));
   };
 
   std::vector<std::string> provided_ports;