Add Zed Servo Node

src/description/ros2_control/drive/drive.servo.ros2_control.xacro

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <xacro:macro name="servo_ros2_control" params="name">
+        <ros2_control name="${name}" type="system">
+            <hardware>
+                <plugin>servo_ros2_control/SERVOHardwareInterface</plugin>
+                <param name="update_rate">20</param> <!-- Hz -->
+                <param name="logger_rate">5</param> <!-- Hz -->
+                <param name="logger_state">1</param> <!-- 0 for off, 1 for on -->
+                <param name="can_interface">can0</param>
+                <param name="can_id">0x80</param>
+            </hardware>
+
+            <joint name="zed_servo_joint">
+                <param name="node_id">0x1</param>
+                <param name="gear_ratio">1</param>
+                <param name="max_pos">235</param> <!-- degrees -->
+                <param name ="meters_per_deg">0.00016666666</param> <!-- (TODO) convert motor position in degrees to displacement in meters -->
+                <param name="servo_type">standard</param> <!-- standard or continuous -->
+                <param name="joint_type">revolute</param> <!-- revolute or prismatic -->
+                <command_interface name="position"/>
+                <command_interface name="velocity"/>
+                <state_interface name="position"/>
+                <state_interface name="velocity"/>
+            </joint>
+        </ros2_control>
+    </xacro:macro>
+</robot>

src/description/urdf/athena_drive.urdf.xacro

@@ -13,6 +13,7 @@
 
   <xacro:include filename="$(find description)/urdf/athena_drive/athena_drive_macro.xacro"/>
   <xacro:include filename="$(find description)/urdf/athena_drive/athena_drive_macro.ros2_control.xacro"/>
+
   <xacro:include filename="imu.xacro"/>
   <xacro:include filename="gps.xacro"/>
   <xacro:include filename="ground_truth_odom.xacro"/>
@@ -42,6 +43,9 @@
 
   <!-- Import power_module ros2_control description -->
   <xacro:include filename="$(find description)/ros2_control/drive/drive.power_module.ros2_control.xacro" />
+
+  <!-- Added servo_drive here -->
+  <xacro:include filename="$(find description)/ros2_control/drive/drive.servo.ros2_control.xacro"/>
 
   <!-- Contains description of drive -->
   <xacro:description/>
@@ -83,6 +87,9 @@
 
     <!-- PowerModule (CAN-based) -->
     <xacro:power_module_ros2_control name="drive_power_module" can_interface="$(arg can_interface)"/>
+
+    <!-- Servo control for Zed camera -->
+    <xacro:servo_ros2_control name="zed_servo_control"/>
   </xacro:unless>
 
 

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

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":

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
   "$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>"
   "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+#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")

src/subsystems/drive/drive_controllers/package.xml

@@ -24,6 +24,8 @@
   <depend>realtime_tools</depend>
   <depend>std_srvs</depend>
   <depend>tf2_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>position_controllers</depend>
 
   <export>
     <build_type>ament_cmake</build_type>

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),

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(<dependency> 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)

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,
         ]

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()