Using Custom Yaml Loader for topological map subscription

topological_navigation/config/bt_tree_default.xml

@@ -6,7 +6,7 @@
           <RecoveryNode number_of_retries="0" name="ComputePathThroughPoses">
             <ReactiveSequence>
               <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="1.5"/>
-              <ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="GoalAligned"/>
+              <ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="GridBased"/>
             </ReactiveSequence>
             <ReactiveFallback name="ComputePathThroughPosesRecoveryFallback">
               <GoalUpdated/>

topological_navigation/config/bt_tree_in_row_operation.xml

@@ -0,0 +1,10 @@
+<root main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <RecoveryNode number_of_retries="0" name="Harvesting">
+        <Wait wait_duration="5"/>
+        <ReactiveFallback name="ComputePathToPoseRecoveryFallback">
+        <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
+        </ReactiveFallback>
+    </RecoveryNode>
+  </BehaviorTree>
+</root>

topological_navigation/config/bt_tree_in_row_recovery.xml

@@ -0,0 +1,10 @@
+<root main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <RecoveryNode number_of_retries="0" name="Harvesting">
+        <Wait wait_duration="5"/>
+        <ReactiveFallback name="ComputePathToPoseRecoveryFallback">
+        <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
+        </ReactiveFallback>
+    </RecoveryNode>
+  </BehaviorTree>
+</root>

topological_navigation/topological_navigation/policy_marker2.py

@@ -1,7 +1,7 @@
 #!/usr/bin/env python3
 
 import math
-
+import yaml
 import rclpy, json 
 from rclpy.node import Node
 from rclpy.qos import QoSProfile, DurabilityPolicy
@@ -17,6 +17,23 @@
 from topological_navigation_msgs.msg import TopologicalMap, NavRoute
 from std_msgs.msg import String 
 
+# this ensures that all the poses and translates 
+# are float-type and not int-type as there is an 
+# assertion in ros2 messages (vector3, pose etc.) 
+# for float-type [x,y,z,w] keys.
+class CustomSafeLoader(yaml.SafeLoader):
+    def construct_mapping(self, node, deep=False):
+        mapping = super().construct_mapping(node, deep=deep)
+
+        # this can be extended to test the validity of the tmap2 
+        # as well at load time (or add missing keys)
+        for key in ['x', 'y', 'z', 'w']:
+            if key in mapping and isinstance(mapping[key], int):
+                mapping[key] = float(mapping[key])
+
+        return mapping
+
+
 class PoliciesVis(Node):
 
     def __init__(self):
@@ -34,7 +51,8 @@ def __init__(self):
     def map_callback(self, msg):
         self.get_logger().info('map callback triggered')
         # self.map = msg
-        self.lnodes = json.loads(msg.data)
+        # self.lnodes = json.loads(msg.data)
+        self.lnodes = yaml.load( msg.data, Loader = CustomSafeLoader)
         self.topol_map = self.lnodes["pointset"]
         self.rsearch = TopologicalRouteSearch2(self.lnodes)
 

topological_navigation/topological_navigation/scripts/actions_bt.py

@@ -7,35 +7,72 @@ def __init__(self):
         self.DRIVE_ON_HEADING = "DriveOnHeading"
 
         self.ROW_TRAVERSAL = "row_traversal"
+        self.ROW_OPERATION = "row_operation"
+        self.ROW_RECOVERY = "row_recovery"
         self.ROW_CHANGE = "row_change"
         self.GOAL_ALIGN = "goal_align"
         self.GOAL_ALIGN_INDEX = ["ca"]
         self.GOAL_ALIGN_GOAL = ["cb"]
-        self.ABORT_NOT_CONTINUE = [self.GOAL_ALIGN, self.ROW_CHANGE, self.ROW_TRAVERSAL, self.NAVIGATE_TO_POSE, self.NAVIGATE_THROUGH_POSES]
+        self.ROW_START_INDEX = "a"
+        self.ROW_COLUMN_START_INDEX = "c"
+        self.ROW_COLUMN_START_NEXT_INDEX = "b"
+        self.OUTSIDE_EDGE_START_INDEX = "WayPoint"
+
+        self.INSIDE_POLYTUNNEL = "INSIDE_POLYTUNNEL"
+        self.OUTSIDE_POLYTUNNEL = "OUTSIDE_POLYTUNNEL"
+        self.TRANSITION_INTO_POLYTUNNEL = "TRANSITION_INTO_POLYTUNNEL"
+
+
+        self.ROBOT_STATUS_PREPARATION_STATE = "PREPARATION_STATE"
+        self.ROBOT_STATUS_AUTONOMOUS_NAVIGATION_STATE = "AUTONOMOUS_NAVIGATION_STATE"
+        self.ROBOT_STATUS_AUTONOMOUS_HARVESTING_STATE = "AUTONOMOUS_HARVESTING_STATE"
+        self.ROBOT_STATUS_AUTONOMOUS_RECOVERY_STATE = "AUTONOMOUS_RECOVERY_STATE"
+        self.ROBOT_STATUS_DISABLE_STATE = "DISABLE_STATE"
+        self.ROBOT_STATUS_NATURAL_STATE = "NATURAL_STATE"
+
+        self.ROBOT_CURRENT_STATUS = {}
+        self.ROBOT_CURRENT_STATUS[self.ROBOT_STATUS_PREPARATION_STATE] = 0
+        self.ROBOT_CURRENT_STATUS[self.ROBOT_STATUS_AUTONOMOUS_NAVIGATION_STATE] = 1
+        self.ROBOT_CURRENT_STATUS[self.ROBOT_STATUS_AUTONOMOUS_HARVESTING_STATE] = 2
+        self.ROBOT_CURRENT_STATUS[self.ROBOT_STATUS_AUTONOMOUS_RECOVERY_STATE] = 3
+        self.ROBOT_CURRENT_STATUS[self.ROBOT_STATUS_DISABLE_STATE] = 4
+        self.ROBOT_CURRENT_STATUS[self.ROBOT_STATUS_NATURAL_STATE] = 5
+
+        self.ABORT_NOT_CONTINUE = [self.GOAL_ALIGN, self.ROW_CHANGE, self.ROW_TRAVERSAL
+                                        , self.NAVIGATE_TO_POSE, self.NAVIGATE_THROUGH_POSES
+                                        , self.ROW_OPERATION, self.ROW_RECOVERY]
 
         self.BT_DEFAULT = "bt_tree_default"
-        self.BT_IN_ROW = "bt_tree_in_row"
         self.BT_GOAL_ALIGN = "bt_tree_goal_align"
-
+        self.BT_IN_ROW = "bt_tree_in_row"
+        self.BT_IN_ROW_OPERATION = "bt_tree_in_row_operation"
+        self.BT_IN_ROW_RECOVERY = "bt_tree_in_row_recovery"
+
         self.navigation_actions = [
             self.NAVIGATE_TO_POSE,
             self.NAVIGATE_THROUGH_POSES,
             self.DRIVE_ON_HEADING,
             self.ROW_CHANGE,
-            self.ROW_TRAVERSAL
+            self.ROW_TRAVERSAL,
+            self.ROW_OPERATION,
+            self.ROW_RECOVERY
         ]
 
         self.bt_tree_types = [
             self.BT_IN_ROW,
             self.BT_DEFAULT,
-            self.BT_GOAL_ALIGN
+            self.BT_GOAL_ALIGN, 
+            self.BT_IN_ROW_OPERATION,
+            self.BT_IN_ROW_RECOVERY,
         ]
 
         self.bt_tree_with_actions = {}
-        self.bt_tree_with_actions[self.ROW_TRAVERSAL] = self.BT_IN_ROW
         self.bt_tree_with_actions[self.NAVIGATE_THROUGH_POSES] = self.BT_DEFAULT
         self.bt_tree_with_actions[self.NAVIGATE_TO_POSE] = self.BT_DEFAULT
         self.bt_tree_with_actions[self.GOAL_ALIGN] = self.BT_GOAL_ALIGN
+        self.bt_tree_with_actions[self.ROW_TRAVERSAL] = self.BT_IN_ROW
+        self.bt_tree_with_actions[self.ROW_OPERATION] = self.BT_IN_ROW_OPERATION
+        self.bt_tree_with_actions[self.ROW_RECOVERY] = self.BT_IN_ROW_RECOVERY
 
         self.status_mapping = {}
         self.status_mapping[0] = "STATUS_UNKNOWN"
@@ -46,11 +83,11 @@ def __init__(self):
         self.status_mapping[5] = "STATUS_CANCELED"
         self.status_mapping[6] = "STATUS_ABORTED"
 
-        self.goal_cancle_error_codes = {} 
-        self.goal_cancle_error_codes[0] = "ERROR_NONE"
-        self.goal_cancle_error_codes[1] = "ERROR_REJECTED"
-        self.goal_cancle_error_codes[2] = "ERROR_UNKNOWN_GOAL_ID"
-        self.goal_cancle_error_codes[3] = "ERROR_GOAL_TERMINATED"
+        self.goal_cancel_error_codes = {} 
+        self.goal_cancel_error_codes[0] = "ERROR_NONE"
+        self.goal_cancel_error_codes[1] = "ERROR_REJECTED"
+        self.goal_cancel_error_codes[2] = "ERROR_UNKNOWN_GOAL_ID"
+        self.goal_cancel_error_codes[3] = "ERROR_GOAL_TERMINATED"
 
         self.planner_with_goal_checker_config = {
             "dwb_core::DWBLocalPlanner": {
@@ -61,13 +98,55 @@ def __init__(self):
                 "goal_checker.xy_goal_tolerance": 0.1,
                 "goal_checker.yaw_goal_tolerance": 0.05,
             },
+            "rownav_local_planner::TebLocalPlannerROS":{
+                "goal_checker.xy_goal_tolerance": 0.1,
+                "goal_checker.yaw_goal_tolerance": 0.6,
+            },
+            "behavioral_controller::BehavioralController":{
+                "goal_checker.xy_goal_tolerance": 0.1,
+                "goal_checker.yaw_goal_tolerance": 0.6,
+            }
         }
 
         self.bt_tree_with_control_server_config = {}
         self.bt_tree_with_control_server_config[self.ROW_TRAVERSAL] = "dwb_core::DWBLocalPlanner"
         self.bt_tree_with_control_server_config[self.NAVIGATE_THROUGH_POSES] = "dwb_core::DWBLocalPlanner"
         self.bt_tree_with_control_server_config[self.NAVIGATE_TO_POSE] = "dwb_core::DWBLocalPlanner"
         self.bt_tree_with_control_server_config[self.GOAL_ALIGN] = "dwb_core::DWBLocalPlanner"
+        self.bt_tree_with_control_server_config[self.ROW_RECOVERY] = "dwb_core::DWBLocalPlanner"
+
+        self.planner_with_pd_regulator_config = {
+            "dwb_core::DWBLocalPlanner": {
+                "kp_v": 40.0,
+                "kd_v": 0.1,
+                "kp_w": 12.0,
+                "kd_w": 0.1,
+            },
+            "teb_local_planner::TebLocalPlannerROS": {
+                "kp_v": 40.0,
+                "kd_v": 0.1,
+                "kp_w": 12.0,
+                "kd_w": 0.1,
+            },
+            "rownav_local_planner::TebLocalPlannerROS":{
+                "kp_v": 40.0,
+                "kd_v": 0.1,
+                "kp_w": 12.0,
+                "kd_w": 0.1,
+            },
+            "behavioral_controller::BehavioralController":{
+                "kp_v": 40.0,
+                "kd_v": 0.1,
+                "kp_w": 12.0,
+                "kd_w": 0.1,
+            }
+        }
+
+    def getCodeForRobotCurrentStatus(self, msg):
+        if msg not in self.ROBOT_CURRENT_STATUS:
+            print("The {} is not one of configured robot status types".format(msg))
+            return self.ROBOT_CURRENT_STATUS[self.ROBOT_STATUS_DISABLE_STATE]
+        return self.ROBOT_CURRENT_STATUS[msg]
 
     def setPlanner(self, planner_name, action_type):
         self.bt_tree_with_control_server_config[action_type] = planner_name
@@ -78,4 +157,13 @@ def setPlannerParams(self, planner_name, xy_goal_tolerance, yaw_goal_tolerance):
             return 
         self.planner_with_goal_checker_config[planner_name]["goal_checker.xy_goal_tolerance"] = xy_goal_tolerance
         self.planner_with_goal_checker_config[planner_name]["goal_checker.yaw_goal_tolerance"] = yaw_goal_tolerance
-        print("Setting planner {} params xy_goal_tolerance: {}, yaw_goal_tolerance: {}".format(planner_name, xy_goal_tolerance, yaw_goal_tolerance))
+        print("Setting planner {} params xy_goal_tolerance: {}, yaw_goal_tolerance: {}".format(planner_name, xy_goal_tolerance, yaw_goal_tolerance))
+
+    def setPDRegulstorParams(self, planner_name, pd_params, robot_controller_name):
+        if not planner_name in self.planner_with_goal_checker_config:
+            print("The planner {} is not one of configured types".format(planner_name))
+            return 
+        parmas_names = ["kp_v", "kd_v", "kp_w", "kd_w"]
+        for index, val in zip(parmas_names, pd_params):
+            self.planner_with_pd_regulator_config[planner_name][index] = val
+        print("Setting planner {} params kp_v: {} kd_v: {} kp_w: {} kd_w: {} ".format(planner_name, pd_params[0], pd_params[1], pd_params[2], pd_params[3]))

topological_navigation/topological_navigation/scripts/get_simple_policy2.py

@@ -7,13 +7,30 @@
 """
 
 import sys
-import rclpy, json 
+import rclpy, json , yaml
 from std_msgs.msg import String
 from topological_navigation_msgs.srv import GetRouteTo, GetRouteBetween
 from topological_navigation.route_search2 import RouteChecker, TopologicalRouteSearch2, get_route_distance
 from rclpy.qos import QoSProfile, HistoryPolicy, ReliabilityPolicy, DurabilityPolicy
 from rclpy.executors import MultiThreadedExecutor
 
+# this ensures that all the poses and translates 
+# are float-type and not int-type as there is an 
+# assertion in ros2 messages (vector3, pose etc.) 
+# for float-type [x,y,z,w] keys.
+class CustomSafeLoader(yaml.SafeLoader):
+    def construct_mapping(self, node, deep=False):
+        mapping = super().construct_mapping(node, deep=deep)
+
+        # this can be extended to test the validity of the tmap2 
+        # as well at load time (or add missing keys)
+        for key in ['x', 'y', 'z', 'w']:
+            if key in mapping and isinstance(mapping[key], int):
+                mapping[key] = float(mapping[key])
+
+        return mapping
+
+
 class SearchPolicyServer(rclpy.node.Node):
 
     def __init__(self, name):
@@ -57,7 +74,8 @@ def get_routeb_cb(self, req, res):
         return res         
 
     def map_callback(self, msg) :
-        self.lnodes = json.loads(msg.data)
+        # self.lnodes = json.loads(msg.data)
+        self.lnodes = yaml.load( msg.data, Loader=CustomSafeLoader)
         self.topol_map = self.lnodes["pointset"]
         self.rsearch = TopologicalRouteSearch2(self.lnodes)
         self._map_received = True