Here is my code.(SendGoal.cpp)
\n#include \"rm_behavior_tree/plugins/action/send_goal.hpp\"\n\nnamespace rm_behavior_tree\n{\n\nSendGoalAction::SendGoalAction(\n const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params)\n: RosActionNode<nav2_msgs::action::NavigateToPose>(name, conf, params)\n{\n}\n\nbool SendGoalAction::setGoal(nav2_msgs::action::NavigateToPose::Goal & goal)\n{\n auto res = getInput<geometry_msgs::msg::PoseStamped>(\"goal_pose\");\n if (!res) {\n throw BT::RuntimeError(\"error reading port [goal_pose]:\", res.error());\n }\n goal.pose = res.value();\n goal.pose.header.frame_id = \"map\";\n goal.pose.header.stamp = rclcpp::Clock().now();\n\n // clang-format off\n std::cout << \"Goal_pose: [ \"\n << std::fixed << std::setprecision(1)\n << goal.pose.pose.position.x << \", \"\n << goal.pose.pose.position.y << \", \"\n << goal.pose.pose.position.z << \", \"\n << goal.pose.pose.orientation.x << \", \"\n << goal.pose.pose.orientation.y << \", \"\n << goal.pose.pose.orientation.z << \", \"\n << goal.pose.pose.orientation.w << \" ]\\n\";\n // clang-format on\n\n return true;\n}\n\nvoid SendGoalAction::onHalt()\n{\n RCLCPP_INFO(node_->get_logger(), \"SendGoalAction has been halted.\");\n}\n\nBT::NodeStatus SendGoalAction::onResultReceived(const WrappedResult & wr)\n{\n switch (wr.code) {\n case rclcpp_action::ResultCode::SUCCEEDED:\n std::cout << \"Success!!!\" << '\\n';\n return BT::NodeStatus::SUCCESS;\n break;\n case rclcpp_action::ResultCode::ABORTED:\n std::cout << \"Goal was aborted\" << '\\n';\n return BT::NodeStatus::FAILURE;\n break;\n case rclcpp_action::ResultCode::CANCELED:\n std::cout << \"Goal was canceled\" << '\\n';\n return BT::NodeStatus::FAILURE;\n break;\n default:\n std::cout << \"Unknown result code\" << '\\n';\n return BT::NodeStatus::FAILURE;\n break;\n }\n}\n\nBT::NodeStatus SendGoalAction::onFeedback(\n const std::shared_ptr<const nav2_msgs::action::NavigateToPose::Feedback> feedback)\n{\n std::cout << \"Distance remaining: \" << feedback->distance_remaining << '\\n';\n return BT::NodeStatus::RUNNING;\n}\n\nBT::NodeStatus SendGoalAction::onFailure(BT::ActionNodeErrorCode error)\n{\n RCLCPP_ERROR(node_->get_logger(), \"SendGoalAction failed with error code: %d\", error);\n return BT::NodeStatus::FAILURE;\n}\n\n} // namespace rm_behavior_tree\n\n#include \"behaviortree_ros2/plugins.hpp\"\nCreateRosNodePlugin(rm_behavior_tree::SendGoalAction, \"SendGoal\");
Additionally, I have noticed that if the program enters the SendGoal Action, and at this point I try to exit the program using Ctrl+C, it remains unable to exit.
\n^C[INFO] [1711783006.794006880] [rclcpp]: signal_handler(signum=2)
I solved the problem myself. By replacing AsyncSequence
with ReactiveSequence
, other nodes will continue to be ticked even if one node returns RUNNING
. By replacing IfThenElse
with WhileDoElse
, the currently executing node will be interrupted if the condition of the if changes. I have a further understanding of the word \"reactive\".
\n
\n
-
Beta Was this translation helpful? Give feedback.
-
Beta Was this translation helpful? Give feedback.
I solved the problem myself. By replacing
AsyncSequence
withReactiveSequence
, other nodes will continue to be ticked even if one node returnsRUNNING
. By replacingIfThenElse
withWhileDoElse
, the currently executing node will be interrupted if the condition of the if changes. I have a further understanding of the word "reactive".