diff --git a/src/guppy_control/config/controller_params.yaml b/src/guppy_control/config/controller_params.yaml index a1d7c54..00de5b4 100644 --- a/src/guppy_control/config/controller_params.yaml +++ b/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 \ No newline at end of file diff --git a/src/guppy_control/src/control_chassis_node.cpp b/src/guppy_control/src/control_chassis_node.cpp index 4c7cab9..9c5ef3d 100644 --- a/src/guppy_control/src/control_chassis_node.cpp +++ b/src/guppy_control/src/control_chassis_node.cpp @@ -41,11 +41,10 @@ class ControlChassis : public rclcpp::Node { load_params_(¶meters); // load parameters from configuration file this->param_subscriber_ = std::make_shared(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>; // get type at pointer/reference and strip const/volatile using R = std::remove_cv_t>>; // final result of calling lambda transformer, ie. transformed type - + if constexpr (std::is_same_v) { // 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(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(&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 transformers = { - { + { "motor_positions", std::function(const rclcpp::Parameter&)>( - [](const rclcpp::Parameter& value) { + [](const rclcpp::Parameter& value) { return ControlChassis::to_motor_coefficients(value.as_double_array()); } ) @@ -212,7 +209,7 @@ class ControlChassis : public rclcpp::Node { } ) }, - { + { "motor_upper_bounds", std::function(const rclcpp::Parameter&)>( [](const rclcpp::Parameter& value) { @@ -220,7 +217,7 @@ class ControlChassis : public rclcpp::Node { } ) }, - { + { "axis_weight_matrix", std::function(const rclcpp::Parameter&)>( [](const rclcpp::Parameter& value) { @@ -228,7 +225,7 @@ class ControlChassis : public rclcpp::Node { } ) }, - { + { "pid_gains_vel_linear", std::function(const rclcpp::Parameter&)>( [](const rclcpp::Parameter& value) { @@ -236,7 +233,7 @@ class ControlChassis : public rclcpp::Node { } ) }, - { + { "pid_gains_vel_angular", std::function(const rclcpp::Parameter&)>( [](const rclcpp::Parameter& value) { @@ -244,14 +241,14 @@ class ControlChassis : public rclcpp::Node { } ) }, - { + { "pid_gains_pose_linear", std::function(const rclcpp::Parameter&)>( [](const rclcpp::Parameter& value) { return value.as_double_array(); } ) }, - { + { "pid_gains_pose_angular", std::function(const rclcpp::Parameter&)>( [](const rclcpp::Parameter& value) { @@ -259,7 +256,7 @@ class ControlChassis : public rclcpp::Node { } ) }, - { + { "pose_lock_deadband", std::function(const rclcpp::Parameter&)>( [](const rclcpp::Parameter& value) { @@ -267,7 +264,7 @@ class ControlChassis : public rclcpp::Node { } ) }, - { + { "drag_coefficients", std::function(const rclcpp::Parameter&)>( [](const rclcpp::Parameter& value) { @@ -275,7 +272,7 @@ class ControlChassis : public rclcpp::Node { } ) }, - { + { "drag_areas", std::function(const rclcpp::Parameter&)>( [](const rclcpp::Parameter& value) { @@ -283,7 +280,7 @@ class ControlChassis : public rclcpp::Node { } ) }, - { + { "drag_effect_matrix", std::function(const rclcpp::Parameter&)>( [](const rclcpp::Parameter& value) { @@ -291,7 +288,7 @@ class ControlChassis : public rclcpp::Node { } ) }, - { + { "water_density", std::function( [](const rclcpp::Parameter& value) { @@ -299,7 +296,7 @@ class ControlChassis : public rclcpp::Node { } ) }, - { + { "robot_volume", std::function( [](const rclcpp::Parameter& value) { @@ -307,7 +304,7 @@ class ControlChassis : public rclcpp::Node { } ) }, - { + { "robot_mass", std::function( [](const rclcpp::Parameter& value) { @@ -315,7 +312,7 @@ class ControlChassis : public rclcpp::Node { } ) }, - { + { "center_of_buoyancy", std::function(const rclcpp::Parameter&)>( [](const rclcpp::Parameter& value) { @@ -323,7 +320,7 @@ class ControlChassis : public rclcpp::Node { } ) }, - { + { "qp_epsilon", std::function( [](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(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>; using R = std::invoke_result_t; - + if constexpr (std::is_same_v) *pointer = transformer(param); }, pointer_to, transformer_it->second); @@ -482,7 +479,7 @@ 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); @@ -490,7 +487,7 @@ class ControlChassis : public rclcpp::Node { 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; -} \ No newline at end of file +} diff --git a/src/guppy_localization/src/combine.cpp b/src/guppy_localization/src/combine.cpp index 1458fd3..f0ac293 100644 --- a/src/guppy_localization/src/combine.cpp +++ b/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); diff --git a/src/guppy_nav/include/guppy_nav/change_state_behavior.hpp b/src/guppy_nav/include/guppy_nav/change_state_behavior.hpp index 923d793..84c2c6a 100644 --- a/src/guppy_nav/include/guppy_nav/change_state_behavior.hpp +++ b/src/guppy_nav/include/guppy_nav/change_state_behavior.hpp @@ -7,8 +7,9 @@ class ChangeStateBehavior : public BT::RosServiceNode(name, conf, params) - {} + : RosServiceNode(name, conf, params) { + RCLCPP_INFO(logger(), "ChangeState behavior initialized."); + } static BT::PortsList providedPorts() { @@ -21,14 +22,15 @@ class ChangeStateBehavior : public BT::RosServiceNodenew_state.state); - RCLCPP_DEBUG(logger(), "Request to change State"); + RCLCPP_INFO(logger(), "Requesting state change to '%u'.", static_cast(request->new_state.state)); return true; } BT::NodeStatus onResponseReceived(const Response::SharedPtr& response) override { - RCLCPP_DEBUG(logger(), "ChangeStateNode success"); + RCLCPP_INFO(logger(), "%s response received from ChangeState action server.", response->success ? "OK" : "BAD"); + return BT::NodeStatus::SUCCESS; } }; diff --git a/src/guppy_nav/include/guppy_nav/move_toward_behavior.hpp b/src/guppy_nav/include/guppy_nav/move_toward_behavior.hpp new file mode 100644 index 0000000..c3dd9fb --- /dev/null +++ b/src/guppy_nav/include/guppy_nav/move_toward_behavior.hpp @@ -0,0 +1,86 @@ +#pragma once + +#include +#include + +#include "guppy_msgs/action/navigate.hpp" +#include "guppy_msgs/msg/corner_detection.hpp" + +#include + +#include +#include + +class MoveTowardBehavior : public BT::RosActionNode { + public: + MoveTowardBehavior(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) : BT::RosActionNode(name, conf, params) {} + + static BT::PortsList providedPorts() { + return providedBasicPorts({BT::InputPort("detection"), BT::InputPort("timeout"), BT::InputPort("continueOnTimeout")}); + } + + bool setGoal(BT::RosActionNode::Goal& goal) override { + RCLCPP_INFO(logger(), "made it to move toward start"); + guppy_msgs::msg::CornerDetection detection; + getInput("detection", detection); + + if (detection.corners.size() < 2) + return false; + + double diagonal_size = 0.0; + for (int i = 0; i < detection.corners.size(); i++) { + for (int j = i + 1; j < detection.corners.size(); j++) { + auto corner1 = detection.corners[i]; + auto corner2 = detection.corners[j]; + diagonal_size = std::max(sqrt(pow(corner2.x - corner1.x, 2) + pow(corner2.y - corner1.y, 2)), diagonal_size); + } + } + if (diagonal_size == 0.0) + return false; + + constexpr double maintain_distance = 0.25; // arbitrary value + constexpr double scale = 0.1; // scaling from pixel units to meters + constexpr double fov_per_pixel = 0.08; // made up number, not calculated from anything + + double angle = fov_per_pixel * diagonal_size / 2; + double distance = (diagonal_size / 2) / tan(angle / 180 * M_PI) * scale; + + goal.pose.position.y = distance > maintain_distance ? distance - maintain_distance : 0.0; + // then set everything else to zero? + goal.pose.position.x = goal.pose.position.z = 0.0; + double roll = 0.0; + double pitch = 0.0; + double yaw = 0.0; + + Eigen::Quaterniond q = Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()); + + goal.pose.orientation.w = q.w(), goal.pose.orientation.x = q.x(), goal.pose.orientation.y = q.y(), goal.pose.orientation.z = q.z(); + + goal.local = true; // local to cameras so has to be local + + getInput("timeout", goal.timeout); + + RCLCPP_INFO(logger(), "passed move"); + + return true; + } + + BT::NodeStatus onResultReceived(const WrappedResult& wrapped) override { + // should do? + return BT::NodeStatus::SUCCESS; + } + + virtual BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override { + bool continueOnTimeout = false; + getInput("continueOnTimeout", continueOnTimeout); + if (continueOnTimeout) { + RCLCPP_INFO(logger(), "pose setter action aborted, continuing..."); + return BT::NodeStatus::SUCCESS; + } else { + RCLCPP_ERROR(logger(), "pose setter node error... %s", BT::toStr(error)); + return BT::NodeStatus::FAILURE; + } + } + + BT::NodeStatus onFeedback(const std::shared_ptr feedback) { return BT::NodeStatus::RUNNING; } +}; diff --git a/src/guppy_nav/include/guppy_nav/pose_setter_behavior.hpp b/src/guppy_nav/include/guppy_nav/pose_setter_behavior.hpp index 163baeb..5c220ad 100644 --- a/src/guppy_nav/include/guppy_nav/pose_setter_behavior.hpp +++ b/src/guppy_nav/include/guppy_nav/pose_setter_behavior.hpp @@ -6,7 +6,10 @@ class NavigateBehavior: public BT::RosActionNode { public: - NavigateBehavior(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) : BT::RosActionNode(name, conf, params) {} + NavigateBehavior(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) + : BT::RosActionNode(name, conf, params) { + RCLCPP_INFO(logger(), "PoseSetter behavior initialized."); + } static BT::PortsList providedPorts() { return providedBasicPorts({ @@ -31,18 +34,18 @@ class NavigateBehavior: public BT::RosActionNode { } BT::NodeStatus onResultReceived(const WrappedResult& wrapped) override { - // should do? + RCLCPP_INFO(logger(), "PoseSetter action server returned results. %s reach target, with perror (%lf, %lf, %lf) qerror (%lf, %lf, %lf, %lf)", wrapped.result->target_reached ? "DID" : "DID NOT", wrapped.result->error.position.x, wrapped.result->error.position.y, wrapped.result->error.position.z, wrapped.result->error.orientation.w, wrapped.result->error.orientation.x, wrapped.result->error.orientation.y, wrapped.result->error.orientation.z); return BT::NodeStatus::SUCCESS; } virtual BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override { - bool continueOnTimeout = false; + bool continueOnTimeout; getInput("continueOnTimeout", continueOnTimeout); if (continueOnTimeout) { - RCLCPP_INFO(logger(), "navigate action aborted, continuing..."); + RCLCPP_INFO(logger(), "PoseSetter action server returned %s, continuing.", BT::toStr(error)); return BT::NodeStatus::SUCCESS; } else { - RCLCPP_ERROR(logger(), "pose setter treenode error... %s", BT::toStr(error)); + RCLCPP_ERROR(logger(), "PoseSetter action server return %s. Node status failing.", BT::toStr(error)); return BT::NodeStatus::FAILURE; } } diff --git a/src/guppy_nav/src/behavior_tree.cpp b/src/guppy_nav/src/behavior_tree.cpp index 1388401..dd8197c 100644 --- a/src/guppy_nav/src/behavior_tree.cpp +++ b/src/guppy_nav/src/behavior_tree.cpp @@ -18,7 +18,7 @@ #include "guppy_nav/face_detection_behavior.hpp" #define TICK_MS 20 -#define TREE_NAME "t-shape" +#define TREE_NAME "practice-run" class NavigationBehaviorTree : public rclcpp::Node { public: @@ -58,6 +58,8 @@ class NavigationBehaviorTree : public rclcpp::Node { auto state_quality = rclcpp::QoS(rclcpp::KeepLast(1)).reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE).durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); _subscription = this->create_subscription("state", state_quality, onState); + + RCLCPP_INFO(get_logger(), "Behavior tree %s initialized with %lu registered nodes.", TREE_NAME, factory.builders().size()); } private: std::unique_ptr _tree; diff --git a/src/guppy_nav/src/pose_setter_server.cpp b/src/guppy_nav/src/pose_setter_server.cpp index f84439d..09d5018 100644 --- a/src/guppy_nav/src/pose_setter_server.cpp +++ b/src/guppy_nav/src/pose_setter_server.cpp @@ -1,7 +1,6 @@ +#include #include -#include #include -#include #include #include @@ -13,13 +12,11 @@ #include #include -#include "guppy_nav/trajectory.hpp" - #include "guppy_msgs/action/navigate.hpp" #include "guppy_msgs/msg/state.hpp" #include "guppy_msgs/srv/set_hold_pose.hpp" + #include "geometry_msgs/msg/pose.hpp" -#include "geometry_msgs/msg/twist.hpp" #include "nav_msgs/msg/odometry.hpp" #define SETPOINTS_EVERY 0.3 // meters @@ -27,25 +24,26 @@ class PoseSetterServer : public rclcpp::Node { public: explicit PoseSetterServer(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) : Node("pose_setter", options) { - auto handleGoal = [this](const rclcpp_action::GoalUUID& _, std::shared_ptr goal) { - RCLCPP_INFO(this->get_logger(), "goal request with pose and relative set to %d", goal->local); //goal->pose + auto handleGoal = [this](const rclcpp_action::GoalUUID& id, std::shared_ptr goal) { + RCLCPP_INFO(this->get_logger(), "Goal %s requested at (%.2lf, %.1lf, %lf)(%.2lf, %.2lf, %.2lf, %.2lf) %s with %lf timeout.", rclcpp_action::to_string(id).c_str(), goal->pose.position.x, goal->pose.position.y, goal->pose.position.z, goal->pose.orientation.w, goal->pose.orientation.x, goal->pose.orientation.y, goal->pose.orientation.z, goal->local ? "LOCAL" : "ABSOLUTE", goal->timeout); if (_state == guppy_msgs::msg::State::NAV) { _cancel = false; return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } - RCLCPP_INFO(this->get_logger(), "rejecting request because not in nav"); //goal->pose + RCLCPP_INFO(this->get_logger(), "Rejecting request because state not NAV."); //goal->pose return rclcpp_action::GoalResponse::REJECT; }; auto handleCancel = [this](const std::shared_ptr> goalHandle) { - RCLCPP_INFO(this->get_logger(), "request to cancel goal"); + RCLCPP_INFO(this->get_logger(), "Request to cancel goal %s.", rclcpp_action::to_string(goalHandle->get_goal_id()).c_str()); this->_cancel = true; return rclcpp_action::CancelResponse::ACCEPT; }; auto handleAccepted = [this](const std::shared_ptr> goalHandle) { + RCLCPP_INFO(this->get_logger(), "Accepted goal %s.", rclcpp_action::to_string(goalHandle->get_goal_id()).c_str()); std::thread{[this, goalHandle]() { execute(goalHandle); }}.detach(); @@ -165,10 +163,14 @@ class PoseSetterServer : public rclcpp::Node { error = pos_list[setpoint_index] - _current_pos; qerror = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); + auto qdistance = _current_quat.inverse() * quat_list[setpoint_index]; + auto qangle = 2 * atan2(qdistance.vec().norm(), qdistance.w()); + if ( abs(error.x()) <= tolerance_list[setpoint_index] && abs(error.y()) <= tolerance_list[setpoint_index] && - abs(error.z()) <= tolerance_list[setpoint_index] + abs(error.z()) <= tolerance_list[setpoint_index] && + abs(qangle) <= M_PI / 36 // 10 degrees ) { setpoint_index++; if (setpoint_index == n_steps) { diff --git a/src/guppy_state/src/navstart.cpp b/src/guppy_state/src/navstart.cpp index fec1283..0dcf475 100644 --- a/src/guppy_state/src/navstart.cpp +++ b/src/guppy_state/src/navstart.cpp @@ -43,17 +43,17 @@ class NavStart : public rclcpp::Node { stateclient->async_send_request(request); was_nav = false; } else if (navon && !was_nav) { - // auto request = std::make_shared(); - // guppy_msgs::msg::State state; - // state.state = guppy_msgs::msg::State::NAV; - // request->new_state = state; - // stateclient->async_send_request(request); + auto request = std::make_shared(); + guppy_msgs::msg::State state; + state.state = guppy_msgs::msg::State::NAV; + request->new_state = state; + stateclient->async_send_request(request); was_nav = true; - std::thread t([](){ - system("/home/robosub/guppy/util/bin/prequal"); - }); - t.detach(); + // std::thread t([](){ + // system("/home/robosub/guppy/util/bin/prequal"); + // }); + // t.detach(); } } diff --git a/src/guppy_state/src/state_manager.cpp b/src/guppy_state/src/state_manager.cpp index 6bbab35..88092cf 100644 --- a/src/guppy_state/src/state_manager.cpp +++ b/src/guppy_state/src/state_manager.cpp @@ -46,7 +46,6 @@ class StateManager : public rclcpp::Node { static const geometry_msgs::msg::Twist zero_twist; } - private: void estopcallback(guppy_msgs::msg::CanFrame msg) { int is_estopped = 0; @@ -61,6 +60,18 @@ class StateManager : public rclcpp::Node { // } } + static std::string to_string(uint8_t state) { + switch (state) { + case guppy_msgs::msg::State::STARTUP: return "STARTUP"; break; + case guppy_msgs::msg::State::HOLDING: return "HOLDING"; break; + case guppy_msgs::msg::State::NAV: return "NAV"; break; + case guppy_msgs::msg::State::TASK: return "TASK"; break; + case guppy_msgs::msg::State::TELEOP: return "TELEOP"; break; + case guppy_msgs::msg::State::DISABLED: return "DISABLED"; break; + case guppy_msgs::msg::State::FAULT: return "FAULT"; break; + } + } + bool is_valid_state(uint8_t state) { switch (state) { case guppy_msgs::msg::State::STARTUP: @@ -80,34 +91,42 @@ class StateManager : public rclcpp::Node { const std::shared_ptr request, std::shared_ptr response ) { - RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "State transition request..."); + RCLCPP_INFO(get_logger(), "State transition to %s requested.", to_string(request->new_state.state).c_str()); - if (request->new_state.state == this->current_state_) { - RCLCPP_ERROR(this->get_logger(), "Already in state!"); + auto new_state = request->new_state.state; + + if (new_state == this->current_state_) { + RCLCPP_WARN(this->get_logger(), "Already in state %s!", to_string(current_state_).c_str()); response->success = false; return; } - if (!is_valid_state(request->new_state.state)) { + if (!is_valid_state(new_state)) { RCLCPP_ERROR(this->get_logger(), "Invalid state passed in transition service!"); response->success = false; return; } if (this->current_state_ == guppy_msgs::msg::State::FAULT) { - RCLCPP_ERROR(this->get_logger(), "You can't exit the fault state!"); // TODO fix loggers! + RCLCPP_WARN(this->get_logger(), "You can't exit the FAULT state!"); response->success = false; return; } - if (request->new_state.state == guppy_msgs::msg::State::HOLDING) { + if (new_state == guppy_msgs::msg::State::HOLDING) { + RCLCPP_ERROR(this->get_logger(), "Resting pose for holding."); auto request = std::make_shared(); resetholdpose->async_send_request(request); } // TODO switch logic should be handled here NOT in StateManager#publishState() - response->success = this->publish_state(request->new_state.state); + auto stale_state = current_state_; + + response->success = this->publish_state(new_state); + + if (response->success) RCLCPP_INFO(this->get_logger(), "Transitioning state from %s -> %s.", to_string(stale_state).c_str(), to_string(new_state).c_str()); + else RCLCPP_ERROR(get_logger(), "Failed to publish state transition from %s -> %s.", to_string(stale_state).c_str(), to_string(new_state).c_str()); } bool publish_state(uint8_t state) { @@ -121,13 +140,13 @@ class StateManager : public rclcpp::Node { void on_timer() { switch (this->current_state_) { - case guppy_msgs::msg::State::STARTUP: this->handle_startup(); break; - case guppy_msgs::msg::State::HOLDING: this->handle_holding(); break; - case guppy_msgs::msg::State::NAV: this->handle_nav(); break; - case guppy_msgs::msg::State::TASK: this->handle_task(); break; - case guppy_msgs::msg::State::TELEOP: this->handle_teleop(); break; - case guppy_msgs::msg::State::DISABLED: this->handle_disabled(); break; - case guppy_msgs::msg::State::FAULT: this->handle_fault(); break; + case guppy_msgs::msg::State::STARTUP: this->handle_startup(); break; + case guppy_msgs::msg::State::HOLDING: this->handle_holding(); break; + case guppy_msgs::msg::State::NAV: this->handle_nav(); break; + case guppy_msgs::msg::State::TASK: this->handle_task(); break; + case guppy_msgs::msg::State::TELEOP: this->handle_teleop(); break; + case guppy_msgs::msg::State::DISABLED: this->handle_disabled(); break; + case guppy_msgs::msg::State::FAULT: this->handle_fault(); break; } } @@ -203,11 +222,8 @@ int main(int argc, char* argv[]) { auto publisher_node = std::make_shared(); - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "State transition server ready."); - rclcpp::spin(publisher_node); - rclcpp::shutdown(); return 0; diff --git a/src/guppy_tasks/resource/fullspin-absolute.xml b/src/guppy_tasks/resource/fullspin-absolute.xml new file mode 100644 index 0000000..6053136 --- /dev/null +++ b/src/guppy_tasks/resource/fullspin-absolute.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/src/guppy_tasks/resource/fullspin-local.xml b/src/guppy_tasks/resource/fullspin-local.xml new file mode 100644 index 0000000..f7694df --- /dev/null +++ b/src/guppy_tasks/resource/fullspin-local.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/src/guppy_tasks/resource/practice-run.xml b/src/guppy_tasks/resource/practice-run.xml new file mode 100644 index 0000000..ef7056d --- /dev/null +++ b/src/guppy_tasks/resource/practice-run.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/guppy_tasks/resource/spin-test.xml b/src/guppy_tasks/resource/spin-test.xml new file mode 100644 index 0000000..dcf3a56 --- /dev/null +++ b/src/guppy_tasks/resource/spin-test.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/src/guppy_tasks/resource/t-shape.xml b/src/guppy_tasks/resource/t-shape.xml index 35798fc..c17542d 100644 --- a/src/guppy_tasks/resource/t-shape.xml +++ b/src/guppy_tasks/resource/t-shape.xml @@ -4,10 +4,12 @@ - - - - + + + + + + diff --git a/src/guppy_tasks/test/test_copyright.py b/src/guppy_tasks/resource/test/test_copyright.py similarity index 100% rename from src/guppy_tasks/test/test_copyright.py rename to src/guppy_tasks/resource/test/test_copyright.py diff --git a/src/guppy_tasks/test/test_flake8.py b/src/guppy_tasks/resource/test/test_flake8.py similarity index 100% rename from src/guppy_tasks/test/test_flake8.py rename to src/guppy_tasks/resource/test/test_flake8.py diff --git a/src/guppy_tasks/test/test_pep257.py b/src/guppy_tasks/resource/test/test_pep257.py similarity index 100% rename from src/guppy_tasks/test/test_pep257.py rename to src/guppy_tasks/resource/test/test_pep257.py diff --git a/src/guppy_vision/CMakeLists.txt b/src/guppy_vision/CMakeLists.txt index 98f78db..f3e6bdc 100644 --- a/src/guppy_vision/CMakeLists.txt +++ b/src/guppy_vision/CMakeLists.txt @@ -66,6 +66,7 @@ ament_python_install_package(${PROJECT_NAME}) # Install Python executables install(PROGRAMS ${PROJECT_NAME}/apriltag_detector.py + ${PROJECT_NAME}/object_detection.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/src/guppy_vision/guppy_vision/object_detection.py b/src/guppy_vision/guppy_vision/object_detection.py new file mode 100644 index 0000000..010d515 --- /dev/null +++ b/src/guppy_vision/guppy_vision/object_detection.py @@ -0,0 +1,81 @@ +#!/usr/bin/env python3 +import json as jsonlib +import rclpy +from rclpy.node import Node +from ultralytics import YOLO +from sensor_msgs.msg import Image +from cv_bridge import CvBridge +from rclpy.qos import qos_profile_sensor_data +import cv2 as cv +import numpy as np +from guppy_msgs.msg import CornerDetection, CornerDetectionList +from vision_msgs.msg import Point2D +from geometry_msgs.msg import Point + +class ObjectDetection(Node): + def __init__(self): + super().__init__('object_detection') + + # Model generously provided by OSU-UWRT + self.model = YOLO("/home/robosub/guppy/src/guppy_vision/resource/ffc_rs_26.pt") + self.bridge = CvBridge() + + self.sub = self.create_subscription(Image, 'cam/test', self.callback, qos_profile_sensor_data) + self.pub = self.create_publisher(CornerDetectionList, "/cam/test/detections", 10) + + def callback(self, msg): + frame = self.bridge.imgmsg_to_cv2(msg) + results = self.model(frame) + + l = CornerDetectionList() + + if results[0].masks is not None: + json = jsonlib.loads(results[0].to_json()) + for i in range(len(results[0].masks.xy)): + det = CornerDetection() + contour = results[0].masks.xy[i].astype(np.float32) + + rect = cv.minAreaRect(contour) + box = cv.boxPoints(rect) + box = np.int32(box).tolist() + + for j in box: + point = Point2D() + point.x = float(j[0]) + point.y = float(j[1]) + det.corners.append(point) + + det.confidence = json[i]["confidence"] + det.name = json[i]["name"] + det.square = True + + points = [ + (-0.08, 0.08, 0), + ( 0.08, 0.08, 0), + ( 0.08, -0.08, 0), + (-0.08, -0.08, 0), + ] + + for x, y, z in points: + point = Point() + point.x = float(x) + point.y = float(y) + point.z = float(z) + det.dimension_points.append(point) + + l.detections.append(det) + + l.header = msg.header + self.pub.publish(l) + + + +def main(args=None): + rclpy.init(args=args) + objdet = ObjectDetection() + rclpy.spin(objdet) + objdet.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/src/guppy_vision/launch/core.xml b/src/guppy_vision/launch/core.xml index aaf9d64..393d3de 100644 --- a/src/guppy_vision/launch/core.xml +++ b/src/guppy_vision/launch/core.xml @@ -4,5 +4,6 @@ - + + diff --git a/src/guppy_vision/resource/ffc_rs_26.pt b/src/guppy_vision/resource/ffc_rs_26.pt new file mode 100644 index 0000000..ad4b77f Binary files /dev/null and b/src/guppy_vision/resource/ffc_rs_26.pt differ diff --git a/src/guppy_vision/src/camera_pub.cpp b/src/guppy_vision/src/camera_pub.cpp index 6b5c1b9..433fd76 100644 --- a/src/guppy_vision/src/camera_pub.cpp +++ b/src/guppy_vision/src/camera_pub.cpp @@ -41,7 +41,7 @@ class FlirPublisher : public rclcpp::Node { break; } } - std::string topic = "/cam/" + pos + "/raw"; + std::string topic = "/cam/" + pos; this->pubs_.push_back(image_transport::create_publisher(this, topic)); // this->publishers_.push_back(this->create_publisher(topic, 1)); this->threads_.emplace_back(&FlirPublisher::acquireFootage, this, i); diff --git a/startup/setup.sh b/startup/setup.sh index 9dfe1e4..e34f2bd 100755 --- a/startup/setup.sh +++ b/startup/setup.sh @@ -6,5 +6,6 @@ until ping -c1 192.168.194.95 &>/dev/null; do sleep 1 done +export LD_LIBRARY_PATH=/opt/spinnaker/lib:$LD_LIBRARY_PATH echo ----------ATTEMPTING TO START GUPPY---------- ros2 launch guppy hw.xml