bt fixed

mgonzs13 · Dec 16, 2023 · 12abacc · 12abacc
1 parent e6b0c02
commit 12abacc
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 36 deletions.
diff --git a/rover_gazebo/launch/gazebo.launch.py b/rover_gazebo/launch/gazebo.launch.py
@@ -111,7 +111,8 @@ def generate_launch_description():
         name="rviz",
         package="rviz2",
         executable="rviz2",
-        arguments=["-d", rviz_config],
+        arguments=["-d", rviz_config,
+                   "--ros-args", "--log-level", "Error"],
         parameters=[{"use_sim_time": True}],
         condition=IfCondition(PythonExpression([launch_rviz])),
     )

diff --git a/rover_localization/launch/rtabmap.launch.py b/rover_localization/launch/rtabmap.launch.py
@@ -123,11 +123,12 @@ def generate_launch_description():
         Node(
             package="rtabmap_slam",
             executable="rtabmap",
-            output="screen",
+            output="log",
             parameters=parameters,
             remappings=remappings,
             arguments=["-d",
-                       "--ros-args", "--log-level", "Warn"]),
+                       "--ros-args", "--log-level", "Error"]
+        ),
 
         Node(
             condition=IfCondition(launch_rtabmapviz),

diff --git a/rover_navigation/behavior_trees/rover_bt.xml b/rover_navigation/behavior_trees/rover_bt.xml
@@ -1,37 +1,18 @@
 <root main_tree_to_execute="MainTree">
   <BehaviorTree ID="MainTree">
-    <RecoveryNode number_of_retries="6" name="NavigateRecovery">
-      <PipelineSequence>
-        <RateController hz="1.0" name="RateControllerComputePathToPose">
-          <RecoveryNode number_of_retries="1" name="RecoveryComputePathToPose">
-            <Fallback name="FallbackComputePathToPose">
-              <ReactiveSequence name="CheckIfNewPathNeeded">
-                <Inverter>
-                  <GlobalUpdatedGoal/>
-                </Inverter>
-                <IsPathValid path="{path}"/>
-              </ReactiveSequence>
-              <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
-            </Fallback>
-            <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
-          </RecoveryNode>
-        </RateController>
-        <RecoveryNode number_of_retries="1" name="RecoveryFollowPath">
-          <FollowPath path="{path}" controller_id="FollowPath"/>
-          <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
-        </RecoveryNode>
-      </PipelineSequence>
-      <ReactiveFallback name="FallbackRecoveries">
-        <GoalUpdated/>
-        <RoundRobin name="RecoveryActions">
-          <Sequence name="ClearingActions">
-            <ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
-            <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
-          </Sequence>
-          <Wait name="WaitRecovery" wait_duration="5"/>
-          <BackUp name="BackUpRecovery" backup_dist="0.1" backup_speed="0.05"/>
-        </RoundRobin>
-      </ReactiveFallback>
-    </RecoveryNode>
+    <PipelineSequence name="NavigateWithReplanning">
+      <RateController hz="1.0">
+        <Fallback> 
+          <ReactiveSequence>
+            <Inverter>
+              <GlobalUpdatedGoal/>
+            </Inverter>
+            <IsPathValid path="{path}"/>
+          </ReactiveSequence>
+          <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
+        </Fallback>
+      </RateController>
+      <FollowPath path="{path}" controller_id="FollowPath"/>
+    </PipelineSequence>
   </BehaviorTree>
 </root>