Merge branch 'dev' into drag-learning

.vscode/c_cpp_properties.json

@@ -0,0 +1,42 @@
+{
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "${default}",
+        "limitSymbolsToIncludedHeaders": false
+      },
+      "includePath": [
+        "/home/hollis/osu-uwrt/development/software/install/riptide_rviz/include/**",
+        "/home/hollis/osu-uwrt/development/software/install/riptide_gyro/include/**",
+        "/home/hollis/osu-uwrt/development/software/install/riptide_msgs2/include/**",
+        "/home/hollis/osu-uwrt/development/software/install/launch_msgs/include/**",
+        "/home/hollis/AMR/install/amrviz/include/**",
+        "/home/hollis/AMR/install/amr_msgs/include/**",
+        "/home/hollis/osu-uwrt/development/dependencies/install/zed_interfaces/include/**",
+        "/home/hollis/osu-uwrt/development/dependencies/install/vectornav/include/**",
+        "/home/hollis/osu-uwrt/development/dependencies/install/vectornav_msgs/include/**",
+        "/home/hollis/osu-uwrt/development/dependencies/install/nortek_dvl_msgs/include/**",
+        "/home/hollis/osu-uwrt/development/dependencies/install/chameleon_tf_msgs/include/**",
+        "/home/hollis/osu-uwrt/dependencies/install/stereo_image_proc/include/**",
+        "/home/hollis/osu-uwrt/dependencies/install/image_proc/include/**",
+        "/home/hollis/osu-uwrt/dependencies/install/tracetools_image_pipeline/include/**",
+        "/home/hollis/osu-uwrt/dependencies/install/nortek_dvl_msgs/include/**",
+        "/home/hollis/osu-uwrt/dependencies/install/microstrain_inertial_msgs/include/**",
+        "/home/hollis/osu-uwrt/dependencies/install/micro_ros_agent/include/**",
+        "/home/hollis/osu-uwrt/dependencies/install/micro_ros_msgs/include/**",
+        "/home/hollis/osu-uwrt/dependencies/install/image_view/include/**",
+        "/home/hollis/osu-uwrt/dependencies/install/image_rotate/include/**",
+        "/home/hollis/osu-uwrt/dependencies/install/image_publisher/include/**",
+        "/home/hollis/osu-uwrt/dependencies/install/depth_image_proc/include/**",
+        "/opt/ros/humble/include/**",
+        "/usr/include/**"
+      ],
+      "name": "ROS",
+      "intelliSenseMode": "gcc-x64",
+      "compilerPath": "/usr/bin/gcc",
+      "cStandard": "gnu11",
+      "cppStandard": "c++14"
+    }
+  ],
+  "version": 4
+}

.vscode/settings.json

@@ -0,0 +1,46 @@
+{
+    "python.autoComplete.extraPaths": [
+        "/home/hollis/osu-uwrt/development/software/install/riptide_hardware2/local/lib/python3.10/dist-packages",
+        "/home/hollis/osu-uwrt/development/software/install/riptide_msgs2/local/lib/python3.10/dist-packages",
+        "/home/hollis/osu-uwrt/development/software/install/riptide_bringup2/lib/python3.10/site-packages",
+        "/home/hollis/osu-uwrt/development/software/install/riptide_SNIB/lib/python3.10/site-packages",
+        "/home/hollis/osu-uwrt/development/software/install/remote_launch/lib/python3.10/site-packages",
+        "/home/hollis/osu-uwrt/development/software/install/launch_msgs/local/lib/python3.10/dist-packages",
+        "/home/hollis/AMR/install/amr_msgs/local/lib/python3.10/dist-packages",
+        "/home/hollis/osu-uwrt/riptide_setup/colcon_riptide/install/colcon-riptide/lib/python3.10/site-packages",
+        "/home/hollis/osu-uwrt/development/dependencies/install/zed_interfaces/local/lib/python3.10/dist-packages",
+        "/home/hollis/osu-uwrt/development/dependencies/install/vectornav_msgs/local/lib/python3.10/dist-packages",
+        "/home/hollis/osu-uwrt/development/dependencies/install/nortek_dvl_msgs/local/lib/python3.10/dist-packages",
+        "/home/hollis/osu-uwrt/development/dependencies/install/chameleon_tf_msgs/local/lib/python3.10/dist-packages",
+        "/home/hollis/osu-uwrt/dependencies/install/nortek_dvl_msgs/local/lib/python3.10/dist-packages",
+        "/home/hollis/osu-uwrt/dependencies/install/microstrain_inertial_rqt/lib/python3.10/site-packages",
+        "/home/hollis/osu-uwrt/dependencies/install/microstrain_inertial_msgs/local/lib/python3.10/dist-packages",
+        "/home/hollis/osu-uwrt/dependencies/install/micro_ros_msgs/local/lib/python3.10/dist-packages",
+        "/home/hollis/osu-uwrt/dependencies/install/echo_video/lib/python3.10/site-packages",
+        "/home/hollis/osu-uwrt/dependencies/install/camera_calibration/lib/python3.10/site-packages",
+        "/opt/ros/humble/lib/python3.10/site-packages",
+        "/opt/ros/humble/local/lib/python3.10/dist-packages"
+    ],
+    "python.analysis.extraPaths": [
+        "/home/hollis/osu-uwrt/development/software/install/riptide_hardware2/local/lib/python3.10/dist-packages",
+        "/home/hollis/osu-uwrt/development/software/install/riptide_msgs2/local/lib/python3.10/dist-packages",
+        "/home/hollis/osu-uwrt/development/software/install/riptide_bringup2/lib/python3.10/site-packages",
+        "/home/hollis/osu-uwrt/development/software/install/riptide_SNIB/lib/python3.10/site-packages",
+        "/home/hollis/osu-uwrt/development/software/install/remote_launch/lib/python3.10/site-packages",
+        "/home/hollis/osu-uwrt/development/software/install/launch_msgs/local/lib/python3.10/dist-packages",
+        "/home/hollis/AMR/install/amr_msgs/local/lib/python3.10/dist-packages",
+        "/home/hollis/osu-uwrt/riptide_setup/colcon_riptide/install/colcon-riptide/lib/python3.10/site-packages",
+        "/home/hollis/osu-uwrt/development/dependencies/install/zed_interfaces/local/lib/python3.10/dist-packages",
+        "/home/hollis/osu-uwrt/development/dependencies/install/vectornav_msgs/local/lib/python3.10/dist-packages",
+        "/home/hollis/osu-uwrt/development/dependencies/install/nortek_dvl_msgs/local/lib/python3.10/dist-packages",
+        "/home/hollis/osu-uwrt/development/dependencies/install/chameleon_tf_msgs/local/lib/python3.10/dist-packages",
+        "/home/hollis/osu-uwrt/dependencies/install/nortek_dvl_msgs/local/lib/python3.10/dist-packages",
+        "/home/hollis/osu-uwrt/dependencies/install/microstrain_inertial_rqt/lib/python3.10/site-packages",
+        "/home/hollis/osu-uwrt/dependencies/install/microstrain_inertial_msgs/local/lib/python3.10/dist-packages",
+        "/home/hollis/osu-uwrt/dependencies/install/micro_ros_msgs/local/lib/python3.10/dist-packages",
+        "/home/hollis/osu-uwrt/dependencies/install/echo_video/lib/python3.10/site-packages",
+        "/home/hollis/osu-uwrt/dependencies/install/camera_calibration/lib/python3.10/site-packages",
+        "/opt/ros/humble/lib/python3.10/site-packages",
+        "/opt/ros/humble/local/lib/python3.10/dist-packages"
+    ]
+}

riptide_controllers/CMakeLists.txt

@@ -47,6 +47,11 @@ install(DIRECTORY launch
   DESTINATION share/${PROJECT_NAME}
 )
 
+#install 
+install(DIRECTORY config 
+  DESTINATION share/${PROJECT_NAME}
+)
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   # the following line skips the linter which checks for copyrights

riptide_controllers/config/talos_autoff.yaml

@@ -0,0 +1 @@
+auto_ff: [0.0,0.0,0.0,0.0,0.0,0.0]

riptide_controllers/src/riptide_controllers/RocketLeague.py

@@ -44,11 +44,11 @@
         "dpad_right":ecodes.BTN_DPAD_RIGHT,
     },
     "SHANWAN":{
-        "left_joy_x":ecodes.ABS_X,
-        "left_joy_y":ecodes.ABS_Y,
+        "left_joy_x":ecodes.ABS_Z,
+        "left_joy_y":ecodes.ABS_RX,
         "left_trigger":ecodes.ABS_BRAKE,        
-        "right_joy_x":ecodes.ABS_Z,
-        "right_joy_y":ecodes.ABS_RX,
+        "right_joy_x":ecodes.ABS_X,
+        "right_joy_y":ecodes.ABS_Y,
         "right_trigger":ecodes.ABS_GAS,
         "a_btn":ecodes.BTN_A,        
         "b_btn":ecodes.BTN_B,        

riptide_controllers/src/riptide_controllers/controller_overseer.py

@@ -115,6 +115,7 @@ def __init__(self, node: 'ControllerOverseer', node_name: str):
         self.known_params = []
         self.reload_param_service = node.create_service(Trigger, f"controller_overseer/update_{self.node_name}_params".lower(), self.reload_parameters_callback)
         self.last_reload_time = node.get_clock().now()
+        self.loaded_params = False # have parameters been loaded
 
 
     # checks if the model is active. Handles when model comes up or goes down
@@ -238,6 +239,8 @@ def set_parameters_done_callback(self, future: rclpy.Future, request):
             for fail in fail_indices:
                 self.overseer_node.get_logger().error(f"Failed to set parameter {request.parameters[fail].name} for {self.node_name}: {result.results[fail].reason}")
 
+        self.loaded_params = True
+
 
     #gets a parameter value and type from the config
     def get_param_value(self, param_name):
@@ -307,6 +310,8 @@ def __init__(self):
         self.escPowerStopsLow = 0
         self.escPowerStopsHigh = 0
         self.configTree = {}
+        self.currentAutoTuneTwist = [0,0,0,0,0,0]
+        self.autoff_config_path = None
 
         #
         # Declare parameters 
@@ -324,6 +329,10 @@ def __init__(self):
         self.declare_parameter("thruster_solver_node_name", "")
         self.thrusterSolverName = self.get_parameter("thruster_solver_node_name").value
 
+        #declare write autotune param
+        self.declare_parameter("write_ff_autotune", True)
+        self.writeFFAutoTune = self.get_parameter("write_ff_autotune").value
+
         self.declare_parameter(FF_PUBLISH_PARAM, False)
 
         # Other setup
@@ -370,6 +379,8 @@ def __init__(self):
         #odometry filtered
         self.create_subscription(Odometry, "odometry/filtered", self.odometryCB, qos_profile_system_default)
 
+        #sub to autotune
+        self.create_subscription(Twist, "ff_auto_tune", self.ffAutoTuneCB, qos_profile_system_default)
 
         #pub for thruster weights
         self.weightsPub = self.create_publisher(Int32MultiArray, THRUSTER_SOLVER_WEIGHT_MATRIX_TOPIC, qos_profile_system_default)
@@ -448,14 +459,28 @@ def setConfigPath(self):
                         self.configPath = path_check
                         self.get_logger().info(f"Discovered source directory, overriding descriptions to use '{self.configPath}'")
                         break
-
-
+
+        #set the path to the autocal saved data file
+        control_share_dir = get_package_share_directory("riptide_controllers2")
+
+        auto_ff_subpath = os.path.join("config", self.robotName + "_autoff.yaml")
+
+         # Set fallback config path if we can't find the one in source
+        self.autoff_config_path = os.path.join(control_share_dir, auto_ff_subpath)
+
     def readConfig(self):
         try:
             with open(self.configPath, "r") as config:
                 self.configTree = yaml.safe_load(config)
         except:
             self.get_logger().error(f"Cannot open config file at {self.configPath}!")
+
+        try:
+            with open(self.autoff_config_path, "r") as config:
+                self.configTree["auto_ff"] = yaml.safe_load(config)
+                self.currentAutoTuneTwist = self.configTree["auto_ff"]["auto_ff"]
+        except:
+            self.get_logger().error(f"Cannot open config file at {self.autoff_config_path}!")
 
         # read specific values used by the class
 
@@ -479,7 +504,7 @@ def readConfig(self):
         self.surfaceWeight      = thruster_solver_info["surfaced_weight"]
         self.disabledWeight     = thruster_solver_info["disable_weight"]
         self.lowDowndraftWeight = thruster_solver_info["low_downdraft_weight"]
-        
+
 
     def thrusterTelemetryCB(self, msg: DshotPartialTelemetry):
         #wether or not the thrusterSolverweigths need adjusted
@@ -761,6 +786,37 @@ def doUpdate(self):
 
             self.ffPub.publish(msg)
 
+    def ffAutoTuneCB(self, msg):
+        #write the autotune save if it has changed
+
+        #if writing is disabled
+        if not self.writeFFAutoTune:
+            return
+
+        #if the overseer has not initialized the controller yet
+        if not self.model_nodes["complete_controller"].loaded_params:
+            return
+
+        #if the auto ff config path doesn't exist
+        if (self.autoff_config_path == None):
+            return 
+
+        #if the twist has been updated
+        if not (self.currentAutoTuneTwist[0] == msg.linear.x and self.currentAutoTuneTwist[1] == msg.linear.y and self.currentAutoTuneTwist[2] == msg.linear.z and 
+           self.currentAutoTuneTwist[3] == msg.angular.x and self.currentAutoTuneTwist[4] == msg.angular.y and self.currentAutoTuneTwist[5] == msg.angular.z):
+            #prepare string to be wrote
+            auto_ff_config_string = f"auto_ff: [{msg.linear.x},{msg.linear.y},{msg.linear.z},{msg.angular.x},{msg.angular.y},{msg.angular.z}]"
+
+            #write config to file
+            try:
+                with open(self.autoff_config_path, "w") as config:
+                    config.write(auto_ff_config_string)
+                    config.close()
+
+            except:
+                self.get_logger().error(f"Cannot open ff auto tune file at: {self.autoff_config_path}")
+
+
 
 def main(args=None):
     rclpy.init(args=args)