From 03d39800a0771b3a9ce728953eb7632456ae40c9 Mon Sep 17 00:00:00 2001 From: Jonathan Date: Tue, 14 May 2024 14:25:13 -0500 Subject: [PATCH] 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 Co-authored-by: umnrobotics Co-authored-by: Braydon Higgins * 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 --------- Co-authored-by: Anthony Brogni Co-authored-by: alex berg Co-authored-by: umnrobotics Co-authored-by: Alex Berg <56053786+BergerKing333@users.noreply.github.com> Co-authored-by: Braydon Higgins --- config/drivetrain_config.yaml | 8 +- config/nav2_isaac_sim.yaml | 26 +-- config/nvblox/nvblox_base.yaml | 2 +- config/rovr_control.yaml | 5 +- config/rviz/zed_example.rviz | 57 +++-- src/apriltag/apriltag/apriltag_node.py | 19 +- src/apriltag/launch/apriltag_launch.py | 4 +- src/drivetrain/drivetrain/drivetrain_node.py | 33 ++- src/gstreamer/gstreamer/client_widget.py | 10 +- src/gstreamer/gstreamer/server_gstreamer.py | 2 +- src/gstreamer/launch/laptop_launch.py | 39 +++- .../launch/EVERYTHING_launch.py | 2 +- .../isaac_ros_launch/launch/isaac_launch.py | 62 +++--- .../isaac_ros_launch/launch/nvblox.launch.py | 2 +- .../isaac_ros_launch/launch/rviz.launch.py | 2 +- src/motor_control/src/motor_control_node.cpp | 10 +- src/rovr_control/rovr_control/costmap_2d.py | 206 ++++++++++++++++++ .../rovr_control/main_control_node.py | 206 ++++++++++++++++-- src/skimmer/skimmer/skimmer_node.py | 13 +- 19 files changed, 563 insertions(+), 145 deletions(-) create mode 100644 src/rovr_control/rovr_control/costmap_2d.py diff --git a/config/drivetrain_config.yaml b/config/drivetrain_config.yaml index 0a0047cc..271d3d3e 100644 --- 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 index e7db6dd4..b2cf952f 100644 --- 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 index c74b3637..f280f39a 100644 --- 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 index 764ce4e6..93830a9b 100644 --- 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 index 69214eb7..43a7c07c 100644 --- 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 index a6697462..e7a865a5 100644 --- 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 index 6fe00c2b..10f08bb4 100644 --- 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 index 3fe003e4..f636f093 100644 --- 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: diff --git a/src/gstreamer/gstreamer/client_widget.py b/src/gstreamer/gstreamer/client_widget.py index 424ce674..a15b79cb 100644 --- a/src/gstreamer/gstreamer/client_widget.py +++ b/src/gstreamer/gstreamer/client_widget.py @@ -86,7 +86,7 @@ def on_camera1_push_button_clicked(self): print("Requesting Camera 1") req = SetActiveCamera.Request() req.srctype = "v4l2src" - req.device = "/dev/video2" + req.device = "/dev/video0" req.width = 640 req.height = 480 req.framerate = 30 @@ -99,7 +99,7 @@ def on_camera2_push_button_clicked(self): print("Requesting Camera 2") req = SetActiveCamera.Request() req.srctype = "v4l2src" - req.device = "/dev/video6" + req.device = "/dev/video5" req.width = 640 req.height = 480 req.framerate = 30 @@ -112,7 +112,7 @@ def on_camera3_push_button_clicked(self): print("Requesting Camera 3") req = SetActiveCamera.Request() req.srctype = "v4l2src" - req.device = "/dev/video4" + req.device = "/dev/video7" req.width = 640 req.height = 480 req.framerate = 30 @@ -125,7 +125,7 @@ def on_camera4_push_button_clicked(self): print("Requesting Camera 4") req = SetActiveCamera.Request() req.srctype = "v4l2src" - req.device = "/dev/video7" + req.device = "/dev/video6" req.width = 640 req.height = 480 req.framerate = 30 @@ -138,7 +138,7 @@ def on_camera5_push_button_clicked(self): print("Requesting Camera 5") req = SetActiveCamera.Request() req.srctype = "v4l2src" - req.device = "/dev/video5" + req.device = "/dev/video4" req.width = 640 req.height = 480 req.framerate = 30 diff --git a/src/gstreamer/gstreamer/server_gstreamer.py b/src/gstreamer/gstreamer/server_gstreamer.py index 97ce7c1b..70e8418e 100644 --- a/src/gstreamer/gstreamer/server_gstreamer.py +++ b/src/gstreamer/gstreamer/server_gstreamer.py @@ -73,7 +73,7 @@ def init_h265(self, input, sink): def init_av1(self, input, sink): # gst-launch-1.0 videotestsrc ! 'video/x-raw, width=(int)640, height=(int)480, format=(string)NV12, framerate=(fraction)30/1' ! nvvideoconvert ! nvv4l2av1enc ! udpsink host=127.0.0.1 port=5000 nvv4l2av1enc = Gst.ElementFactory.make("nvv4l2av1enc", "nvv4l2av1enc") - nvv4l2av1enc.set_property("bitrate", 200000) + nvv4l2av1enc.set_property("bitrate", 1000000) self.pipeline.add(nvv4l2av1enc) input.link(nvv4l2av1enc) diff --git a/src/gstreamer/launch/laptop_launch.py b/src/gstreamer/launch/laptop_launch.py index 6cb7d805..7ef2e840 100644 --- a/src/gstreamer/launch/laptop_launch.py +++ b/src/gstreamer/launch/laptop_launch.py @@ -1,23 +1,46 @@ +# import os from launch import LaunchDescription -from launch_ros.actions import Node +# from launch_ros.actions import Node from launch.actions import ExecuteProcess - +# from launch.conditions import IfCondition +# from launch.actions import DeclareLaunchArgument +# from launch.substitutions import LaunchConfiguration, PathJoinSubstitution # Launches the joystick node and the gstreamer camera client on the operator laptop def generate_launch_description(): ld = LaunchDescription() - joystick_node = Node( - package="joy", - executable="joy_node", - parameters=["config/joy_node.yaml"], - ) + # bringup_dir = os.path.join("config", "rviz") + + # # Config + # config_name = LaunchConfiguration("config_name", default="zed_example.rviz") + # config_path = PathJoinSubstitution([bringup_dir, config_name]) + # global_frame = LaunchConfiguration("global_frame", default="map") + + # run_rviz_arg = DeclareLaunchArgument("run_rviz_client", default_value="True", description="Whether to start RVIZ") + + # joystick_node = Node( + # package="joy", + # executable="joy_node", + # parameters=["config/joy_node.yaml"], + # ) start_gStreamer_client = ExecuteProcess( cmd=["rqt", "--force-discover", "--standalone", "CameraClient"], shell=True, output="screen" ) - ld.add_action(joystick_node) + # rviz_node = Node( + # package="rviz2", + # executable="rviz2", + # name="rviz2", + # output="screen", + # arguments=["-d", config_path, "-f", global_frame], + # condition=IfCondition(LaunchConfiguration("run_rviz_client")), + # ) + + # 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(start_gStreamer_client) + # ld.add_action(rviz_node) return ld diff --git a/src/isaac_ros/isaac_ros_launch/launch/EVERYTHING_launch.py b/src/isaac_ros/isaac_ros_launch/launch/EVERYTHING_launch.py index 11a4437a..e8dd940c 100644 --- a/src/isaac_ros/isaac_ros_launch/launch/EVERYTHING_launch.py +++ b/src/isaac_ros/isaac_ros_launch/launch/EVERYTHING_launch.py @@ -23,7 +23,7 @@ def generate_launch_description(): "setup_for_zed": "True", "setup_for_gazebo": "False", "use_nvblox": "True", - "run_rviz": "False", # We don't need to run RViz during matches + "run_rviz_robot": "False", # We don't need to run RViz during matches "record_svo": "True", # Record match data to an SVO file }.items(), ) diff --git a/src/isaac_ros/isaac_ros_launch/launch/isaac_launch.py b/src/isaac_ros/isaac_ros_launch/launch/isaac_launch.py index 235ae768..9bbd5e7d 100644 --- a/src/isaac_ros/isaac_ros_launch/launch/isaac_launch.py +++ b/src/isaac_ros/isaac_ros_launch/launch/isaac_launch.py @@ -19,17 +19,17 @@ def generate_launch_description(): apriltag_bringup_dir = get_package_share_directory("apriltag") # Launch Arguments - run_rviz_arg = DeclareLaunchArgument("run_rviz", default_value="True", description="Whether to start RVIZ") + run_rviz_arg = DeclareLaunchArgument("run_rviz_robot", default_value="True", description="Whether to start RVIZ") setup_for_zed_arg = DeclareLaunchArgument( "setup_for_zed", 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="odom") + global_frame = LaunchConfiguration("global_frame", default="map") # Create a shared container to hold composable nodes # for speed ups through intra process communication. @@ -64,19 +64,19 @@ 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( @@ -84,7 +84,7 @@ def generate_launch_description(): 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(), @@ -105,12 +105,12 @@ def generate_launch_description(): "config_name": "zed_example.rviz", "global_frame": global_frame, }.items(), - condition=IfCondition(LaunchConfiguration("run_rviz")), + condition=IfCondition(LaunchConfiguration("run_rviz_robot")), ) # Nav2 params nav2_param_file = os.path.join("config", "nav2_isaac_sim.yaml") - param_substitutions = {"global_frame": LaunchConfiguration("global_frame", default="odom")} + param_substitutions = {"global_frame": LaunchConfiguration("global_frame", default="map")} 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, ] ) diff --git a/src/isaac_ros/isaac_ros_launch/launch/nvblox.launch.py b/src/isaac_ros/isaac_ros_launch/launch/nvblox.launch.py index 374dd0a4..6360ac93 100644 --- a/src/isaac_ros/isaac_ros_launch/launch/nvblox.launch.py +++ b/src/isaac_ros/isaac_ros_launch/launch/nvblox.launch.py @@ -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="odom")), + SetParameter(name="global_frame", value=LaunchConfiguration("global_frame", default="map")), # 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), diff --git a/src/isaac_ros/isaac_ros_launch/launch/rviz.launch.py b/src/isaac_ros/isaac_ros_launch/launch/rviz.launch.py index 10372152..3dcba4c1 100644 --- a/src/isaac_ros/isaac_ros_launch/launch/rviz.launch.py +++ b/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="odom") + global_frame = LaunchConfiguration("global_frame", default="map") # Rviz node rviz = Node( diff --git a/src/motor_control/src/motor_control_node.cpp b/src/motor_control/src/motor_control_node.cpp index f04cd36d..5966cc0c 100644 --- a/src/motor_control/src/motor_control_node.cpp +++ b/src/motor_control/src/motor_control_node.cpp @@ -253,10 +253,10 @@ class MotorControlNode : public rclcpp::Node { "motor/get", std::bind(&MotorControlNode::get_callback, this, _1, _2)); // Instantiate all of our PIDControllers here - this->pid_controllers[this->get_parameter("BACK_LEFT_TURN").as_int()] = new PIDController(42, 0.005, 0.0, 0.0, 0.0, 10, 0.5); - this->pid_controllers[this->get_parameter("FRONT_LEFT_TURN").as_int()] = new PIDController(42, 0.005, 0.0, 0.0, 0.0, 10, 0.5); - this->pid_controllers[this->get_parameter("BACK_RIGHT_TURN").as_int()] = new PIDController(42, 0.005, 0.0, 0.0, 0.0, 10, 0.5); - this->pid_controllers[this->get_parameter("FRONT_RIGHT_TURN").as_int()] = new PIDController(42, 0.005, 0.0, 0.0, 0.0, 10, 0.5); + this->pid_controllers[this->get_parameter("BACK_LEFT_TURN").as_int()] = new PIDController(42, 0.005, 0.0, 0.0, 0.0, 10, 0.4); + this->pid_controllers[this->get_parameter("FRONT_LEFT_TURN").as_int()] = new PIDController(42, 0.005, 0.0, 0.0, 0.0, 10, 0.4); + this->pid_controllers[this->get_parameter("BACK_RIGHT_TURN").as_int()] = new PIDController(42, 0.005, 0.0, 0.0, 0.0, 10, 0.4); + this->pid_controllers[this->get_parameter("FRONT_RIGHT_TURN").as_int()] = new PIDController(42, 0.005, 0.0, 0.0, 0.0, 10, 0.4); this->pid_controllers[this->get_parameter("SKIMMER_LIFT_MOTOR").as_int()] = new PIDController(42, 0.005, 0.0, 0.0, 0.0, 20, 0.05); // Enable continuous input for the swerve module PID controllers @@ -282,7 +282,7 @@ class MotorControlNode : public rclcpp::Node { for (auto pair : this->current_msg) { uint32_t motorId = pair.first; // If the motor controller has previously received a command, send the most recent command again - if ((this->pid_controllers[motorId] && this->pid_controllers[motorId]->isActive == false) || !this->pid_controllers[motorId]) { + if ((this->pid_controllers[motorId] && this->pid_controllers[motorId]->isActive == false) || this->pid_controllers[motorId] == NULL) { send_can(std::get<0>(this->current_msg[motorId]), std::get<1>(this->current_msg[motorId])); } } diff --git a/src/rovr_control/rovr_control/costmap_2d.py b/src/rovr_control/rovr_control/costmap_2d.py new file mode 100644 index 00000000..fdfefae5 --- /dev/null +++ b/src/rovr_control/rovr_control/costmap_2d.py @@ -0,0 +1,206 @@ +#! /usr/bin/env python3 +# Copyright 2021 Samsung Research America +# Copyright 2022 Stevedan Ogochukwu Omodolor +# Copyright 2022 Jaehun Jackson Kim +# Copyright 2022 Afif Swaidan +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +""" +This is a Python3 API for costmap 2d messages from the stack. + +It provides the basic conversion, get/set, +and handling semantics found in the costmap 2d C++ API. +""" + +import numpy as np +from typing import Tuple + +class PyCostmap2D: + """ + PyCostmap2D. + + Costmap Python3 API for OccupancyGrids to populate from published messages + """ + + def __init__(self, costmap): + """ + Initialize costmap2D. + + Args + ---- + occupancy_map (OccupancyGrid): 2D OccupancyGrid Map + + Returns + ------- + None + + """ + self.size_x = costmap.metadata.size_x + self.size_y = costmap.metadata.size_y + self.resolution = costmap.metadata.resolution + self.origin_x = costmap.metadata.origin.position.x + self.origin_y = costmap.metadata.origin.position.y + self.global_frame_id = costmap.header.frame_id + self.costmap_timestamp = costmap.header.stamp + # Extract costmap + self.costmap = np.array(costmap.data, dtype=np.uint8).reshape(self.size_x, self.size_y) + + def getSizeInCellsX(self): + """Get map width in cells.""" + return self.size_x + + def getSizeInCellsY(self): + """Get map height in cells.""" + return self.size_y + + def getSizeInMetersX(self): + """Get x axis map size in meters.""" + return (self.size_x - 1 + 0.5) * self.resolution + + def getSizeInMetersY(self): + """Get y axis map size in meters.""" + return (self.size_y - 1 + 0.5) * self.resolution + + def getOriginX(self): + """Get the origin x axis of the map [m].""" + return self.origin_x + + def getOriginY(self): + """Get the origin y axis of the map [m].""" + return self.origin_y + + def getResolution(self): + """Get map resolution [m/cell].""" + return self.resolution + + def getGlobalFrameID(self): + """Get global frame_id.""" + return self.global_frame_id + + def getCostmapTimestamp(self): + """Get costmap timestamp.""" + return self.costmap_timestamp + + def getCostXY(self, mx: int, my: int) -> np.uint8: + """ + Get the cost of a cell in the costmap using map coordinate XY. + + Args + ---- + mx (int): map coordinate X to get cost + my (int): map coordinate Y to get cost + + Returns + ------- + np.uint8: cost of a cell + + """ + return self.costmap[self.getIndex(mx, my)] + + def getCostIdx(self, index: int) -> np.uint8: + """ + Get the cost of a cell in the costmap using Index. + + Args + ---- + index (int): index of cell to get cost + + Returns + ------- + np.uint8: cost of a cell + + """ + return self.costmap[index] + + def setCost(self, mx: int, my: int, cost: np.uint8) -> None: + """ + Set the cost of a cell in the costmap using map coordinate XY. + + Args + ---- + mx (int): map coordinate X to get cost + my (int): map coordinate Y to get cost + cost (np.uint8): The cost to set the cell + + Returns + ------- + None + + """ + self.costmap[self.getIndex(mx, my)] = cost + + def mapToWorld(self, mx: int, my: int) -> Tuple[float, float]: + """ + Get the world coordinate XY using map coordinate XY. + + Args + ---- + mx (int): map coordinate X to get world coordinate + my (int): map coordinate Y to get world coordinate + + Returns + ------- + tuple of float: wx, wy + wx (float) [m]: world coordinate X + wy (float) [m]: world coordinate Y + + """ + wx = self.origin_x + (mx + 0.5) * self.resolution + wy = self.origin_y + (my + 0.5) * self.resolution + return (wx, wy) + + def worldToMapValidated(self, wx: float, wy: float): + """ + Get the map coordinate XY using world coordinate XY. + + Args + ---- + wx (float) [m]: world coordinate X to get map coordinate + wy (float) [m]: world coordinate Y to get map coordinate + + Returns + ------- + (None, None): if coordinates are invalid + tuple of int: mx, my (if coordinates are valid) + mx (int): map coordinate X + my (int): map coordinate Y + + """ + if wx < self.origin_x or wy < self.origin_y: + return (None, None) + mx = int((wx - self.origin_x) // self.resolution) + my = int((wy - self.origin_y) // self.resolution) + if mx < self.size_x and my < self.size_y: + return (mx, my) + return (None, None) + + def getIndex(self, mx: int, my: int) -> int: + """ + Get the index of the cell using map coordinate XY. + + Args + ---- + mx (int): map coordinate X to get Index + my (int): map coordinate Y to get Index + + Returns + ------- + int: The index of the cell + + """ + 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)]) \ No newline at end of file diff --git a/src/rovr_control/rovr_control/main_control_node.py b/src/rovr_control/rovr_control/main_control_node.py index 6d975057..7c3c1595 100644 --- a/src/rovr_control/rovr_control/main_control_node.py +++ b/src/rovr_control/rovr_control/main_control_node.py @@ -3,13 +3,21 @@ # Maintainer: Anthony Brogni # Last Updated: November 2023 +import sys + # Import the ROS 2 module import rclpy from rclpy.node import Node from rclpy.executors import SingleThreadedExecutor # This is needed to run multiple callbacks in a single thread +# Provides a “navigation as a library” capability +from nav2_simple_commander.robot_navigator import ( + BasicNavigator, + TaskResult, +) + # Import ROS 2 formatted message types -from geometry_msgs.msg import Twist, Vector3 +from geometry_msgs.msg import Twist, Vector3, PoseStamped from sensor_msgs.msg import Joy from std_msgs.msg import Bool @@ -19,9 +27,7 @@ # Import Python Modules import asyncio # Allows the use of asynchronous methods! - -# Provides a “navigation as a library” capability -from nav2_simple_commander.robot_navigator import BasicNavigator +from scipy.spatial.transform import Rotation as R # Import our logitech gamepad button mappings from .gamepad_constants import * @@ -29,10 +35,26 @@ # Uncomment the line below to use the Xbox controller mappings instead # from .xbox_controller_constants import * +from .costmap_2d import PyCostmap2D + # GLOBAL VARIABLES # buttons = [0] * 11 # This is to help with button press detection # Define the possible states of our robot -states = {"Teleop": 0, "Auto_Dig": 1, "Auto_Offload": 2, "Calibrating": 3} +states = {"Teleop": 0, "Autonomous": 1} + + +# Helper Method +def create_pose_stamped(x, y, yaw): + pose_stamped_msg = PoseStamped() + pose_stamped_msg.header.frame_id = "map" + pose_stamped_msg.pose.position.x = x + pose_stamped_msg.pose.position.y = y + quat = R.from_euler("z", yaw - 15, degrees=True).as_quat() # TODO: Why does this need to be offset? + pose_stamped_msg.pose.orientation.x = quat[0] + pose_stamped_msg.pose.orientation.y = quat[1] + pose_stamped_msg.pose.orientation.z = quat[2] + pose_stamped_msg.pose.orientation.w = quat[3] + return pose_stamped_msg class MainControlNode(Node): @@ -45,9 +67,11 @@ def __init__(self) -> None: self.declare_parameter("max_drive_power", 1.0) # Measured in Duty Cycle (0.0-1.0) self.declare_parameter("max_turn_power", 1.0) # Measured in Duty Cycle (0.0-1.0) self.declare_parameter("skimmer_belt_power", -0.3) # Measured in Duty Cycle (0.0-1.0) + self.declare_parameter("autonomous_field_type", "bottom") # The type of field ("top", "bottom", "nasa") self.declare_parameter("skimmer_lift_manual_power", 0.075) # Measured in Duty Cycle (0.0-1.0) self.declare_parameter("lift_dumping_position", -1000) # Measured in encoder counts - self.declare_parameter("lift_digging_position", -3200) # Measured in encoder counts + self.declare_parameter("lift_digging_start_position", -3050) # Measured in encoder counts + self.declare_parameter("lift_digging_end_position", -3150) # Measured in encoder counts # Assign the ROS Parameters to member variables below # self.autonomous_driving_power = self.get_parameter("autonomous_driving_power").value @@ -55,8 +79,10 @@ def __init__(self) -> None: self.max_turn_power = self.get_parameter("max_turn_power").value self.skimmer_belt_power = self.get_parameter("skimmer_belt_power").value self.skimmer_lift_manual_power = self.get_parameter("skimmer_lift_manual_power").value + self.autonomous_field_type = self.get_parameter("autonomous_field_type").value self.lift_dumping_position = self.get_parameter("lift_dumping_position").value * 360 / 42 # Convert encoder counts to degrees - self.lift_digging_position = self.get_parameter("lift_digging_position").value * 360 / 42 # Convert encoder counts to degrees + self.lift_digging_start_position = self.get_parameter("lift_digging_start_position").value * 360 / 42 # Convert encoder counts to degrees + self.lift_digging_end_position = self.get_parameter("lift_digging_end_position").value * 360 / 42 # Convert encoder counts to degrees # Print the ROS Parameters to the terminal below # self.get_logger().info("autonomous_driving_power has been set to: " + str(self.autonomous_driving_power)) @@ -64,8 +90,10 @@ def __init__(self) -> None: self.get_logger().info("max_turn_power has been set to: " + str(self.max_turn_power)) self.get_logger().info("skimmer_belt_power has been set to: " + str(self.skimmer_belt_power)) self.get_logger().info("skimmer_lift_manual_power has been set to: " + str(self.skimmer_lift_manual_power)) + self.get_logger().info("autonomous_field_type has been set to: " + str(self.autonomous_field_type)) self.get_logger().info("lift_dumping_position has been set to: " + str(self.lift_dumping_position)) - self.get_logger().info("lift_digging_position has been set to: " + str(self.lift_digging_position)) + self.get_logger().info("lift_digging_start_position has been set to: " + str(self.lift_digging_start_position)) + self.get_logger().info("lift_digging_end_position has been set to: " + str(self.lift_digging_end_position)) # Define some initial states here self.state = states["Teleop"] @@ -74,8 +102,24 @@ def __init__(self) -> None: self.back_camera = None self.autonomous_digging_process = None self.autonomous_offload_process = None + self.autonomous_cycle_process = None + self.travel_automation_process = None self.skimmer_goal_reached = True + self.DANGER_THRESHOLD = 1 + self.REAL_DANGER_THRESHOLD = 100 + + # Define important map locations + if self.autonomous_field_type == "top": + self.autonomous_berm_location = create_pose_stamped(7.25, -3.2, 90) # TODO: Test this location in simulation + self.travel_automation_location = create_pose_stamped(6.2, -1.2, 0) + elif self.autonomous_field_type == "bottom": + self.autonomous_berm_location = create_pose_stamped(7.25, -1.4, 270) + self.travel_automation_location = create_pose_stamped(6.2, -3.2, 0) + elif self.autonomous_field_type == "nasa": + self.autonomous_berm_location = create_pose_stamped(1.3, -0.6, 90) # TODO: Test this location in simulation + self.travel_automation_location = create_pose_stamped(6.2, -1.2, 0) # TODO: Test this location in simulation + # Define timers here self.apriltag_timer = self.create_timer(0.1, self.start_calibration_callback) self.apriltag_timer.cancel() # Cancel the apriltag timer initially @@ -87,6 +131,7 @@ def __init__(self) -> None: self.cli_lift_setPosition = self.create_client(SetPosition, "lift/setPosition") self.cli_drivetrain_stop = self.create_client(Stop, "drivetrain/stop") self.cli_drivetrain_drive = self.create_client(Drive, "drivetrain/drive") + self.cli_drivetrain_calibrate = self.create_client(Stop, "drivetrain/calibrate") self.cli_motor_get = self.create_client(MotorCommandGet, "motor/get") self.cli_lift_stop = self.create_client(Stop, "lift/stop") self.cli_lift_zero = self.create_client(Stop, "lift/zero") @@ -106,6 +151,40 @@ def __init__(self) -> None: while not self.cli_lift_zero.wait_for_service(timeout_sec=1): self.get_logger().warn("Waiting for the lift/zero service to be available (BLOCKING)") self.cli_lift_zero.call_async(Stop.Request()) # Zero the lift by slowly raising it up + + + def optimal_dig_location(self) -> list: + available_dig_spots = [] + + try: + costmap = PyCostmap2D(self.nav2.getGlobalCostmap()) + resolution = costmap.getResolution() + + # NEEDED MEASUREMENTS: + robot_width = 1.749 / 2 + robot_width_pixels = robot_width // resolution + danger_threshold, real_danger_threshold = 50, 150 + dig_zone_depth, dig_zone_start, dig_zone_end = 2.57 // 2, 4.07, 8.14 + dig_zone_border_y = 2.0 + + + while len(available_dig_spots) == 0: + if danger_threshold > real_danger_threshold: + self.get_logger().warn("No safe digging spots available. Switch to manual control.") + return None + i = dig_zone_start + robot_width + while i <= dig_zone_end - robot_width: + if costmap.getDigCost(i, dig_zone_border_y, robot_width_pixels, dig_zone_depth) <= self.DANGER_THRESHOLD: + available_dig_spots.append(create_pose_stamped(i, -dig_zone_border_y, 90)) + i += robot_width + else: + i += resolution + if len(available_dig_spots) > 0: + return available_dig_spots + danger_threshold += 5 + except Exception as e: + self.get_logger().error(f"Error in optimal_dig_location: {e} on line {sys.exc_info()[-1].tb_lineno}") + return None def start_calibration_callback(self) -> None: """This method publishes the odometry of the robot.""" @@ -150,21 +229,50 @@ async def calibrate_field_coordinates(self) -> None: self.apriltag_timer.cancel() self.end_autonomous() # Return to Teleop mode + async def travel_automation(self) -> None: + """This method is used to automate the travel of the robot to the excavation zone.""" + self.get_logger().info("Starting Travel Automation!") + try: + await self.calibrate_field_coordinates() # Calibrate the field coordinates first + self.nav2.goToPose(self.travel_automation_location) # Navigate to the excavation zone + while not self.nav2.isTaskComplete(): # Wait for the excavation zone to be reached + await asyncio.sleep(0.1) # Allows other async tasks to continue running (this is non-blocking) + if self.nav2.getResult() == TaskResult.FAILED: + self.get_logger().error("Failed to reach the excavation zone!") + self.end_autonomous() # Return to Teleop mode + return + self.get_logger().info("Travel Automation Complete!") + if self.travel_automation_process is None: + self.end_autonomous() # Return to Teleop mode + except asyncio.CancelledError: + self.get_logger().warn("Travel Automation Terminated!") + self.end_autonomous() # Return to Teleop mode + # TODO: This autonomous routine has not been tested yet! async def auto_dig_procedure(self) -> None: """This method lays out the procedure for autonomously digging!""" self.get_logger().info("\nStarting Autonomous Digging Procedure!") try: # Wrap the autonomous procedure in a try-except - await self.cli_lift_setPosition.call_async(SetPosition.Request(position=self.lift_digging_position)) # Lower the skimmer into the ground + await self.cli_lift_zero.call_async(Stop.Request()) + await self.cli_lift_setPosition.call_async(SetPosition.Request(position=self.lift_digging_start_position)) # Lower the skimmer onto the ground self.skimmer_goal_reached = False # Wait for the goal height to be reached while not self.skimmer_goal_reached: - self.get_logger().info("Moving skimmer to the goal") + self.get_logger().info("Moving skimmer to starting dig position") await asyncio.sleep(0.1) # Allows other async tasks to continue running (this is non-blocking) await self.cli_skimmer_setPower.call_async(SetPower.Request(power=self.skimmer_belt_power)) # Drive forward while digging - self.nav2.driveOnHeading(dist=0.75, speed=0.1) # TODO: Tune these values - while not self.nav2.isTaskComplete(): # Wait for the end of the driveOnHeading task + start_time = self.get_clock().now().nanoseconds + elapsed = self.get_clock().now().nanoseconds - start_time + # accelerate for 2 seconds + while elapsed < 2e9: + await self.cli_lift_set_power.call_async(SetPower.Request(power=-0.05e-9*(elapsed))) + await self.cli_drivetrain_drive.call_async(Drive.Request(forward_power=0.0, horizontal_power=0.25e-9*(elapsed), turning_power=0.0)) + self.get_logger().info("Accelerating lift and drive train") + elapsed = self.get_clock().now().nanoseconds - start_time + await asyncio.sleep(0.1) # Allows other async tasks to continue running (this is non-blocking) + # keep digging at full speed for the remaining 10 seconds + while self.get_clock().now().nanoseconds - start_time < 12e9: self.get_logger().info("Auto Driving") await asyncio.sleep(0.1) # Allows other async tasks to continue running (this is non-blocking) await self.cli_drivetrain_stop.call_async(Stop.Request()) @@ -173,7 +281,7 @@ async def auto_dig_procedure(self) -> None: self.skimmer_goal_reached = False # Wait for the lift goal to be reached while not self.skimmer_goal_reached: - self.get_logger().info("Moving skimmer to the goal") + self.get_logger().info("Moving skimmer to dumping position") await asyncio.sleep(0.1) # Allows other async tasks to continue running (this is non-blocking) self.get_logger().info("Autonomous Digging Procedure Complete!\n") self.end_autonomous() # Return to Teleop mode @@ -186,7 +294,15 @@ async def auto_offload_procedure(self) -> None: """This method lays out the procedure for autonomously offloading!""" self.get_logger().info("\nStarting Autonomous Offload Procedure!") try: # Wrap the autonomous procedure in a try-except - await self.cli_lift_setPosition.call_async(SetPosition.Request(position=self.lift_dumping_position)) # Raise up the skimmer in preparation for dumping + # Drive backward into the berm zone + await self.cli_drivetrain_drive.call_async(Drive.Request(forward_power=0.0, horizontal_power=-0.25, turning_power=0.0)) + start_time = self.get_clock().now().nanoseconds + while self.get_clock().now().nanoseconds - start_time < 10e9: + self.get_logger().info("Auto Driving") + await asyncio.sleep(0.1) # Allows other async tasks to continue running (this is non-blocking) + await self.cli_drivetrain_stop.call_async(Stop.Request()) + # Raise up the skimmer in preparation for dumping + await self.cli_lift_setPosition.call_async(SetPosition.Request(position=self.lift_dumping_position)) self.skimmer_goal_reached = False # Wait for the lift goal to be reached while not self.skimmer_goal_reached: @@ -197,11 +313,52 @@ async def auto_offload_procedure(self) -> None: await asyncio.sleep(8 / abs(self.skimmer_belt_power)) # How long to offload for # TODO: Adjust this time factor as needed await self.cli_skimmer_stop.call_async(Stop.Request()) # Stop the skimmer belt self.get_logger().info("Autonomous Offload Procedure Complete!\n") - self.end_autonomous() # Return to Teleop mode + if self.autonomous_cycle_process is None: + self.end_autonomous() # Return to Teleop mode except asyncio.CancelledError: # Put termination code here self.get_logger().warn("Autonomous Offload Procedure Terminated\n") self.end_autonomous() # Return to Teleop mode + # TODO: This autonomous routine has not been tested yet! + async def auto_cycle_procedure(self) -> None: + """This method lays out the procedure for doing a complete autonomous cycle!""" + self.get_logger().info("\nStarting an Autonomous Cycle!") + try: # Wrap the autonomous procedure in a try-except + ## Navigate to the dig_location, run the dig procedure, then navigate to the berm zone and run the offload procedure ## + if not self.field_calibrated: + self.get_logger().error("Field coordinates must be calibrated first!") + self.end_autonomous() # Return to Teleop mode + return + self.nav2.goToPose(self.optimal_dig_location()[0]) # Navigate to the dig location + while not self.nav2.isTaskComplete(): # Wait for the dig location to be reached + await asyncio.sleep(0.1) # Allows other async tasks to continue running (this is non-blocking) + if self.nav2.getResult() == TaskResult.FAILED: + self.get_logger().error("Failed to reach the dig location!") + self.end_autonomous() # Return to Teleop mode + return + self.autonomous_digging_process = asyncio.ensure_future( + self.auto_dig_procedure() + ) # Start the auto dig process + while not self.autonomous_digging_process.done(): # Wait for the dig process to complete + await asyncio.sleep(0.1) # Allows other async tasks to continue running (this is non-blocking) + self.nav2.goToPose(self.autonomous_berm_location) # Navigate to the berm zone + while not self.nav2.isTaskComplete(): # Wait for the berm zone to be reached + await asyncio.sleep(0.1) # Allows other async tasks to continue running (this is non-blocking) + if self.nav2.getResult() == TaskResult.FAILED: + self.get_logger().error("Failed to reach the berm zone!") + self.end_autonomous() # Return to Teleop mode + return + self.autonomous_offload_process = asyncio.ensure_future( + self.auto_offload_procedure() + ) # Start the auto offload process + while not self.autonomous_offload_process.done(): # Wait for the offload process to complete + await asyncio.sleep(0.1) # Allows other async tasks to continue running (this is non-blocking) + self.get_logger().info("Completed an Autonomous Cycle!\n") + self.end_autonomous() # Return to Teleop mode + except asyncio.CancelledError: # Put termination code here + self.get_logger().info("Autonomous Cycle Terminated\n") + self.end_autonomous() # Return to Teleop mode + def skimmer_goal_callback(self, msg: Bool) -> None: """Update the member variable accordingly.""" self.skimmer_goal_reached = msg.data @@ -234,7 +391,7 @@ def joystick_callback(self, msg: Joy) -> None: # Check if the lift digging position button is pressed # if msg.buttons[A_BUTTON] == 1 and buttons[A_BUTTON] == 0: - self.cli_lift_setPosition.call_async(SetPosition.Request(position=self.lift_digging_position)) + self.cli_lift_setPosition.call_async(SetPosition.Request(position=self.lift_digging_start_position)) # Manually adjust the height of the skimmer with the left and right triggers if msg.buttons[RIGHT_TRIGGER] == 1 and buttons[RIGHT_TRIGGER] == 0: @@ -246,6 +403,10 @@ def joystick_callback(self, msg: Joy) -> None: elif msg.buttons[LEFT_TRIGGER] == 0 and buttons[LEFT_TRIGGER] == 1: self.cli_lift_stop.call_async(Stop.Request()) + # Check if the calibration button is pressed + if msg.buttons[RIGHT_BUMPER] == 1 and buttons[RIGHT_BUMPER] == 0: + self.cli_drivetrain_calibrate.call_async(Stop.Request()) + # THE CONTROLS BELOW ALWAYS WORK # # Check if the Apriltag calibration button is pressed @@ -254,7 +415,7 @@ def joystick_callback(self, msg: Joy) -> None: if self.apriltag_timer.is_canceled(): self.started_calibration = False self.field_calibrated = False - self.state = states["Calibrating"] # Exit Teleop mode + self.state = states["Autonomous"] # Exit Teleop mode self.apriltag_timer.reset() # Stop the field calibration process else: @@ -266,23 +427,25 @@ def joystick_callback(self, msg: Joy) -> None: if msg.buttons[BACK_BUTTON] == 1 and buttons[BACK_BUTTON] == 0: if self.state == states["Teleop"]: self.stop_all_subsystems() # Stop all subsystems - self.state = states["Auto_Dig"] + self.state = states["Autonomous"] self.autonomous_digging_process = asyncio.ensure_future( self.auto_dig_procedure() ) # Start the auto dig process - elif self.state == states["Auto_Dig"]: + elif self.state == states["Autonomous"]: self.autonomous_digging_process.cancel() # Terminate the auto dig process + self.autonomous_digging_process = None # Check if the autonomous offload button is pressed if msg.buttons[LEFT_BUMPER] == 1 and buttons[LEFT_BUMPER] == 0: if self.state == states["Teleop"]: self.stop_all_subsystems() # Stop all subsystems - self.state = states["Auto_Offload"] + self.state = states["Autonomous"] self.autonomous_offload_process = asyncio.ensure_future( self.auto_offload_procedure() ) # Start the auto dig process - elif self.state == states["Auto_Offload"]: + elif self.state == states["Autonomous"]: self.autonomous_offload_process.cancel() # Terminate the auto offload process + self.autonomous_offload_process = None # Update button states (this allows us to detect changing button states) for index in range(len(buttons)): @@ -309,6 +472,7 @@ def main(args=None) -> None: loop.run_until_complete(spin(executor)) # Run the spin function in the event loop # Clean up and shutdown + node.nav2.lifecycleShutdown() node.destroy_node() rclpy.shutdown() diff --git a/src/skimmer/skimmer/skimmer_node.py b/src/skimmer/skimmer/skimmer_node.py index 19811b63..f33cde82 100644 --- a/src/skimmer/skimmer/skimmer_node.py +++ b/src/skimmer/skimmer/skimmer_node.py @@ -64,7 +64,7 @@ def __init__(self): # Current position of the lift motor in degrees self.current_position_degrees = 0 # Relative encoders always initialize to 0 # Goal Threshold (if abs(self.current_goal_position - ACTUAL VALUE) <= self.goal_threshold then we should publish True to /skimmer/goal_reached) - self.goal_threshold = 180 # in degrees of the motor # TODO: Tune this threshold if needed + self.goal_threshold = 320 # in degrees of the motor # TODO: Tune this threshold if needed # Current state of the lift system self.lift_running = False @@ -188,9 +188,8 @@ def timer_callback(self): def done_callback(self, future): self.current_position_degrees = future.result().data - goal_reached_msg = Bool( - data=abs(self.current_goal_position - self.current_position_degrees) <= self.goal_threshold + data=abs(self.current_goal_position + self.lift_encoder_offset - self.current_position_degrees) <= self.goal_threshold ) self.publisher_goal_reached.publish(goal_reached_msg) @@ -205,12 +204,12 @@ def limit_switch_callback(self, limit_switches_msg): self.bottom_limit_pressed = limit_switches_msg.bottom_limit_switch if self.top_limit_pressed: # If the top limit switch is pressed self.lift_encoder_offset = self.current_position_degrees - self.get_logger().info("Current position in degrees: " + str(self.current_position_degrees)) - self.get_logger().info("New lift encoder offset: " + str(self.lift_encoder_offset)) + self.get_logger().debug("Current position in degrees: " + str(self.current_position_degrees)) + self.get_logger().debug("New lift encoder offset: " + str(self.lift_encoder_offset)) elif self.bottom_limit_pressed: # If the bottom limit switch is pressed self.lift_encoder_offset = self.current_position_degrees - self.MAX_ENCODER_DEGREES - self.get_logger().info("Current position in degrees: " + str(self.current_position_degrees)) - self.get_logger().info("New lift encoder offset: " + str(self.lift_encoder_offset)) + self.get_logger().debug("Current position in degrees: " + str(self.current_position_degrees)) + self.get_logger().debug("New lift encoder offset: " + str(self.lift_encoder_offset)) def main(args=None):