feat: andy messes up

src/guppy_control/config/controller_params.yaml

@@ -23,10 +23,10 @@ control_chassis:
     ] # flattend 6 * 6 matrix
 
     pid_gains_vel_linear: [ 20000.0, 0.0, 0.0 ]
-    pid_gains_vel_angular: [ 1000.0, 0.0, 0.0 ]
+    pid_gains_vel_angular: [ 100.0, 0.0, 0.0 ]
 
     pid_gains_pose_linear: [ 300.0, 0.0, 12.0 ]
-    pid_gains_pose_angular: [ 100.0, 0.0, 10.0 ]
+    pid_gains_pose_angular: [ 60.0, 0.0, 10.0 ]
 
     # pid_gains_vel_linear: [ 0.0, 0.0, 0.0 ]
     # pid_gains_vel_angular: [ 0.0, 0.0, 0.0 ]
@@ -49,6 +49,6 @@ control_chassis:
     drag_areas: [ 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 ]
     water_density: 1000.0 # kg/m^3
     robot_volume: 0.03963 # m^3
-    robot_mass: 35.0 # kg
-    center_of_buoyancy: [ 0.0, 0.0, -0.08 ]
+    robot_mass: 33.5 # kg
+    center_of_buoyancy: [ 0.0, 0.0, 0.0 ]
     qp_epsilon: 0.001

src/guppy_control/src/control_chassis_node.cpp

@@ -41,11 +41,10 @@ class ControlChassis : public rclcpp::Node {
     load_params_(&parameters); // load parameters from configuration file
 
     this->param_subscriber_ = std::make_shared<rclcpp::ParameterEventHandler>(this); // subscribe to parameter change event
-    
+
     // parameter callback to update parameters on event
 
     auto param_callback = [this](const rcl_interfaces::msg::ParameterEvent & parameter_event) {
-      RCLCPP_INFO(this->get_logger(), "Received parameter event from node for \"%s\"!", parameter_event.node.c_str());
       if (parameter_event.node != this->get_fully_qualified_name()) return; // quit if for another node
 
       auto controller_params = this->controller->get_param_struct(); // get copy of current parameters
@@ -55,11 +54,9 @@ class ControlChassis : public rclcpp::Node {
       bool update = false;
 
       for (const auto& param : parameter_event.changed_parameters) {
-        const std::string name = rclcpp::Parameter::from_parameter_msg(param).get_name();
+        const auto name = rclcpp::Parameter::from_parameter_msg(param).get_name();
         const auto value = rclcpp::Parameter::from_parameter_msg(param);
 
-        RCLCPP_INFO(this->get_logger(), "Parameter event received for '%s', ", parameter_event.node.c_str());
-
         // get entries (from maps below) with values of lambda/pointer based on the parameter name key
         auto pointer_it = param_map.find(name); // get pointer to parameter member in struct (pointer_it->second refers to value)
         auto transformer_it = transformers.find(name); // get transformer lambda to change simple vector to complex eigen type
@@ -73,19 +70,19 @@ class ControlChassis : public rclcpp::Node {
 
           using P = std::remove_cv_t<std::remove_reference_t<decltype(*pointer)>>; // get type at pointer/reference and strip const/volatile
           using R = std::remove_cv_t<std::remove_reference_t<std::invoke_result_t<decltype(transformer), const rclcpp::Parameter&>>>; // final result of calling lambda transformer, ie. transformed type
-          
+
           if constexpr (std::is_same_v<P, R>) { // ensure transformed type and type at pointer are the same (so you don't set bad data)
             std::string before, after;
             before = printer_it->second(static_cast<const void*>(pointer)); // ignore type of pointer as you pass to printer to format for debugging
-          
+
             auto tmp = transformer(value); // store transformed value (from double vector to actual parameter type ie. Eigen::Matrix) based on parameter name
-          
+
             after = printer_it->second(static_cast<const void*>(&tmp)); // ignore type and format result for debugging
-          
+
             *pointer = std::move(tmp); // fast copy for vector
 
             update = true; // parameter struct is dirty and should be updated
-          
+
             RCLCPP_INFO(this->get_logger(), "Parameter '%s' changed (%s)->(%s)", name.c_str(), before.c_str(), after.c_str());
           }
         }, pointer_it->second, transformer_it->second); // pass values into visit, must belong to defined variants
@@ -117,7 +114,7 @@ class ControlChassis : public rclcpp::Node {
 
     auto timer_callback = [this]() -> void {
       auto thrusts = controller->get_motor_thrusts();
-      
+
       for (int i = 0; i < 8; i++) {
         std_msgs::msg::Float32 thrust;
         thrust.data = (double)thrusts[i];
@@ -148,7 +145,7 @@ class ControlChassis : public rclcpp::Node {
   void cmdvel_callback(geometry_msgs::msg::Twist::SharedPtr msg) {
     controller->update_desired_state(msg);
   }
-  
+
   void state_callback(guppy_msgs::msg::State::SharedPtr msg) {
       if (msg->state == guppy_msgs::msg::State::DISABLED) {
           this->thruster_interface->set_enabled(false);
@@ -196,10 +193,10 @@ class ControlChassis : public rclcpp::Node {
 
   // transform simple vector/double types into complex eigen types with a lambda, based on parameter name
   std::unordered_map<std::string, ParameterTypes> transformers = {
-    { 
+    {
       "motor_positions",
       std::function<Eigen::Matrix<double, 6, N_MOTORS>(const rclcpp::Parameter&)>(
-        [](const rclcpp::Parameter& value) {  
+        [](const rclcpp::Parameter& value) {
           return ControlChassis::to_motor_coefficients<N_MOTORS>(value.as_double_array());
         }
       )
@@ -212,118 +209,118 @@ class ControlChassis : public rclcpp::Node {
         }
       )
     },
-    { 
+    {
       "motor_upper_bounds",
       std::function<Eigen::Matrix<double, N_MOTORS, 1>(const rclcpp::Parameter&)>(
         [](const rclcpp::Parameter& value) {
           return to_eigen_vec<N_MOTORS>(value.as_double_array());
         }
       )
     },
-    { 
+    {
       "axis_weight_matrix",
       std::function<Eigen::Matrix<double, 6, 6>(const rclcpp::Parameter&)>(
         [](const rclcpp::Parameter& value) {
           return to_eigen_matrix<6,6>(value.as_double_array());
         }
       )
     },
-    { 
+    {
       "pid_gains_vel_linear",
       std::function<std::vector<double>(const rclcpp::Parameter&)>(
         [](const rclcpp::Parameter& value) {
           return value.as_double_array();
         }
       )
     },
-    { 
+    {
       "pid_gains_vel_angular",
       std::function<std::vector<double>(const rclcpp::Parameter&)>(
         [](const rclcpp::Parameter& value) {
           return value.as_double_array();
         }
       )
     },
-    { 
+    {
       "pid_gains_pose_linear",
       std::function<std::vector<double>(const rclcpp::Parameter&)>(
         [](const rclcpp::Parameter& value) { return value.as_double_array();
         }
       )
     },
-    { 
+    {
       "pid_gains_pose_angular",
       std::function<std::vector<double>(const rclcpp::Parameter&)>(
         [](const rclcpp::Parameter& value) {
           return value.as_double_array();
         }
       )
     },
-    { 
+    {
       "pose_lock_deadband",
       std::function<Eigen::Matrix<double, 6, 1>(const rclcpp::Parameter&)>(
         [](const rclcpp::Parameter& value) {
           return to_eigen_vec<6>(value.as_double_array());
         }
       )
     },
-    { 
+    {
       "drag_coefficients",
       std::function<Eigen::Matrix<double, 6, 1>(const rclcpp::Parameter&)>(
         [](const rclcpp::Parameter& value) {
           return to_eigen_vec<6>(value.as_double_array());
         }
       )
     },
-    { 
+    {
       "drag_areas",
       std::function<Eigen::Matrix<double, 6, 1>(const rclcpp::Parameter&)>(
         [](const rclcpp::Parameter& value) {
           return to_eigen_vec<6>(value.as_double_array());
         }
       )
     },
-    { 
+    {
       "drag_effect_matrix",
       std::function<Eigen::Matrix<double, 6, 6>(const rclcpp::Parameter&)>(
         [](const rclcpp::Parameter& value) {
           return to_eigen_matrix<6,6>(value.as_double_array());
         }
       )
     },
-    { 
+    {
       "water_density",
       std::function<double(const rclcpp::Parameter&)>(
         [](const rclcpp::Parameter& value) {
           return value.as_double();
         }
       )
     },
-    { 
+    {
       "robot_volume",
       std::function<double(const rclcpp::Parameter&)>(
         [](const rclcpp::Parameter& value) {
           return value.as_double();
         }
       )
     },
-    { 
+    {
       "robot_mass",
       std::function<double(const rclcpp::Parameter&)>(
         [](const rclcpp::Parameter& value) {
           return value.as_double();
         }
       )
     },
-    { 
+    {
       "center_of_buoyancy",
       std::function<Eigen::Matrix<double, 3, 1>(const rclcpp::Parameter&)>(
         [](const rclcpp::Parameter& value) {
           return to_eigen_vec<3>(value.as_double_array());
         }
       )
     },
-    { 
+    {
       "qp_epsilon",
       std::function<double(const rclcpp::Parameter&)>(
         [](const rclcpp::Parameter& value) {
@@ -332,7 +329,7 @@ class ControlChassis : public rclcpp::Node {
       )
     }
   };
-  
+
   // formatting for eigen matrices/vectors
   inline static const Eigen::IOFormat LineFormat = Eigen::IOFormat(3, 0, ", ", "\n", "[", "]");
   inline static const Eigen::IOFormat InlineFormat = Eigen::IOFormat(3, 0, ", ", "", "", "");
@@ -434,7 +431,7 @@ class ControlChassis : public rclcpp::Node {
     },
     {
       "robot_volume",
-      [](const void* pointer) { 
+      [](const void* pointer) {
         return std::to_string(*static_cast<const double*>(pointer));
       }
     },
@@ -451,7 +448,7 @@ class ControlChassis : public rclcpp::Node {
       }
     }
   };
-  
+
   T200Interface* thruster_interface;
   ChassisController* controller;
 
@@ -472,7 +469,7 @@ class ControlChassis : public rclcpp::Node {
       std::visit([&](auto* pointer, auto& transformer) {
         using P = std::remove_cv_t<std::remove_reference_t<decltype(*pointer)>>;
         using R = std::invoke_result_t<decltype(transformer), const rclcpp::Parameter&>;
-        
+
         if constexpr (std::is_same_v<P, R>)
           *pointer = transformer(param);
       }, pointer_to, transformer_it->second);
@@ -482,15 +479,15 @@ class ControlChassis : public rclcpp::Node {
   // print current values of a parameter struct
   void print_params_struct_(ChassisController::ChassisControllerParams* params) {
     auto param_map = get_param_map(params); // get pointers to parameter struct members by parameter name
-    
+
     for (auto& [name, pointer_to] : param_map) {
       const auto printer_it = printers.find(name);
 
       if (printer_it == printers.end()) { // skip if no formatting lambda
         RCLCPP_WARN(this->get_logger(), "No printer registered for '%s', skipping.", name.c_str());
         continue;
       }
-      
+
       std::string rendered;
       std::visit(
         [&](auto* pointer) {
@@ -636,4 +633,4 @@ int main(int argc, char * argv[]) {
   rclcpp::shutdown();
 
   return 0;
-}
+}

src/guppy_localization/src/combine.cpp

@@ -54,36 +54,57 @@ void baro_callback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
     publish_transform();    
   }
 
-  void dvl_callback(nav_msgs::msg::Odometry::SharedPtr msg) {
-    Eigen::Vector3d axis_vector(0.0, 1.0, 0.0);
-    Eigen::AngleAxisd angle_axis(M_PI, axis_vector);
-    Eigen::Quaterniond quat(angle_axis);
+  void dvl_callback(nav_msgs::msg::Odometry::SharedPtr msg)
+{
+    // Rotate DVL frame -> body frame (your existing transform)
+    Eigen::Quaterniond q_dvl_to_body(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitY()));
 
-    Eigen::Vector3d position(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z);
-    Eigen::Vector3d twist(msg->twist.twist.linear.x, msg->twist.twist.linear.y, msg->twist.twist.linear.z);
+    Eigen::Vector3d position(msg->pose.pose.position.x,
+                             msg->pose.pose.position.y,
+                             msg->pose.pose.position.z);
 
-    position = quat * position;
-    twist = quat * twist;
+    Eigen::Vector3d twist(msg->twist.twist.linear.x,
+                          msg->twist.twist.linear.y,
+                          msg->twist.twist.linear.z);
 
-    odom.pose.pose.position.x = position[0] - initial_pose.x;
-    odom.pose.pose.position.y = position[1] - initial_pose.y;
-    // odom.pose.pose.position.z = position[2];
+    position = q_dvl_to_body * position;
+    twist    = q_dvl_to_body * twist;
 
-    double rx = 0.2;
-    double ry = 0.0;
-    double rz = -0.125;
-    double wx = odom.twist.twist.angular.x;
-    double wy = odom.twist.twist.angular.y;
-    double wz = odom.twist.twist.angular.z;
+    // DVL location relative to vehicle origin (body frame)
+    Eigen::Vector3d r_body(0.2, 0.0, -0.125);
 
-    odom.twist.twist.linear.x = twist[0] - (wy*rz - wz*ry);
-    odom.twist.twist.linear.y = twist[1] - (wz*rx - wx*rz);
-    odom.twist.twist.linear.z = twist[2] - (wx*ry - wy*rx);
+    // Current vehicle orientation (replace with your IMU orientation)
+    Eigen::Quaterniond q_world_body(
+        odom.pose.pose.orientation.w,
+        odom.pose.pose.orientation.x,
+        odom.pose.pose.orientation.y,
+        odom.pose.pose.orientation.z);
 
-    odom_pub_->publish(odom);
+    // Rotate lever arm into world frame
+    Eigen::Vector3d r_world = q_world_body * r_body;
 
-    publish_transform();    
-  }
+    // Correct position to vehicle origin
+    Eigen::Vector3d corrected_position = position - r_world;
+
+    odom.pose.pose.position.x = corrected_position.x() - initial_pose.x;
+    odom.pose.pose.position.y = corrected_position.y() - initial_pose.y;
+    odom.pose.pose.position.z = corrected_position.z() - initial_pose.z;
+
+    // Lever arm velocity correction
+    Eigen::Vector3d omega(
+        odom.twist.twist.angular.x,
+        odom.twist.twist.angular.y,
+        odom.twist.twist.angular.z);
+
+    Eigen::Vector3d corrected_twist = twist - omega.cross(r_body);
+
+    odom.twist.twist.linear.x = corrected_twist.x();
+    odom.twist.twist.linear.y = corrected_twist.y();
+    odom.twist.twist.linear.z = corrected_twist.z();
+
+    odom_pub_->publish(odom);
+    publish_transform();
+}
 
   void imu_callback(sensor_msgs::msg::Imu::SharedPtr msg) {
     Eigen::Vector3d axis_vector(0.0, 0.0, 1.0);