change to behavior to publisher

clubcapra · Jun 22, 2024 · a16d9dc · a16d9dc
1 parent 3dae26f
commit a16d9dc
Show file tree

Hide file tree

Showing 4 changed files with 24 additions and 22 deletions.
diff --git a/src/rove_navigation/config/follow_dynamic_point.xml b/src/rove_navigation/config/follow_dynamic_point.xml
@@ -1,19 +1,17 @@
 <root main_tree_to_execute="MainTree">
     <BehaviorTree ID="MainTree">
-        <PipelineSequence name="NavigateWithReplanning">
-            <ControllerSelector selected_controller="{selected_controller}" default_controller="FollowPath" topic_name="controller_selector"/>
-            <PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased" topic_name="planner_selector"/>
-            <RateController hz="1.0">
-                <Sequence>
-                    <GoalUpdater input_goal="{goal}" output_goal="{updated_goal}">
-                        <ComputePathToPose goal="{updated_goal}" path="{path}" planner_id="{selected_planner}"/>
-                    </GoalUpdater>
-                    <TruncatePath distance="3.0" input_path="{path}" output_path="{truncated_path}"/>
-                </Sequence>
-            </RateController>
-            <KeepRunningUntilFailure>
-                <FollowPath path="{truncated_path}" controller_id="{selected_controller}"/>
-            </KeepRunningUntilFailure>
-        </PipelineSequence>
+      <PipelineSequence name="NavigateWithReplanning">
+        <RateController hz="1.0">
+          <Sequence>
+            <GoalUpdater input_goal="{goal}" output_goal="{updated_goal}">
+              <ComputePathToPose goal="{updated_goal}" path="{path}" planner_id="GridBased"/>
+            </GoalUpdater>
+           <TruncatePath distance="2.0" input_path="{path}" output_path="{truncated_path}"/>
+          </Sequence>
+        </RateController>
+        <KeepRunningUntilFailure>
+          <FollowPath path="{truncated_path}" controller_id="FollowPath"/>
+        </KeepRunningUntilFailure>
+      </PipelineSequence>
     </BehaviorTree>
-</root>
+  </root>
diff --git a/src/rove_navigation/config/rove_nav_params.yaml b/src/rove_navigation/config/rove_nav_params.yaml
@@ -54,7 +54,6 @@ bt_navigator:
     robot_base_frame: base_link
     odom_topic: /odom
     bt_loop_duration: 10
-    default_nav_to_pose_bt_xml: ./src/rove_navigation/config/follow_dynamic_point.xml
     default_server_timeout: 20
     enable_groot_monitoring: true
     groot_zmq_publisher_port: 1666

diff --git a/src/rove_navigation/launch/navigation.launch.py b/src/rove_navigation/launch/navigation.launch.py
@@ -38,11 +38,10 @@ def generate_launch_description():
         launch_arguments={
             'use_sim_time': 'true',
             'params_file': nav2_config_path,
-            'bt_xml_filename': bt_xml_path,
         }.items()
     )
 
     return LaunchDescription([
         nav,
-        navigation_to_person_node,
+        #navigation_to_person_node,
     ])
diff --git a/src/rove_navigation/rove_navigation/navigate_to_person.py b/src/rove_navigation/rove_navigation/navigate_to_person.py
@@ -14,7 +14,12 @@ def __init__(self):
             '/person_position',
             self.navigate_to_person,
             10)
-        self.navigator = BasicNavigator()
+
+        self.goal_update_pub = self.create_publisher(
+            PoseStamped,
+            '/goal_update',
+            10)
+
         self.tf_buffer = Buffer()
         self.tf_listener = TransformListener(self.tf_buffer, self)
 
@@ -39,8 +44,9 @@ def navigate_to_person(self, msg):
             # Log the navigation target for debugging
             self.get_logger().info(f'Navigating to transformed goal: {goal_pose.pose.position.x} {goal_pose.pose.position.y}...')
 
-            # Command the robot to navigate to the goal
-            self.navigator.goToPose(goal_pose)
+            # Publish the goal
+            self.goal_update_pub.publish(goal_pose)
+
 
 def main(args=None):
     rclpy.init(args=args)