William dev

CMakeLists.txt

@@ -84,7 +84,7 @@ target_include_directories(bno_node
         $<INSTALL_INTERFACE:include>)
 
 ament_target_dependencies(bno_node rclcpp std_msgs sensor_msgs geometry_msgs)
-ament_target_dependencies(thrusters rclcpp std_msgs sensor_msgs ament_index_cpp)
+ament_target_dependencies(thrusters rclcpp std_msgs sensor_msgs geometry_msgs ament_index_cpp)
 
 install(TARGETS
         sh2lib

README.md

@@ -19,6 +19,54 @@
 ## Build
 1. `cd [YOUR WORKSPACE]` (Don't build inside the package)
 2. `colcon build`
+
+## Launch / Run
+After building and sourcing the workspace, you can run nodes individually or use the provided launch file to start both IMU and thruster nodes.
+
+Run nodes individually:
+```bash
+source install/local_setup.bash
+ros2 run wu25 bno_node
+ros2 run wu25 thrusters
+```
+
+Run both nodes with the packaged launch file (this launch uses a `sudo -E` prefix to preserve environment variables when running nodes that require root access):
+```bash
+source install/local_setup.bash
+ros2 launch wu25 start_nodes.launch.py
+```
+
+Systemd (run server on boot)
+--------------------------------
+You can register the control server (the FastAPI app in `tools/rov_control_server.py`) as a systemd service on the Orange Pi so it starts on boot.
+
+Create `/etc/systemd/system/rov-control.service` with the following contents (adjust paths and user):
+
+```ini
+[Unit]
+Description=ROV Control Server
+After=network.target
+
+[Service]
+Type=simple
+User=pi
+WorkingDirectory=/home/pi/ros2_ws/src/WU25
+Environment=PYTHONUNBUFFERED=1
+ExecStart=/home/pi/ros2_ws/src/WU25/.venv/bin/python3 tools/rov_control_server.py
+Restart=on-failure
+
+[Install]
+WantedBy=multi-user.target
+```
+
+Then enable and start:
+```bash
+sudo systemctl daemon-reload
+sudo systemctl enable rov-control.service
+sudo systemctl start rov-control.service
+sudo journalctl -u rov-control.service -f
+```
+
 ## Enabling `spidev` (Orange Pi 5)
 1. Copy device tree blob file from firmware files 
 

include/thrusters/thrusters.hpp

@@ -171,6 +171,12 @@ class Thrusters {
 
     void SetDesiredRotation(const Eigen::Quaternionf &q);
 
+    void SetRotationPIDGains(float p, float i, float d,
+                             float i_zone = std::numeric_limits<float>::max(),
+                             float max_output = std::numeric_limits<float>::max());
+
+    void SetDepthPIDGains(float kp, float ki, float kd);
+
     static Eigen::Vector3f CalculateAngVel(const Eigen::Quaternionf &q1, const Eigen::Quaternionf &q2, float dt);
 
     static float Thrusters::CalculateInclination(const Eigen::Quaternionf& rot);
@@ -228,18 +234,18 @@ class Thrusters {
     ThrusterDecomp decomp_horizontal_;
     ThrusterDecomp decomp_vertical_;
 
-    QuatPIDController::PIDParams x_params_;
-    QuatPIDController::PIDParams y_params_;
-    QuatPIDController::PIDParams z_params_;
+    QuatPIDController::PIDParams x_params_{.p = 1.0f};
+    QuatPIDController::PIDParams y_params_{.p = 1.0f};
+    QuatPIDController::PIDParams z_params_{.p = 1.0f};
     QuatPIDController idle_rotation_controller_;
 
     PIDController x_omega_controller_{0, 0, 0};
     PIDController y_omega_controller_{0, 0, 0};
     PIDController z_omega_controller_{0, 0, 0};
     PIDController idle_depth_controller_{0, 0, 0};
 
-    std::chrono::system_clock::time_point rotation_recieved_time_ns_;
-    std::chrono::system_clock::time_point previous_rotation_recieved_time_;
+    std::chrono::high_resolution_clock::time_point rotation_recieved_time_ns_;
+    std::chrono::high_resolution_clock::time_point previous_rotation_recieved_time_;
 
     std::array<float, 8> pwm_outputs_{};
 

include/util/pid_controller.hpp

@@ -14,6 +14,8 @@ class PIDController {
 
     void SetSetpoint(float setpoint);
 
+    void SetGains(float kP, float kI, float kD, float kFF = 0.0f);
+
     float Calculate(float current_state);
 
 private:

include/util/quaternion_pid.hpp

@@ -29,6 +29,11 @@ class QuatPIDController {
 
     void SetSetpoint(const Eigen::Quaternionf &setpoint);
 
+    void SetParams(float p, float i, float d,
+                   float i_zone = std::numeric_limits<float>::max(),
+                   float i_max_accum = std::numeric_limits<float>::max(),
+                   float max_output = std::numeric_limits<float>::max());
+
     Eigen::Vector3f Calculate(const Eigen::Quaternionf &current_rotation);
 
     Eigen::Quaternionf GetError() const;

launch/start_nodes.launch.py

@@ -0,0 +1,38 @@
+from launch import LaunchDescription
+from launch.actions import ExecuteProcess
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    # Prefix to run nodes under sudo while preserving environment variables
+    sudo_prefix = [
+        "sudo -E env \"PYTHONPATH=$PYTHONPATH\" \"LD_LIBRARY_PATH=$LD_LIBRARY_PATH\" \"PATH=$PATH\" \"USER=$USER\"  bash -c "
+    ]
+
+    return LaunchDescription([
+        Node(
+            package='wu25',
+            executable='bno_node',
+            name='bno_node',
+            output='screen',
+            prefix=sudo_prefix,
+            shell=True,
+        ),
+        Node(
+            package='wu25',
+            executable='thrusters',
+            name='thrusters',
+            output='screen',
+            prefix=sudo_prefix,
+            shell=True,
+        ),
+        # Start the ROS->HTTP bridge script as part of the launch (with sudo -E env)
+        ExecuteProcess(
+            cmd=[
+                'sudo', '-E', 'env', 'PYTHONPATH=$PYTHONPATH', 'LD_LIBRARY_PATH=$LD_LIBRARY_PATH',
+                'PATH=$PATH', 'USER=$USER', 'bash', '-lc',
+                "source ~/ros2_ws/install/local_setup.bash >/dev/null 2>&1; export ROS_DOMAIN_ID=42; nohup python3 /home/pi/ros2_ws/src/WU25/tools/ros_to_http.py >/tmp/ros_to_http.log 2>&1 &"
+            ],
+            output='screen',
+        ),
+    ])

requirements.txt

@@ -0,0 +1,3 @@
+fastapi
+uvicorn[standard]
+paramiko