diff --git a/docs/source/index.rst b/docs/source/index.rst index 8378868..eb983f9 100644 --- a/docs/source/index.rst +++ b/docs/source/index.rst @@ -23,16 +23,16 @@ Program outputs: - A CMakeLists.txt file that incorporates the generated files. -Required parameters: +Required parameter: - ``--input_filename``: The path to the SCXML file that describes the behavior of the skill. -- ``--model_filename``: The path to the XML file that describes the full model of the program. -- ``--interface_filename``: The path to the XML file that describes the interfaces used. By default, the program generates the code in the same directory as the SCXML file specified by the ``--input_filename`` parameter. However, you can select a different output directory by using the ``--output_path`` parameter. -Additionally, the program uses files from the ``templates`` directory by default to generate the code, but you can specify a different directory with the ``--templates_path`` parameter. -The ``--verbose_mode`` parameter can be used to enable verbose logging. +Additionally, the program uses files from the ``template_skill`` directory by default to generate the code, but you can specify a different directory with the ``--template_path`` parameter. +The executable also supports ``--verbose_mode``, ``--debug_mode``, ``--translate_mode``, ``--generate_mode``, and ``--datamodel_mode``. + +At the moment, the system-model and interface XML files are taken from built-in default paths in the executable rather than from command-line flags. The repository reference files are still available under ``tutorials/specifications/``. The generated skills are based on a behavior tree structure. Skills corresponding to condition nodes will have a ROS2 tick service, while skills corresponding to action nodes will have both tick and halt services. @@ -42,4 +42,4 @@ Contents :maxdepth: 2 tutorials - API/api \ No newline at end of file + API/api diff --git a/docs/source/tutorials.rst b/docs/source/tutorials.rst index d4d246e..e666324 100644 --- a/docs/source/tutorials.rst +++ b/docs/source/tutorials.rst @@ -1,69 +1,136 @@ Tutorials ========= -This section provides a tutorial on the model2code tool and how to use it to generate skill-level code from the high-level SCXML model of a skill. +This page gives a practical introduction to ``model2code`` using the tutorial skills shipped with the repository. -The output code is a ROS package that can be compiled and run on a ROS2 system, that handles the SCXML events and translate them into ROS2 service calls or responses. +The main idea is simple: you describe a skill as an SCXML state machine, and ``model2code`` generates the ROS2 package that wraps that state machine with the required ROS interfaces. Prerequisites --------------- -MODEL2CODE has been tested on Ubuntu 22.04 with Python 3.10.12. +------------- +``model2code`` has been tested on Ubuntu 22.04. Before running the tutorial, build and install the tool as described in the repository ``README``. -Before you can use the model2code tool, you need to follow the dependency installation and compiling steps in the model2code repository `README `_. +Current CLI Notes +----------------- +The current executable accepts the following options: -Code generation guide +- ``--input_filename``: required path to the SCXML skill model. +- ``--output_path``: optional path where generated files are written. +- ``--template_path``: optional path to the template package directory. +- ``--verbose_mode``: print the internal log after execution. +- ``--debug_mode``: inject debug logging tags into the translated state machine. +- ``--translate_mode``: run only the translation stage. +- ``--generate_mode``: run only the generation stage. +- ``--datamodel_mode``: enable datamodel file generation. + +At the moment, the executable uses built-in default paths for the system model and interface XML files instead of parsing ``--model_filename`` and ``--interface_filename`` from the command line. + +The repository still provides the reference specification files in: + +- ``tutorials/specifications/full-model.xml`` +- ``tutorials/specifications/interfaces.xml`` + +If your local build expects those files in a different location, update your local setup before running the examples. + +Tutorial 1: Action Skill with a Service Call -------------------------------------------- -Input and Output Models -``````````````` -The main input is an SCXML model, extended with ROS-specific features, that describes the behavior of the skill. Visit the `(ROS) SCXML Model Implementation `_ description page for more information on how to create an SCXML model of a ROS node. -The required inputs are also the files defining the system model and interfaces. The `full-model.xml `_ file contains a list of the components and specifies their interfaces, which are described in the `interfaces.xml `_ file. -The output is a ROS2 package containing the generated code. The package includes the following files: a standard SCXML file, C++ source and header files, a main C++ file, a package.xml file, and a CMakeLists.txt file. The templates for the generated files are located in the `templates `_ folder. - -The `tutorial_skills `_ are written for a system based on a behavior tree structure. The leaf nodes of the behavior tree are conditions or actions handled by plugins that propagate the tick from the behavior tree to the skills. -Skills corresponding to condition nodes will have a ROS2 tick service, while skills corresponding to action nodes will have both tick and halt services. Skills can interface with components through ROS2 services and topics. -The interface of the 'TemplateComponent' is the 'template_interfaces', which includes two functions 'Function1' and 'Function2', respectively a sample service and a topic. - -Parameters -``````````````` -To run the model2code use the following parameters: - - `--input_filename` (required): The path to the SCXML file that describes the behavior of the skill. - - `--model_filename` (required): The path to the XML file that describes the full model of the program. - - `--interface_filename` (required): The path to the XML file that describes the interfaces used. - - `--template_path`: The path to the directory containing the templates for the files to be generated. By default, the program uses the `templates` directory. - - `--output_path`: The path to the directory where the generated files will be placed. By default, the program generates the code in the same directory as the SCXML file specified by the `--input_filename` parameter. - - `--verbose_mode`: To enable logging. By default, the program does not log. - -First example -``````````````` -The `first_tutorial_skill `_ is a simple skill corresponding to an action node of the behavior tree. -It provides ROS2 services for tick and halt, and it is a client for the 'Function1' service provided by the 'TemplateComponent'. -The model is composed of two states. In the 'idle' state, when the skill receives a tick request it sends a request to the 'Function1' service and goes to the 'start' state. In the 'start' state it waits for the response from the 'Function1' service and eventually responds to the tick request. In both states when the skill receives a halt request it responds right away. - -To generate the code for this skill, move to the model2code folder and run the following command: +The first example is ``FirstTutorialSkill``: + +- it exposes ``/FirstTutorialSkill/tick``, +- it exposes ``/FirstTutorialSkill/halt``, +- it calls ``/TemplateComponent/Function1`` when ticked, +- it returns success or failure after the service response. + +The input model is: + +``tutorials/skills/first_tutorial_skill/src/FirstTutorialSkill.scxml`` + +Conceptually, the skill works like this: + +1. In ``idle``, a tick request is received. +2. The skill sends a request to ``/TemplateComponent/Function1`` and moves to ``start``. +3. In ``start``, the skill waits for the service response. +4. The response is converted into a tick reply. +5. A halt request can be handled immediately from either state. + +Generate the package +```````````````````` + +From the repository root, run: .. code-block:: bash - - model2code --input_filename "tutorials/skills/first_tutorial_skill/src/FirstTutorialSkill.scxml" --model_filename "tutorials/specifications/full-model.xml" --interface_filename "tutorials/specifications/interfaces.xml" --output_path "tutorials/skills/first_tutorial_skill" -The generated code will be placed in the `tutorials/skills/first_tutorial_skill` folder. -The 'FirstTutorialSkillSM.scxml' file is the translation of the input SCXML model in standard SCXML, so that it is runnable with any SCXML compiler. -The 'FirstTutorialSkill.cpp' and 'FirstTutorialSkill.h' files contain the C++ code of the skill that handles the SCXML events and ROS2 communication. -The 'main.cpp' file contains the main function of the skill. -The CMakeLists.txt file incorporates the generated files. -The package.xml file with information about the ROS package and the inclusion of the 'template_interfaces' package as a dependency. + model2code \ + --input_filename tutorials/skills/first_tutorial_skill/src/FirstTutorialSkill.scxml \ + --output_path tutorials/skills/first_tutorial_skill + +If you want extra logs while learning the flow, add ``--verbose_mode``. + +What gets generated +``````````````````` + +The output folder will contain the main generated package files: + +- ``src/FirstTutorialSkillSM.scxml``: the translated standard SCXML model. +- ``src/FirstTutorialSkill.cpp`` and ``include/FirstTutorialSkill.h``: the generated ROS/C++ wrapper. +- ``src/main.cpp``: the executable entry point. +- ``CMakeLists.txt`` and ``package.xml``: build and package metadata. +What to look for +```````````````` -Second example -``````````````` -The `second_tutorial_skill `_ corresponds to a condition node of the behavior tree, therefore it does not provide a halt service. -It provides ROS2 services for tick, and is a subscriber for the 'Function2' topic provided by the 'TemplateComponent'. +This example is useful because it shows the core contract of ``model2code``: -To generate the code for this skill, move to the model2code folder and run the following command: +- the SCXML file contains the skill logic, +- the generated C++ files contain the ROS integration layer. + +At runtime, a ROS request becomes an SCXML event, the state machine processes it, and the generated wrapper sends the corresponding ROS response or outgoing request. + +Tutorial 2: Condition Skill with a Topic Subscription +----------------------------------------------------- + +The second example is ``SecondTutorialSkill``: + +- it exposes only ``/SecondTutorialSkill/tick``, +- it subscribes to ``/TemplateComponent/Function2``, +- it stores the latest topic value in the datamodel, +- it returns success or failure when ticked depending on that value. + +The input model is: + +``tutorials/skills/second_tutorial_skill/src/SecondTutorialSkill.scxml`` + +Generate the package +```````````````````` + +From the repository root, run: .. code-block:: bash - - model2code --input_filename "tutorials/skills/second_tutorial_skill/src/SecondTutorialSkill.scxml" --model_filename "tutorials/specifications/full-model.xml" --interface_filename "tutorials/specifications/interfaces.xml" --output_path "tutorials/skills/second_tutorial_skill" --verbose_mode -The generated code will be placed in the `tutorials/skills/second_tutorial_skill` folder. The structure of generated files follows that of the first example, with the only difference being that 'SecondTutorialSkill.cpp' omits the halt service and includes a subscriber for 'Function2'. -In this example the `--verbose_mode` parameter is used to enable verbose logging. \ No newline at end of file + model2code \ + --input_filename tutorials/skills/second_tutorial_skill/src/SecondTutorialSkill.scxml \ + --output_path tutorials/skills/second_tutorial_skill \ + --verbose_mode + +Why this example matters +```````````````````````` + +This second skill shows a different communication pattern: + +- there is no halt service, +- the decision is based on subscribed topic data, +- the generated code contains a topic subscriber instead of a service client. + +Taken together, the two tutorial skills show how the generated package changes when the SCXML model describes a different kind of ROS interaction. + +Suggested Learning Path +----------------------- + +If you are new to the project, this order works well: + +1. Read ``FirstTutorialSkill.scxml`` to understand the behavior. +2. Generate the package and inspect the translated ``SM.scxml`` file. +3. Open the generated ``.cpp`` and ``.h`` files to see how ROS interfaces were created. +4. Repeat the same process with ``SecondTutorialSkill`` and compare the differences. + +That comparison gives a solid first understanding of how ``model2code`` maps high-level SCXML models into concrete ROS2 packages. diff --git a/src/ExtractFromXML.cpp b/src/ExtractFromXML.cpp index 434a7b1..6ca81f7 100644 --- a/src/ExtractFromXML.cpp +++ b/src/ExtractFromXML.cpp @@ -308,30 +308,33 @@ bool findInterfaceType(const fileDataStr fileData, eventDataStr& eventData, tiny bool is_action_server = findElementByTagAndAttValue(root, std::string("ros_action_server"), std::string("action_name"), std::string("/" + eventData.componentName + "/" + eventData.functionName), element); + if (!is_action_server && !eventData.componentName.empty()) { + is_action_server = findElementByTagAndAttValue(root, std::string("ros_action_server"), std::string("action_name"), std::string("/" + eventData.componentName), element); + } add_to_log("is_action_server: " + std::to_string(is_action_server) + " at line " + std::to_string(__LINE__)); if (is_action_server) { getElementAttValue(element, std::string("type"), eventData.messageInterfaceType); + getElementAttValue(element, std::string("action_name"), eventData.serverName); eventData.rosInterfaceType = "action-server"; // type of the interface in ROS eventData.interfaceName = eventData.messageInterfaceType.substr(0, eventData.messageInterfaceType.find_last_of("/")); add_to_log("interfaceName: " + eventData.interfaceName + " at line " + std::to_string(__LINE__)); eventData.interfaceType = "action"; - // eventData.serverName = "/" + eventData.componentName + "/" + eventData.functionName; return true; } - - - bool is_action_client = findElementByTagAndAttValue(root, std::string("ros_action_client"), std::string("action_name"), std::string("/" + eventData.componentName + "/" + eventData.functionName), element); + if (!is_action_client && !eventData.componentName.empty()) { + is_action_client = findElementByTagAndAttValue(root, std::string("ros_action_client"), std::string("action_name"), std::string("/" + eventData.componentName), element); + } add_to_log("is_action_client: " + std::to_string(is_action_client) + " at line " + std::to_string(__LINE__)); if (is_action_client) { getElementAttValue(element, std::string("type"), eventData.messageInterfaceType); + getElementAttValue(element, std::string("action_name"), eventData.clientName); eventData.rosInterfaceType = "action-client"; // type of the interface in ROS eventData.interfaceName = eventData.messageInterfaceType.substr(0, eventData.messageInterfaceType.find_last_of("/")); add_to_log("interfaceName: " + eventData.interfaceName + " at line " + std::to_string(__LINE__)); eventData.interfaceType = "action"; - eventData.clientName = "/" + eventData.componentName + "/" + eventData.functionName; return true; } @@ -528,4 +531,4 @@ bool extractFromSCXML(tinyxml2::XMLDocument& doc, const std::string fileName, st return true; } /** @} */ // end of readFromXMLFiles subgroup -/** @} */ // end of ExtractFromXMLFunctions group \ No newline at end of file +/** @} */ // end of ExtractFromXMLFunctions group diff --git a/src/Replacer.cpp b/src/Replacer.cpp index a7a4680..5644c76 100644 --- a/src/Replacer.cpp +++ b/src/Replacer.cpp @@ -217,6 +217,8 @@ void replaceCommonEventPlaceholders(std::string& code, const eventDataStr& event replaceAll(code, "$eventData.event$", eventData.event); replaceAll(code, "$eventData.componentName$", eventData.componentName); replaceAll(code, "$eventData.functionName$", eventData.functionName); + replaceAll(code, "$eventData.clientName$", eventData.clientName); + replaceAll(code, "$eventData.serverName$", eventData.serverName); replaceAll(code, "$eventData.serviceTypeName$", eventData.serviceTypeName); replaceAll(code, "$eventData.serviceTypeNameSnakeCase$", eventData.serviceTypeNameSnakeCase); replaceAll(code, "$eventData.nodeName$", eventData.nodeName); @@ -509,6 +511,8 @@ void handleGenericEvent(const eventDataStr eventData, const savedCodeStr savedCo std::string eventFeedbackReturn = "FeedbackReturn"; std::string eventGoalResponse = "GoalResponse"; std::string eventResultResponse = "ResultResponse"; + std::string eventAbortedResultResponse = "AbortedResultResponse"; + std::string eventCancelResult = "CancelResult"; if (std::string(eventData.eventName).find(eventFeedbackReturn) != std::string::npos) { std::string actionFeedbackCallback = savedCode.actionFeedbackCallback; @@ -545,15 +549,23 @@ void handleGenericEvent(const eventDataStr eventData, const savedCodeStr savedCo replaceCommonEventPlaceholders(actionGoalResponseFnc, eventData); writeAfterCommand(str, "/*ACTION_FNC_LIST*/", actionGoalResponseFnc); } - else if(std::string(eventData.eventName).find(eventResultResponse) != std::string::npos) + else if(std::string(eventData.eventName).find(eventResultResponse) != std::string::npos + || std::string(eventData.eventName).find(eventAbortedResultResponse) != std::string::npos + || std::string(eventData.eventName).find(eventCancelResult) != std::string::npos) { std::string actionResultCallbackFnc = savedCode.actionResultCallbackFnc; replaceCommonEventPlaceholders(actionResultCallbackFnc, eventData); - writeAfterCommand(str, "/*ACTION_FNC_LIST*/", actionResultCallbackFnc); + if(!checkIfStrPresent(str, actionResultCallbackFnc)){ + writeAfterCommand(str, "/*ACTION_FNC_LIST*/", actionResultCallbackFnc); + } - std::string actionResultRequestLambda = savedCode.actionResultRequestLambda; - replaceCommonEventPlaceholders(actionResultRequestLambda, eventData); - writeAfterCommand(str, "/*ACTION_LAMBDA_LIST*/", actionResultRequestLambda); + if (std::string(eventData.eventName).find(eventResultResponse) != std::string::npos) { + std::string actionResultRequestLambda = savedCode.actionResultRequestLambda; + replaceCommonEventPlaceholders(actionResultRequestLambda, eventData); + if(!checkIfStrPresent(str, actionResultRequestLambda)){ + writeAfterCommand(str, "/*ACTION_LAMBDA_LIST*/", actionResultRequestLambda); + } + } } } @@ -821,4 +833,4 @@ bool Replacer(fileDataStr& fileData, templateFileDataStr& templateFileData) writeFile(fileData.outputPathSrc, fileData.outputMainFileName, codeMap["mainCode"]); return true; -} \ No newline at end of file +} diff --git a/src/Translator.cpp b/src/Translator.cpp index aeb2e4b..e4ae275 100644 --- a/src/Translator.cpp +++ b/src/Translator.cpp @@ -115,23 +115,31 @@ bool deleteElementFromVector(std::vector& elements) */ bool replaceTagName(tinyxml2::XMLDocument* doc, tinyxml2::XMLElement* element, const std::string& newTagName) { + std::cout << "Replacing tag name '" << element->Value() << "' with '" << newTagName << "'" << std::endl; if (!element || !doc) { std::cerr << "Invalid document or element." << std::endl; return false; } + std::cout << " line " << __LINE__ << std::endl; // Create a new element with the new tag name tinyxml2::XMLElement* newElement = doc->NewElement(newTagName.c_str()); + std::cout << " line " << __LINE__ << std::endl; // Copy attributes from the old element to the new element + std::cout << " line " << __LINE__ << std::endl; for (const tinyxml2::XMLAttribute* attr = element->FirstAttribute(); attr; attr = attr->Next()) { newElement->SetAttribute(attr->Name(), attr->Value()); + std::cout << " line " << __LINE__ << std::endl; } + std::cout << " line " << __LINE__ << std::endl; // Copy the text content of the old element to the new element + std::cout << " line " << __LINE__ << std::endl; if (element->GetText()) { newElement->SetText(element->GetText()); } + std::cout << " line " << __LINE__ << std::endl; // Move child elements from the old element to the new element for (tinyxml2::XMLElement* child = element->FirstChildElement(); child; ) { @@ -140,9 +148,11 @@ bool replaceTagName(tinyxml2::XMLDocument* doc, tinyxml2::XMLElement* element, c child = next; } + std::cout << " line " << __LINE__ << std::endl; // Replace the old element with the new element in the document element->Parent()->InsertAfterChild(element, newElement); element->Parent()->DeleteChild(element); + std::cout << " line " << __LINE__ << std::endl; return true; } @@ -504,6 +514,43 @@ bool findElementByTagAndAttValueContaining(tinyxml2::XMLElement* root, const std return false; } +std::string buildCanonicalActionEventPrefix(const char* actionNameAttr, const char* actionTypeAttr) +{ + if (!actionNameAttr) { + return ""; + } + + std::string actionName(actionNameAttr); + if (actionName.empty()) { + return actionName; + } + + if (actionName.front() == '/') { + actionName.erase(0, 1); + } + + // Multi-segment action names already match the expected /component/function shape. + if (actionName.find('/') != std::string::npos) { + return "/" + actionName; + } + + std::string actionTypeName; + if (actionTypeAttr) { + actionTypeName = actionTypeAttr; + size_t lastSlash = actionTypeName.find_last_of('/'); + if (lastSlash != std::string::npos) { + actionTypeName = actionTypeName.substr(lastSlash + 1); + } + } + + if (actionTypeName.empty()) { + return "/" + actionName; + } + + // Single-segment action names still need a stable function segment for codegen. + return "/" + actionName + "/" + actionTypeName; +} + /** * @brief replaces the value of the event attribute in all elements in a vector from / to . * @@ -725,20 +772,28 @@ bool Translator(fileDataStr& fileData){ if( !readHLXMLFile(doc, fileContent, fileData.inputFileName)){ return false; } - + std::cout << "line " << __LINE__ << std::endl; // Get Root and SkillData + std::cout << "line " << __LINE__ << std::endl; tinyxml2::XMLElement* root = doc.RootElement(); if (!root) { std::cerr << "No root element found" << std::endl; return false; + std::cout << "line " << __LINE__ << std::endl; } + std::cout << "line " << __LINE__ << std::endl; getDataFromRootNameHighLevel(root->Attribute("name"), skillData); + std::cout << "line " << __LINE__ << std::endl; // Add log print for each state entry + std::cout << "line " << __LINE__ << std::endl; if (fileData.debug_mode) { addLogToStateEntries(&doc, root); } + std::cout << "line " << __LINE__ << std::endl; + // replace ascxml tag with scxml + std::cout << "line " << __LINE__ << std::endl; // Get Skill Type tinyxml2::XMLElement* haltServerElement; if(findElementByTagAndAttValueContaining(root, std::string("ros_service_server"), std::string("type"), std::string("bt_interfaces_dummy/HaltAction"), haltServerElement) || @@ -777,9 +832,10 @@ bool Translator(fileDataStr& fileData){ for (tinyxml2::XMLElement* element : actionVector) { const char* name = element->Attribute("name"); const char* actionName = element->Attribute("action_name"); + const char* actionType = element->Attribute("type"); if (name && actionName) { - nameToActionNameMap[name] = actionName; + nameToActionNameMap[name] = buildCanonicalActionEventPrefix(actionName, actionType); } else { std::cerr << "Missing attribute in ros_action_client tag\n"; } @@ -937,6 +993,29 @@ bool Translator(fileDataStr& fileData){ replaceTagNameFromVector(&doc, actionHandleSuccessResultVector, "transition"); replaceAttributeValueSubstringFromVector(assignVector, "expr", "_wrapped_result.result.", "_event.data."); + // Translate elements with tag ros_action_handle_aborted_result + std::vector actionHandleAbortedResultVector; + findElementVectorByTag(root, std::string("ros_action_handle_aborted_result"), actionHandleAbortedResultVector); + replaceAttributeNameFromVector(actionHandleAbortedResultVector, "name", "event"); + for (auto& element : actionHandleAbortedResultVector) { + const char* eventValue = element->Attribute("event"); + if (eventValue) { + std::string eventStr(eventValue); + auto it = nameToActionNameMap.find(eventStr); + if (it != nameToActionNameMap.end()) { + element->SetAttribute("event", it->second.c_str()); + add_to_log("Updating: " + eventStr + " -> " + it->second); + } else { + add_to_log("No update for: " + eventStr); + } + } else { + add_to_log("No 'event' attribute found"); + } + } + replaceEventValueFromVectorFromSlashToPoint(actionHandleAbortedResultVector); + appendAttributeValueFromVector(actionHandleAbortedResultVector, "event", ".AbortedResultResponse"); + replaceTagNameFromVector(&doc, actionHandleAbortedResultVector, "transition"); + // Translate elements with tag ros_action_handle_cancel_response std::vector actionHandleCancelResultVector; findElementVectorByTag(root, std::string("ros_action_handle_cancel_result"), actionHandleCancelResultVector); diff --git a/template_skill/src/TemplateSkill.cpp b/template_skill/src/TemplateSkill.cpp index 02073f3..2a2a5c4 100644 --- a/template_skill/src/TemplateSkill.cpp +++ b/template_skill/src/TemplateSkill.cpp @@ -86,7 +86,7 @@ bool $className$::start(int argc, char*argv[]) std::placeholders::_2)); m_haltService->configure_introspection(m_node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS);/*END_HALT*/ /*ACTION_LIST_C*//*ACTION_C*/ - m_actionClient = rclcpp_action::create_client<$eventData.interfaceName$::action::$eventData.functionName$>(m_node, "/$eventData.componentName$/$eventData.functionName$"); + m_actionClient = rclcpp_action::create_client<$eventData.interfaceName$::action::$eventData.functionName$>(m_node, "$eventData.clientName$"); m_send_goal_options.goal_response_callback = std::bind(&$className$::goal_response_callback, this, std::placeholders::_1); m_send_goal_options.feedback_callback = std::bind(&$className$::feedback_callback, this, std::placeholders::_1, std::placeholders::_2); m_send_goal_options.result_callback = std::bind(&$className$::result_callback, this, std::placeholders::_1); @@ -179,7 +179,7 @@ bool $className$::start(int argc, char*argv[]) RCLCPP_INFO(m_node->get_logger(), "calling send goal"); std::shared_ptr node$eventData.functionName$ = rclcpp::Node::make_shared(m_name + "SkillNode$eventData.functionName$"); rclcpp_action::Client<$eventData.interfaceName$::action::$eventData.functionName$>::SharedPtr client$eventData.functionName$ = - rclcpp_action::create_client<$eventData.interfaceName$::action::$eventData.functionName$>(node$eventData.functionName$, "/$eventData.componentName$/$eventData.functionName$"); + rclcpp_action::create_client<$eventData.interfaceName$::action::$eventData.functionName$>(node$eventData.functionName$, "$eventData.clientName$"); $eventData.interfaceName$::action::$eventData.functionName$::Goal goal_msg; /*SEND_PARAM_LIST*//*SEND_PARAM*/ std::string temp = event.data().toMap()["$IT->FIRST$"].toString().toStdString(); @@ -193,7 +193,7 @@ bool $className$::start(int argc, char*argv[]) RCLCPP_INFO(m_node->get_logger(), "$className$::$eventData.componentName$.$eventData.functionName$.ResultRequest"); std::shared_ptr node$eventData.functionName$ = rclcpp::Node::make_shared(m_name + "SkillNode$eventData.functionName$"); rclcpp_action::Client<$eventData.interfaceName$::action::$eventData.functionName$>::SharedPtr client$eventData.functionName$ = - rclcpp_action::create_client<$eventData.interfaceName$::action::$eventData.functionName$>(node$eventData.functionName$, "/$eventData.componentName$/GoToPoi"); + rclcpp_action::create_client<$eventData.interfaceName$::action::$eventData.functionName$>(node$eventData.functionName$, "$eventData.clientName$"); RCLCPP_INFO(m_node->get_logger(), "result request"); }); /*END_ACTION_RESULT_REQUEST*/ @@ -346,14 +346,21 @@ void $className$::feedback_callback( /*ACTION_RESULT_CALLBACK_FNC*/ void $className$::result_callback(const rclcpp_action::ClientGoalHandle<$eventData.interfaceName$::action::$eventData.functionName$>::WrappedResult & result) { + QVariantMap data; switch (result.code) { case rclcpp_action::ResultCode::SUCCEEDED: + m_stateMachine.submitEvent("$eventData.componentName$.$eventData.functionName$.ResultResponse", data); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "$eventData.componentName$.$eventData.functionName$.ResultResponse"); break; case rclcpp_action::ResultCode::ABORTED: RCLCPP_ERROR(m_node->get_logger(), "Goal was aborted"); + m_stateMachine.submitEvent("$eventData.componentName$.$eventData.functionName$.AbortedResultResponse", data); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "$eventData.componentName$.$eventData.functionName$.AbortedResultResponse"); break; case rclcpp_action::ResultCode::CANCELED: RCLCPP_ERROR(m_node->get_logger(), "Goal was canceled"); + m_stateMachine.submitEvent("$eventData.componentName$.$eventData.functionName$.CancelResult", data); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "$eventData.componentName$.$eventData.functionName$.CancelResult"); break; default: RCLCPP_ERROR(m_node->get_logger(), "Unknown result code"); @@ -361,8 +368,4 @@ void $className$::result_callback(const rclcpp_action::ClientGoalHandle<$eventD } //std::cout << "Result received: " << result.result->is_ok << std::endl; // RCLCPP_INFO(m_node->get_logger(), "Result received: %d ", result.result->is_ok); - QVariantMap data; - // data.insert("is_ok", result.result->is_ok); - m_stateMachine.submitEvent("$eventData.componentName$.$eventData.functionName$.ResultResponse", data); - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "$eventData.componentName$.$eventData.functionName$.ResultResponse"); }/*END_ACTION_RESULT_CALLBACK_FNC*/