Auto Dig Nav Offload + Cameras (#408)

* auto dig improvement

* improvements

* working caemra compression

* initial try, will need to be tested

* small changes

* added to everything launch

* added to launch file and whatnot

* Added a client for the new action server to the main control node

* it works

* removed duplicate

* nav2 and zed config changes

* Tuned backward_distance

* disable some everything launch features

* testing changes

* decrease global costmap size

* auto format

* auto format

---------

Co-authored-by: Alex Berg <56053786+BergerKing333@users.noreply.github.com>
Co-authored-by: umnrobotics <robotics@umn.edu>
Co-authored-by: alex berg <ber00221@umn.edu>

Author: 3 people

arduino/sensors/sensors.ino

@@ -13,8 +13,8 @@ struct SensorData {
 #define DIGGER_BOTTOM_LIMIT_SWITCH 3
 #define DUMPER_TOP_LIMIT_SWITCH 4
 #define DUMPER_BOTTOM_LIMIT_SWITCH 5
-#define LEFT_MOTOR_POT_PIN A0
-#define RIGHT_MOTOR_POT_PIN A1
+#define LEFT_MOTOR_POT_PIN A2
+#define RIGHT_MOTOR_POT_PIN A3
 
 void setup() {
   // Initialize serial communication

config/motor_control.yaml

@@ -19,5 +19,5 @@
     DIGGER_SAFETY_ZONE: 120 # Measured in potentiometer units (0 to 1023)
     CURRENT_SPIKE_THRESHOLD: 1.8 # Threshold for current spike detection (measured in Amps)
     CURRENT_SPIKE_TIME: 0.2 # Time in seconds to consider it a current spike (measured in seconds)
-    BUCKETS_CURRENT_SPIKE_THRESHOLD: 8.0 # Threshold for current spike detection (measured in Amps)
+    BUCKETS_CURRENT_SPIKE_THRESHOLD: 10.0 # Threshold for current spike detection (measured in Amps)
     BUCKETS_CURRENT_SPIKE_TIME: 0.2 # Time in seconds to consider it a current spike (measured in seconds)

config/nav2_isaac_sim.yaml

@@ -28,7 +28,6 @@ bt_navigator:
       - nav2_follow_path_action_bt_node
       - nav2_spin_action_bt_node
       - nav2_wait_action_bt_node
-      - nav2_assisted_teleop_action_bt_node
       - nav2_back_up_action_bt_node
       - nav2_drive_on_heading_bt_node
       - nav2_clear_costmap_service_bt_node
@@ -54,7 +53,6 @@ bt_navigator:
       - nav2_distance_traveled_condition_bt_node
       - nav2_single_trigger_bt_node
       - nav2_goal_updated_controller_bt_node
-      - nav2_is_battery_low_condition_bt_node
       - nav2_navigate_through_poses_action_bt_node
       - nav2_navigate_to_pose_action_bt_node
       - nav2_remove_passed_goals_action_bt_node
@@ -66,9 +64,7 @@ bt_navigator:
       - nav2_wait_cancel_bt_node
       - nav2_spin_cancel_bt_node
       - nav2_back_up_cancel_bt_node
-      - nav2_assisted_teleop_cancel_bt_node
       - nav2_drive_on_heading_cancel_bt_node
-      - nav2_is_battery_charging_condition_bt_node
 bt_navigator_navigate_through_poses_rclcpp_node:
   ros__parameters:
     use_sim_time: False
@@ -215,8 +211,8 @@ global_costmap:
       robot_base_frame: base_link
       use_sim_time: False
       rolling_window: True
-      width: 200
-      height: 200
+      width: 20
+      height: 20
       footprint: "[ [0.595, 0.38], [-0.595, 0.38,], [-0.595, -0.38], [0.595, -0.38] ]"
       # resolution: 0.1
       origin_x: -100.0
@@ -295,8 +291,7 @@ behavior_server:
     costmap_topic: local_costmap/costmap_raw
     footprint_topic: local_costmap/published_footprint
     cycle_frequency: 10.0
-    behavior_plugins:
-      ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
+    behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"]
     spin:
       plugin: "nav2_behaviors/Spin"
     backup:
@@ -305,28 +300,15 @@ behavior_server:
       plugin: "nav2_behaviors/DriveOnHeading"
     wait:
       plugin: "nav2_behaviors/Wait"
-    assisted_teleop:
-      plugin: "nav2_behaviors/AssistedTeleop"
     global_frame: odom
     robot_base_frame: base_link
     transform_tolerance: 0.1
     use_sim_time: False
-    simulate_ahead_time: 2.0
-    max_rotational_vel: 1.0
-    min_rotational_vel: 0.4
-    rotational_acc_lim: 3.2
+    simulate_ahead_time: 1.0
+    max_rotational_vel: 0.75
+    min_rotational_vel: -0.75
+    rotational_acc_lim: 0.75
 
 robot_state_publisher:
   ros__parameters:
     use_sim_time: False
-
-waypoint_follower:
-  ros__parameters:
-    use_sim_time: False
-    loop_rate: 20
-    stop_on_failure: false
-    waypoint_task_executor_plugin: "wait_at_waypoint"
-    wait_at_waypoint:
-      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
-      enabled: True
-      waypoint_pause_duration: 200

config/rovr_control.yaml

@@ -2,7 +2,7 @@
   ros__parameters:
     max_drive_power: 1.0 # Measured in Duty Cycle (0.0-1.0)
     max_turn_power: 1.0 # Measured in Duty Cycle (0.0-1.0)
-    digger_chain_power: 0.2 # Measured in Duty Cycle (0.0-1.0)
+    digger_chain_power: 0.18 # Measured in Duty Cycle (0.0-1.0)
     digger_lift_manual_power_down: 0.12 # Measured in Duty Cycle (0.0-1.0)
     digger_lift_manual_power_up: 0.5 # Measured in Duty Cycle (0.0-1.0)
     autonomous_field_type: "cosmic" # The type of field ("cosmic", "top", "bottom", "nasa")

config/sensors/zed_common.yaml

@@ -64,7 +64,7 @@
 
     pos_tracking:
       pos_tracking_enabled: true # True to enable positional tracking from start
-      pos_tracking_mode: "GEN_2" # Matches the ZED SDK setting: 'GEN_1', 'GEN_2'
+      pos_tracking_mode: "GEN_1" # Matches the ZED SDK setting: 'GEN_1', 'GEN_2'
       imu_fusion: true # enable/disable IMU fusion. When set to false, only the optical odometry will be used.
       publish_tf: true # [usually overwritten by launch file] publish `odom -> camera_link` TF
       publish_map_tf: false # [usually overwritten by launch file] publish `map -> odom` TF

scripts/laptop.sh

@@ -1,4 +1,4 @@
 #!/bin/bash
 
 source install/setup.bash && \
-ros2 launch gstreamer laptop_launch.py
+ros2 run rqt_image_view rqt_image_view

src/isaac_ros/isaac_ros_launch/launch/EVERYTHING_launch.py

@@ -1,6 +1,5 @@
 from launch_ros.substitutions import FindPackageShare
 
-from launch_ros.actions import Node
 from launch import LaunchDescription
 from launch.actions import IncludeLaunchDescription
 from launch.launch_description_sources import PythonLaunchDescriptionSource
@@ -21,26 +20,23 @@ def generate_launch_description():
         launch_arguments={
             "setup_for_zed": "True",
             "setup_for_gazebo": "False",
-            "use_nvblox": "True",
+            "use_nvblox": "False",
             "use_nav2": "True",
             "run_rviz_robot": "False",  # We don't need to run RViz during matches
             "zed_multicam": "True",  # Use multiple ZED cameras
             "record_svo": "True",  # Record match data to an SVO file
+            "use_apriltags": "False",
         }.items(),
     )
 
-    gstreamer_server = Node(
-        package="gstreamer",
-        executable="server_node",
-        name="gstreamer_server_node",
-        output="screen",
-        emulate_tty=True,
+    camera_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare("rovr_control"), "camera_launch.py"]))
     )
 
     # Add all of the actions to the launch description
     ld.add_action(main_launch)
-    ld.add_action(gstreamer_server)
     ld.add_action(isaac_launch)
+    ld.add_action(camera_launch)
 
     # Return the launch description
     return ld

src/isaac_ros/isaac_ros_launch/launch/isaac_launch.py

@@ -5,7 +5,7 @@
     DeclareLaunchArgument,
     IncludeLaunchDescription,
 )
-from launch.conditions import IfCondition, UnlessCondition
+from launch.conditions import IfCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression
 from nav2_common.launch import RewrittenYaml
@@ -17,6 +17,8 @@ def generate_launch_description():
     setup_for_zed = LaunchConfiguration("setup_for_zed", default="True")
     use_nvblox = LaunchConfiguration("use_nvblox", default="True")
     zed_multicam = LaunchConfiguration("zed_multicam", default="False")
+    use_apriltags = LaunchConfiguration("use_apriltags", default="True")
+    setup_for_gazebo = LaunchConfiguration("setup_for_gazebo", default="False")
 
     # Launch Arguments
     run_rviz_arg = DeclareLaunchArgument("run_rviz_robot", default_value="True", description="Whether to start RVIZ")
@@ -55,6 +57,11 @@ def generate_launch_description():
         default_value="False",
         description="Whether to use two ZED cameras",
     )
+    use_apriltags_arg = DeclareLaunchArgument(
+        "use_apriltags",
+        default_value="True",
+        description="Whether to run isaac ros apriltags",
+    )
 
     global_frame = LaunchConfiguration("global_frame", default="odom")
 
@@ -161,14 +168,14 @@ def generate_launch_description():
     # apriltag launch
     apriltag_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([PathJoinSubstitution([FindPackageShare("apriltag"), "apriltag_launch.py"])]),
-        condition=UnlessCondition(LaunchConfiguration("setup_for_gazebo")),
+        condition=IfCondition(PythonExpression([use_apriltags, " and not ", setup_for_gazebo])),
     )
     # apriltag (gazebo) launch
     apriltag_gazebo_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             [PathJoinSubstitution([FindPackageShare("apriltag"), "apriltag_gazebo_launch.py"])]
         ),
-        condition=IfCondition(LaunchConfiguration("setup_for_gazebo")),
+        condition=IfCondition(PythonExpression([use_apriltags, " and ", setup_for_gazebo])),
     )
 
     return LaunchDescription(
@@ -180,6 +187,7 @@ def generate_launch_description():
             use_nvblox_arg,
             use_nav2_arg,
             zed_multicam_arg,
+            use_apriltags_arg,
             shared_container,
             nvblox_launch,
             nvblox_multicam_launch,

src/motor_control/src/motor_control_node.cpp

@@ -490,15 +490,15 @@ class MotorControlNode : public rclcpp::Node {
       // Log an error message
       RCLCPP_ERROR(this->get_logger(), "ERROR: Position difference between linear actuators is too high! Stopping both motors.");
     }
-    else if ((msg.left_motor_pot == 1023) || (msg.right_motor_pot == 1023)) {
+    else if ((msg.left_motor_pot >= 1023) || (msg.right_motor_pot >= 1023)) {
       // Stop both motors!
       this->digger_lift_goal = { "duty_cycle", 0.0 };
       vesc_set_duty_cycle(this->get_parameter("DIGGER_LEFT_LINEAR_ACTUATOR").as_int(), 0.0);
       vesc_set_duty_cycle(this->get_parameter("DIGGER_RIGHT_LINEAR_ACTUATOR").as_int(), 0.0);
       // Log an error message
       RCLCPP_ERROR(this->get_logger(), "ERROR: Potentiometer has reached max value! Stopping both motors, check if one is unplugged");
     }
-    else if ((msg.left_motor_pot == 0) || (msg.right_motor_pot == 0)) {
+    else if ((msg.left_motor_pot <= 0) || (msg.right_motor_pot <= 0)) {
       // Stop both motors!
       this->digger_lift_goal = { "duty_cycle", 0.0 };
       vesc_set_duty_cycle(this->get_parameter("DIGGER_LEFT_LINEAR_ACTUATOR").as_int(), 0.0);

src/rovr_control/launch/camera_launch.py

@@ -0,0 +1,41 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_camera_nodes(camera_name, video_device, resolution=(640, 480), quality=60):
+    return [
+        # Camera driver node
+        Node(
+            package="v4l2_camera",
+            executable="v4l2_camera_node",
+            namespace=camera_name,
+            name="camera",
+            parameters=[
+                {"video_device": video_device},
+                {"image_size": [resolution[0], resolution[1]]},
+                {"pixel_format": "YUYV"},
+                {".image_raw.jpeg_quality": quality},
+            ],
+            remappings=[("/image_raw", "image_raw"), ("/camera_info", "camera_info")],  # scoped to namespace
+        ),
+        # Compressed republisher
+        Node(
+            package="image_transport",
+            executable="republish",
+            namespace=camera_name,
+            name="republish_compressed",
+            arguments=["raw", "compressed"],
+            remappings=[("in", f"{video_device}/image_raw"), ("out", "image_raw/compressed")],
+        ),
+    ]
+
+
+def generate_launch_description():
+    return LaunchDescription(
+        generate_camera_nodes("left", "/dev/video4", (1280, 720), quality=60)
+        # + generate_camera_nodes('back', '/dev/video0')
+        # + generate_camera_nodes('left', '/dev/video0')
+        # + generate_camera_nodes('right', '/dev/video0')
+        # + generate_camera_nodes('dumper', '/dev/video0')
+        # + generate_camera_nodes('digger', '/dev/video0')
+    )