Aj/diff launch configs

.env

@@ -30,28 +30,26 @@ ROBOT_LAUNCH_PACKAGE="robot_bringup"
 ROBOT_LAUNCH_FILE="robot.launch.xml"
 
 # See robot-base-docker-compose.yaml for how these variables get propagated. 
-# We use relative launch path (not file) because these are launch files get included
-# from the top level robot launch file, and the include-format requires a path.
 AUTONOMY_LAUNCH_PACKAGE="autonomy_bringup"
-AUTONOMY_LAUNCH_PATH="launch/autonomy.launch.xml"
+AUTONOMY_LAUNCH_FILE="autonomy.launch.xml"
 # --
 INTERFACE_LAUNCH_PACKAGE="interface_bringup"
-INTERFACE_LAUNCH_PATH="launch/interface.launch.xml"
+INTERFACE_LAUNCH_FILE="interface.launch.xml"
 # --
 SENSORS_LAUNCH_PACKAGE="sensors_bringup"
-SENSORS_LAUNCH_PATH="launch/sensors.launch.xml"
+SENSORS_LAUNCH_FILE="sensors.launch.xml"
 # --
 PERCEPTION_LAUNCH_PACKAGE="perception_bringup"
-PERCEPTION_LAUNCH_PATH="launch/perception.launch.xml"
+PERCEPTION_LAUNCH_FILE="perception.launch.xml"
 # --
 LOCAL_LAUNCH_PACKAGE="local_bringup"
-LOCAL_LAUNCH_PATH="launch/local.launch.xml"
+LOCAL_LAUNCH_FILE="local.launch.xml"
 # --
 GLOBAL_LAUNCH_PACKAGE="global_bringup"
-GLOBAL_LAUNCH_PATH="launch/global.launch.xml"
+GLOBAL_LAUNCH_FILE="global.launch.xml"
 # --
 BEHAVIOR_LAUNCH_PACKAGE="behavior_bringup"
-BEHAVIOR_LAUNCH_PATH="launch/behavior.launch.xml"
+BEHAVIOR_LAUNCH_FILE="behavior.launch.xml"
 # ===============================================
 
 # =========== GROUND CONTROL STATION ============

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

@@ -12,33 +12,33 @@ services:
       - QT_X11_NO_MITSHM=1
       # docker compose interpolation to env variables
       - AUTONOMY_LAUNCH_PACKAGE=${AUTONOMY_LAUNCH_PACKAGE}
-      - AUTONOMY_LAUNCH_PATH=${AUTONOMY_LAUNCH_PATH}
+      - AUTONOMY_LAUNCH_FILE=${AUTONOMY_LAUNCH_FILE}
       # --
       - INTERFACE_LAUNCH_PACKAGE=${INTERFACE_LAUNCH_PACKAGE}
-      - INTERFACE_LAUNCH_PATH=${INTERFACE_LAUNCH_PATH}
+      - INTERFACE_LAUNCH_FILE=${INTERFACE_LAUNCH_FILE}
       # --
       - SENSORS_LAUNCH_PACKAGE=${SENSORS_LAUNCH_PACKAGE}
-      - SENSORS_LAUNCH_PATH=${SENSORS_LAUNCH_PATH}
+      - SENSORS_LAUNCH_FILE=${SENSORS_LAUNCH_FILE}
       # --
       - PERCEPTION_LAUNCH_PACKAGE=${PERCEPTION_LAUNCH_PACKAGE}
-      - PERCEPTION_LAUNCH_PATH=${PERCEPTION_LAUNCH_PATH}
+      - PERCEPTION_LAUNCH_FILE=${PERCEPTION_LAUNCH_FILE}
       # --
       - LOCAL_LAUNCH_PACKAGE=${LOCAL_LAUNCH_PACKAGE}
-      - LOCAL_LAUNCH_PATH=${LOCAL_LAUNCH_PATH}
+      - LOCAL_LAUNCH_FILE=${LOCAL_LAUNCH_FILE}
       # --
       - GLOBAL_LAUNCH_PACKAGE=${GLOBAL_LAUNCH_PACKAGE}
-      - GLOBAL_LAUNCH_PATH=${GLOBAL_LAUNCH_PATH}
+      - GLOBAL_LAUNCH_FILE=${GLOBAL_LAUNCH_FILE}
       # --
       - BEHAVIOR_LAUNCH_PACKAGE=${BEHAVIOR_LAUNCH_PACKAGE}
-      - BEHAVIOR_LAUNCH_PATH=${BEHAVIOR_LAUNCH_PATH}
+      - BEHAVIOR_LAUNCH_FILE=${BEHAVIOR_LAUNCH_FILE}
     deploy:
       # let it use the GPU
       resources:
         reservations:
           devices:
             - driver: nvidia # https://stackoverflow.com/a/70761193
               count: 1
-              capabilities: [ gpu ]
+              capabilities: [gpu]
     volumes:
       # display stuff
       - $HOME/.Xauthority:/root/.Xauthority
@@ -49,7 +49,7 @@ services:
       - /var/run/docker.sock:/var/run/docker.sock # access docker API for container name
       - ../../common/inputrc:/etc/inputrc:rw # for using page up/down to search through command history
       # autonomy stack stuff
-      - ../../common/ros_packages:/root/ros_ws/src/common:rw  # common ROS packages
-      - ../../common/ros_packages/fastdds.xml:/root/ros_ws/fastdds.xml:rw  # fastdds.xml
-      - ../ros_ws:/root/ros_ws:rw  # robot-specific ROS packages
+      - ../../common/ros_packages:/root/ros_ws/src/common:rw # common ROS packages
+      - ../../common/ros_packages/fastdds.xml:/root/ros_ws/fastdds.xml:rw # fastdds.xml
+      - ../ros_ws:/root/ros_ws:rw # robot-specific ROS packages
 

robot/ros_ws/src/autonomy/0_interface/interface_bringup/launch/interface.launch.xml

@@ -1,87 +1,57 @@
 <launch>
+    <!-- Args -->
 
-    <!-- the below is a hack to get ground truth TF from Isaac. this is to remove map noise for the
-    sponsor demo. set use_sim_ground_truth_tf to true for demo -->
-    <!-- basically we publish the ground truth base link tf from isaac. we then substitute that to
-	 the base_link tf -->
-    <!--
-    <arg name="use_sim_ground_truth_tf" default="false" />
+    <arg name="odometry_in_topic" default="/$(env ROBOT_NAME)/interface/mavros/local_position/odom" />
 
-    <let name="convert_estimated_odom_to_tf" value="true" unless="$(var use_sim_ground_truth_tf)" />
-    <let name="convert_estimated_odom_to_tf" value="false" if="$(var use_sim_ground_truth_tf)" />
-    -->
+    <!-- Nodes -->
 
     <node pkg="robot_interface" exec="robot_interface_node"
         namespace="interface"
         output="screen">
         <param name="interface" value="mavros_interface::MAVROSInterface" />
         <param name="is_ardupilot" value="true" />
         <param name="post_takeoff_command_delay_time" value="10." />
-	<param name="do_global_pose_command" value="false" />
-
-	<remap from="ardupilot_takeoff" to="/$(env ROBOT_NAME)/takeoff_landing_planner/ardupilot_takeoff" />
-	<remap from="reset_integrators" to="/$(env ROBOT_NAME)/control/reset_integrators" />
+        <param name="do_global_pose_command" value="false" />
+
+        <remap from="ardupilot_takeoff"
+            to="/$(env ROBOT_NAME)/takeoff_landing_planner/ardupilot_takeoff" />
+        <remap from="reset_integrators" to="/$(env ROBOT_NAME)/control/reset_integrators" />
     </node>
 
     <node pkg="mavros_interface" exec="position_setpoint_pub.py"
         namespace="interface"
         output="screen">
-      
-      <param name="command_type" value="2" />
-      <param name="max_velocity" value="3.0" />
-      <param name="target_frame" value="base_link" />
-      <param name="publish_goal" value="false" />
-      
+
+        <param name="command_type" value="2" />
+        <param name="max_velocity" value="3.0" />
+        <param name="target_frame" value="base_link" />
+        <param name="publish_goal" value="false" />
+
     </node>
 
+    <!-- Converts odometry topic into a TF -->
     <node pkg="robot_interface" namespace="odometry_conversion" exec="odometry_conversion"
         output="screen">
-        <remap from="odometry_in" to="/$(env ROBOT_NAME)/interface/mavros/local_position/odom" />
+        <remap from="odometry_in" to="$(var odometry_in_topic)" />
         <remap from="odometry_out" to="odometry" />
 
         <param name="odom_input_qos_is_best_effort" value="true" />
         <param name="new_frame_id" value="map" />
         <param name="new_child_frame_id" value="base_link" />
-        <param name="odometry_output_type" value="2" />
+        <param name="odometry_output_type" value="1" />
 
-	<param name="convert_odometry_to_transform" value="true" />
+        <param name="convert_odometry_to_transform" value="true" />
         <param name="convert_odometry_to_stabilized_transform" value="true" />
         <param name="restamp_now_post" value="false" />
 
-        <!-- no longer publish TF from estimated odom, this is the hack -->
-	<!--
-        <param name="convert_odometry_to_transform" value="$(var convert_estimated_odom_to_tf)" />
-        <param name="convert_odometry_to_stabilized_transform"
-            value="$(var convert_estimated_odom_to_tf)" />
-        <param name="restamp_now_post" value="true" />
-	-->
     </node>
 
-    <!--
-    <node
-        pkg="tf2_ros"
-        exec="static_transform_publisher"
-        name="tf_base_link_is_ground_truth"
-        args="0 0 0 0 0 0 base_link_ground_truth base_link" />
-    -->
-
-    <!-- If using ground truth TF from sim, then create the stabilized TF from the ground truth TF, since 
-	 odometry_conversion now isn't creating this from odometry estimate  -->
-    <!--
-    <group if="$(var use_sim_ground_truth_tf)">
-        <node pkg="robot_interface" exec="stabilized_tf_node" output="screen">
-            <param name="parent_frame" value="map_FLU" />
-            <param name="child_frame" value="base_link" />
-        </node>
-    </group>
-    -->
-
     <node pkg="drone_safety_monitor" exec="drone_safety_monitor"
-          namespace="drone_safety_monitor"
-          output="screen">
-      <param name="state_estimate_timeout" value="1.0" />
-      
-      <remap from="state_estimate" to="/$(env ROBOT_NAME)/odometry_conversion/odometry" />
+        namespace="drone_safety_monitor"
+        output="screen">
+        <param name="state_estimate_timeout" value="1.0" />
+
+        <remap from="state_estimate" to="/$(env ROBOT_NAME)/odometry_conversion/odometry" />
     </node>
-    
-</launch>
+
+</launch>

robot/ros_ws/src/autonomy/0_interface/robot_interface/src/odometry_conversion.cpp

@@ -26,8 +26,8 @@
 /**
  * @brief
  * Does several things
- * - if there's an odometry, republishes it with a new frame_id
- * - if there's an odometry, republishes it as a transform
+ * - if the input odometry is in a different frame, either transform it or overwrite it to new frame_id
+ * - if there's an input odometry, republishes into the tf2 transform tree
  * - converts MAVROS odometry BEST_EFFORT to RELIABLE
  *
  */

robot/ros_ws/src/autonomy/1_sensors/camera_param_server/config/camera_config.yaml

@@ -3,5 +3,5 @@ camera_list:
   - camera_name: "front_stereo"
     camera_type: "stereo"
     camera_info_sub_topic: "camera_info"
-    left_camera_frame_id: "left_camera" # not sure if this is needed because it should be standardized
-    right_camera_frame_id: "right_camera"
+    left_camera_frame_id: "CameraLeft" 
+    right_camera_frame_id: "CameraRight"

robot/ros_ws/src/autonomy/2_perception/MAC-VO-ROS/config/interface_config.yaml

@@ -2,8 +2,7 @@
   ros__parameters:
     camera_name: "front_stereo"
     camera_param_server_client_topic: "sensors/get_camera_params"
-    # coordinate_frame: "robot_origin_ned"
-    coordinate_frame: "map_ned"
+    coordinate_frame: "macvo_ned"
     imageL_sub_topic: "left/image_rect"
     imageR_sub_topic: "right/image_rect"
     pose_pub_topic: "pose"

robot/ros_ws/src/autonomy/2_perception/MAC-VO-ROS/macvo/macvo.py

@@ -40,14 +40,14 @@
 class macvoNode(Node):
     def __init__(self) -> None:
         super().__init__("macvo_node")
-        self.coord_frame = "map_ned"  # FIXME: this is probably wrong? Should be the camera optical center frame
+        self.coord_frame = "macvo_ned"  # Coordinate frame for the camera, in NED (North-East-Down) convention
         self.frame_id = 0      # Frame ID
         self.init_time = None  # ROS2 time stamp
         self.get_logger().set_level(logging.INFO)
         self.get_logger().info(f"{os.getcwd()}")
         self.declared_parameters = set()
 
-        self.coord_frame = self.get_string_param("coordinate_frame")  # FIXME: this is probably wrong? Should be the camera optical center frame
+        self.coord_frame = self.get_string_param("coordinate_frame") 
         self.frame_id = 0  # Frame ID
 
         # Load the Camera model ------------------------------------

robot/ros_ws/src/autonomy/2_perception/perception_bringup/launch/perception.launch.xml

@@ -1,8 +1,19 @@
 <!-- PERCEPTION -->
 <launch>
+
+
     <!-- State and depth estimation -->
+    <!-- Camera is in optical frame. This is Z-forward, x right, y down.  -->
+     <!-- the tf rotation applies in argument order (yaw first, then pitch, then roll) -->
+    <node
+        pkg="tf2_ros"
+        exec="static_transform_publisher"
+        name="static_transform_publisher"
+        args="--x 0 --y 0 --z 0 --yaw 1.5708 --pitch -1.5708 --roll 3.14159 --frame-id CameraLeft --child-frame-id macvo_ned" />
+
     <node pkg="macvo" exec="macvo" namespace="macvo">
         <param name="camera_config" value="$(find-pkg-share macvo)/config/model_config.yaml" />
+        <!-- override -->
         <param from="$(find-pkg-share macvo)/config/interface_config.yaml" />
         <remap from="left/image_rect"
             to="/$(env ROBOT_NAME)/sensors/front_stereo/left/image_rect" />
@@ -11,4 +22,5 @@
         <remap from="sensors/get_camera_params"
             to="/$(env ROBOT_NAME)/sensors/get_camera_params" />
     </node>
+
 </launch>

robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/config/disparity_expansion_params.yaml

@@ -4,7 +4,6 @@
     metric_depth_scale: 1.0  # units size of a meter. depth value corresponds to this many meters
     downsample_scale: 0.5  # the ratio to scale down the disparity image before processing. DROAN expects 960x600. So if the incoming image is 480x300, set this to 0.5 so that it scales up
     baseline_fallback: 0.12 # if the baseline is 0 from the camera_info, use this value instead
-    lut_max_disparity: 180  # maximum disparity value in the disparity image
     # expansion_radius: 0.325  # 0.325 is the spirit drone blade to blade
     expansion_radius: 0.1  # debug hack to make disparity expansion not crash
     bg_multiplier: 1.0