Pool time + nav part 2

.gitignore

@@ -2,7 +2,7 @@ devel/
 logs/
 build/
 log/
-bin/
+# bin/
 lib/
 install/
 msg_gen/

src/guppy_control/CMakeLists.txt

@@ -17,13 +17,14 @@ if(NOT CMAKE_CXX_STANDARD)
     find_package(geometry_msgs REQUIRED)
     find_package(nav_msgs REQUIRED)
     find_package(guppy_msgs REQUIRED)
+    find_package(std_srvs REQUIRED)
     find_package(eigen3_cmake_module REQUIRED)
     find_package(Eigen3)
 
     include_directories(include)
 
     add_executable(control_chassis src/control_chassis_node.cpp src/chassis_controller.cpp src/t200_interface.cpp)
-    ament_target_dependencies(control_chassis control_toolbox geometry_msgs nav_msgs guppy_msgs Eigen3 proxsuite)
+    ament_target_dependencies(control_chassis std_srvs control_toolbox geometry_msgs nav_msgs guppy_msgs Eigen3 proxsuite)
     set_source_files_properties(src/chassis_controller.cpp PROPERTIES
       COMPILE_OPTIONS "-O3;-DNDEBUG;-DPROXSUITE_VECTORIZE"
     )

src/guppy_control/config/controller_params.yaml

@@ -22,11 +22,11 @@ control_chassis:
        0.0,   0.0,   0.0,  0.0,  0.0,  1.0,
     ] # flattend 6 * 6 matrix
 
-    pid_gains_vel_linear: [ 1000.0, 0.0, 0.0 ]
+    pid_gains_vel_linear: [ 20000.0, 0.0, 0.0 ]
     pid_gains_vel_angular: [ 1000.0, 0.0, 0.0 ]
 
-    pid_gains_pose_linear: [ 90.0, 0.0, 30.0 ]
-    pid_gains_pose_angular: [ 50.0, 0.0, 50.0 ]
+    pid_gains_pose_linear: [ 300.0, 0.0, 12.0 ]
+    pid_gains_pose_angular: [ 100.0, 0.0, 5.0 ]
 
     # pid_gains_vel_linear: [ 0.0, 0.0, 0.0 ]
     # pid_gains_vel_angular: [ 0.0, 0.0, 0.0 ]
@@ -48,7 +48,7 @@ control_chassis:
     drag_coefficients: [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ]
     drag_areas: [ 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 ]
     water_density: 1000.0 # kg/m^3
-    robot_volume: 0.0 #0.03963 # m^3
-    robot_mass: 0.0 #34.1 # kg
-    center_of_buoyancy: [0.0, 0.0, 0.0] #[ 0.0, 0.0, -0.08 ]
+    robot_volume: 0.03963 # m^3
+    robot_mass: 34.1 # kg
+    center_of_buoyancy: [ 0.0, 0.0, -0.08 ]
     qp_epsilon: 0.001

src/guppy_control/include/guppy_control/chassis_controller.hpp

@@ -16,6 +16,7 @@
 #include <control_toolbox/control_toolbox/pid.hpp>
 #include "geometry_msgs/msg/twist.hpp"
 #include "nav_msgs/msg/odometry.hpp"
+#include "guppy_msgs/srv/set_hold_pose.hpp"
 
 #include "guppy_control/t200_interface.hpp"
 
@@ -142,6 +143,16 @@ class ChassisController {
     */
     void update_desired_state(geometry_msgs::msg::Twist::SharedPtr msg);
 
+    /*
+        @brief resets the holding pose
+    */
+    void reset_holding_pose(guppy_msgs::srv::SetHoldPose::Request::SharedPtr msg);
+
+    /*
+        @brief enable pose pid
+    */
+    void enable_pose_pid(bool enabled);
+
     /*
         @brief update the configuration parameters
         @param parameters the new configuration options object
@@ -219,6 +230,9 @@ class ChassisController {
     /* the control thread object */
     std::thread control_thread_;
 
+    /* whether or not pose pid is enabled */
+    bool pose_pid_enabled = false;
+
     // private methods
 
     /*

src/guppy_control/package.xml

@@ -13,6 +13,7 @@
   <depend>proxsuite</depend>
   <depend>control_toolbox</depend>
   <depend>guppy_msgs</depend>
+  <depend>std_srvs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

src/guppy_control/src/chassis_controller.cpp

@@ -22,8 +22,11 @@ bool ChassisController::control_loop(bool debug) {
 
   // calculate buoyant effect on sub
   Eigen::Vector3d buoyancy_force; buoyancy_force << 0, 0, params_.water_density * params_.robot_volume * GRAVITY;
-  Eigen::Vector3d r_vec = current_orientation_state_ * params_.center_of_buoyancy;
-  Eigen::Vector3d buoyancy_torque = -1 * r_vec.cross(buoyancy_force);
+  Eigen::Vector3d r_vec = current_orientation_state_.inverse() * params_.center_of_buoyancy;
+  Eigen::Vector3d buoyancy_torque = 1 * r_vec.cross(buoyancy_force);
+  // if (pose_pid_enabled) {
+    buoyancy_torque = -1 * r_vec.cross(buoyancy_force);
+  // }
   Eigen::Vector3d buoyancy_force_rotated = current_orientation_state_.inverse() * buoyancy_force;
   Eigen::Vector<double, 6> buoyancy_wrench;
   buoyancy_wrench << buoyancy_force_rotated[0], buoyancy_force_rotated[1], buoyancy_force_rotated[2], buoyancy_torque;
@@ -50,7 +53,7 @@ bool ChassisController::control_loop(bool debug) {
   Eigen::Vector3d position_err = desired_position_state_ - current_position_state_;
   position_err = current_orientation_state_.inverse() * position_err;
   for (int i=0; i<3; i++) {
-    if (abs(desired_velocity_state_[i]) < params_.pose_lock_deadband[i]) {
+    if (abs(desired_velocity_state_[i]) < params_.pose_lock_deadband[i] && pose_pid_enabled) {
       position_nudge[i] = pose_pid[i].compute_command(position_err[i], (dt_us_ / 1000000.0));
     } else {
       position_nudge[i] = 0;
@@ -96,7 +99,41 @@ bool ChassisController::control_loop(bool debug) {
   return success;
 }
 
+void ChassisController::reset_holding_pose(guppy_msgs::srv::SetHoldPose::Request::SharedPtr request) {
+  if (request->type == request->guppy_msgs::srv::SetHoldPose::Request::RESET) {
+    this->desired_orientation_state_ = this->current_orientation_state_;
+    this->desired_position_state_ = this->current_position_state_;
+  }
+
+  else {
+    auto ros_quat = request->pose.orientation;
+    auto ros_pos = request->pose.position;
+
+    Eigen::Quaterniond quat(ros_quat.w, ros_quat.x, ros_quat.y, ros_quat.z);
+    Eigen::Vector3d pos; pos << ros_pos.x, ros_pos.y, ros_pos.z;
+
+    if (request->type == request->guppy_msgs::srv::SetHoldPose::Request::GLOBAL) {
+      this->desired_position_state_ = pos;
+      this->desired_orientation_state_ = quat;
+    }
+
+    else if (request->type == request->guppy_msgs::srv::SetHoldPose::Request::RELATIVE) {
+      this->desired_orientation_state_ = this->current_orientation_state_ * quat;
+      this->desired_position_state_ = this->current_position_state_ + pos;
+    }
+
+    else if (request->type == request->guppy_msgs::srv::SetHoldPose::Request::LOCAL) {
+      this->desired_orientation_state_ = this->current_orientation_state_ * quat;
+      this->desired_position_state_ = this->current_position_state_ + (this->current_orientation_state_.inverse() * pos);
+    }
+  }  
+}
+
 Eigen::Vector3d ChassisController::calculate_rotational_nudge(bool debug) {
+  if (!pose_pid_enabled) {
+    Eigen::Vector3d out(0.0, 0.0, 0.0);
+    return out;
+  } 
   // the new state flags of the rotational locks
   int new_orientation_lock = ALL_FREE; // == 0
 
@@ -137,6 +174,10 @@ Eigen::Vector3d ChassisController::calculate_rotational_nudge(bool debug) {
   return output_nudge;
 }
 
+void ChassisController::enable_pose_pid(bool enabled) {
+  this->pose_pid_enabled = enabled;
+}
+
 ChassisController::ChassisController(ChassisControllerParams parameters, T200Interface* hw_interface, int dt_us)
   : interface_(hw_interface), current_orientation_state_(1, 0, 0, 0), desired_orientation_state_(1, 0, 0, 0),\
     params_(parameters), qp_(N_MOTORS, 0, N_MOTORS), dt_us_(dt_us), THREAD_RUNNING_(false) {
@@ -217,6 +258,10 @@ void ChassisController::update_desired_state(geometry_msgs::msg::Twist::SharedPt
     ros_twist->angular.y, \
     ros_twist->angular.z;
 
+  if (std::isnan(ros_twist->linear.x) || std::isnan(ros_twist->linear.y) || std::isnan(ros_twist->linear.z)) {
+    return;
+  }
+
   this->desired_velocity_state_ = new_desired_state;
 }
 

src/guppy_control/src/control_chassis_node.cpp

@@ -5,6 +5,7 @@
 
 #include "std_msgs/msg/float32.hpp"
 #include "std_msgs/msg/float64.hpp"
+#include "guppy_msgs/srv/set_hold_pose.hpp"
 #include "guppy_msgs/msg/state.hpp"
 
 using namespace std::chrono_literals;
@@ -109,6 +110,11 @@ class ControlChassis : public rclcpp::Node {
     cmd_vel_subscription_ = this->create_subscription<geometry_msgs::msg::Twist>("/cmd_vel",10,std::bind(&ControlChassis::cmdvel_callback, this, std::placeholders::_1));
     state_subscription_ = this->create_subscription<guppy_msgs::msg::State>("/state", 10, std::bind(&ControlChassis::state_callback, this, std::placeholders::_1));
 
+    reset_service = this->create_service<guppy_msgs::srv::SetHoldPose>("reset_holding_pose", [&](const std::shared_ptr<guppy_msgs::srv::SetHoldPose::Request> request, std::shared_ptr<guppy_msgs::srv::SetHoldPose::Response> response) {
+      controller->reset_holding_pose(request);
+    }, 10);
+
+
     auto timer_callback = [this]() -> void {
       auto thrusts = controller->get_motor_thrusts();
 
@@ -149,6 +155,11 @@ class ControlChassis : public rclcpp::Node {
       } else {
           this->thruster_interface->set_enabled(true);
       }
+      if (msg->state == guppy_msgs::msg::State::NAV) {
+          this->controller->enable_pose_pid(true);
+      } else {
+          this->controller->enable_pose_pid(false);
+      }
   }
 
 private:
@@ -157,6 +168,8 @@ class ControlChassis : public rclcpp::Node {
   rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_subscription_;
   rclcpp::Subscription<guppy_msgs::msg::State>::SharedPtr state_subscription_;
   rclcpp::TimerBase::SharedPtr timer_;
+  rclcpp::Service<guppy_msgs::srv::SetHoldPose>::SharedPtr reset_service;
+
 
   std::shared_ptr<rclcpp::ParameterEventHandler> param_subscriber_;
   std::shared_ptr<rclcpp::ParameterEventCallbackHandle> param_event_callback_handle_;

src/guppy_localization/CMakeLists.txt

@@ -18,13 +18,14 @@ find_package(nav_msgs REQUIRED)
 find_package(eigen3_cmake_module REQUIRED)
 find_package(Eigen3)
 find_package(tf2 REQUIRED)
+find_package(guppy_msgs REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
 
 
 add_executable(barometer_pub src/barometer_pub.cpp)
 add_executable(combine_sensors src/combine.cpp)
-ament_target_dependencies(barometer_pub rclcpp sensor_msgs geometry_msgs std_msgs)
+ament_target_dependencies(barometer_pub rclcpp sensor_msgs geometry_msgs guppy_msgs std_msgs)
 ament_target_dependencies(combine_sensors tf2 tf2_geometry_msgs tf2_ros rclcpp sensor_msgs geometry_msgs nav_msgs std_msgs Eigen3)
 
 install(TARGETS

src/guppy_localization/package.xml

@@ -14,6 +14,7 @@
   <depend>geometry_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>nav_msgs</depend>
+  <depend>guppy_msgs</depend>
 
   <exec_depend>ros2launch</exec_depend>
 

src/guppy_localization/src/barometer_pub.cpp

@@ -4,31 +4,39 @@
 
 #include "rclcpp/rclcpp.hpp"
 #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
+#include "guppy_msgs/msg/can_frame.hpp"
 #include "std_msgs/msg/byte_multi_array.hpp"
 
 using namespace std::chrono_literals;
 
 class BarometerPublisher : public rclcpp::Node {
 public:
   BarometerPublisher() : Node("barometer_publisher") {
-    this->declare_parameter("can_id", 0x001);
     this->declare_parameter("tf_frame", "barometer");
 
-    char can_topic[30];
-    sprintf(can_topic, "/can/id_%x", (unsigned int)this->get_parameter("can_id").as_int());
 
     publisher_ = this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("/barometer", 10);
-    subscription_ = this->create_subscription<std_msgs::msg::ByteMultiArray>(
-      can_topic, 10,
+    subscription_ = this->create_subscription<guppy_msgs::msg::CanFrame>(
+      "/can/id_0x26", 10,
       std::bind(&BarometerPublisher::callback, this, std::placeholders::_1)
     );
 
     RCLCPP_INFO(this->get_logger(), "setup publisher and subscriber");
   }
 
-  void callback(std_msgs::msg::ByteMultiArray::SharedPtr msg) {
-    double depth = 0;
-    memcpy(&depth, msg->data.data(), sizeof(double));
+  void callback(guppy_msgs::msg::CanFrame msg) {
+    float depth = 0;
+    memcpy(&depth, msg.data.data(), sizeof(float));
+
+
+    if (!initialized) {
+      offset = depth;
+      initialized = true;
+    }
+
+    depth -= offset;
+    depth *= -1;
+    // RCLCPP_INFO(this->get_logger(), "depth: %f", depth);
 
     geometry_msgs::msg::PoseWithCovarianceStamped pose_out;
     pose_out.header.frame_id = this->get_parameter("tf_frame").as_string();
@@ -52,7 +60,9 @@ class BarometerPublisher : public rclcpp::Node {
 
 private:
   rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr publisher_;
-  rclcpp::Subscription<std_msgs::msg::ByteMultiArray>::SharedPtr subscription_;
+  rclcpp::Subscription<guppy_msgs::msg::CanFrame>::SharedPtr subscription_;
+  double offset = 0;
+  bool initialized = false;
 };
 
 int main(int argc, char * argv[]) {

src/guppy_localization/src/combine.cpp

@@ -1,5 +1,6 @@
 #include "rclcpp/rclcpp.hpp"
 #include "nav_msgs/msg/odometry.hpp"
+#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
 #include "sensor_msgs/msg/imu.hpp"
 #include "tf2_ros/transform_broadcaster.hpp"
 
@@ -12,6 +13,7 @@ class CombineSensors : public rclcpp::Node {
   CombineSensors() : Node("combine_sensors") {
     imu_subscription_ = this->create_subscription<sensor_msgs::msg::Imu>("/vectornav/imu",10,std::bind(&CombineSensors::imu_callback, this, std::placeholders::_1));
     dvl_subscription_ = this->create_subscription<nav_msgs::msg::Odometry>("/waterlinked_dvl_driver/odom",10,std::bind(&CombineSensors::dvl_callback, this, std::placeholders::_1));
+    baro_subscription_ = this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>("/barometer",10,std::bind(&CombineSensors::baro_callback, this, std::placeholders::_1));
     odom_pub_ = this->create_publisher<nav_msgs::msg::Odometry>("/odometry/filtered", 10);
     tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
 
@@ -26,6 +28,14 @@ class CombineSensors : public rclcpp::Node {
     // ...
   }
 
+void baro_callback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) {
+    odom.pose.pose.position.z = msg->pose.pose.position.z;
+
+    odom_pub_->publish(odom);
+
+    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);
@@ -39,7 +49,7 @@ class CombineSensors : public rclcpp::Node {
 
     odom.pose.pose.position.x = position[0];
     odom.pose.pose.position.y = position[1];
-    odom.pose.pose.position.z = position[2];
+    // odom.pose.pose.position.z = position[2];
 
     double rx = 0.2;
     double ry = 0.0;
@@ -108,6 +118,7 @@ class CombineSensors : public rclcpp::Node {
   rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
   rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_subscription_;
   rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr dvl_subscription_;
+  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr baro_subscription_;
   std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
 
   nav_msgs::msg::Odometry odom;

src/guppy_msgs/CMakeLists.txt

@@ -20,6 +20,8 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/CanFrame.msg"
   "srv/SendCan.srv"
 
+  "srv/SetHoldPose.srv"
+
   "msg/Toast.msg"
 
   "msg/CornerDetection.msg"

src/guppy_msgs/action/Navigate.action

@@ -1,8 +1,11 @@
 geometry_msgs/Pose pose
 bool local true
-float64 duration
+float64 timeout 10
+float64 tolerance 0.1
 ---
 geometry_msgs/Pose pose #final relative position
+geometry_msgs/Pose error
 bool target_reached #if guppy made it to the target pose
 ---
 geometry_msgs/Pose progress #current progess relative to start
+float64 percent_done

src/guppy_msgs/srv/SetHoldPose.srv

@@ -0,0 +1,9 @@
+# constants
+uint8 RESET     = 0
+uint8 GLOBAL    = 1
+uint8 LOCAL     = 2
+uint8 RELATIVE  = 3
+
+uint8 type
+geometry_msgs/Pose pose
+---