Gazebo and Auto fixes during the competition + some cleanup after (#246)

* Add rviz config to client since bandwidth cap was removed

* Ifx namespace conflict
It was possible that the launch configurations could conflict this prevents that form happening

* Update laptop_launch.py

* Update EVERYTHING_launch.py

* fixing apriltag TF timestamp issue

* comp changes

* Autonomy logic (#244)

* Initial Commit

* Added 2 reminders

* Added autonomus_field_type to rovr_control.yaml and coordinates to main_control_node

* Added cycle condition for end_autonomous()

* started optimal_dig_location func

* Changed optimal_dig_location function and location of the function

* theoretical way algorithm to find dig strips in test file

* TF broadcaster improvements

* Swapped the field path to a config file parameter.

* Finish the addition of the ROS param

* more work in test file

* Added  a frame_id_renamer so the apriltag TFs  can be visualized in rviz

* Commented tings out and addded file save output

* fixed robot starting rotated

* Applied an offset so that odom starts at the zed

* Added the basicnavigator

* better starting spot on the field

* reverted an old "fix" and pushing the part that works so far

* current working state

* minor change

* working calibration routine for a parallel zed

* improvements

* delete testing code

* minor change

* remove unused imports

* bring back translation_end _vector for easier accurate positioning

* getting close

* use the known map coordinates as an offset

* no more averaging

* remove unused import

* actually use the orientation of the tags properly

* digging pseudo code

* some cleanup

* adding things

* moving to rov_control

* moving to rovr control + swapping to lineCost

* moving to while loop

* increased this time because our robot is slow

* deleted the weird combine_esdf file

* minor changes

* more correct

* small fix

* things work now

* happy with this nav2 performance

* auto format

* minor improvements

* more improvements

* creating a custom costmap2d object and using that to generate lineCost/ dig locations

* bug fixes

* bug fixes

* Fixing bugs in autuo dig

* Commenting out so it doesnt move to dig right away

* Deleted the testing_auto.py file

* fixes

* final fixes

* fix

* fix

---------

Co-authored-by: Isopod00 <[email protected]>
Co-authored-by: umnrobotics <[email protected]>
Co-authored-by: Braydon Higgins <[email protected]>

* Updating camera info in client_widget

* Some of this needs to be reverted/merged with main

* reverted global frame back to odom and reverted a lot of the gazebo "fixes"

* auto formatting and minor fixes

* Bring calibrate_field_coordinates back to normal

* added back a missing "self.end_autonomous()"

* deleted this "fix" because it didnt actually work

* deleted an old comment

---------

Co-authored-by: Jonathan Blixt <[email protected]>
Co-authored-by: alex berg <[email protected]>
Co-authored-by: umnrobotics <[email protected]>
Co-authored-by: Alex Berg <[email protected]>
Co-authored-by: Braydon Higgins <[email protected]>

config/nav2_isaac_sim.yaml

@@ -19,7 +19,7 @@
 bt_navigator:
   ros__parameters:
     use_sim_time: False
-    global_frame: map
+    global_frame: odom
     robot_base_frame: zed2i_camera_link
     odom_topic: /odom
     bt_loop_duration: 20
@@ -79,7 +79,7 @@ bt_navigator_rclcpp_node:
 
 controller_server:
   ros__parameters:
-    use_sim_time: false
+    use_sim_time: False
     controller_frequency: 10.0
     min_x_velocity_threshold: 0.001
     min_y_velocity_threshold: 0.001
@@ -176,7 +176,7 @@ local_costmap:
     ros__parameters:
       update_frequency: 10.0
       publish_frequency: 10.0
-      global_frame: map
+      global_frame: odom
       robot_base_frame: zed2i_camera_link
       use_sim_time: False
       rolling_window: True
@@ -208,7 +208,7 @@ global_costmap:
     ros__parameters:
       update_frequency: 10.0
       publish_frequency: 10.0
-      global_frame: map
+      global_frame: odom
       robot_base_frame: zed2i_camera_link
       use_sim_time: False
       rolling_window: True
@@ -284,7 +284,7 @@ behavior_server:
       plugin: "nav2_behaviors/DriveOnHeading"
     wait:
       plugin: "nav2_behaviors/Wait"
-    global_frame: map
+    global_frame: odom
     robot_base_frame: zed2i_camera_link
     transform_tolerance: 0.2
     use_sim_time: False

config/nvblox/nvblox_base.yaml

@@ -44,9 +44,9 @@
     # Used for crater detection.
     # one is the floor, two is hole level
     # by going significantly above 'ground', we ignore all the noise from uneven terrain while still detecting obstacles.
-    esdf_above_ground_slice_height: 0.4  # TODO: This might need to be adjusted but it works for large obstacles
-    esdf_underground_detection: -1.0  # TODO: This NEEDS to be adjusted
-    combine_pointcloud: False
+    esdf_above_ground_slice_height: 0.4  # TODO: This might need to be adjusted
+    esdf_underground_detection: -1.0  # TODO: This might need to be adjusted
+    combine_pointcloud: True
     combine_costmap: True
     invert_below_ground: False
     above_ground_denoise_level: 0.5

src/apriltag/apriltag/apriltag_node.py

@@ -125,15 +125,8 @@ def tagDetectionSub(self, msg):
 
     def broadcast_transform(self):
         """Broadcasts the map -> odom transform"""
-        # Lookup the odom to zed2i_camera_link tf from the tf buffer
-        try:
-            odom_to_camera_link_transform = self.tf_buffer.lookup_transform("odom", "zed2i_camera_link", rclpy.time.Time())
-            self.map_transform.header.stamp = odom_to_camera_link_transform.header.stamp
-            self.tf_broadcaster.sendTransform(self.map_transform)
-        except TransformException as ex:
-            self.get_logger().warn(f"Could not transform odom to zed2i_camera_link: {ex}")
-            self.map_transform.header.stamp = self.get_clock().now().to_msg()
-            self.tf_broadcaster.sendTransform(self.map_transform)
+        self.map_transform.header.stamp = self.get_clock().now().to_msg()
+        self.tf_broadcaster.sendTransform(self.map_transform)
 
 
 def main(args=None):

src/gazebo/gazebo_files/worlds/UCF_field.sdf

@@ -31,7 +31,7 @@
 
     <include>
       <uri>package://robot_description/models/master_ASM</uri>
-      <pose>3 1 0.65 0 0.44 0</pose>
+      <pose>3 -1 0.65 0 0.44 0</pose>
       <!-- Global odometer -->
       <plugin
       filename="ignition-gazebo-odometry-publisher-system"

src/gstreamer/launch/laptop_launch.py

@@ -6,6 +6,7 @@
 from launch.actions import DeclareLaunchArgument
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 
+
 # Launches the joystick node and rviz and the gstreamer camera client on the operator laptop
 def generate_launch_description():
     ld = LaunchDescription()
@@ -15,7 +16,7 @@ def generate_launch_description():
     # Config
     config_name = LaunchConfiguration("config_name", default="zed_example.rviz")
     config_path = PathJoinSubstitution([bringup_dir, config_name])
-    global_frame = LaunchConfiguration("global_frame", default="map")
+    global_frame = LaunchConfiguration("global_frame", default="odom")
 
     run_rviz_arg = DeclareLaunchArgument("run_rviz_client", default_value="True", description="Whether to start RVIZ")
 
@@ -39,7 +40,7 @@ def generate_launch_description():
     )
 
     ld.add_action(run_rviz_arg)
-    ld.add_action(joystick_node) # TODO: The joystick node currently does not work inside the container!
+    ld.add_action(joystick_node)  # TODO: The joystick node currently does not work inside the container!
     ld.add_action(start_gStreamer_client)
     ld.add_action(rviz_node)
 

src/isaac_ros/isaac_ros_launch/launch/isaac_launch.py

@@ -25,11 +25,11 @@ def generate_launch_description():
         default_value="True",
         description="Whether to run from live zed data",
     )
-    # setup_for_gazebo_arg = DeclareLaunchArgument(
-    #     "setup_for_gazebo",
-    #     default_value="False",
-    #     description="Whether to run in gazebo",
-    # )
+    setup_for_gazebo_arg = DeclareLaunchArgument(
+        "setup_for_gazebo",
+        default_value="False",
+        description="Whether to run in gazebo",
+    )
     use_nvblox_arg = DeclareLaunchArgument(
         "use_nvblox",
         default_value="True",
@@ -41,7 +41,7 @@ def generate_launch_description():
         description="Whether to record a ZED svo file",
     )
 
-    global_frame = LaunchConfiguration("global_frame", default="map")
+    global_frame = LaunchConfiguration("global_frame", default="odom")
 
     # Create a shared container to hold composable nodes
     # for speed ups through intra process communication.
@@ -64,27 +64,27 @@ def generate_launch_description():
         }.items(),
         condition=IfCondition(LaunchConfiguration("setup_for_zed")),
     )
-    # # Gazebo
-    # gazebo_launch = IncludeLaunchDescription(
-    #     PythonLaunchDescriptionSource(
-    #         [
-    #             os.path.join(
-    #                 get_package_share_directory("ros_gz_launch"),
-    #                 "launch",
-    #                 "UCF_field.launch.py",
-    #             )
-    #         ]
-    #     ),
-    #     condition=IfCondition(LaunchConfiguration("setup_for_gazebo")),
-    # )
+    # Gazebo
+    gazebo_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [
+                os.path.join(
+                    get_package_share_directory("ros_gz_launch"),
+                    "launch",
+                    "UCF_field.launch.py",
+                )
+            ]
+        ),
+        condition=IfCondition(LaunchConfiguration("setup_for_gazebo")),
+    )
 
     # Nvblox
     nvblox_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([os.path.join(bringup_dir, "nvblox.launch.py")]),
         launch_arguments={
             "global_frame": global_frame,
             "setup_for_zed": LaunchConfiguration("setup_for_zed"),
-            # "setup_for_gazebo": LaunchConfiguration("setup_for_gazebo"),
+            "setup_for_gazebo": LaunchConfiguration("setup_for_gazebo"),
             "attach_to_shared_component_container": "True",
             "component_container_name": shared_container_name,
         }.items(),
@@ -110,7 +110,7 @@ def generate_launch_description():
 
     # Nav2 params
     nav2_param_file = os.path.join("config", "nav2_isaac_sim.yaml")
-    param_substitutions = {"global_frame": LaunchConfiguration("global_frame", default="map")}
+    param_substitutions = {"global_frame": LaunchConfiguration("global_frame", default="odom")}
     configured_params = RewrittenYaml(
         source_file=nav2_param_file,
         root_key="",
@@ -131,28 +131,28 @@ def generate_launch_description():
     # apriltag launch
     apriltag_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([os.path.join(apriltag_bringup_dir, "apriltag_launch.py")]),
-        # condition=UnlessCondition(LaunchConfiguration("setup_for_gazebo")),
+        condition=UnlessCondition(LaunchConfiguration("setup_for_gazebo")),
     )
     # apriltag (gazebo) launch
-    # apriltag_gazebo_launch = IncludeLaunchDescription(
-    #     PythonLaunchDescriptionSource([os.path.join(apriltag_bringup_dir, "apriltag_gazebo_launch.py")]),
-    #     condition=IfCondition(LaunchConfiguration("setup_for_gazebo")),
-    # )
+    apriltag_gazebo_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([os.path.join(apriltag_bringup_dir, "apriltag_gazebo_launch.py")]),
+        condition=IfCondition(LaunchConfiguration("setup_for_gazebo")),
+    )
 
     return LaunchDescription(
         [
             run_rviz_arg,
             setup_for_zed_arg,
-            # setup_for_gazebo_arg,
+            setup_for_gazebo_arg,
             record_svo_arg,
             use_nvblox_arg,
             shared_container,
             nvblox_launch,
             nav2_launch,
             zed_launch,
-            # gazebo_launch,
+            gazebo_launch,
             rviz_launch,
             apriltag_launch,
-            # apriltag_gazebo_launch,
+            apriltag_gazebo_launch,
         ]
     )

src/isaac_ros/isaac_ros_launch/launch/nvblox.launch.py

@@ -40,7 +40,7 @@ def generate_launch_description():
     # Conditionals for setup
     setup_for_zed = IfCondition(LaunchConfiguration("setup_for_zed", default="False"))
     setup_for_gazebo = IfCondition(LaunchConfiguration("setup_for_gazebo", default="False"))
-    
+
     # Option to attach the nodes to a shared component container for speed ups through intra process communication.
     # Make sure to set the 'component_container_name' to the name of the component container you want to attach to.
     attach_to_shared_component_container_arg = LaunchConfiguration(
@@ -70,7 +70,7 @@ def generate_launch_description():
             SetParametersFromFile(base_config),
             SetParametersFromFile(zed_config, condition=setup_for_zed),
             SetParametersFromFile(gazebo_simulation_config, condition=setup_for_gazebo),
-            SetParameter(name="global_frame", value=LaunchConfiguration("global_frame", default="map")),
+            SetParameter(name="global_frame", value=LaunchConfiguration("global_frame", default="odom")),
             # Remappings for zed data
             SetRemap(src=["depth/image"], dst=["/zed2i/zed_node/depth/depth_registered"], condition=setup_for_zed),
             SetRemap(src=["depth/camera_info"], dst=["/zed2i/zed_node/depth/camera_info"], condition=setup_for_zed),

src/isaac_ros/isaac_ros_launch/launch/rviz.launch.py

@@ -29,7 +29,7 @@ def generate_launch_description():
     # Config
     config_name = LaunchConfiguration("config_name", default="zed_example.rviz")
     config_path = PathJoinSubstitution([bringup_dir, config_name])
-    global_frame = LaunchConfiguration("global_frame", default="map")
+    global_frame = LaunchConfiguration("global_frame", default="odom")
 
     # Rviz node
     rviz = Node(

src/rovr_control/rovr_control/costmap_2d.py

@@ -26,6 +26,7 @@
 import numpy as np
 from typing import Tuple
 
+
 class PyCostmap2D:
     """
     PyCostmap2D.
@@ -200,7 +201,7 @@ def getIndex(self, mx: int, my: int) -> int:
 
         """
         return my * self.size_x + mx
-    
+
     def getDigCost(self, wx, wy, robot_width, dig_length):
         mx, my = self.worldToMapValidated(wx, wy)
-        return np.amax(self.costmap[int(mx-robot_width):int(mx+robot_width), int(my):int(my+dig_length)])
+        return np.amax(self.costmap[int(mx - robot_width) : int(mx + robot_width), int(my) : int(my + dig_length)])