Merge pull request #64 from clubcapra/following_behavior

Following behavior
clubcapra · Jun 22, 2024 · 604dd0f · 604dd0f
2 parents 0f207fd + 2186266
commit 604dd0f
Show file tree

Hide file tree

Showing 19 changed files with 165 additions and 217 deletions.
diff --git a/src/rove_behavior/bt_xml/patrolling.xml b/src/rove_behavior/bt_xml/patrolling.xml
diff --git a/src/rove_behavior/package.xml b/src/rove_behavior/package.xml
diff --git a/src/rove_behavior/resource/rove_behavior b/src/rove_behavior/resource/rove_behavior
diff --git a/src/rove_behavior/setup.cfg b/src/rove_behavior/setup.cfg
diff --git a/src/rove_behavior/setup.py b/src/rove_behavior/setup.py
diff --git a/src/rove_behavior/test/test_copyright.py b/src/rove_behavior/test/test_copyright.py
diff --git a/src/rove_behavior/test/test_flake8.py b/src/rove_behavior/test/test_flake8.py
diff --git a/src/rove_behavior/test/test_pep257.py b/src/rove_behavior/test/test_pep257.py
diff --git a/src/rove_bringup/launch/sim.launch.py b/src/rove_bringup/launch/sim.launch.py
@@ -8,6 +8,7 @@
 from launch.substitutions import Command
 from launch_ros.actions import Node
 from launch_ros.parameter_descriptions import ParameterValue
+from math import pi
 
 
 def generate_launch_description():
@@ -56,18 +57,22 @@ def generate_launch_description():
         output='screen',
     )
 
+    yaw = -pi / 2
+
     # Spawn robot
     spawn_rove = Node(
-        package='ros_gz_sim',
-        executable='create',
-        arguments=['-name', 'rove',
-                   '-topic', 'robot_description',
-                   '-x', '-2',
-                   '-y', '0',
-                   '-z', '0.1',
-                   ],
-        output='screen',
-    )
+    package='ros_gz_sim',
+    executable='create',
+    arguments=[
+        '-name', 'rove',
+        '-topic', 'robot_description',
+        '-x', '0',
+        '-y', '0',
+        '-z', '0.1',
+        '-Y', str(yaw),
+    ],
+    output='screen',
+)
 
     # Takes the description and joint angles as inputs and publishes
     # the 3D poses of the robot links
@@ -84,7 +89,7 @@ def generate_launch_description():
 
     # fake human tracker
     human_tracker = Node(
-            package='rove_behavior',
+            package='rove_navigation',
             executable='green_person_tracker',
             name='green_person_tracker',
             output='screen',

diff --git a/src/rove_description/worlds/actor.sdf b/src/rove_description/worlds/actor.sdf
@@ -32,28 +32,54 @@
         </animation>
         <script>
             <trajectory id="0" type="walking">
+                <!-- Start position, initially facing West (starting to move West) -->
                 <waypoint>
-                    <time>0</time> <!-- Move right -->
-                    <pose>2 -9 1 0 0 -1.57</pose>
+                    <time>0</time>
+                    <pose>2 -9 1 0 0 3.14</pose> <!-- Facing West -->
+                </waypoint>
+
+                <!-- Move West -->
+                <waypoint>
+                    <time>4</time>
+                    <pose>-2 -9 1 0 0 3.14</pose>
+                </waypoint>
+
+                <!-- Turn to face South before moving -->
+                <waypoint>
+                    <time>4.5</time>
+                    <pose>-2 -9 1 0 0 1.57</pose> <!-- Facing South -->
                 </waypoint>
-                <waypoint> <!--Turn
-                    around -->
-                    <time>5</time>
-                    <pose>2 -9 1 0 0 1.57</pose>
+
+                <!-- Move South -->
+                <waypoint>
+                    <time>14</time>
+                    <pose>-2 7 1 0 0 1.57</pose>
                 </waypoint>
-                <waypoint> <!-- Move left -->
-                    <time>25</time>
-                    <pose>2 7 1 0 0 1.57</pose>
+
+                <!-- Turn to face East before moving -->
+                <waypoint>
+                    <time>14.5</time>
+                    <pose>-2 7 1 0 0 0</pose> <!-- Facing East -->
                 </waypoint>
-                <waypoint> <!-- Turn around -->
-                    <time>30</time>
-                    <pose>2 7 1 0 0 -1.57</pose>
+
+                <!-- Move East -->
+                <waypoint>
+                    <time>18.5</time>
+                    <pose>2 7 1 0 0 0</pose>
+                </waypoint>
+
+                <!-- Turn to face North before moving -->
+                <waypoint>
+                    <time>19</time>
+                    <pose>2 7 1 0 0 -1.57</pose> <!-- Facing North -->
                 </waypoint>
+
+                <!-- Move North to complete the square -->
                 <waypoint>
-                    <time>50</time>
+                    <time>29</time>
                     <pose>2 -9 1 0 0 -1.57</pose>
                 </waypoint>
-            </trajectory>
+            </trajectory>        
         </script>
     </actor>
 </sdf>
diff --git a/src/rove_navigation/config/bt_waypoints.xml b/src/rove_navigation/config/bt_waypoints.xml
diff --git a/src/rove_navigation/launch/navigation.launch.py b/src/rove_navigation/launch/navigation.launch.py
@@ -6,6 +6,8 @@
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import PathJoinSubstitution
 from launch_ros.substitutions import FindPackageShare
+
+from launch_ros.actions import Node
 from launch_ros.actions import SetRemap
 
 def generate_launch_description():
@@ -17,6 +19,18 @@ def generate_launch_description():
 
     nav2_config_path = os.path.join(pkg_rove_navigation, 'config',
                                     'rove_nav_params.yaml')
+
+    bt_xml_path = PathJoinSubstitution(
+        [pkg_rove_navigation, 'config', 'follow_dynamic_point.xml']
+    )
+
+    person_following_node = Node(
+            package="rove_navigation",
+            executable='person_following',
+            name='person_following',
+            output='screen',
+        )
+
 
     nav = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(navigation_launch_path),
@@ -27,6 +41,7 @@ def generate_launch_description():
     )
 
     return LaunchDescription([
+        person_following_node,
         GroupAction(
             actions=[
                 SetRemap(src='cmd_vel', dst='nav_vel'),

diff --git a/src/rove_behavior/rove_behavior/__init__.py → ...tion/rove_navigation/behavior/__init__.py b/src/rove_behavior/rove_behavior/__init__.py → ...tion/rove_navigation/behavior/__init__.py
diff --git a/...rove_behavior/rove_behavior/navigation.py → ...on/rove_navigation/behavior/navigation.py b/...rove_behavior/rove_behavior/navigation.py → ...on/rove_navigation/behavior/navigation.py
@@ -6,6 +6,11 @@
 from rclpy.action import ActionClient
 from nav2_msgs.action import NavigateToPose
 
+import numpy as np
+from geometry_msgs.msg import PointStamped
+
+from rclpy.node import Node
+
 
 class GetLocationFromQueue(py_trees.behaviour.Behaviour):
     """Gets a location name from the queue"""

diff --git a/...behavior/rove_behavior/square_behavior.py → ..._navigation/behavior_square_pattroling.py b/...behavior/rove_behavior/square_behavior.py → ..._navigation/behavior_square_pattroling.py
@@ -20,7 +20,7 @@
 from py_trees.common import OneShotPolicy
 from ament_index_python.packages import get_package_share_directory
 
-from rove_behavior.navigation import GoToPose, GetLocationFromQueue
+from rove_navigation.behavior.navigation import GoToPose, GetLocationFromQueue
 
 
 class AutonomyBehavior(Node):
@@ -54,6 +54,7 @@ def create_naive_tree(self):
         for loc in self.loc_list:
             pose = self.locations[loc]
             seq.add_child(GoToPose(f"go_to_{loc}", pose, self))
+            self.get_logger().info(f"Added GoToPose node for {loc} with pose {pose}")
 
         return tree
 
@@ -71,4 +72,4 @@ def main(args=None):
     rclpy.shutdown()
 
 if __name__ == "__main__":
-    main()
+    main()