Behavior server error codes (ros-navigation#3539)

* empty error codes

* add error codes for behaviors

* updated assisted teleop

* pass tests

* added error codes to bt nodes

* Enable Visualizations for Dev Container (ros-navigation#3523)

* Add visualizer stage
to install demo dependencies

* Install foxglove

* Install gzweb

* Add hack for resolvable mesh URIs
located by the aws SDL model files
- aws-robotics/aws-robomaker-small-warehouse-world#24

* Revert hack and use fork
that fixes issues with deploy.sh
- osrf/gzweb#248

* Update target stage to visualizer

* Comment out gzclient and rviz for debugging

* Add hack for resolvable mesh URIs
as migrating the python3 scripts still hasn't resolved the issue

* Reorder stages for readability
by keeping sequential builder and tester stages adjacent
while keeping tester stage the default exported target

* fix typo

* Install gdb for launching ros launch files
using the ROS VS Code extension
- ms-iot/vscode-ros#588

* Add vscode tasks file

* Add Start Gzweb task

* Add Start Foxglove tasks
for bridge and studio

* Add Start Foxglove compound task
using dependsOn

* Set default problemMatcher to empty
to avoid nagging the user to select one
as currently none really support our use case

* Source overlay before running foxglove_bridge
to ensure nav2 message types are defined
by inlining all args into command
and sourcing workspace setup

* Formatting

* Generalize and simplify hack

* Generalize gazebo model discovery

* Patch gzserver to run headless using xvfb
to avoid host/platform specific x11 quirks
exposed by vscode automatic x11 forwarding

This is needed to provide gazebo a virtual frame buffer
as it still need one after all these years.
This also avoids the need modifying launch files to call xvfb-run

- microsoft/vscode-remote-release#8031
- gazebosim/gazebo-classic#1602

* Set isBackground for start tasks

* Add stop tasks

* Add restart foxglove task

* Switch to shell for commanding pkill
to gracefully return 0 when process is not running
allowing sequence of dependsOn tasks to run
such as for the restart tasks

* Add icons to tasks
for readability

* Add restart gzweb task

* Add global start, stop, and restart tasks
for all background visualization tasks

* Formatting

* Hide tasks users need not run manually
to avoid cluttering up the run task quick pick

* Shorten label for background tasks
so they succinctly show from the running task list

* Show global start and stop visualizations tasks
as they may be too helpful to hide

* Revert "Comment out gzclient and rviz for debugging"

This reverts commit 0addae2.

* Add --ipc=host to runArgs
to enable shared memory transport
- https://community.rti.com/kb/communicate-between-two-docker-containers-using-rti-connext-dds-and-shared-memory

* Add --pid=host to runArgs
to simplify discovery
- https://community.rti.com/kb/communicate-between-two-docker-containers-using-rti-connext-dds-and-shared-memory

* Add to runArgs
to simplify debugging
- https://code.visualstudio.com/docs/devcontainers/create-dev-container#_use-docker-compose

* Add comments

* Comment out runArgs unintended side effects
or cross talk between containers by default
also avoids interfering with vscode's X11 forwarding

* ignore warning (ros-navigation#3543)

* Split overlay setup into multiple steps
by skipping slower to build leaf packages during preparation,
then store cache and repeat setup without skipping packages

* Skip restore steps after already preping overlay
to avoid needlessly downloading the same overlay cache

* Revert resource_class to default medium
as the build resource usage seldom maxes out 4 cores
nor uses more than 2GB RAM

* Fix circleci config syntax
by setting skip default as empty string
to keep it an optional parameter

* Fix circleci config syntax
missing angle brackets

* ignore warning

* Revert "Revert resource_class to default medium"

This reverts commit 44375a1.

* Fix nested defaults
to avoid dropping of cache after storing during test jobs
by ensuring restore_overlay_workspace still sets restore: true

---------

Co-authored-by: ruffsl <roxfoxpox@gmail.com>

* code review

* code review

* removed unsigned short

* lint errer

* error codes in main bts

* behavior error code range change

---------

Co-authored-by: Ruffin <roxfoxpox@gmail.com>
Signed-off-by: enricosutera <enricosutera@outlook.com>

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp

@@ -29,6 +29,10 @@ namespace nav2_behavior_tree
  */
 class AssistedTeleopAction : public BtActionNode<nav2_msgs::action::AssistedTeleop>
 {
+  using Action = nav2_msgs::action::AssistedTeleop;
+  using ActionGoal = Action::Goal;
+  using ActionResult = Action::Result;
+
 public:
   /**
    * @brief A constructor for nav2_behavior_tree::nav2_msgs::action::AssistedTeleop
@@ -46,8 +50,21 @@ class AssistedTeleopAction : public BtActionNode<nav2_msgs::action::AssistedTele
    */
   void on_tick() override;
 
+  /**
+   * @brief Function to perform some user-defined operation upon successful completion of the action
+   */
+  BT::NodeStatus on_success() override;
+
+  /**
+   * @brief Function to perform some user-defined operation upon abortion of the action
+   */
   BT::NodeStatus on_aborted() override;
 
+  /**
+   * @brief Function to perform some user-defined operation upon cancellation of the action
+   */
+  BT::NodeStatus on_cancelled() override;
+
   /**
    * @brief Creates list of BT ports
    * @return BT::PortsList Containing basic ports along with node-specific ports
@@ -57,7 +74,9 @@ class AssistedTeleopAction : public BtActionNode<nav2_msgs::action::AssistedTele
     return providedBasicPorts(
       {
         BT::InputPort<double>("time_allowance", 10.0, "Allowed time for running assisted teleop"),
-        BT::InputPort<bool>("is_recovery", false, "If true the recovery count will be incremented")
+        BT::InputPort<bool>("is_recovery", false, "If true the recovery count will be incremented"),
+        BT::OutputPort<ActionResult::_error_code_type>(
+          "error_code_id", "The assisted teleop behavior server error code")
       });
   }
 

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp

@@ -28,6 +28,10 @@ namespace nav2_behavior_tree
  */
 class BackUpAction : public BtActionNode<nav2_msgs::action::BackUp>
 {
+  using Action = nav2_msgs::action::BackUp;
+  using ActionGoal = Action::Goal;
+  using ActionResult = Action::Result;
+
 public:
   /**
    * @brief A constructor for nav2_behavior_tree::BackUpAction
@@ -45,6 +49,22 @@ class BackUpAction : public BtActionNode<nav2_msgs::action::BackUp>
    */
   void on_tick() override;
 
+
+  /**
+ * @brief Function to perform some user-defined operation upon successful completion of the action
+ */
+  BT::NodeStatus on_success() override;
+
+  /**
+   * @brief Function to perform some user-defined operation upon abortion of the action
+   */
+  BT::NodeStatus on_aborted() override;
+
+  /**
+   * @brief Function to perform some user-defined operation upon cancellation of the action
+   */
+  BT::NodeStatus on_cancelled() override;
+
   /**
    * @brief Creates list of BT ports
    * @return BT::PortsList Containing basic ports along with node-specific ports
@@ -55,7 +75,9 @@ class BackUpAction : public BtActionNode<nav2_msgs::action::BackUp>
       {
         BT::InputPort<double>("backup_dist", 0.15, "Distance to backup"),
         BT::InputPort<double>("backup_speed", 0.025, "Speed at which to backup"),
-        BT::InputPort<double>("time_allowance", 10.0, "Allowed time for reversing")
+        BT::InputPort<double>("time_allowance", 10.0, "Allowed time for reversing"),
+        BT::OutputPort<ActionResult::_error_code_type>(
+          "error_code_id", "The back up behavior server error code")
       });
   }
 };

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp

@@ -28,6 +28,10 @@ namespace nav2_behavior_tree
  */
 class DriveOnHeadingAction : public BtActionNode<nav2_msgs::action::DriveOnHeading>
 {
+  using Action = nav2_msgs::action::DriveOnHeading;
+  using ActionGoal = Action::Goal;
+  using ActionResult = Action::Goal;
+
 public:
   /**
    * @brief A constructor for nav2_behavior_tree::DriveOnHeadingAction
@@ -50,11 +54,27 @@ class DriveOnHeadingAction : public BtActionNode<nav2_msgs::action::DriveOnHeadi
       {
         BT::InputPort<double>("dist_to_travel", 0.15, "Distance to travel"),
         BT::InputPort<double>("speed", 0.025, "Speed at which to travel"),
-        BT::InputPort<double>("time_allowance", 10.0, "Allowed time for driving on heading")
+        BT::InputPort<double>("time_allowance", 10.0, "Allowed time for driving on heading"),
+        BT::OutputPort<Action::Result::_error_code_type>(
+          "error_code_id", "The drive on heading behavior server error code")
       });
   }
+
+  /**
+ * @brief Function to perform some user-defined operation upon successful completion of the action
+ */
+  BT::NodeStatus on_success() override;
+
+  /**
+   * @brief Function to perform some user-defined operation upon abortion of the action
+   */
+  BT::NodeStatus on_aborted() override;
+
+  /**
+   * @brief Function to perform some user-defined operation upon cancellation of the action
+   */
+  BT::NodeStatus on_cancelled() override;
 };
 
 }  // namespace nav2_behavior_tree
-
 #endif  // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__DRIVE_ON_HEADING_ACTION_HPP_

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp

@@ -28,6 +28,10 @@ namespace nav2_behavior_tree
  */
 class SpinAction : public BtActionNode<nav2_msgs::action::Spin>
 {
+  using Action = nav2_msgs::action::Spin;
+  using ActionResult = Action::Result;
+  using ActionGoal = Action::Goal;
+
 public:
   /**
    * @brief A constructor for nav2_behavior_tree::SpinAction
@@ -55,10 +59,27 @@ 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::InputPort<bool>("is_recovery", true, "True if recovery"),
+        BT::OutputPort<ActionResult::_error_code_type>(
+          "error_code_id", "The spin behavior error code")
       });
   }
 
+  /**
+   * @brief Function to perform some user-defined operation upon successful completion of the action
+   */
+  BT::NodeStatus on_success() override;
+
+  /**
+   * @brief Function to perform some user-defined operation upon abortion of the action
+   */
+  BT::NodeStatus on_aborted() override;
+
+  /**
+   * @brief Function to perform some user-defined operation upon cancellation of the action
+   */
+  BT::NodeStatus on_cancelled() override;
+
 private:
   bool is_recovery_;
 };

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -13,6 +13,7 @@
       <input_port name="time_allowance">Allowed time for reversing</input_port>
       <input_port name="server_name">Server name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
+      <output_port name="error_code_id">"Back up error code"</output_port>
     </Action>
 
     <Action ID="DriveOnHeading">
@@ -21,6 +22,7 @@
       <input_port name="time_allowance">Allowed time for reversing</input_port>
       <input_port name="server_name">Server name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
+      <output_port name="error_code_id">"Drive on heading error code"</output_port>
     </Action>
 
     <Action ID="CancelControl">
@@ -184,6 +186,7 @@
       <input_port name="time_allowance">Allowed time for spinning</input_port>
       <input_port name="server_name">Server name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
+      <output_port name="error_code_id">Spin error code</output_port>
     </Action>
 
     <Action ID="Wait">
@@ -197,6 +200,7 @@
       <input_port name="is_recovery">If true recovery count will be incremented</input_port>
       <input_port name="server_name">Service name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
+      <output_port name="error_code_id">Assisted teleop error code</output_port>
     </Action>
 
     <!-- ############################### CONDITION NODES ############################## -->

nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp

@@ -41,11 +41,24 @@ void AssistedTeleopAction::on_tick()
   }
 }
 
+BT::NodeStatus AssistedTeleopAction::on_success()
+{
+  setOutput("error_code_id", ActionGoal::NONE);
+  return BT::NodeStatus::SUCCESS;
+}
+
 BT::NodeStatus AssistedTeleopAction::on_aborted()
 {
+  setOutput("error_code_id", result_.result->error_code);
   return is_recovery_ ? BT::NodeStatus::FAILURE : BT::NodeStatus::SUCCESS;
 }
 
+BT::NodeStatus AssistedTeleopAction::on_cancelled()
+{
+  setOutput("error_code_id", ActionGoal::NONE);
+  return BT::NodeStatus::SUCCESS;
+}
+
 }  // namespace nav2_behavior_tree
 
 #include "behaviortree_cpp_v3/bt_factory.h"

nav2_behavior_tree/plugins/action/back_up_action.cpp

@@ -46,6 +46,24 @@ void BackUpAction::on_tick()
   increment_recovery_count();
 }
 
+BT::NodeStatus BackUpAction::on_success()
+{
+  setOutput("error_code_id", ActionGoal::NONE);
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus BackUpAction::on_aborted()
+{
+  setOutput("error_code_id", result_.result->error_code);
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus BackUpAction::on_cancelled()
+{
+  setOutput("error_code_id", ActionGoal::NONE);
+  return BT::NodeStatus::SUCCESS;
+}
+
 }  // namespace nav2_behavior_tree
 
 #include "behaviortree_cpp_v3/bt_factory.h"

nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp

@@ -41,6 +41,24 @@ DriveOnHeadingAction::DriveOnHeadingAction(
   goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance);
 }
 
+BT::NodeStatus DriveOnHeadingAction::on_success()
+{
+  setOutput("error_code_id", ActionGoal::NONE);
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus DriveOnHeadingAction::on_aborted()
+{
+  setOutput("error_code_id", result_.result->error_code);
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus DriveOnHeadingAction::on_cancelled()
+{
+  setOutput("error_code_id", ActionGoal::NONE);
+  return BT::NodeStatus::SUCCESS;
+}
+
 }  // namespace nav2_behavior_tree
 
 #include "behaviortree_cpp_v3/bt_factory.h"

nav2_behavior_tree/plugins/action/spin_action.cpp

@@ -12,7 +12,6 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <string>
 #include <memory>
 
 #include "nav2_behavior_tree/plugins/action/spin_action.hpp"
@@ -42,6 +41,24 @@ void SpinAction::on_tick()
   }
 }
 
+BT::NodeStatus SpinAction::on_success()
+{
+  setOutput("error_code_id", ActionGoal::NONE);
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus SpinAction::on_aborted()
+{
+  setOutput("error_code_id", result_.result->error_code);
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus SpinAction::on_cancelled()
+{
+  setOutput("error_code_id", ActionGoal::NONE);
+  return BT::NodeStatus::SUCCESS;
+}
+
 }  // namespace nav2_behavior_tree
 
 #include "behaviortree_cpp_v3/bt_factory.h"