Implement for spin

redvinaa · redvinaa · commit 0acecdf2f7c0 · 2024-11-13T10:55:35.000+01:00
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp
@@ -65,6 +65,8 @@ class SpinAction : public BtActionNode<nav2_msgs::action::Spin>
         BT::InputPort<double>("spin_dist", 1.57, "Spin distance"),
         BT::InputPort<double>("time_allowance", 10.0, "Allowed time for spinning"),
         BT::InputPort<bool>("is_recovery", true, "True if recovery"),
+        BT::OutputPort<double>(
+          "angular_distance_traveled", "Angular distance traveled (may be less than requested)"),
         BT::OutputPort<ActionResult::_error_code_type>(
           "error_code_id", "The spin behavior error code")
       });
@@ -85,6 +87,18 @@ class SpinAction : public BtActionNode<nav2_msgs::action::Spin>
    */
   BT::NodeStatus on_cancelled() override;
 
+  /**
+   * @brief Function to perform some user-defined operation after a timeout
+   * waiting for a result that hasn't been received yet. Also provides access to
+   * the latest feedback message from the action server. Feedback will be nullptr
+   * in subsequent calls to this function if no new feedback is received while waiting for a result.
+   * @param feedback shared_ptr to latest feedback message, nullptr if no new feedback was received
+   */
+  void on_wait_for_result(std::shared_ptr<const typename Action::Feedback> feedback) override;
+
+protected:
+  double distance_traveled_;
+
 private:
   bool is_recovery_;
 };
diff --git a/nav2_behavior_tree/plugins/action/spin_action.cpp b/nav2_behavior_tree/plugins/action/spin_action.cpp
@@ -49,22 +49,33 @@ void SpinAction::on_tick()
 
 BT::NodeStatus SpinAction::on_success()
 {
+  setOutput("angular_distance_traveled", distance_traveled_);
   setOutput("error_code_id", ActionGoal::NONE);
   return BT::NodeStatus::SUCCESS;
 }
 
 BT::NodeStatus SpinAction::on_aborted()
 {
+  setOutput("angular_distance_traveled", distance_traveled_);
   setOutput("error_code_id", result_.result->error_code);
   return BT::NodeStatus::FAILURE;
 }
 
 BT::NodeStatus SpinAction::on_cancelled()
 {
+  setOutput("angular_distance_traveled", distance_traveled_);
   setOutput("error_code_id", ActionGoal::NONE);
   return BT::NodeStatus::SUCCESS;
 }
 
+void SpinAction::on_wait_for_result(std::shared_ptr<const typename Action::Feedback> feedback)
+{
+  if (!feedback) {
+    return;
+  }
+  distance_traveled_ = feedback->angular_distance_traveled;
+}
+
 }  // namespace nav2_behavior_tree
 
 #include "behaviortree_cpp_v3/bt_factory.h"

Original file line number	Diff line number	Diff line change
`@@ -49,22 +49,33 @@ void SpinAction::on_tick()`
`49`	`49`
`50`	`50`	`BT::NodeStatus SpinAction::on_success()`
`51`	`51`	`{`
	`52`	`+ setOutput("angular_distance_traveled", distance_traveled_);`
`52`	`53`	`setOutput("error_code_id", ActionGoal::NONE);`
`53`	`54`	`return BT::NodeStatus::SUCCESS;`
`54`	`55`	`}`
`55`	`56`
`56`	`57`	`BT::NodeStatus SpinAction::on_aborted()`
`57`	`58`	`{`
	`59`	`+ setOutput("angular_distance_traveled", distance_traveled_);`
`58`	`60`	`setOutput("error_code_id", result_.result->error_code);`
`59`	`61`	`return BT::NodeStatus::FAILURE;`
`60`	`62`	`}`
`61`	`63`
`62`	`64`	`BT::NodeStatus SpinAction::on_cancelled()`
`63`	`65`	`{`
	`66`	`+ setOutput("angular_distance_traveled", distance_traveled_);`
`64`	`67`	`setOutput("error_code_id", ActionGoal::NONE);`
`65`	`68`	`return BT::NodeStatus::SUCCESS;`
`66`	`69`	`}`
`67`	`70`
	`71`	`+void SpinAction::on_wait_for_result(std::shared_ptr<const typename Action::Feedback> feedback)`
	`72`	`+{`
	`73`	`+ if (!feedback) {`
	`74`	`+ return;`
	`75`	`+ }`
	`76`	`+ distance_traveled_ = feedback->angular_distance_traveled;`
	`77`	`+}`
	`78`	`+`
`68`	`79`	`} // namespace nav2_behavior_tree`
`69`	`80`
`70`	`81`	`#include "behaviortree_cpp_v3/bt_factory.h"`