Working better

RoboJackets · May 26, 2024 · 36f0268 · 36f0268
1 parent 5b1caa4
commit 36f0268
Show file tree

Hide file tree

Showing 9 changed files with 493 additions and 403 deletions.
diff --git a/mapping/CMakeLists.txt b/mapping/CMakeLists.txt
@@ -59,6 +59,7 @@ rclcpp_components_register_node(
 install(
   DIRECTORY
   config
+  launch
   DESTINATION share/${PROJECT_NAME}/
 )
 

diff --git a/mapping/launch/mapping.launch.py b/mapping/launch/mapping.launch.py
@@ -0,0 +1,27 @@
+from launch import LaunchDescription
+from launch.substitutions import PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    elevation_mapping_node = Node(
+        package="mapping",
+        executable="mapping_ElevationMapping",
+        output="screen",
+        parameters=[
+            PathJoinSubstitution(
+                [
+                    FindPackageShare("mapping"),
+                    "config",
+                    "mapping_params.yaml",
+                ]
+            )
+        ],
+    )
+
+    return LaunchDescription(
+        [
+            elevation_mapping_node,
+        ]
+    )
diff --git a/urc_bringup/launch/bringup_simulation.launch.py b/urc_bringup/launch/bringup_simulation.launch.py
@@ -1,10 +1,13 @@
 import os
 from xacro import process_file
 from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription
-from launch.actions import SetEnvironmentVariable, RegisterEventHandler
-from launch.substitutions import LaunchConfiguration
-from launch.event_handlers import OnProcessExit
+from launch.actions import (
+    IncludeLaunchDescription,
+    SetEnvironmentVariable,
+    RegisterEventHandler,
+)
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.event_handlers import OnProcessExit, OnProcessStart
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
@@ -142,6 +145,21 @@ def generate_launch_description():
         )
     )
 
+    elevation_mapping_node = Node(
+        package="mapping",
+        executable="mapping_ElevationMapping",
+        output="screen",
+        parameters=[
+            PathJoinSubstitution(
+                [
+                    FindPackageShare("mapping"),
+                    "config",
+                    "mapping_params.yaml",
+                ]
+            )
+        ],
+    )
+
     dummy_costmap_publisher = Node(
         package="urc_test", executable="costmap_generator", output="screen"
     )
@@ -155,7 +173,19 @@ def generate_launch_description():
                         load_joint_state_broadcaster,
                         load_drivetrain_controller,
                         teleop_launch,
-                        dummy_costmap_publisher,
+                    ],
+                )
+            ),
+            RegisterEventHandler(
+                event_handler=OnProcessExit(
+                    target_action=load_drivetrain_controller,
+                    on_exit=[elevation_mapping_node],
+                )
+            ),
+            RegisterEventHandler(
+                event_handler=OnProcessStart(
+                    target_action=elevation_mapping_node,
+                    on_start=[
                         path_planning_launch,
                         trajectory_following_launch,
                         bt_launch,

diff --git a/urc_bringup/strategies/bt_test.xml b/urc_bringup/strategies/bt_test.xml
@@ -1,33 +1,30 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <root BTCPP_format="4">
   <BehaviorTree ID="test">
-    <Sequence>
-      <Delay delay_msec="1000">
-        <LogInfo logger="{ros_log}"
-                 message="Trying to Plan Path..."/>
-      </Delay>
-      <Fallback>
-        <Sequence>
+    <RunOnce then_skip="true">
+      <Sequence>
+        <Delay delay_msec="1000">
           <LogInfo logger="{ros_log}"
-                   message="Path Planning Looping..."/>
-          <RetryUntilSuccessful num_attempts="5">
-            <CallGeneratePlan start_pose="0,0,0"
-                              goal_pose="10,10,2"
-                              path="{generated_path}"
-                              service_name="/plan"/>
-          </RetryUntilSuccessful>
-          <LogInfo logger="{ros_log}"
-                   message="Path Planning Successful!"/>
-          <FollowPath path="{generated_path}"
-                      action_name="/follow_path"/>
-        </Sequence>
-        <Sequence>
-          <LogInfo logger="{ros_log}"
-                   message="Nope."/>
-          <CallTrigger service_name="/bt_orchestrator/stop_bt"/>
-        </Sequence>
-      </Fallback>
-    </Sequence>
+                   message="Trying to Plan Path..."/>
+        </Delay>
+        <KeepRunningUntilFailure>
+          <Inverter>
+            <Sequence>
+              <LogInfo logger="{ros_log}"
+                       message="Path Planning Looping..."/>
+              <CallGeneratePlan start_pose="0,0,0"
+                                goal_pose="5,5,2"
+                                path="{generated_path}"
+                                service_name="/plan"/>
+              <LogInfo logger="{ros_log}"
+                       message="Path Planning Successful!"/>
+              <FollowPath path="{generated_path}"
+                          action_name="/follow_path"/>
+            </Sequence>
+          </Inverter>
+        </KeepRunningUntilFailure>
+      </Sequence>
+    </RunOnce>
   </BehaviorTree>
 
   <!-- Description of Node Models (used by Groot) -->
@@ -39,11 +36,6 @@
       <output_port name="path"/>
       <input_port name="service_name"/>
     </Action>
-    <Action ID="CallTrigger"
-            editable="true">
-      <input_port name="service_name"
-                  default="&quot;&quot;">Service name.</input_port>
-    </Action>
     <Action ID="FollowPath"
             editable="true">
       <input_port name="path"/>