Merge branch 'main' into ui

.github/workflows/build.yml

@@ -3,7 +3,7 @@ run-name: ${{ github.actor }} is working on ROS2 rove build
 on: [push]
 jobs:
   Build_rove:
-    runs-on: [self-hosted, linux, docker]
+    runs-on: ubuntu-latest
     permissions:
       packages: write
       contents: read

src/rove_bringup/config/test.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<root BTCPP_format="4"
+      main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <Delay delay_msec="2000">
+      <Nav2Client goal="-0.579;-1.341;0.0;1.0"/>
+    </Delay>
+  </BehaviorTree>
+
+  <BehaviorTree ID="MoveRobot">
+    <Sequence name="SetGoal">
+      <Nav2Client goal="{target}"/>
+    </Sequence>
+  </BehaviorTree>
+
+  <!-- Description of Node Models (used by Groot) -->
+  <TreeNodesModel>
+    <Action ID="Nav2Client"
+            editable="true">
+      <input_port name="goal"/>
+    </Action>
+  </TreeNodesModel>
+
+</root>

src/rove_navigation/config/twist_mux.yaml → src/rove_bringup/config/twist_mux.yaml

@@ -1,11 +1,15 @@
-twist_mux:
-  ros_parameters:
+/twist_mux:
+  ros__parameters:
     topics:
       navigation:
-        topic: cmd_vel
+        topic: nav_vel
         timeout: 0.5
         priority: 10
       joystick:
-        topic: cmd_vel_joy
+        topic: joy_vel
         timeout: 0.5
         priority: 20
+      others:
+        topic: cmd_vel
+        timeout: 0.5
+        priority: 1

src/rove_bringup/launch/autonomy.launch.py

@@ -24,7 +24,7 @@ def generate_launch_description():
             os.path.join(slam_pkg_path, "launch", "online_async_launch.py"),
         ),
         launch_arguments={
-            "use_sim_time": "true",
+            "use_sim_time": use_sim_time,
             "slam_params_file": os.path.join(
                 pkg_rove_slam, "config", "slam_config.yaml"
             )
@@ -34,7 +34,7 @@ def generate_launch_description():
 
     slam3d = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
-            os.path.join(pkg_rove_slam, "launch", "slam3d_full.launch.py"),
+            os.path.join(pkg_rove_slam, "launch", "velodyne_3d.launch.py"),
         ),
         launch_arguments={
             "use_sim_time": use_sim_time,

src/rove_bringup/launch/behavior.launch.py

@@ -0,0 +1,24 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions.node import Node
+
+def generate_launch_description():
+    # Path
+    bt_xml_dir = os.path.join(get_package_share_directory('rove_bringup'), 'config')
+
+    # Parameters
+    bt_xml = LaunchConfiguration('bt_xml', default=bt_xml_dir+'/test.xml')
+
+    behavior_tree = Node(
+        package='bt_ros2',
+        executable='bt_ros2',
+        parameters=[{'bt_xml': bt_xml}],
+        output='screen'
+    )
+
+    ld = LaunchDescription()
+    ld.add_action(behavior_tree)
+    return ld

src/rove_bringup/launch/common.launch.py

@@ -97,12 +97,22 @@ def generate_launch_description():
             "use_slam3d": "true",
         }.items(),
     )
+
+    twist_mux = Node(
+      package='twist_mux',
+      executable='twist_mux',
+      output='screen',
+      parameters=[os.path.join(pkg_rove_bringup, 'config/twist_mux.yaml')],
+      remappings={
+          ('/cmd_vel_out', '/diff_drive_controller/cmd_vel_unstamped')
+      })
 
     return LaunchDescription([
             robot_state_publisher,
             robot_localization_node_local,
-            #robot_localization_node_global,
+            # robot_localization_node_global,
             #navsat_transform,
+            twist_mux,
             rviz,
             teleop,
             autonomy,

src/rove_bringup/launch/real.launch.py

@@ -131,7 +131,7 @@ def create_controller_node(node_name:str):
     zed = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             os.path.join(pkg_rove_zed, 'launch', 'zed_mapping.launch.py'),
-        )
+        ),
     )
 
     return LaunchDescription([

src/rove_bringup/launch/rove_controller_bluetooth.launch.py

@@ -25,7 +25,7 @@ def generate_launch_description():
             parameters=[teleop_joy_params_file],
             remappings=[
                 ('/joy', '/joy'),
-                # ('/cmd_vel', '/model/rove/cmd_vel')
+                ('/cmd_vel', '/joy_vel'),
             ],
         ),
         Node(

src/rove_bringup/launch/rove_controller_usb.launch.py

@@ -25,6 +25,7 @@ def generate_launch_description():
             parameters=[teleop_joy_params_file],
             remappings=[
                 ('/joy', '/joy'),
+                ('/cmd_vel', '/joy_vel'),
             ],
         ),
         Node(

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():
@@ -31,19 +32,48 @@ def generate_launch_description():
         launch_arguments={'gz_args': "-v 4 -r " + world}.items(),
     )
 
-    # Spawn robot
-    create = Node(
+    walls_file_path = os.path.join(pkg_rove_description, 'worlds', 'walls.sdf')
+    spawn_walls = Node(
         package='ros_gz_sim',
         executable='create',
-        arguments=['-name', 'rove',
-                   '-topic', 'robot_description',
+        arguments=['-file', walls_file_path,
+                   '-name', 'walls',
                    '-x', '0',
                    '-y', '0',
-                   '-z', '0.1',
-                   ],
+                   '-z', '0'],
         output='screen',
     )
 
+    actor_file_path = os.path.join(pkg_rove_description, 'worlds', 'actor.sdf')
+    spawn_actor = Node(
+        package='ros_gz_sim',
+        executable='create',
+        arguments=['-file', actor_file_path,
+                   '-name', 'actor',
+                   '-topic', 'actor_pose',
+                   '-x', '0',
+                   '-y', '0',
+                   '-z', '0.1'],
+        output='screen',
+    )
+
+    yaw = -pi / 2
+
+    # Spawn robot
+    spawn_rove = Node(
+    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
     robot_state_publisher = Node(
@@ -57,6 +87,14 @@ def generate_launch_description():
         ]
     )
 
+    # fake human tracker
+    human_tracker = Node(
+            package='rove_navigation',
+            executable='green_person_tracker',
+            name='green_person_tracker',
+            output='screen',
+    )
+
     # Bridge ROS topics and Gazebo messages for establishing communication
     bridge = Node(
         package='ros_gz_bridge',
@@ -82,7 +120,10 @@ def generate_launch_description():
     return LaunchDescription([
             gz_sim,
             bridge,
+            spawn_walls,
+            spawn_actor,
             robot_state_publisher,
-            create,
+            spawn_rove,
             common,
+            human_tracker,
             ])

src/rove_bringup/package.xml

@@ -9,12 +9,14 @@
 
   <build_depend>joy</build_depend>
   <build_depend>teleop_twist_joy</build_depend>
+  <build_depend>twist_mux</build_depend>
   <build_depend>std_msgs</build_depend>
   <build_depend>geometry_msgs</build_depend>
   <build_depend>control_msgs</build_depend>
 
   <exec_depend>joy</exec_depend>
   <exec_depend>teleop_twist_joy</exec_depend>
+  <exec_depend>twist_mux</exec_depend>
   <exec_depend>control_msgs</exec_depend>
   <exec_depend>foxglove_bridge</exec_depend>