AGVFM-546 AGV Errors: Add error code to the AGV errors on the camera …

…server (#75)

* add error_type code for error messages

* add custom message type AGVError

* add custom message type AgvError

* use message type AGVError

* fix numbering

* fix typo

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp

@@ -22,7 +22,7 @@
 #include "nav2_util/node_utils.hpp"
 #include "rclcpp_action/rclcpp_action.hpp"
 #include "nav2_behavior_tree/bt_conversions.hpp"
-#include "std_msgs/msg/string.hpp"
+#include "nav2_msgs/msg/agv_error.hpp"
 
 namespace nav2_behavior_tree
 {
@@ -54,7 +54,7 @@ class BtActionNode : public BT::ActionNodeBase
     }
     createActionClient(action_name_);
 
-    error_publisher_ = node_->create_publisher<std_msgs::msg::String>("action_server_error_message", rclcpp::SystemDefaultsQoS());
+    error_publisher_ = node_->create_publisher<nav2_msgs::msg::AgvError>("action_server_error_message", rclcpp::SystemDefaultsQoS());
     // Give the derive class a chance to do any initialization
     RCLCPP_INFO(node_->get_logger(), "\"%s\" BtActionNode initialized", xml_tag_name.c_str());
   }
@@ -145,7 +145,8 @@ class BtActionNode : public BT::ActionNodeBase
       } catch (const std::runtime_error& e) {
           auto error =  "BT navigator, action " + action_name_  + " : on_new_goal_received exception occurred, " + std::string(e.what());
           RCLCPP_ERROR(node_->get_logger(), error);
-          message.data = error;
+          message.error_message = error;
+          message.error_type = 26;
           error_publisher_->publish(message);
           goal_result_available_ = true;
           result_.code = rclcpp_action::ResultCode::ABORTED;
@@ -277,8 +278,8 @@ class BtActionNode : public BT::ActionNodeBase
   // The timeout value while waiting for response from a server when a
   // new action goal is sent or canceled
   std::chrono::milliseconds server_timeout_;
-  std_msgs::msg::String message;
-  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr error_publisher_;
+  nav2_msgs::msg::AgvError message;
+  rclcpp::Publisher<nav2_msgs::msg::AgvError>::SharedPtr error_publisher_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_controller/src/nav2_controller.cpp

@@ -361,7 +361,7 @@ void ControllerServer::computeControl()
   } catch (nav2_core::PlannerException & e) {
     // RCLCPP ERROR is sometimes not shown, use info as well to be sure
     RCLCPP_INFO(get_logger(), "An error occured");
-    error_publisher_.publish(e.what());
+    error_publisher_.publish(e.what(), 29);
     RCLCPP_INFO(get_logger(), e.what());
     publishZeroVelocity();
     action_server_->terminate_current();

nav2_msgs/CMakeLists.txt

@@ -16,6 +16,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/Costmap.msg"
   "msg/CostmapMetaData.msg"
   "msg/LidarStatus.msg"
+  "msg/AgvError.msg"
   "msg/VoxelGrid.msg"
   "msg/BehaviorTreeStatusChange.msg"
   "msg/BehaviorTreeLog.msg"

nav2_msgs/msg/AgvError.msg

@@ -0,0 +1,2 @@
+string error_message
+uint32 error_type

nav2_planner/src/planner_server.cpp

@@ -223,7 +223,7 @@ PlannerServer::computePlan()
 
     geometry_msgs::msg::PoseStamped start;
     if (!costmap_ros_->getRobotPose(start)) {
-      error_publisher_.publish("Couldn't get robot current position");
+      error_publisher_.publish("Couldn't get robot current position", 27);
       action_server_->terminate_current();
       return;
     }
@@ -268,7 +268,7 @@ PlannerServer::computePlan()
       get_logger(), "%s plugin failed to plan calculation to (%.2f, %.2f): \"%s\"",
       goal->planner_id.c_str(), goal->pose.pose.position.x,
       goal->pose.pose.position.y, ex.what());
-      error_publisher_.publish(goal->planner_id + " plugin failed to plan calculation: " + ex.what());
+      error_publisher_.publish(goal->planner_id + " plugin failed to plan calculation: " + ex.what(), 25);
     // TODO(orduno): provide information about fail error to parent task,
     //               for example: couldn't get costmap update
     action_server_->terminate_current();

nav2_util/include/nav2_util/error_publisher.hpp

@@ -7,6 +7,7 @@
 #include <string>
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 #include "std_msgs/msg/string.hpp"
+#include "nav2_msgs/msg/agv_error.hpp"
 
 namespace nav2_util
 {
@@ -21,20 +22,21 @@ class ErrorPublisher
     ErrorPublisher() = default;
 
     ErrorPublisher(rclcpp_lifecycle::LifecycleNode * node, std::string error_message_prefix): message_prefix(error_message_prefix), action_server_node_(node) {
-        publisher_ = node->create_publisher<std_msgs::msg::String>("action_server_error_message", rclcpp::SystemDefaultsQoS());
+        publisher_ = node->create_publisher<nav2_msgs::msg::AgvError>("action_server_error_message", rclcpp::SystemDefaultsQoS());
     }
 
     void on_activate() { publisher_->on_activate(); }
 
-    void publish(std::string msg) {
-        auto message = std_msgs::msg::String();
-        message.data = message_prefix + " : " + msg;
+    void publish(std::string msg, int error_type) {
+        auto message = nav2_msgs::msg::AgvError();
+        message.error_message = message_prefix + " : " + msg;
+        message.error_type = error_type;
         publisher_->publish(message);
         RCLCPP_ERROR(action_server_node_->get_logger(), msg);
     }
 
 private:
-    rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::String>::SharedPtr publisher_;
+    rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::AgvError>::SharedPtr publisher_;
     std::string message_prefix;
     rclcpp_lifecycle::LifecycleNode * action_server_node_;
 };

smac_planner/src/smac_planner.cpp

@@ -230,7 +230,7 @@ nav_msgs::msg::Path SmacPlanner::createPlan(
   if (!_costmap_ros->is_static_layer_costmap_initialized()) {
        // no map for static layer was published
        // abort planning
-      error_publisher_.publish("static layer costmap hasn't received any initial occupancy grid");
+      error_publisher_.publish("static layer costmap hasn't received any initial occupancy grid", 31);
       nav_msgs::msg::Path plan;
       plan.header.stamp = _clock->now();
       plan.header.frame_id = _global_frame;
@@ -310,7 +310,7 @@ nav_msgs::msg::Path SmacPlanner::createPlan(
       _logger,
       "%s: failed to create plan, %s.",
       _name.c_str(), error.c_str());
-    error_publisher_.publish(_name + ": failed to create plan, error:  " + error);
+    error_publisher_.publish(_name + ": failed to create plan, error:  " + error, 25);
     return plan;
   }