Syncing develop with main

.env

@@ -1,5 +1,5 @@
 PROJECT_NAME="airstack"
-PROJECT_VERSION="1.0.3"
+PROJECT_VERSION="1.0.4"
 # can replace with your docker hub username
 PROJECT_DOCKER_REGISTRY="airlab-storage.andrew.cmu.edu:5001/shared"
 DEFAULT_ISAAC_SCENE="omniverse://airlab-storage.andrew.cmu.edu:8443/Projects/AirStack/fire_academy.scene.usd"

ground_control_station/docker/docker-compose.yaml

@@ -1,52 +1,24 @@
 services:
   ground-control-station:
-    image: &gcs_image ${PROJECT_DOCKER_REGISTRY}/${PROJECT_NAME}:v${PROJECT_VERSION}_gcs
-    build:
-      context: ../
-      dockerfile: docker/Dockerfile.gcs
-      tags:
-        - *gcs_image
-    container_name: ground-control-station
-    entrypoint: ""
-    command: >
-      bash -c "ssh service restart;
-      tmux new -d -s gcs_bringup
-      && tmux send-keys -t gcs_bringup 
-      'if [ ! -f "/root/ros_ws/install/setup.bash" ]; then bws && sws; fi;
-      ros2 launch gcs_bringup gcs.launch.xml' ENTER
-      && sleep infinity"
-    # Interactive shell
-    stdin_open: true # docker run -i
-    tty: true # docker run -t
-    # Needed to display graphical applications
-    # ipc: host
-    privileged: true
+    profiles:
+      - ""
+      - sitl
+    extends:
+      file: ./ground-control-station-base-docker-compose.yaml
+      service: ground-control-station-base
     networks:
       - airstack_network
-    environment:
-      - DISPLAY
-      - QT_X11_NO_MITSHM=1
-    deploy:
-      # let it use the GPU
-      resources:
-        reservations:
-          devices:
-            - driver: nvidia # https://stackoverflow.com/a/70761193
-              count: 1
-              capabilities: [gpu]
     ports:
       - 2222:22 # for ssh
-    volumes:
-      # display stuff
-      - $HOME/.Xauthority:/root/.Xauthority
-      - /tmp/.X11-unix:/tmp/.X11-unix
-      # developer stuff
-      - .bashrc:/root/.bashrc:rw # bash config
-      - /var/run/docker.sock:/var/run/docker.sock # access docker API for container name
-      # autonomy stack stuff
-      - ../../common/ros_packages:/root/ros_ws/src/common:rw # common ROS packages
-      - ../ros_ws:/root/ros_ws:rw # gcs-specific ROS packages
-      - ../../common/ros_packages/fastdds.xml:/root/ros_ws/fastdds.xml:rw # fastdds.xml
+
+  ground-control-station-real:
+    profiles:
+      - hitl
+      - deploy
+    extends:
+      file: ./ground-control-station-base-docker-compose.yaml
+      service: ground-control-station-base
+    network_mode: host
 
 # include: 
   # - ./tak-docker-compose.yaml

ground_control_station/docker/ground-control-station-base-docker-compose.yaml

@@ -0,0 +1,46 @@
+# docker compose file
+services:
+  ground-control-station-base:
+    image: &gcs_image ${PROJECT_DOCKER_REGISTRY}/${PROJECT_NAME}:v${PROJECT_VERSION}_gcs
+    build:
+      context: ../
+      dockerfile: docker/Dockerfile.gcs
+      tags:
+        - *gcs_image
+    container_name: ground-control-station
+    entrypoint: ""
+    command: >
+      bash -c "ssh service restart;
+      tmux new -d -s gcs_bringup
+      && tmux send-keys -t gcs_bringup 
+      'if [ ! -f "/root/ros_ws/install/setup.bash" ]; then bws && sws; fi;
+      ros2 launch gcs_bringup gcs.launch.xml' ENTER
+      && sleep infinity"
+    # Interactive shell
+    stdin_open: true # docker run -i
+    tty: true # docker run -t
+    # Needed to display graphical applications
+    # ipc: host
+    privileged: true
+    environment:
+      - DISPLAY
+      - QT_X11_NO_MITSHM=1
+    deploy:
+      # let it use the GPU
+      resources:
+        reservations:
+          devices:
+            - driver: nvidia # https://stackoverflow.com/a/70761193
+              count: 1
+              capabilities: [gpu]
+    volumes:
+      # display stuff
+      - $HOME/.Xauthority:/root/.Xauthority
+      - /tmp/.X11-unix:/tmp/.X11-unix
+      # developer stuff
+      - .bashrc:/root/.bashrc:rw # bash config
+      - /var/run/docker.sock:/var/run/docker.sock # access docker API for container name
+      # autonomy stack stuff
+      - ../../common/ros_packages:/root/ros_ws/src/common:rw # common ROS packages
+      - ../ros_ws:/root/ros_ws:rw # gcs-specific ROS packages
+      - ../../common/ros_packages/fastdds.xml:/root/ros_ws/fastdds.xml:rw # fastdds.xml

ground_control_station/ros_ws/src/gcs_bringup/config/domain_bridge.yaml

@@ -1,11 +1,26 @@
 name: my_bridge
-from_domain: 0
-to_domain: 1
 topics:
   /robot_1/behavior/behavior_tree_commands:
     type: behavior_tree_msgs/msg/BehaviorTreeCommands
+    from_domain: 0
+    to_domain: 1
+  /robot_2/behavior/behavior_tree_commands:
+    type: behavior_tree_msgs/msg/BehaviorTreeCommands
+    from_domain: 0
+    to_domain: 2
+  /robot_3/behavior/behavior_tree_commands:
+    type: behavior_tree_msgs/msg/BehaviorTreeCommands
+    from_domain: 0
+    to_domain: 3
   /robot_1/fixed_trajectory_generator/fixed_trajectory_command:
     type: airstack_msgs/msg/FixedTrajectory
-  /robot_1/behavior/behavior_tree_graphviz:
-    type: std_msgs/msg/String
-    reversed: True
+    from_domain: 0
+    to_domain: 1
+  /robot_2/fixed_trajectory_generator/fixed_trajectory_command:
+    type: airstack_msgs/msg/FixedTrajectory
+    from_domain: 0
+    to_domain: 2
+  /robot_3/fixed_trajectory_generator/fixed_trajectory_command:
+    type: airstack_msgs/msg/FixedTrajectory
+    from_domain: 0
+    to_domain: 3

robot/docker/.bashrc

@@ -176,5 +176,18 @@ CONTAINER_PREFIX="airstack-"
 export ROBOT_NAME=$(echo "$container_name" | sed "s#/$CONTAINER_PREFIX##" | sed 's#-#_#')
 export ROS_DOMAIN_ID=$(echo "$ROBOT_NAME" | awk -F'_' '{print $NF}')
 
+if [ "$ROBOT_NAME" == "null" ]; then
+    num=$(hostname | awk -F'-' '{print $2}') # get number from hostname
+    num=$((num)) #remove leading zeros
+
+    if [[ "$num" == 0 ]]; then
+	export ROBOT_NAME="ERROR"
+	export ROS_DOMAIN_ID="ERROR"
+    else
+	export ROBOT_NAME="robot_$num"
+	export ROS_DOMAIN_ID=$num
+    fi
+fi
+
 export RCUTILS_COLORIZED_OUTPUT=1  # get colored output from ROS2 tools
 

robot/docker/Dockerfile.robot

@@ -120,7 +120,18 @@ RUN sed -i 's/#PasswordAuthentication yes/PasswordAuthentication yes/' /etc/ssh/
 RUN sed 's@session\s*required\s*pam_loginuid.so@session optional pam_loginuid.so@g' -i /etc/pam.d/sshd
 
 EXPOSE 22
-
+
+ARG REAL_ROBOT=false
+RUN if [ "$REAL_ROBOT"  = "true" ]; then \
+  # Put commands here that should run for the real robot but not the sim
+
+  echo "REAL_ROBOT is true"; \
+  apt-get update && apt-get install -y libimath-dev; \
+else \
+  # Put commands here that should be run for the sim but not the real robot
+
+  echo "REAL_ROBOT is false"; \
+fi
 
 # Cleanup. Prevent people accidentally doing git commits as root in Docker
 RUN apt purge git -y \

robot/docker/docker-compose.yaml

@@ -17,6 +17,14 @@ services:
         BASE_IMAGE: ubuntu:22.04
       tags:
         - *desktop_image
+    # we use tmux send-keys so that the session stays alive
+    command: >
+      bash -c "ssh service restart;
+      tmux new -d -s robot_bringup
+      && tmux send-keys -t robot_bringup 
+      'if [ ! -f "/root/ros_ws/install/setup.bash" ]; then bws && sws; fi;
+      ros2 launch robot_bringup robot.launch.xml' ENTER
+      && sleep infinity"
     # assumes you're connected to work internet, so creates a network to isolate from other developers on your work internet
     networks:
       - airstack_network
@@ -38,8 +46,18 @@ services:
     build:
       dockerfile: ./Dockerfile.robot
       args:
-        BASE_IMAGE: nvcr.io/nvidia/l4t-pytorch:r35.2.1-pth2.0-py3
+        BASE_IMAGE: nvcr.io/nvidia/l4t-jetpack:r36.4.0
+        REAL_ROBOT: true
       tags:
         - *l4t_image
+    # we use tmux send-keys so that the session stays alive
+    command: >
+      bash -c "ssh service restart;
+      tmux new -d -s robot_bringup
+      && tmux send-keys -t robot_bringup 
+      'if [ ! -f "/root/ros_ws/install/setup.bash" ]; then bws && sws; fi;
+      ros2 launch robot_bringup robot.launch.xml sim:="false" ' ENTER
+      && sleep infinity"
+    runtime: nvidia
     # assumes network isolation via a physical router, so uses network_mode=host
     network_mode: host

robot/docker/robot-base-docker-compose.yaml

@@ -2,14 +2,6 @@
 services:
   robot_base:
     entrypoint: ""
-    # we use tmux send-keys so that the session stays alive
-    command: >
-      bash -c "ssh service restart;
-      tmux new -d -s robot_bringup
-      && tmux send-keys -t robot_bringup 
-      'if [ ! -f "/root/ros_ws/install/setup.bash" ]; then bws && sws; fi;
-      ros2 launch robot_bringup robot.launch.xml' ENTER
-      && sleep infinity"
     # Interactive shell
     stdin_open: true # docker run -i
     tty: true # docker run -t

robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/src/trajectory_library.cpp

@@ -483,12 +483,12 @@ bool Trajectory::get_waypoint_sphere_intersection(double initial_time, double ah
             wp_end = wp_start.interpolate(
                 wp_end, (ahead_distance - current_path_distance) / segment_distance);
             if (end_waypoint != NULL) *end_waypoint = wp_end;
-        } else if (wp_end.get_time() > time_end) {
+        }/* else if (wp_end.get_time() > time_end) {
             should_break = true;
             wp_end = wp_start.interpolate(wp_end, (time_end - wp_start.get_time()) /
                                                       (wp_end.get_time() - wp_start.get_time()));
             if (end_waypoint != NULL) *end_waypoint = wp_end;
-        } else
+        }*/ else
             current_path_distance += segment_distance;
 
         // sphere line intersection equations:

robot/ros_ws/src/autonomy/3_local/local_bringup/launch/local.launch.xml

@@ -60,7 +60,7 @@
         <param name="tracking_point_distance_limit" value="10.5" />
         <param name="velocity_look_ahead_time" value="0.9" />
         <!-- look ahead time controls the speed, greater is faster -->
-        <param name="look_ahead_time" value="2.0" />
+        <param name="look_ahead_time" value="1.0" />
         <param name="virtual_tracking_ahead_time" value="0.5" />
         <param name="min_virtual_tracking_velocity" value="0.1" />
         <param name="sphere_radius" value="1." />
@@ -83,4 +83,4 @@
     output="screen" respawn="true" respawn_delay="1" />
   ?>
 
-</launch>
+</launch>

robot/ros_ws/src/autonomy/5_behavior/behavior_bringup/launch/behavior.launch.xml

@@ -1,12 +1,6 @@
 <launch>
-    <!-- GUI interface -->
     <group>
         <push-ros-namespace namespace="behavior" />
-        <node pkg="rqt_gui" exec="rqt_gui"
-            args="--perspective-file $(find-pkg-share robot_bringup)/config/core.perspective">
-            <remap from="fixed_trajectory_command"
-                to="/$(env ROBOT_NAME)/fixed_trajectory_generator/fixed_trajectory_command" />
-        </node>
 
         <!-- Behavior Tree -->
         <node pkg="behavior_tree" exec="behavior_tree_implementation">

robot/ros_ws/src/robot_bringup/launch/robot.launch.xml

@@ -1,9 +1,35 @@
 <!-- ROBOT -->
 <launch>
+
+  <arg name="sim" default="true" />
+
   <push_ros_namespace namespace="$(env ROBOT_NAME)" />
 
-  <!-- TODO: parameterize this -->
-  <set_parameter name="use_sim_time" value="true" />
+  <set_parameter name="use_sim_time" value="true" if="$(var sim)" />
+
+  <!-- Sim -->
+  <group if="$(var sim)">
+    <!-- GUI -->
+    <node pkg="rviz2" exec="rviz2"
+	  args="-d $(find-pkg-share robot_bringup)/rviz/robot.rviz --ros-args --log-level INFO"
+	  output="screen" respawn="true" respawn_delay="1" />
+
+    <push-ros-namespace namespace="behavior" />
+    <node pkg="rqt_gui" exec="rqt_gui"
+          args="--perspective-file $(find-pkg-share robot_bringup)/config/core.perspective">
+      <remap from="fixed_trajectory_command"
+             to="/$(env ROBOT_NAME)/fixed_trajectory_generator/fixed_trajectory_command" />
+    </node>
+  </group>
+  <!-- Real -->
+  <group unless="$(var sim)">
+    <push-ros-namespace namespace="interface" />
+    <include file="$(find-pkg-share mavros)/launch/apm.launch">
+      <arg name="fcu_url" value="/dev/ttyTHS1:115200" />
+    </include>
+  </group>
+
+
 
   <!-- Static TFs -->
   <include file="$(find-pkg-share robot_bringup)/launch/static_transforms.launch.xml" />
@@ -14,14 +40,9 @@
   <!-- Logging -->
   <include file="$(find-pkg-share logging_bringup)/launch/logging.launch.xml" />
 
-  <!-- GUI -->
-
-  <node pkg="rviz2" exec="rviz2"
-    args="-d $(find-pkg-share robot_bringup)/rviz/robot.rviz --ros-args --log-level INFO"
-    output="screen" respawn="true" respawn_delay="1" />
-
+  <!-- Domain Bridge -->
   <node pkg="domain_bridge" exec="domain_bridge"
     args="/root/ros_ws/src/robot_bringup/params/domain_bridge.yaml"
     output="screen" respawn="true" respawn_delay="1" />
 
-</launch>
+</launch>

robot/ros_ws/src/robot_bringup/params/domain_bridge.yaml

@@ -44,6 +44,11 @@ topics:
     from_domain: 2
     to_domain: 1
 
+  /robot_3/behavior/behavior_tree_graphviz:
+    type: std_msgs/msg/String
+    from_domain: 3
+    to_domain: 0
+
   # Bridge "/clock" topic from doman ID 2 to domain ID 3,
   # Override durability to be 'volatile' and override depth to be 1
   clock: