Skip to content
\n

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\");
\n

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)
","upvoteCount":1,"answerCount":1,"acceptedAnswer":{"@type":"Answer","text":"

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\"截图

","upvoteCount":0,"url":"https://github.com/BehaviorTree/BehaviorTree.CPP/discussions/797#discussioncomment-8959204"}}}

SendGoal Action not allowing other nodes to tick and unable to halt properly #797

Discussion options

You must be logged in to vote

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".


Replies: 1 comment

Comment options

You must be logged in to vote
0 replies
Answer selected by facontidavide
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Category
Q&A
Labels
None yet
1 participant