Lunabotics 2024 Competition Changes (#243)

* 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 * auto dig working * changed the fixed frame * changed "top" to "bottom" * added a calibrate button * only use gstreamer in the laptop launch * Added a drive to auto offload * changes * Merge autodig changes into client-rviz (#245) * auto dig changes * removed old comments * Update rovr_control.yaml --------- Co-authored-by: umnrobotics <[email protected]> --------- Co-authored-by: Anthony Brogni <[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]>
GOFIRST-Robotics · May 14, 2024 · 03d3980 · 03d3980
1 parent 35e8021
commit 03d3980
Show file tree

Hide file tree

Showing 19 changed files with 563 additions and 145 deletions.
diff --git a/config/drivetrain_config.yaml b/config/drivetrain_config.yaml
@@ -3,9 +3,9 @@
     HALF_WHEEL_BASE: 0.55959375 # HALF of the wheelbase in meters
     HALF_TRACK_WIDTH: 0.2746375 # HALF of the trackwidth in meters
     STEERING_MOTOR_GEAR_RATIO: 40 # Gear ratio of the steering motor (40:1)
-    FRONT_LEFT_MAGNET_OFFSET: 823 # (in encoder counts)
-    FRONT_RIGHT_MAGNET_OFFSET: 378 # (in encoder counts)
-    BACK_LEFT_MAGNET_OFFSET: 906 # (in encoder counts)
-    BACK_RIGHT_MAGNET_OFFSET: 392 # (in encoder counts)
+    FRONT_LEFT_MAGNET_OFFSET: 97 # (in encoder counts)
+    FRONT_RIGHT_MAGNET_OFFSET: 873 # (in encoder counts)
+    BACK_LEFT_MAGNET_OFFSET: 50 # (in encoder counts)
+    BACK_RIGHT_MAGNET_OFFSET: 647 # (in encoder counts)
     ABSOLUTE_ENCODER_COUNTS: 1024 # (in encoder counts)
     GAZEBO_SIMULATION: False # Set to true if running in Gazebo
diff --git a/config/nav2_isaac_sim.yaml b/config/nav2_isaac_sim.yaml
@@ -19,7 +19,7 @@
 bt_navigator:
   ros__parameters:
     use_sim_time: False
-    global_frame: odom
+    global_frame: map
     robot_base_frame: zed2i_camera_link
     odom_topic: /odom
     bt_loop_duration: 20
@@ -102,24 +102,6 @@ controller_server:
       stateful: True
     # DWB parameters
     FollowPath:
-      # NOTE(remos):
-      #   The RotationShimController did not work for Isaac Sim 2023.1.0 at the time of the release.
-      #   Therefore we comment it here and use the DWBLocalPlanner for the moment.
-      # TODO(remos):
-      #   Bring back the RotationShimController when the issue is fixed
-      #   as it offers better navigation performance when doing narrow turns.
-
-      # Rotation Shim Controller params
-      # This will turn the robot in place when the heading difference between the robot
-      # and the path is too big.
-      # plugin: "nav2_rotation_shim_controller::RotationShimController"
-      # primary_controller: "dwb_core::DWBLocalPlanner"
-      # angular_dist_threshold: 0.3 # ~= 17 degree
-      # forward_sampling_distance: 1.0
-      # rotate_to_heading_angular_vel: 1.8
-      # max_angular_accel: 3.2
-      # simulate_ahead_time: 1.0
-
       # Primary controller params (DWBLocalPlanner)
       plugin: "dwb_core::DWBLocalPlanner"
       debug_trajectory_details: True
@@ -194,7 +176,7 @@ local_costmap:
     ros__parameters:
       update_frequency: 10.0
       publish_frequency: 10.0
-      global_frame: odom
+      global_frame: map
       robot_base_frame: zed2i_camera_link
       use_sim_time: False
       rolling_window: True
@@ -226,7 +208,7 @@ global_costmap:
     ros__parameters:
       update_frequency: 10.0
       publish_frequency: 10.0
-      global_frame: odom
+      global_frame: map
       robot_base_frame: zed2i_camera_link
       use_sim_time: False
       rolling_window: True
@@ -302,7 +284,7 @@ behavior_server:
       plugin: "nav2_behaviors/DriveOnHeading"
     wait:
       plugin: "nav2_behaviors/Wait"
-    global_frame: odom
+    global_frame: map
     robot_base_frame: zed2i_camera_link
     transform_tolerance: 0.2
     use_sim_time: False

diff --git a/config/nvblox/nvblox_base.yaml b/config/nvblox/nvblox_base.yaml
@@ -46,7 +46,7 @@
     # 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: True
+    combine_pointcloud: False
     combine_costmap: True
     invert_below_ground: False
     above_ground_denoise_level: 0.5

diff --git a/config/rovr_control.yaml b/config/rovr_control.yaml
@@ -5,6 +5,7 @@
     max_turn_power: 1.0 # Measured in Duty Cycle (0.0-1.0)
     skimmer_belt_power: -0.3 # Measured in Duty Cycle (0.0-1.0)
     skimmer_lift_manual_power: 0.075 # Measured in Duty Cycle (0.0-1.0)
-    autonomous_field_type: "top" # The type of field ("top", "bottom", "nasa")
+    autonomous_field_type: "bottom" # The type of field ("top", "bottom", "nasa")
     lift_dumping_position: -1000 # Measured in encoder counts!
-    lift_digging_position: -3200 # Measured in encoder counts!
+    lift_digging_start_position: -3050 # Measured in encoder counts!
+    lift_digging_end_position: -3150 # Measured in encoder counts!
diff --git a/config/rviz/zed_example.rviz b/config/rviz/zed_example.rviz
@@ -4,6 +4,7 @@ Panels:
     Name: Displays
     Property Tree Widget:
       Expanded:
+        - /Global Options1
         - /Status1
         - /TF1
         - /TF1/Frames1
@@ -14,7 +15,7 @@ Panels:
         - /Plan1
         - /RobotModel1
       Splitter Ratio: 0.5896806120872498
-    Tree Height: 632
+    Tree Height: 733
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -36,10 +37,14 @@ Visualization Manager:
       Frame Timeout: 15
       Frames:
         All Enabled: false
-        base_link:
+        frame_link:
+          Value: true
+        map:
           Value: true
         odom:
           Value: true
+        translation_end_link:
+          Value: true
         zed2i_camera_center:
           Value: false
         zed2i_camera_link:
@@ -60,19 +65,21 @@ Visualization Manager:
       Show Axes: true
       Show Names: false
       Tree:
-        odom:
-          zed2i_camera_link:
-            base_link:
-              {}
-            zed2i_camera_center:
-              zed2i_left_camera_frame:
-                zed2i_imu_link:
-                  {}
-                zed2i_left_camera_optical_frame:
-                  {}
-              zed2i_right_camera_frame:
-                zed2i_right_camera_optical_frame:
+        map:
+          odom:
+            zed2i_camera_link:
+              translation_end_link:
+                frame_link:
                   {}
+              zed2i_camera_center:
+                zed2i_left_camera_frame:
+                  zed2i_imu_link:
+                    {}
+                  zed2i_left_camera_optical_frame:
+                    {}
+                zed2i_right_camera_frame:
+                  zed2i_right_camera_optical_frame:
+                    {}
       Update Interval: 0
       Value: true
     - Alpha: 0.5
@@ -374,7 +381,7 @@ Visualization Manager:
         Expand Link Details: false
         Expand Tree: false
         Link Tree Style: Links in Alphabetic Order
-        base_link:
+        frame_link:
           Alpha: 1
           Show Axes: false
           Show Trail: false
@@ -399,6 +406,10 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
+        translation_end_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
         wheel1_Link:
           Alpha: 1
           Show Axes: false
@@ -455,7 +466,7 @@ Visualization Manager:
   Enabled: true
   Global Options:
     Background Color: 212; 212; 212
-    Fixed Frame: odom
+    Fixed Frame: map
     Frame Rate: 30
   Name: root
   Tools:
@@ -498,7 +509,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 14.920734405517578
+      Distance: 18.716569900512695
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -513,10 +524,10 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 1.5597963333129883
+      Pitch: 0.7597967386245728
       Target Frame: camera_link
       Value: Orbit (rviz)
-      Yaw: 2.205397605895996
+      Yaw: 2.24539852142334
     Saved: ~
 Window Geometry:
   Color Image:
@@ -528,13 +539,13 @@ Window Geometry:
   Height: 1016
   Hide Left Dock: false
   Hide Right Dock: true
-  QMainWindow State: 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
+  QMainWindow State: 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
   Selection:
     collapsed: false
   Tool Properties:
     collapsed: false
   Views:
     collapsed: true
-  Width: 1850
-  X: 944
-  Y: 134
+  Width: 1848
+  X: 72
+  Y: 27
diff --git a/src/apriltag/apriltag/apriltag_node.py b/src/apriltag/apriltag/apriltag_node.py
@@ -18,7 +18,7 @@ def __init__(self):
         super().__init__("apriltag_node")
         current_dir = os.getcwd()
 
-        self.declare_parameter("autonomous_field_type", "top")  # The type of field ("top", "bottom", "nasa")
+        self.declare_parameter("autonomous_field_type", "bottom")  # The type of field ("top", "bottom", "nasa")
         field_type = self.get_parameter("autonomous_field_type").value
         paths = {
             "top": "src/apriltag/apriltag/apriltag_location_ucf_top.urdf.xarco",
@@ -49,7 +49,7 @@ def __init__(self):
         self.create_service(ResetOdom, "resetOdom", self.reset_callback)
 
         # Create a timer to broadcast the map -> odom transform
-        self.timer = self.create_timer(0.1, self.broadcast_transform)
+        self.timer = self.create_timer(0.01, self.broadcast_transform)
 
     # Service callback definition
     def reset_callback(self, request, response):
@@ -85,11 +85,11 @@ def tagDetectionSub(self, msg):
             rpy_values = [element.attrib["rpy"] for element in rpy_elements]
             rpy = rpy_values[0].split(" ")
 
-            # Lookup the odom to zed2i_camera_link tf from the tf buffer
+            # Lookup the odom to detected tag tf from the tf buffer
             try:
                 odom_to_tag_transform = self.tf_buffer.lookup_transform("odom", f"{tag.family}:{id}", rclpy.time.Time())
             except TransformException as ex:
-                self.get_logger().warn(f"Could not transform odom to zed2i_camera_link: {ex}")
+                self.get_logger().warn(f"Could not transform odom to the detected tag: {ex}")
                 return
 
             odom_to_tag_transform.child_frame_id = "odom"
@@ -125,8 +125,15 @@ def tagDetectionSub(self, msg):
 
     def broadcast_transform(self):
         """Broadcasts the map -> odom transform"""
-        self.map_transform.header.stamp = self.get_clock().now().to_msg()
-        self.tf_broadcaster.sendTransform(self.map_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)
 
 
 def main(args=None):

diff --git a/src/apriltag/launch/apriltag_launch.py b/src/apriltag/launch/apriltag_launch.py
@@ -12,7 +12,7 @@ def generate_launch_description():
         parameters=["config/rovr_control.yaml"],
     )
 
-    apriltag_node = ComposableNode(
+    isaac_ros_apriltag = ComposableNode(
         package="isaac_ros_apriltag",
         plugin="nvidia::isaac_ros::apriltag::AprilTagNode",
         name="isaac_ros_apriltag",
@@ -37,7 +37,7 @@ def generate_launch_description():
         name="apriltag_container",
         namespace="",
         executable="component_container_mt",
-        composable_node_descriptions=[apriltag_node, image_format_converter_node_left],
+        composable_node_descriptions=[isaac_ros_apriltag, image_format_converter_node_left],
         output="screen",
     )
 

diff --git a/src/drivetrain/drivetrain/drivetrain_node.py b/src/drivetrain/drivetrain/drivetrain_node.py
@@ -86,10 +86,10 @@ def __init__(self):
         self.declare_parameter("HALF_WHEEL_BASE", 0.5)
         self.declare_parameter("HALF_TRACK_WIDTH", 0.5)
         self.declare_parameter("STEERING_MOTOR_GEAR_RATIO", 40)
-        self.declare_parameter("FRONT_LEFT_MAGNET_OFFSET", 823)
-        self.declare_parameter("FRONT_RIGHT_MAGNET_OFFSET", 378)
-        self.declare_parameter("BACK_LEFT_MAGNET_OFFSET", 906)
-        self.declare_parameter("BACK_RIGHT_MAGNET_OFFSET", 392)
+        self.declare_parameter("FRONT_LEFT_MAGNET_OFFSET", 97)
+        self.declare_parameter("FRONT_RIGHT_MAGNET_OFFSET", 873)
+        self.declare_parameter("BACK_LEFT_MAGNET_OFFSET", 50)
+        self.declare_parameter("BACK_RIGHT_MAGNET_OFFSET", 647)
         self.declare_parameter("ABSOLUTE_ENCODER_COUNTS", 1024)
         self.declare_parameter("GAZEBO_SIMULATION", False)
 
@@ -135,6 +135,7 @@ def __init__(self):
         # Define services (methods callable from the outside) here
         self.srv_stop = self.create_service(Stop, "drivetrain/stop", self.stop_callback)
         self.srv_drive = self.create_service(Drive, "drivetrain/drive", self.drive_callback)
+        self.srv_calibrate = self.create_service(Stop, "drivetrain/calibrate", self.calibrate_callback)
 
         # Define timers here
         self.absolute_angle_timer = self.create_timer(0.05, self.absolute_angle_reset)
@@ -291,6 +292,30 @@ def drive_callback(self, request, response):
         response.success = 0  # indicates success
         return response
 
+    def calibrate_callback(self, request, response):
+        """This service request calibrates the drivetrain."""
+        if self.front_left.current_absolute_angle is not None:
+            # future.result().data will contain the position of the MOTOR (not the wheel) in degrees. Divide this by the gear ratio to get the wheel position.
+            front_left_future = self.cli_motor_get.call_async(MotorCommandGet.Request(type="position", can_id=self.FRONT_LEFT_TURN))
+            front_left_future.add_done_callback(lambda future: self.front_left.reset(future.result().data / self.STEERING_MOTOR_GEAR_RATIO))
+
+            # future.result().data will contain the position of the MOTOR (not the wheel) in degrees. Divide this by the gear ratio to get the wheel position.
+            front_right_future = self.cli_motor_get.call_async(MotorCommandGet.Request(type="position", can_id=self.FRONT_RIGHT_TURN))
+            front_right_future.add_done_callback(lambda future: self.front_right.reset(future.result().data / self.STEERING_MOTOR_GEAR_RATIO))
+
+            # future.result().data will contain the position of the MOTOR (not the wheel) in degrees. Divide this by the gear ratio to get the wheel position.
+            back_left_future = self.cli_motor_get.call_async(MotorCommandGet.Request(type="position", can_id=self.BACK_LEFT_TURN))
+            back_left_future.add_done_callback(lambda future: self.back_left.reset(future.result().data / self.STEERING_MOTOR_GEAR_RATIO))
+
+            # future.result().data will contain the position of the MOTOR (not the wheel) in degrees. Divide this by the gear ratio to get the wheel position.
+            back_right_future = self.cli_motor_get.call_async(MotorCommandGet.Request(type="position", can_id=self.BACK_RIGHT_TURN))
+            back_right_future.add_done_callback(lambda future: self.back_right.reset(future.result().data / self.STEERING_MOTOR_GEAR_RATIO))
+
+            response.success = 0  # indicates success
+        else:
+            response.success = 1  # indicates failure
+        return response
+
     # Define subscriber callback methods here
 
     def cmd_vel_callback(self, msg: Twist) -> None: