diff --git a/nav2_config b/nav2_config new file mode 160000 index 00000000..890800cb --- /dev/null +++ b/nav2_config @@ -0,0 +1 @@ +Subproject commit 890800cbc0b1410cbcbbf97fa9bf3a7204d44856 diff --git a/src/description/ros2_control/drive/drive.servo.ros2_control.xacro b/src/description/ros2_control/drive/drive.servo.ros2_control.xacro new file mode 100644 index 00000000..3d5d06fc --- /dev/null +++ b/src/description/ros2_control/drive/drive.servo.ros2_control.xacro @@ -0,0 +1,28 @@ + + + + + + servo_ros2_control/SERVOHardwareInterface + 20 + 5 + 1 + can0 + 0x80 + + + + 0x1 + 1 + 235 + 0.00016666666 + standard + revolute + + + + + + + + diff --git a/src/description/urdf/athena_drive.urdf.xacro b/src/description/urdf/athena_drive.urdf.xacro index 294ff484..aaaa84cb 100644 --- a/src/description/urdf/athena_drive.urdf.xacro +++ b/src/description/urdf/athena_drive.urdf.xacro @@ -13,6 +13,7 @@ + @@ -42,6 +43,9 @@ + + + @@ -83,6 +87,9 @@ + + + diff --git a/src/subsystems/drive/drive_bringup/config/athena_drive_controllers.yaml b/src/subsystems/drive/drive_bringup/config/athena_drive_controllers.yaml index 27319c2c..99deacc6 100644 --- a/src/subsystems/drive/drive_bringup/config/athena_drive_controllers.yaml +++ b/src/subsystems/drive/drive_bringup/config/athena_drive_controllers.yaml @@ -54,7 +54,15 @@ controller_manager: rear_ackermann_controller: type: athena_drive_controllers/RearAckermannController - + + #Generic Position Controller that exists in ros2 humble workspace + zed_servo_controller: + type: position_controllers/JointGroupPositionController + #Generic Position Controller that exists in ros2 humble workspace + zed_servo_controller: + type: position_controllers/JointGroupPositionController + #zed_servo_controller: + #type:athena_drive_controllers/ZedServoController ackermann_steering_controller: # unsure why but ackermann controller only goes about half of scale_linear.x in teleop_twist # ackermann controller turns to at max scale_angular.yaw*pi. Seems to ignore how much you input, relies more on how fast rover is going @@ -196,3 +204,9 @@ rear_ackermann_controller: wheel_radius: 0.125 max_speed: 0.59 max_steer_angle: 0.785398 + +#Added this controller +zed_servo_controller: + ros__parameters: + joints: + - zed_servo_joint diff --git a/src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py b/src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py index 91999751..b8612897 100644 --- a/src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py +++ b/src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py @@ -256,6 +256,12 @@ def launch_setup(context, *args, **kwargs): arguments=["motor_status_controller", "-c", "/controller_manager"], ) + zed_servo_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["zed_servo_contoller", "-c", "/controller_manager"], + ) + robot_controller_names = [robot_controller] robot_controller_spawners = [] for controller in robot_controller_names: @@ -303,7 +309,6 @@ def launch_setup(context, *args, **kwargs): ) ) ] - controller_switcher_node = RegisterEventHandler( event_handler=OnProcessExit( target_action=inactive_robot_controller_spawners[-1], @@ -378,6 +383,7 @@ def launch_setup(context, *args, **kwargs): jetson_actions = [ control_node, + zed_servo_spawner, robot_state_pub_node, delay_joint_state_broadcaster_spawner_after_ros2_control_node, delay_motor_status_controller_after_joint_state_broadcaster, @@ -391,7 +397,6 @@ def launch_setup(context, *args, **kwargs): joystick_publisher, teleop_twist_joy, ] - if mode == "jetson": return jetson_actions elif mode == "base_station": diff --git a/src/subsystems/drive/drive_controllers/CMakeLists.txt b/src/subsystems/drive/drive_controllers/CMakeLists.txt index 20bc96b9..39c4d4e2 100644 --- a/src/subsystems/drive/drive_controllers/CMakeLists.txt +++ b/src/subsystems/drive/drive_controllers/CMakeLists.txt @@ -13,11 +13,13 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS hardware_interface nav_msgs pluginlib + position_controllers rclcpp rclcpp_lifecycle realtime_tools std_srvs tf2_msgs + std_msgs ) find_package(ament_cmake REQUIRED) @@ -55,6 +57,12 @@ generate_parameter_library(front_ackermann_controller_parameters generate_parameter_library(rear_ackermann_controller_parameters config/rear_ackermann_controller.yaml ) +##.yaml or .hpp? Added the hpp if needed +##included controller_parameters +# generate_parameter_library(zed_servo_controller_parameters +# config/zed_servo_controller.yaml +# include/athena_drive_controllers/validate_drive_controller_parameters.hpp +# ) taken out for testing since we dont need to generate a library for this add_library( drive_controllers @@ -63,10 +71,12 @@ add_library( src/double_ackermann_controller.cpp src/front_ackermann_controller.cpp src/rear_ackermann_controller.cpp +# src/zed_servo_controller.cpp ) target_include_directories(drive_controllers PUBLIC "$" "$") +#zed_servo_controller_parameters target_link_libraries(drive_controllers swerve_drive_controller_parameters double_ackermann_controller_parameters crab_steering_controller_parameters front_ackermann_controller_parameters rear_ackermann_controller_parameters umdloop_can_library::umdloop_can_library) ament_target_dependencies(drive_controllers ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_compile_definitions(drive_controllers PRIVATE "SWERVE_DRIVE_CONTROLLER_BUILDING_DLL") diff --git a/src/subsystems/drive/drive_controllers/package.xml b/src/subsystems/drive/drive_controllers/package.xml index cbca0ab3..b195f87f 100644 --- a/src/subsystems/drive/drive_controllers/package.xml +++ b/src/subsystems/drive/drive_controllers/package.xml @@ -24,6 +24,8 @@ realtime_tools std_srvs tf2_msgs + std_msgs + position_controllers ament_cmake diff --git a/src/subsystems/navigation/athena_planner/launch/navigation.launch.py b/src/subsystems/navigation/athena_planner/launch/navigation.launch.py index 5cd7535e..336ec615 100644 --- a/src/subsystems/navigation/athena_planner/launch/navigation.launch.py +++ b/src/subsystems/navigation/athena_planner/launch/navigation.launch.py @@ -21,14 +21,14 @@ def generate_launch_description(): localizer_launch_file = os.path.join(localizer_share, 'launch', 'localizer.launch.py') zed_tf_publisher_launch_file = os.path.join(localizer_share, 'launch', 'zed_tf_publisher.launch.py') - gps_goal_share = get_package_share_directory('gps_goal') - gps_goal_launch_file = os.path.join(gps_goal_share, 'launch', 'gps_goal_server.launch.py') + # gps_goal_share = get_package_share_directory('gps_goal') + # gps_goal_launch_file = os.path.join(gps_goal_share, 'launch', 'gps_goal_server.launch.py') sensors_share = get_package_share_directory('athena_sensors') sensors_launch_file = os.path.join(sensors_share, 'launch', 'sensors.launch.py') - aruco_bt_share = get_package_share_directory('aruco_bt') - aruco_launch_file = os.path.join(aruco_bt_share, 'launch', 'aruco.launch.py') + # aruco_bt_share = get_package_share_directory('aruco_bt') + # aruco_launch_file = os.path.join(aruco_bt_share, 'launch', 'aruco.launch.py') waypoint_share = get_package_share_directory('waypoint_manager') waypoint_launch_file = os.path.join(waypoint_share, 'launch', 'waypoint_manager.launch.py') @@ -102,10 +102,10 @@ def generate_launch_description(): }.items(), ) - aruco_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource(aruco_launch_file), - launch_arguments={'use_sim_time': sim, 'marker_size': '0.20'}.items() - ) + # aruco_launch = IncludeLaunchDescription( + # PythonLaunchDescriptionSource(aruco_launch_file), + # launch_arguments={'use_sim_time': sim, 'marker_size': '0.20'}.items() + # ) waypoint_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource(waypoint_launch_file), @@ -197,7 +197,7 @@ def generate_launch_description(): yolo_ros_launch, point_cloud_filterer_sim, point_cloud_relay, - gps_goal_launch, + # gps_goal_launch, IncludeLaunchDescription( PythonLaunchDescriptionSource(nav2_nav), diff --git a/src/subsystems/science/science_bringup/CMakeLists.txt b/src/subsystems/science/science_bringup/CMakeLists.txt index 280abdff..7614227d 100644 --- a/src/subsystems/science/science_bringup/CMakeLists.txt +++ b/src/subsystems/science/science_bringup/CMakeLists.txt @@ -7,18 +7,39 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) - +find_package(rosidl_default_generators REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(rosidl_default_runtime REQUIRED) +rosidl_generate_interfaces(${PROJECT_NAME} + "srv/SetController.srv" + "srv/TakePanorama.srv" + DEPENDENCIES builtin_interfaces +) + +rosidl_generate_interfaces(${PROJECT_NAME} + "srv/SetController.srv" + "srv/TakePanorama.srv" + DEPENDENCIES builtin_interfaces +) + install( DIRECTORY config launch DESTINATION share/${PROJECT_NAME} ) +install(PROGRAMS +<<<<<<< HEAD + scripts/zed_servo_node.py +======= + scripts/zed_arduino_node.py +>>>>>>> e6025d0 (Drive Changes) + DESTINATION lib/${PROJECT_NAME} +) +install(PROGRAMS + scripts/zed_arduino_node.py + DESTINATION lib/${PROJECT_NAME} +) if(BUILD_TESTING) diff --git a/src/subsystems/science/science_bringup/launch/athena_science.launch.py b/src/subsystems/science/science_bringup/launch/athena_science.launch.py index fdf3be13..51f19dfc 100644 --- a/src/subsystems/science/science_bringup/launch/athena_science.launch.py +++ b/src/subsystems/science/science_bringup/launch/athena_science.launch.py @@ -181,6 +181,32 @@ def generate_launch_description(): output="both", parameters=[robot_description, robot_controllers], ) + #controller in drive + #node in science + #can launch drive + #package sciecne_bringup + zed_servo_node = RegisterEventHandler( + event_handler=OnProcessStart( + target_action=control_node, + on_start=[TimerAction( + period=8.0, # enough time for everything to settle + actions=[ + Node( + package="science_bringup", + executable="zed_servo_node.py", + name="zed_servo_node", + output="screen" + ), + Node( + package="science_bringup", + executable="zed_arduino_node.py", + name="zed_arduino_node", + output="screen" + ) + ] + )] + ) + ) robot_state_pub_node = Node( package="robot_state_publisher", executable="robot_state_publisher", @@ -206,7 +232,6 @@ def generate_launch_description(): executable="spawner", arguments=["motor_status_controller", "-c", "/controller_manager"], ) - # CONTROLLER MANAGERS '''robot_controller_spawner = Node( @@ -359,6 +384,7 @@ def generate_launch_description(): delay_joint_state_broadcaster_spawner_after_ros2_control_node, delay_motor_status_controller_after_joint_state_broadcaster, # umdloop_can_node, + zed_servo_node, controller_switcher_node, joystick_publisher, ] diff --git a/src/subsystems/science/science_bringup/scripts/zed_arduino_node.py b/src/subsystems/science/science_bringup/scripts/zed_arduino_node.py new file mode 100644 index 00000000..cfae62e9 --- /dev/null +++ b/src/subsystems/science/science_bringup/scripts/zed_arduino_node.py @@ -0,0 +1,44 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +#from std_msgs.msg import String,Int32, Float64MultiArray + +from std_msgs.msg import Int32,Float32 +import pyzed.sl as sl +import cv2 as cv +import math +import numpy as np +from pyfirmata import Arduino, util +import time + +class Zed_Servo_Subscriber(Node): + #angle: 0.0 - 360.0 | frames = 100 - 2000 + # ros2 run node 360, 2000 0 0 + def __init__(self): + super().__init__(node_name='zed_servo_subscriber') + self.get_logger().info("Called Zed Servo Subscriber") + self.board = Arduino("COM4")#Whichever port it is, can edit + self.servo = self.board.get_pin('d:9:s') #Servo on pin 9 Also can edit + time.sleep(2) # Wait for board to initialize + self.board.samplingOn() + self.subscriber = self.create_subscription( + Float32, + 'zed_servo_data', + self.servo_callback, + 10 + ) + def servo_callback(self,msg): + angle = msg.data + try: + self.servo.write(angle)#[0] + except Exception as e: + self.get_logger().info(f"Failed to rotate servo:{e}") +def main(args=None): + rclpy.init(args=args) # Initializes the ros2 communication + node = Zed_Servo_Subscriber() + rclpy.spin(node=node) # Node spin = Keep this Node alive until we kill it + #Maybe spin it up once then kill the subscription to get the location + rclpy.shutdown() #Shutsdown a Node + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/src/subsystems/science/science_bringup/scripts/zed_servo_node.py b/src/subsystems/science/science_bringup/scripts/zed_servo_node.py new file mode 100644 index 00000000..4766e698 --- /dev/null +++ b/src/subsystems/science/science_bringup/scripts/zed_servo_node.py @@ -0,0 +1,227 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from std_msgs.msg import String,Int32, Float64MultiArray,Float32 +from science_bringup.srv import TakePanorama +import pyzed.sl as sl +import cv2 as cv +import math +import numpy as np +from pathlib import Path +import time +import os +#To run the following node: +#ros2 run drive_bringup zed_servo_node.py (I kinda forgot where the node was supposed to be) +#ros2 run science_bringup zed_servo_node.py +# +#Panorama Service sits in science +#ros2 service call /take_panorama science_bringup/TakePanorama "{take_panorama: True, frames: 31, max_angle: 270.0}" + +#I think I was implied to make this but I never made this +#Zed Servo rotate left/right +#ros2 service call /servo_turn science_bringup/Servo_Turn "data:[]" (degree to turn to)/270 + +#Mulitple nodes within the same porgram... + +cam_dir = [sl.VIEW.LEFT,sl.VIEW.SIDE_BY_SIDE,sl.VIEW.RIGHT] +SUCCESS_CODE = sl.ERROR_CODE.SUCCESS + +#MAT object to CV2 object +def slMat2cvMat(sl_mat:sl.Mat) -> cv.Mat: + return cv.cvtColor(crop_black_borders(sl_mat.get_data()),cv.COLOR_BGRA2BGR) +def crop_black_borders(image): + gray = cv.cvtColor(image, cv.COLOR_BGR2GRAY) + _, thresh = cv.threshold(gray, 1, 255, cv.THRESH_BINARY) + contours, _ = cv.findContours(thresh, cv.RETR_EXTERNAL, cv.CHAIN_APPROX_SIMPLE) + x, y, w, h = cv.boundingRect(contours[0]) + return image[y:y+h, x:x+w] + +class Zed_Servo_Node(Node): + #Node Parameters: take_panorama:bool <-- Do we take the panorama? + #angle: 0.0 - 360.0 | frames = 100 - 2000 + def __init__(self): + #Ros2 service call + super().__init__(node_name="zed_servo_node") + #Node needs to be turned on/off + # Then in your camera init: + + self.init_param = sl.InitParameters() + self.runtime_param = sl.RuntimeParameters() + self.tracking_parameter = sl.PositionalTrackingParameters() + self.cv_image_list = [] + self.camera = sl.Camera() + self.pose = sl.Pose() + self.get_logger().info(f"Called Panorama Node") + + self.service = self.create_service( + TakePanorama, + 'take_panorama', + self.handlePanorama + ) + + self.servo_publisher = self.create_publisher( + Float32, + 'zed_servo_publisher', + 10 + ) + #Make this a service + #frames to take + #take_panorama.srv + # take_panorama = take pictures or not + # max_angle = angle to turn to, + # frames = Frames to capture + + #creates the GPS Subscription here + + ##Subscribe to the heading + ##heading_topic + self.subscription = self.create_subscription(#can you check if the subscription is correct? + String, + "sensor_msgs/msg/NavSatFix", + self.set_header, + 10 + ) + + #########Initializes all Camera Parameters############## + self.init_param.camera_resolution=sl.RESOLUTION.HD2K#Test with 1080|2K #We have to use 720, because images aren't wide enough + self.init_param.camera_fps = 15 + + # # Refer here: https://www.stereolabs.com/docs/positional-tracking/coordinate-frames + # RIGHT_HANDED_Z_UP_X_FWD is apparently ROS2 standard + + self.init_param.coordinate_system = sl.COORDINATE_SYSTEM.RIGHT_HANDED_Z_UP_X_FWD + self.init_param.coordinate_units = sl.UNIT.METER + + #May be changed from depth mode into an active depth mode + self.init_param.depth_mode = sl.DEPTH_MODE.PERFORMANCE #PERFORMANCE is fastest + + cam_status = self.camera.open(self.init_param) + if(cam_status!=SUCCESS_CODE): + self.get_logger().info("Camera unable to be fetched") + exit(1) + + #Gets the camera information to confirm serial number called + cam_info = self.camera.get_camera_information() + self.get_logger().info(f"Confirm Camera serial Number: {cam_info.serial_number}") + + ###Camera status to check if positional tracking status has been opened successfully + positional_status = self.camera.enable_positional_tracking(self.tracking_parameter) + if(positional_status!=SUCCESS_CODE): + self.get_logger().info("Error: Positional Tracking not working") + exit(1) + + def handlePanorama(self,request,response): + if(request.take_panorama): + self.get_logger().info("Panormic Service called") + try: + self.max_angle = request.max_angle + frames = request.frames + #And then we pass this off into the take_panorama for it's initial position + #self.take_panorama() + self.get_logger().info(f"Service call: {self.max_angle} degrees and {frames}") + response.message = "Panoramic Shots successfully captured" + response.success = True + except Exception as e: + self.get_logger().info(f"Panoramic service Failed: \n{e}") + response.success = False + else: + #Deactivate the panoramic shots + self.get_logger().info("Deactivating Panoramic Shots") + response.message = "Deactivated Panoramic photos" + response.success = False + return response + + def take_panorama(self): + total_angle = self.max_angle + frames = self.frames + + #If Current object at the current run time parameter is active + if(frames%2==0): + self.get_logger().info("Requires odd amount of images") + return -1 + if(frames<3): + self.get_logger().info("More than 2 photos required") + return -1 + #Initialize Matrix here + img = sl.Mat() + i = 0 + cv_image_list = [] + current_angle = 0.0 + #Makes directory called temp_photos if one doesn't exist + dir_path = "temp_photos"#Please Help me name this something official (or whatever is good with base station) + os.makedirs(dir_path, exist_ok=True) + #Iterate through X Amount of images + Rotations + rotate_amount = total_angle/(frames-1) + self.get_logger().info(f"Rotate_amount:{rotate_amount}") + + #31/2 = 15.5 -> 16 - 1 = 15 + mid_point = math.ceil(frames/2) + while i < frames: + #Move servo first before taking picture, except for the first picture + write_path = f"{dir_path}/img_"+str(i+1)+".png" + if i > 0: + current_angle += rotate_amount + #Actuate servo here + self.servo_publisher.publish(current_angle) + time.sleep(1.0) # Wait for servo to fully stop + if i == mid_point: + self.get_logger().info(f"Mid Point Reached at image {i+1}, current angle: {current_angle} degrees") + #GPS node can get "Header information first" + #Request or + #Takes picture + self.camera.grab(self.runtime_param) + self.camera.retrieve_image(img,sl.VIEW.LEFT) + + #Gets current time stamp + timestamp = self.camera.get_timestamp(sl.TIME_REFERENCE.IMAGE) + self.get_logger().info("Image resolution: {0} x {1} || Image timestamp: {2}\n".format(img.get_width(), img.get_height(), timestamp.get_milliseconds())) + #Converts image from sl object into cv2 object + cv_img = slMat2cvMat(img) + cv.imwrite(write_path, cv_img) + cv_image_list.append(write_path) + self.get_logger().info(f"Image: {i+1} captured") + i+=1 + #Read every photo from cv to convert into JPEG + self.get_logger().info(f"Stitching photos from {dir_path}") + self.stitch_photos(dir_path) + + def set_header(self, msg): + self.get_logger().info(f"Received: {msg.data}") + + #Helper Method to sstitch photos together + def stitch_photos(self,dir_path="pano_photos",out_image_name="panoramic_image"): + # Read the images from this path + folder_path = Path(dir_path) + photo_list = [] + # Iterate over all items in the directory and only adds the ok images + for item in folder_path.iterdir(): + if item.is_file() and item.name.startswith("img_") and item.name.endswith(".png"): + photo_list.append(item.name) + #Ensures images are read in order + photo_list.sort(key=lambda x: int(x.split('_')[1].split('.')[0])) + images = [cv.imread(str(folder_path / photo)) for photo in photo_list] + + # Create a stitcher object + stitcher = cv.Stitcher_create() + + # Stitch the images together + status, stitched_image = stitcher.stitch(images) + + if status == cv.Stitcher_OK: + # Save the stitched image + cv.imwrite(f"{dir_path}/{out_image_name}.png", stitched_image) + self.get_logger().info(f"Stitching completed successfully. Image saved as '{dir_path}/{out_image_name}.png'.") + else: + self.get_logger().info("Stitching failed with status code:", status) + +def main(args=None): + #Checks if Cython is being run + + rclpy.init(args=args) # Initializes the ros2 communication + node = Zed_Servo_Node() + rclpy.spin(node=node) # Node spin = Keep this Node alive until we kill it + #Maybe spin it up once then kill the subscription to get the location + rclpy.shutdown() #Shutsdown a Node + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/src/subsystems/science/science_bringup/srv/TakePanorama.srv b/src/subsystems/science/science_bringup/srv/TakePanorama.srv new file mode 100644 index 00000000..6fe1a1fc --- /dev/null +++ b/src/subsystems/science/science_bringup/srv/TakePanorama.srv @@ -0,0 +1,6 @@ +float32 max_angle #angle to rotate to +int32 frames +bool take_panorama +--- +bool success +string message \ No newline at end of file diff --git a/src/subsystems/science/science_controllers/package.xml b/src/subsystems/science/science_controllers/package.xml index 0917cbe1..be2c79b8 100644 --- a/src/subsystems/science/science_controllers/package.xml +++ b/src/subsystems/science/science_controllers/package.xml @@ -26,7 +26,14 @@ realtime_tools std_srvs - + + std_msgs + + position_controllers + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages ament_cmake