From d9232420eda31d472fb79e735b5cb6b302fb54d6 Mon Sep 17 00:00:00 2001 From: Ryden Handsaker Date: Wed, 8 Apr 2026 04:56:23 +0000 Subject: [PATCH 01/18] feat: base navigation action --- src/guppy/package.xml | 2 +- src/guppy_msgs/CMakeLists.txt | 5 ++ src/guppy_msgs/action/Navigate.action | 7 ++ src/guppy_msgs/package.xml | 1 + src/guppy_nav/CMakeLists.txt | 39 +++++++++ src/{guppy_nav2 => guppy_nav}/package.xml | 7 +- src/guppy_nav/src/navigate_action_server.cpp | 92 ++++++++++++++++++++ src/guppy_nav2/CMakeLists.txt | 26 ------ 8 files changed, 150 insertions(+), 29 deletions(-) create mode 100644 src/guppy_msgs/action/Navigate.action create mode 100644 src/guppy_nav/CMakeLists.txt rename src/{guppy_nav2 => guppy_nav}/package.xml (66%) create mode 100644 src/guppy_nav/src/navigate_action_server.cpp delete mode 100644 src/guppy_nav2/CMakeLists.txt diff --git a/src/guppy/package.xml b/src/guppy/package.xml index 7e387ff..6eb19b7 100644 --- a/src/guppy/package.xml +++ b/src/guppy/package.xml @@ -15,7 +15,7 @@ guppy_localization guppy_moveit guppy_msgs - guppy_nav2 + guppy_nav guppy_safety guppy_state guppy_tasks diff --git a/src/guppy_msgs/CMakeLists.txt b/src/guppy_msgs/CMakeLists.txt index 1661950..e4a8aaf 100644 --- a/src/guppy_msgs/CMakeLists.txt +++ b/src/guppy_msgs/CMakeLists.txt @@ -10,14 +10,19 @@ find_package(ament_cmake REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(builtin_interfaces REQUIRED) +find_package(geometry_msgs REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/CanFrame.msg" "srv/SendCan.srv" + "msg/State.msg" "srv/ChangeState.srv" + + "action/Navigate.action" DEPENDENCIES builtin_interfaces + DEPENDENCIES geometry_msgs ) if(BUILD_TESTING) diff --git a/src/guppy_msgs/action/Navigate.action b/src/guppy_msgs/action/Navigate.action new file mode 100644 index 0000000..fa32c04 --- /dev/null +++ b/src/guppy_msgs/action/Navigate.action @@ -0,0 +1,7 @@ +geometry_msgs/Pose pose +bool relative true #use global pose or relative pose +--- +geometry_msgs/Pose pose #final relative position +bool target_reached #if guppy made it to the target pose +--- +geometry_msgs/Pose progress #current progess relative to start diff --git a/src/guppy_msgs/package.xml b/src/guppy_msgs/package.xml index 86263b0..1d5b2e0 100644 --- a/src/guppy_msgs/package.xml +++ b/src/guppy_msgs/package.xml @@ -7,6 +7,7 @@ Palouse RoboSub MIT + geometry_msgs builtin_interfaces rosidl_default_generators rosidl_default_runtime diff --git a/src/guppy_nav/CMakeLists.txt b/src/guppy_nav/CMakeLists.txt new file mode 100644 index 0000000..205840b --- /dev/null +++ b/src/guppy_nav/CMakeLists.txt @@ -0,0 +1,39 @@ +cmake_minimum_required(VERSION 3.8) +project(guppy_nav) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) + +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(guppy_msgs REQUIRED) + +add_library(action_server SHARED src/navigate_action_server.cpp) + +#target_include_directories(action_server PRIVATE +# $ +# $ +#) + +#add_executable(navigate_action_server src/navigate_action_server.cpp) +ament_target_dependencies(action_server rclcpp rclcpp_action rclcpp_components guppy_msgs geometry_msgs) +rclcpp_components_register_node(action_server PLUGIN "NavigateActionServer" EXECUTABLE navigate_action_server) + + +install(TARGETS + action_server + + DESTINATION lib/${PROJECT_NAME} +) +#install(DIRECTORY +# launch +# +# DESTINATION share/${PROJECT_NAME}/ +#) +#ament_package() diff --git a/src/guppy_nav2/package.xml b/src/guppy_nav/package.xml similarity index 66% rename from src/guppy_nav2/package.xml rename to src/guppy_nav/package.xml index a2cb939..02cff8f 100644 --- a/src/guppy_nav2/package.xml +++ b/src/guppy_nav/package.xml @@ -1,14 +1,17 @@ - guppy_nav2 + guppy_nav 0.0.0 - Nav2 configuration files and behavior trees for the 2026 Guppy AUV. This includes paths, and task ordering/strategy. + Navigation configuration files and behavior trees for the 2026 Guppy AUV. This includes paths, and task ordering/strategy. Palouse RoboSub MIT ament_cmake + guppy_msgs + geometry_msgs + ament_lint_auto ament_lint_common diff --git a/src/guppy_nav/src/navigate_action_server.cpp b/src/guppy_nav/src/navigate_action_server.cpp new file mode 100644 index 0000000..bc168f2 --- /dev/null +++ b/src/guppy_nav/src/navigate_action_server.cpp @@ -0,0 +1,92 @@ +#include +#include +#include + +#include +#include "rclcpp_action/rclcpp_action.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +#include "guppy_msgs/action/navigate.hpp" +#include "geometry_msgs/msg/twist.hpp" + +#define THRESHOLD 0.1 + +class NavigateActionServer : public rclcpp::Node { +public: + explicit NavigateActionServer(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) : Node("navigate_action_server", options) { + auto handle_goal = [this](const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal) { + RCLCPP_INFO(this->get_logger(), "goal request with pose and relative set to %d", goal->relative); //goal->pose + + (void)uuid; // silence unused parameter warning + + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + auto handle_cancel = [this](const std::shared_ptr> goal_handle) { + RCLCPP_INFO(this->get_logger(), "request to cancel goal"); + + (void)goal_handle; // silence unused parameter warning + + return rclcpp_action::CancelResponse::ACCEPT; + }; + + auto handle_accepted = [this](const std::shared_ptr> goal_handle) { + // to avoid blocking calling thread, return lambda quickly and execute on seperate thread + auto execute_in_thread = [this, goal_handle](){ + return this->execute(goal_handle); + }; + + std::thread{execute_in_thread}.detach(); + }; + + this->action_server_ = rclcpp_action::create_server( + this, + "navigate", + handle_goal, + handle_cancel, + handle_accepted + ); + } +private: + rclcpp_action::Server::SharedPtr action_server_; + + void execute(const std::shared_ptr> goal_handle) { + RCLCPP_INFO(this->get_logger(), "executing goal"); + + rclcpp::Rate loop_rate(1); //? + + const auto goal = goal_handle->get_goal(); + + auto feedback = std::make_shared(); + + auto result = std::make_shared(); + + for(int i = 1; /*(i < goal->order) &&*/ rclcpp::ok(); ++i) { //what is this? + if (goal_handle->is_canceling()) { + //result->sequence = sequenece; + goal_handle->canceled(result); + + RCLCPP_INFO(this->get_logger(), "goal cancelled"); + + return; + } + + goal_handle->publish_feedback(feedback); + + RCLCPP_INFO(this->get_logger(), "published feeback"); + + loop_rate.sleep(); + } + + if (rclcpp::ok()) { + // result->pose = ... + // result-> + + goal_handle->succeed(result); + + RCLCPP_INFO(this->get_logger(), "goal succeeded"); + } + }; +}; + +RCLCPP_COMPONENTS_REGISTER_NODE(NavigateActionServer) \ No newline at end of file diff --git a/src/guppy_nav2/CMakeLists.txt b/src/guppy_nav2/CMakeLists.txt deleted file mode 100644 index f2348ed..0000000 --- a/src/guppy_nav2/CMakeLists.txt +++ /dev/null @@ -1,26 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(guppy_nav2) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() From 9af147226e6b63c30e283c6c7ce49706c7e203d0 Mon Sep 17 00:00:00 2001 From: Ryden Handsaker Date: Sat, 25 Apr 2026 15:33:58 +0000 Subject: [PATCH 02/18] feat: nav action sever logic --- src/guppy_msgs/action/Navigate.action | 1 + src/guppy_nav/CMakeLists.txt | 27 +- .../include/guppy_nav/trajectory.hpp | 51 ++++ src/guppy_nav/package.xml | 11 +- src/guppy_nav/src/navigate_action_server.cpp | 233 +++++++++++++++--- 5 files changed, 268 insertions(+), 55 deletions(-) create mode 100644 src/guppy_nav/include/guppy_nav/trajectory.hpp diff --git a/src/guppy_msgs/action/Navigate.action b/src/guppy_msgs/action/Navigate.action index fa32c04..40ce756 100644 --- a/src/guppy_msgs/action/Navigate.action +++ b/src/guppy_msgs/action/Navigate.action @@ -1,5 +1,6 @@ geometry_msgs/Pose pose bool relative true #use global pose or relative pose +float64 time --- geometry_msgs/Pose pose #final relative position bool target_reached #if guppy made it to the target pose diff --git a/src/guppy_nav/CMakeLists.txt b/src/guppy_nav/CMakeLists.txt index 205840b..74612b3 100644 --- a/src/guppy_nav/CMakeLists.txt +++ b/src/guppy_nav/CMakeLists.txt @@ -11,18 +11,25 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(rclcpp_components REQUIRED) -find_package(geometry_msgs REQUIRED) + find_package(guppy_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) + +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) + +find_package(control_toolbox REQUIRED) + add_library(action_server SHARED src/navigate_action_server.cpp) -#target_include_directories(action_server PRIVATE -# $ -# $ -#) +target_include_directories(action_server PRIVATE + $ + $ +) -#add_executable(navigate_action_server src/navigate_action_server.cpp) -ament_target_dependencies(action_server rclcpp rclcpp_action rclcpp_components guppy_msgs geometry_msgs) +ament_target_dependencies(action_server rclcpp rclcpp_action rclcpp_components guppy_msgs nav_msgs geometry_msgs tf2 tf2_geometry_msgs control_toolbox) rclcpp_components_register_node(action_server PLUGIN "NavigateActionServer" EXECUTABLE navigate_action_server) @@ -31,9 +38,3 @@ install(TARGETS DESTINATION lib/${PROJECT_NAME} ) -#install(DIRECTORY -# launch -# -# DESTINATION share/${PROJECT_NAME}/ -#) -#ament_package() diff --git a/src/guppy_nav/include/guppy_nav/trajectory.hpp b/src/guppy_nav/include/guppy_nav/trajectory.hpp new file mode 100644 index 0000000..2b44170 --- /dev/null +++ b/src/guppy_nav/include/guppy_nav/trajectory.hpp @@ -0,0 +1,51 @@ +#include "geometry_msgs/msg/pose.hpp" + +struct Trajectory { + double startVelocity, endVelocity, totalTime, targetPosition; // given + double attackTime, decayTime, maxVelocity; // computed + double k1, c2, c3, k3; // precomputed constants + + explicit Trajectory(double startVelocity, double endVelocity, double attack, double decay, double totalTime, double targetPosition) : + startVelocity(startVelocity), endVelocity(endVelocity), totalTime(totalTime), targetPosition(targetPosition) { + attackTime = totalTime * attack, decayTime = totalTime * (1 - decay), maxVelocity = computeMaxVelocity(); + k1 = (maxVelocity - startVelocity) / attackTime, c2 = computePosition1(attackTime), c3 = computePosition2(decayTime), k3 = (endVelocity - maxVelocity) / (totalTime - decayTime); + } + + double getTargetVelocity(double time) const { + if (time <= attackTime) return (time < attackTime) ? computeVelocity1(time) : maxVelocity; + else if (time <= decayTime) return maxVelocity; + else return (time < totalTime) ? computeVelocity3(time) : endVelocity; + } + + double getTargetPosition(double time) const { + if (time <= attackTime) return (time < attackTime) ? computePosition1(time) : c2; + else if (time <= decayTime) return (time < decayTime) ? computePosition2(time) : c3; + else return (time < totalTime) ? computePosition3(time) : targetPosition; + } +private: + double inline computeMaxVelocity() const { + return (2 * targetPosition - attackTime * startVelocity - (totalTime - decayTime) * endVelocity) / (decayTime - attackTime + totalTime); + } + + double inline computePosition1(double time) const { + return time * (k1 * 0.5 * time + startVelocity); + } + + double inline computePosition2(double time) const { + return maxVelocity * time + c2; + } + + double inline computePosition3(double time) const { + const double dt = time - decayTime; + + return c3 + dt * (k3 * dt + maxVelocity); + } + + double inline computeVelocity1(double time) const { + return k1 * time + startVelocity; + } + + double inline computeVelocity3(double time) const { + return k3 * (time - decayTime) + maxVelocity; + } +}; \ No newline at end of file diff --git a/src/guppy_nav/package.xml b/src/guppy_nav/package.xml index 02cff8f..6bca6a3 100644 --- a/src/guppy_nav/package.xml +++ b/src/guppy_nav/package.xml @@ -7,10 +7,17 @@ Palouse RoboSub MIT - ament_cmake - guppy_msgs + geometry_msgs + nav_msgs + + tf2 + tf2_geometry_msgs + + control_toolbox + + ament_cmake ament_lint_auto ament_lint_common diff --git a/src/guppy_nav/src/navigate_action_server.cpp b/src/guppy_nav/src/navigate_action_server.cpp index bc168f2..81e1c0a 100644 --- a/src/guppy_nav/src/navigate_action_server.cpp +++ b/src/guppy_nav/src/navigate_action_server.cpp @@ -1,20 +1,36 @@ -#include #include -#include +#include #include -#include "rclcpp_action/rclcpp_action.hpp" -#include "rclcpp_components/register_node_macro.hpp" +#include +#include +#include + +#include + +#include +#include +#include + +#include "guppy_nav/trajectory.hpp" #include "guppy_msgs/action/navigate.hpp" +#include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/twist.hpp" +#include "nav_msgs/msg/odometry.hpp" #define THRESHOLD 0.1 +#define TOTAL_TIME 10 // in seconds +#define ATTACK 0.25 // from 0.0 - 1.0, ATTACK + DECAY must <= 1.0 +#define DECAY 0.25 + +#define TARGET_RATE_MS 10 + class NavigateActionServer : public rclcpp::Node { public: explicit NavigateActionServer(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) : Node("navigate_action_server", options) { - auto handle_goal = [this](const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal) { + auto handleGoal = [this](const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal) { RCLCPP_INFO(this->get_logger(), "goal request with pose and relative set to %d", goal->relative); //goal->pose (void)uuid; // silence unused parameter warning @@ -22,71 +38,208 @@ class NavigateActionServer : public rclcpp::Node { return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }; - auto handle_cancel = [this](const std::shared_ptr> goal_handle) { + auto handleCancel = [this](const std::shared_ptr> goalHandle) { RCLCPP_INFO(this->get_logger(), "request to cancel goal"); - (void)goal_handle; // silence unused parameter warning + (void)goalHandle; // silence unused parameter warning return rclcpp_action::CancelResponse::ACCEPT; }; - auto handle_accepted = [this](const std::shared_ptr> goal_handle) { - // to avoid blocking calling thread, return lambda quickly and execute on seperate thread - auto execute_in_thread = [this, goal_handle](){ - return this->execute(goal_handle); - }; - - std::thread{execute_in_thread}.detach(); + auto handleAccepted = [this](const std::shared_ptr> goalHandle) { + execute(goalHandle); }; - - this->action_server_ = rclcpp_action::create_server( + + _actionServer = rclcpp_action::create_server( this, "navigate", - handle_goal, - handle_cancel, - handle_accepted + handleGoal, + handleCancel, + handleAccepted ); + + _commandVelocityPublisher = create_publisher("cmd_vel/nav", 10); + _odomSubscription = create_subscription("/odometry/filtered",10,std::bind(&NavigateActionServer::odometryCallback, this, std::placeholders::_1)); + + _xPid.set_gains(1.5, 0.0, 0.1, std::numeric_limits::infinity(), -std::numeric_limits::infinity(), antiWindupStrategy); // this broke please help + _yPid.set_gains(1.5, 0.0, 0.1, std::numeric_limits::infinity(), -std::numeric_limits::infinity(), antiWindupStrategy); + _zPid.set_gains(1.5, 0.0, 0.1, std::numeric_limits::infinity(), -std::numeric_limits::infinity(), antiWindupStrategy); + } + + void odometryCallback(nav_msgs::msg::Odometry::SharedPtr msg) { + std::lock_guard lock(_kinematiStateMutex); + _kinematicState.pose = msg->pose.pose; + _kinematicState.twist = msg->twist.twist; } private: - rclcpp_action::Server::SharedPtr action_server_; + rclcpp_action::Server::SharedPtr _actionServer; + + rclcpp::Publisher::SharedPtr _commandVelocityPublisher; + rclcpp::Subscription::SharedPtr _odomSubscription; - void execute(const std::shared_ptr> goal_handle) { - RCLCPP_INFO(this->get_logger(), "executing goal"); + struct KinematicState { + geometry_msgs::msg::Pose pose; + geometry_msgs::msg::Twist twist; + }; - rclcpp::Rate loop_rate(1); //? + KinematicState _kinematicState; + std::mutex _kinematiStateMutex; - const auto goal = goal_handle->get_goal(); + control_toolbox::Pid _xPid, _yPid, _zPid; - auto feedback = std::make_shared(); + struct Vector3 { + double x{0}, y{0}, z{0}; - auto result = std::make_shared(); + Vector3() = default; + Vector3(double x, double y, double z) : x(x), y(y), z(z) {} + }; + + struct Trajectory3 { + Trajectory x, y, z; + + explicit Trajectory3(const Vector3& startVelocity, const Vector3& endVelocity, double attack, double decay, double totalTime, const Vector3& targetPosition) : + x(Trajectory(startVelocity.x, endVelocity.x, attack, decay, totalTime, targetPosition.x)), + y(Trajectory(startVelocity.y, endVelocity.y, attack, decay, totalTime, targetPosition.y)), + z(Trajectory(startVelocity.z, endVelocity.z, attack, decay, totalTime, targetPosition.z)) {} + + Vector3 velocity(double time) const { + return {x.getTargetVelocity(time), y.getTargetVelocity(time), z.getTargetVelocity(time)}; + } + + Vector3 position(double time) const { + return {x.getTargetPosition(time), y.getTargetPosition(time), z.getTargetPosition(time)}; + } + }; + + inline static const control_toolbox::AntiWindupStrategy antiWindupStrategy = []{ + control_toolbox::AntiWindupStrategy strategy; + strategy.set_type("back_calculation"); + return strategy; + }(); + + KinematicState getKinematicState() { + std::lock_guard lock(_kinematiStateMutex); + return _kinematicState; + } + + Vector3 getRPY(const geometry_msgs::msg::Quaternion& msg) { + tf2::Quaternion quaternion; + tf2::fromMsg(msg, quaternion); + tf2::Matrix3x3 matrix(quaternion); + + Vector3 orientation; + matrix.getRPY(orientation.x, orientation.y, orientation.z); // (x) roll, (y) pitch, (z) yaw + return orientation; + } + + + Vector3 transformWorldToLocal(const Vector3& world, const Vector3& rpy) { + Vector3 local; + + local.x = cos(rpy.z) * world.x + sin(rpy.z) * world.y; + local.y = -sin(rpy.z) * world.x + cos(rpy.z) * world.y; - for(int i = 1; /*(i < goal->order) &&*/ rclcpp::ok(); ++i) { //what is this? - if (goal_handle->is_canceling()) { - //result->sequence = sequenece; - goal_handle->canceled(result); + local.z = cos(rpy.y) * world.z - sin(rpy.y) * local.x; - RCLCPP_INFO(this->get_logger(), "goal cancelled"); + return local; + } + + geometry_msgs::msg::Twist computeCommandVelocity(const Trajectory3& trajectory, double time, double dt, const KinematicState& state, const Vector3& initialPosition) { + auto targetPosition = trajectory.position(time); + auto targetVelocity = trajectory.velocity(time); + + Vector3 relativePosition{state.pose.position.x - initialPosition.x, state.pose.position.y - initialPosition.y, state.pose.position.z - initialPosition.z}; + Vector3 worldError{targetPosition.x - relativePosition.x, targetPosition.y - relativePosition.y, targetPosition.z - relativePosition.z}; // need to translate the state to relative coordinates, i.e. from initial position + + auto rpy = getRPY(state.pose.orientation); + auto localError = transformWorldToLocal(worldError, rpy); + + geometry_msgs::msg::Twist commandVelocity; + + commandVelocity.linear.x = targetVelocity.x + _xPid.compute_command(localError.x, rclcpp::Duration::from_seconds(dt)); + commandVelocity.linear.y = targetVelocity.y + _yPid.compute_command(localError.y, rclcpp::Duration::from_seconds(dt)); + commandVelocity.linear.z = targetVelocity.z + _zPid.compute_command(localError.z, rclcpp::Duration::from_seconds(dt)); + + return commandVelocity; + }; + + void execute(const std::shared_ptr> goalHandle) { + const auto& goal = goalHandle->get_goal(); + + auto initialState = getKinematicState(); + + Vector3 initialPosition{initialState.pose.position.x, initialState.pose.position.y, initialState.pose.position.z}; + auto finalPose = goal->pose; // not relative + Vector3 relativeFinalPosition = goal->relative ? Vector3{finalPose.position.x, finalPose.position.y, finalPose.position.z} : Vector3{finalPose.position.x - initialPosition.x, finalPose.position.y - initialPosition.y, finalPose.position.z - initialPosition.z}; + + auto initialLinearVelocity = initialState.twist.linear; + Vector3 startVelocity{initialLinearVelocity.x, initialLinearVelocity.y, initialLinearVelocity.z}; + + Trajectory3 trajectory(startVelocity, {0, 0, 0}, ATTACK, DECAY, TOTAL_TIME, relativeFinalPosition); + + rclcpp::Rate rate(1000.0 / TARGET_RATE_MS); + + _xPid.reset(); + _yPid.reset(); + _zPid.reset(); + + auto clock = this->get_clock(); + rclcpp::Time start = clock->now(); + rclcpp::Time last = start; + + auto feedback = std::make_shared(); + auto result = std::make_shared(); + + while (rclcpp::ok()) { + if (goalHandle->is_canceling()) { + result->target_reached = false; + goalHandle->canceled(result); return; } - goal_handle->publish_feedback(feedback); - - RCLCPP_INFO(this->get_logger(), "published feeback"); + auto now = clock->now(); + double elapsed = (now - start).seconds(); + double delta = std::clamp((now - last).seconds(), 1e-4, 0.05); + last = now; + + auto state = getKinematicState(); + auto commandVelocity = computeCommandVelocity(trajectory, elapsed, delta, state, initialPosition); - loop_rate.sleep(); + feedback->progress = state.pose; + + _commandVelocityPublisher->publish(commandVelocity); + goalHandle->publish_feedback(feedback); + + if (elapsed >= TOTAL_TIME) break; + + rate.sleep(); } if (rclcpp::ok()) { - // result->pose = ... - // result-> + result->pose = getKinematicState().pose; + result->target_reached = true; // shouldn't assume true, fix that by calcing the hypotenuse - goal_handle->succeed(result); + goalHandle->succeed(result); RCLCPP_INFO(this->get_logger(), "goal succeeded"); } - }; + } }; -RCLCPP_COMPONENTS_REGISTER_NODE(NavigateActionServer) \ No newline at end of file +RCLCPP_COMPONENTS_REGISTER_NODE(NavigateActionServer) + +int main(int argc, char* argv[]) { + rclcpp::init(argc, argv); + + auto node = std::make_shared(); + + rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(), 2); + + executor.add_node(node); + executor.spin(); + + rclcpp::shutdown(); + + return 0; +} \ No newline at end of file From 7ad0c836046abbee531afa43307728e230898795 Mon Sep 17 00:00:00 2001 From: Ryden Handsaker Date: Sun, 26 Apr 2026 04:16:49 +0000 Subject: [PATCH 03/18] feat: added orientation slerp --- src/guppy_nav/CMakeLists.txt | 4 +- .../include/guppy_nav/trajectory.hpp | 2 +- src/guppy_nav/package.xml | 3 - src/guppy_nav/src/navigate_action_server.cpp | 134 ++++++++++-------- 4 files changed, 78 insertions(+), 65 deletions(-) diff --git a/src/guppy_nav/CMakeLists.txt b/src/guppy_nav/CMakeLists.txt index 74612b3..e419682 100644 --- a/src/guppy_nav/CMakeLists.txt +++ b/src/guppy_nav/CMakeLists.txt @@ -17,8 +17,8 @@ find_package(guppy_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(geometry_msgs REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3) find_package(control_toolbox REQUIRED) diff --git a/src/guppy_nav/include/guppy_nav/trajectory.hpp b/src/guppy_nav/include/guppy_nav/trajectory.hpp index 2b44170..2bf084f 100644 --- a/src/guppy_nav/include/guppy_nav/trajectory.hpp +++ b/src/guppy_nav/include/guppy_nav/trajectory.hpp @@ -1,4 +1,4 @@ -#include "geometry_msgs/msg/pose.hpp" +#pragma once struct Trajectory { double startVelocity, endVelocity, totalTime, targetPosition; // given diff --git a/src/guppy_nav/package.xml b/src/guppy_nav/package.xml index 6bca6a3..4b88183 100644 --- a/src/guppy_nav/package.xml +++ b/src/guppy_nav/package.xml @@ -12,9 +12,6 @@ geometry_msgs nav_msgs - tf2 - tf2_geometry_msgs - control_toolbox ament_cmake diff --git a/src/guppy_nav/src/navigate_action_server.cpp b/src/guppy_nav/src/navigate_action_server.cpp index 81e1c0a..c83ce09 100644 --- a/src/guppy_nav/src/navigate_action_server.cpp +++ b/src/guppy_nav/src/navigate_action_server.cpp @@ -8,9 +8,8 @@ #include -#include -#include -#include +#include +#include #include "guppy_nav/trajectory.hpp" @@ -61,9 +60,12 @@ class NavigateActionServer : public rclcpp::Node { _commandVelocityPublisher = create_publisher("cmd_vel/nav", 10); _odomSubscription = create_subscription("/odometry/filtered",10,std::bind(&NavigateActionServer::odometryCallback, this, std::placeholders::_1)); - _xPid.set_gains(1.5, 0.0, 0.1, std::numeric_limits::infinity(), -std::numeric_limits::infinity(), antiWindupStrategy); // this broke please help + _xPid.set_gains(1.5, 0.0, 0.1, std::numeric_limits::infinity(), -std::numeric_limits::infinity(), antiWindupStrategy); _yPid.set_gains(1.5, 0.0, 0.1, std::numeric_limits::infinity(), -std::numeric_limits::infinity(), antiWindupStrategy); _zPid.set_gains(1.5, 0.0, 0.1, std::numeric_limits::infinity(), -std::numeric_limits::infinity(), antiWindupStrategy); + _yawPid.set_gains(1.5, 0.0, 0.1, std::numeric_limits::infinity(), -std::numeric_limits::infinity(), antiWindupStrategy); + _pitchPid.set_gains(1.5, 0.0, 0.1, std::numeric_limits::infinity(), -std::numeric_limits::infinity(), antiWindupStrategy); + _rollPid.set_gains(1.5, 0.0, 0.1, std::numeric_limits::infinity(), -std::numeric_limits::infinity(), antiWindupStrategy); } void odometryCallback(nav_msgs::msg::Odometry::SharedPtr msg) { @@ -85,29 +87,41 @@ class NavigateActionServer : public rclcpp::Node { KinematicState _kinematicState; std::mutex _kinematiStateMutex; - control_toolbox::Pid _xPid, _yPid, _zPid; - - struct Vector3 { - double x{0}, y{0}, z{0}; - - Vector3() = default; - Vector3(double x, double y, double z) : x(x), y(y), z(z) {} - }; + control_toolbox::Pid _xPid, _yPid, _zPid, _yawPid, _pitchPid, _rollPid; struct Trajectory3 { Trajectory x, y, z; - explicit Trajectory3(const Vector3& startVelocity, const Vector3& endVelocity, double attack, double decay, double totalTime, const Vector3& targetPosition) : - x(Trajectory(startVelocity.x, endVelocity.x, attack, decay, totalTime, targetPosition.x)), - y(Trajectory(startVelocity.y, endVelocity.y, attack, decay, totalTime, targetPosition.y)), - z(Trajectory(startVelocity.z, endVelocity.z, attack, decay, totalTime, targetPosition.z)) {} + explicit Trajectory3(const Eigen::Vector3d& startVelocity, const Eigen::Vector3d& endVelocity, double attack, double decay, double totalTime, const Eigen::Vector3d& targetPosition) : + x(Trajectory(startVelocity.x(), endVelocity.x(), attack, decay, totalTime, targetPosition.x())), + y(Trajectory(startVelocity.y(), endVelocity.y(), attack, decay, totalTime, targetPosition.y())), + z(Trajectory(startVelocity.z(), endVelocity.z(), attack, decay, totalTime, targetPosition.z())) {} - Vector3 velocity(double time) const { - return {x.getTargetVelocity(time), y.getTargetVelocity(time), z.getTargetVelocity(time)}; + Eigen::Vector3d velocity(double elapsed) const { + return Eigen::Vector3d(x.getTargetVelocity(elapsed), y.getTargetVelocity(elapsed), z.getTargetVelocity(elapsed)); } - Vector3 position(double time) const { - return {x.getTargetPosition(time), y.getTargetPosition(time), z.getTargetPosition(time)}; + Eigen::Vector3d position(double elapsed) const { + return Eigen::Vector3d(x.getTargetPosition(elapsed), y.getTargetPosition(elapsed), z.getTargetPosition(elapsed)); + } + }; + + struct OrientationSolver { + Eigen::Quaterniond initialOrientation, finalOrientation; + double totalTime; + + explicit OrientationSolver(const Eigen::Quaterniond& initialOrientation, const Eigen::Quaterniond& finalOrientation, double totalTime) : + initialOrientation(initialOrientation), finalOrientation(finalOrientation), totalTime(totalTime) {} + + Eigen::Vector3d error(double elapsed, const Eigen::Quaterniond& currentOrientation) const { + double alpha = std::clamp(elapsed / totalTime, 0.0, 1.0); + + Eigen::Quaterniond targetOrientation = initialOrientation.slerp(alpha, finalOrientation); + + Eigen::Quaterniond orientationError = currentOrientation.inverse() * targetOrientation; + Eigen::AngleAxisd angleAxisError(orientationError); + + return angleAxisError.axis() * angleAxisError.angle(); } }; @@ -122,43 +136,30 @@ class NavigateActionServer : public rclcpp::Node { return _kinematicState; } - Vector3 getRPY(const geometry_msgs::msg::Quaternion& msg) { - tf2::Quaternion quaternion; - tf2::fromMsg(msg, quaternion); - tf2::Matrix3x3 matrix(quaternion); + geometry_msgs::msg::Twist computeCommandVelocity(const Trajectory3& trajectory, const OrientationSolver& orientationSolver, double elapsed, double delta, const KinematicState& state, const Eigen::Vector3d& initialPosition) { + const auto targetPositionWorld = trajectory.position(elapsed); // world + const auto targetVelocityWorld = trajectory.velocity(elapsed); // world - Vector3 orientation; - matrix.getRPY(orientation.x, orientation.y, orientation.z); // (x) roll, (y) pitch, (z) yaw - return orientation; - } + const Eigen::Vector3d currentPosition(state.pose.position.x, state.pose.position.y, state.pose.position.z); // world! + const auto relativePosition = currentPosition - initialPosition; // still world + const auto worldError = targetPositionWorld - relativePosition; + Eigen::Quaterniond currentOrientation(state.pose.orientation.w, state.pose.orientation.x, state.pose.orientation.y, state.pose.orientation.z); // world! + currentOrientation.normalize(); - Vector3 transformWorldToLocal(const Vector3& world, const Vector3& rpy) { - Vector3 local; - - local.x = cos(rpy.z) * world.x + sin(rpy.z) * world.y; - local.y = -sin(rpy.z) * world.x + cos(rpy.z) * world.y; - - local.z = cos(rpy.y) * world.z - sin(rpy.y) * local.x; - - return local; - } - - geometry_msgs::msg::Twist computeCommandVelocity(const Trajectory3& trajectory, double time, double dt, const KinematicState& state, const Vector3& initialPosition) { - auto targetPosition = trajectory.position(time); - auto targetVelocity = trajectory.velocity(time); - - Vector3 relativePosition{state.pose.position.x - initialPosition.x, state.pose.position.y - initialPosition.y, state.pose.position.z - initialPosition.z}; - Vector3 worldError{targetPosition.x - relativePosition.x, targetPosition.y - relativePosition.y, targetPosition.z - relativePosition.z}; // need to translate the state to relative coordinates, i.e. from initial position - - auto rpy = getRPY(state.pose.orientation); - auto localError = transformWorldToLocal(worldError, rpy); + const auto localError = currentOrientation.inverse() * worldError; + const auto targetVelocityLocal = currentOrientation.inverse() * targetVelocityWorld; + const auto angularErrorLocal = orientationSolver.error(elapsed, currentOrientation); geometry_msgs::msg::Twist commandVelocity; - commandVelocity.linear.x = targetVelocity.x + _xPid.compute_command(localError.x, rclcpp::Duration::from_seconds(dt)); - commandVelocity.linear.y = targetVelocity.y + _yPid.compute_command(localError.y, rclcpp::Duration::from_seconds(dt)); - commandVelocity.linear.z = targetVelocity.z + _zPid.compute_command(localError.z, rclcpp::Duration::from_seconds(dt)); + commandVelocity.angular.z = _yawPid.compute_command(angularErrorLocal.z(), rclcpp::Duration::from_seconds(delta)); + commandVelocity.angular.y = _pitchPid.compute_command(angularErrorLocal.y(), rclcpp::Duration::from_seconds(delta)); + commandVelocity.angular.x = _rollPid.compute_command(angularErrorLocal.x(), rclcpp::Duration::from_seconds(delta)); + + commandVelocity.linear.x = targetVelocityLocal.x() + _xPid.compute_command(localError.x(), rclcpp::Duration::from_seconds(delta)); + commandVelocity.linear.y = targetVelocityLocal.y() + _yPid.compute_command(localError.y(), rclcpp::Duration::from_seconds(delta)); + commandVelocity.linear.z = targetVelocityLocal.z() + _zPid.compute_command(localError.z(), rclcpp::Duration::from_seconds(delta)); return commandVelocity; }; @@ -166,22 +167,37 @@ class NavigateActionServer : public rclcpp::Node { void execute(const std::shared_ptr> goalHandle) { const auto& goal = goalHandle->get_goal(); - auto initialState = getKinematicState(); + const auto initialState = getKinematicState(); + + Eigen::Vector3d initialPosition(initialState.pose.position.x, initialState.pose.position.y, initialState.pose.position.z); //world + Eigen::Quaterniond initialOrientation(initialState.pose.orientation.w, initialState.pose.orientation.x, initialState.pose.orientation.y, initialState.pose.orientation.z); //world + initialOrientation.normalize(); + + Eigen::Vector3d relativeFinalPosition; // still world, just makes origin (0, 0, 0) (may have to be converted if user submitted local coordinates) + + const auto& finalPose = goal->pose; + Eigen::Quaterniond finalOrientation(finalPose.orientation.w, finalPose.orientation.x, finalPose.orientation.y, finalPose.orientation.z); // will end up in world + finalOrientation.normalize(); + + if (goal->relative) { + relativeFinalPosition << finalPose.position.x, finalPose.position.y, finalPose.position.z; + finalOrientation = initialOrientation * finalOrientation; + } else relativeFinalPosition << finalPose.position.x - initialPosition.x(), finalPose.position.y - initialPosition.y(), finalPose.position.z - initialPosition.z(); - Vector3 initialPosition{initialState.pose.position.x, initialState.pose.position.y, initialState.pose.position.z}; - auto finalPose = goal->pose; // not relative - Vector3 relativeFinalPosition = goal->relative ? Vector3{finalPose.position.x, finalPose.position.y, finalPose.position.z} : Vector3{finalPose.position.x - initialPosition.x, finalPose.position.y - initialPosition.y, finalPose.position.z - initialPosition.z}; + Eigen::Vector3d startVelocity(initialState.twist.linear.x, initialState.twist.linear.y, initialState.twist.linear.z); // world - auto initialLinearVelocity = initialState.twist.linear; - Vector3 startVelocity{initialLinearVelocity.x, initialLinearVelocity.y, initialLinearVelocity.z}; + Trajectory3 trajectory(startVelocity, Eigen::Vector3d::Zero(), ATTACK, DECAY, TOTAL_TIME, relativeFinalPosition); - Trajectory3 trajectory(startVelocity, {0, 0, 0}, ATTACK, DECAY, TOTAL_TIME, relativeFinalPosition); + OrientationSolver orientationSolver(initialOrientation, finalOrientation, TOTAL_TIME); rclcpp::Rate rate(1000.0 / TARGET_RATE_MS); _xPid.reset(); _yPid.reset(); _zPid.reset(); + _yawPid.reset(); + _pitchPid.reset(); + _rollPid.reset(); auto clock = this->get_clock(); rclcpp::Time start = clock->now(); @@ -204,7 +220,7 @@ class NavigateActionServer : public rclcpp::Node { last = now; auto state = getKinematicState(); - auto commandVelocity = computeCommandVelocity(trajectory, elapsed, delta, state, initialPosition); + auto commandVelocity = computeCommandVelocity(trajectory, orientationSolver, elapsed, delta, state, initialPosition); feedback->progress = state.pose; From 6779391c78e43d87e09635bc8c3da6139a87ed3b Mon Sep 17 00:00:00 2001 From: Ryden Handsaker Date: Sun, 26 Apr 2026 04:23:52 +0000 Subject: [PATCH 04/18] fix: cmakelists merge --- src/guppy_msgs/CMakeLists.txt | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/guppy_msgs/CMakeLists.txt b/src/guppy_msgs/CMakeLists.txt index 795e05a..1fd5e69 100644 --- a/src/guppy_msgs/CMakeLists.txt +++ b/src/guppy_msgs/CMakeLists.txt @@ -15,17 +15,16 @@ find_package(geometry_msgs REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "srv/ChangeState.srv" - "srv/SendCan.srv" + "msg/State.msg" "msg/CanFrame.msg" "srv/SendCan.srv" - "msg/State.msg" "msg/Toast.msg" + "msg/CornerDetection.msg" "msg/CornerDetectionList.msg" "msg/TransformList.msg" - "srv/ChangeState.srv" "action/Navigate.action" From 25d814e7311da08e086fc9ef9a93a3111e169abf Mon Sep 17 00:00:00 2001 From: Ryden Handsaker Date: Sun, 26 Apr 2026 04:42:02 +0000 Subject: [PATCH 05/18] fix: added launch files --- src/guppy/launch/hw.xml | 1 + src/guppy/launch/sim.xml | 2 ++ src/guppy_nav/CMakeLists.txt | 1 - src/guppy_nav/launch/core.xml | 4 ++++ src/guppy_nav/launch/sim.xml | 4 ++++ src/guppy_nav/src/navigate_action_server.cpp | 2 +- 6 files changed, 12 insertions(+), 2 deletions(-) create mode 100644 src/guppy_nav/launch/core.xml create mode 100644 src/guppy_nav/launch/sim.xml diff --git a/src/guppy/launch/hw.xml b/src/guppy/launch/hw.xml index 3386cbb..f848055 100644 --- a/src/guppy/launch/hw.xml +++ b/src/guppy/launch/hw.xml @@ -11,4 +11,5 @@ + \ No newline at end of file diff --git a/src/guppy/launch/sim.xml b/src/guppy/launch/sim.xml index 89f92ae..59a4269 100644 --- a/src/guppy/launch/sim.xml +++ b/src/guppy/launch/sim.xml @@ -10,4 +10,6 @@ + + \ No newline at end of file diff --git a/src/guppy_nav/CMakeLists.txt b/src/guppy_nav/CMakeLists.txt index e419682..ebc507a 100644 --- a/src/guppy_nav/CMakeLists.txt +++ b/src/guppy_nav/CMakeLists.txt @@ -32,7 +32,6 @@ target_include_directories(action_server PRIVATE ament_target_dependencies(action_server rclcpp rclcpp_action rclcpp_components guppy_msgs nav_msgs geometry_msgs tf2 tf2_geometry_msgs control_toolbox) rclcpp_components_register_node(action_server PLUGIN "NavigateActionServer" EXECUTABLE navigate_action_server) - install(TARGETS action_server diff --git a/src/guppy_nav/launch/core.xml b/src/guppy_nav/launch/core.xml new file mode 100644 index 0000000..e62fe3a --- /dev/null +++ b/src/guppy_nav/launch/core.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/src/guppy_nav/launch/sim.xml b/src/guppy_nav/launch/sim.xml new file mode 100644 index 0000000..6750491 --- /dev/null +++ b/src/guppy_nav/launch/sim.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/src/guppy_nav/src/navigate_action_server.cpp b/src/guppy_nav/src/navigate_action_server.cpp index c83ce09..8221d91 100644 --- a/src/guppy_nav/src/navigate_action_server.cpp +++ b/src/guppy_nav/src/navigate_action_server.cpp @@ -51,7 +51,7 @@ class NavigateActionServer : public rclcpp::Node { _actionServer = rclcpp_action::create_server( this, - "navigate", + "/navigate", handleGoal, handleCancel, handleAccepted From 950b35a1d309887c4e6acfeca8aab48595c22dbe Mon Sep 17 00:00:00 2001 From: Ryden Handsaker Date: Sun, 26 Apr 2026 04:49:36 +0000 Subject: [PATCH 06/18] fix: changed cmakelists --- src/guppy_nav/CMakeLists.txt | 21 ++++++++++++++++----- 1 file changed, 16 insertions(+), 5 deletions(-) diff --git a/src/guppy_nav/CMakeLists.txt b/src/guppy_nav/CMakeLists.txt index ebc507a..54194b0 100644 --- a/src/guppy_nav/CMakeLists.txt +++ b/src/guppy_nav/CMakeLists.txt @@ -24,12 +24,15 @@ find_package(control_toolbox REQUIRED) add_library(action_server SHARED src/navigate_action_server.cpp) -target_include_directories(action_server PRIVATE - $ - $ -) +#target_include_directories(action_server PRIVATE +# $ +# $ +#) + +include_directories(include) -ament_target_dependencies(action_server rclcpp rclcpp_action rclcpp_components guppy_msgs nav_msgs geometry_msgs tf2 tf2_geometry_msgs control_toolbox) +add_executable(action_server src/navigate_action_server.cpp) +ament_target_dependencies(action_server rclcpp rclcpp_action rclcpp_components guppy_msgs nav_msgs geometry_msgs Eigen3 control_toolbox) rclcpp_components_register_node(action_server PLUGIN "NavigateActionServer" EXECUTABLE navigate_action_server) install(TARGETS @@ -37,3 +40,11 @@ install(TARGETS DESTINATION lib/${PROJECT_NAME} ) + +install(DIRECTORY + launch + + DESTINATION share/${PROJECT_NAME}/ +) + +ament_package() From a16d53f3ba29e5db7103660d2e7a453421284619 Mon Sep 17 00:00:00 2001 From: Ryden Handsaker Date: Sun, 26 Apr 2026 04:51:06 +0000 Subject: [PATCH 07/18] fix: changed package.xml --- src/guppy_nav/CMakeLists.txt | 2 +- src/guppy_nav/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/guppy_nav/CMakeLists.txt b/src/guppy_nav/CMakeLists.txt index 54194b0..3dacedc 100644 --- a/src/guppy_nav/CMakeLists.txt +++ b/src/guppy_nav/CMakeLists.txt @@ -31,7 +31,7 @@ add_library(action_server SHARED src/navigate_action_server.cpp) include_directories(include) -add_executable(action_server src/navigate_action_server.cpp) +#add_executable(action_server src/navigate_action_server.cpp) ament_target_dependencies(action_server rclcpp rclcpp_action rclcpp_components guppy_msgs nav_msgs geometry_msgs Eigen3 control_toolbox) rclcpp_components_register_node(action_server PLUGIN "NavigateActionServer" EXECUTABLE navigate_action_server) diff --git a/src/guppy_nav/package.xml b/src/guppy_nav/package.xml index 4b88183..a33cb6c 100644 --- a/src/guppy_nav/package.xml +++ b/src/guppy_nav/package.xml @@ -12,7 +12,7 @@ geometry_msgs nav_msgs - control_toolbox + control_toolbox ament_cmake From 131bd55e35b49aa1874d9d65ac943fccfb21f2cc Mon Sep 17 00:00:00 2001 From: Ryden Handsaker Date: Sun, 26 Apr 2026 05:05:34 +0000 Subject: [PATCH 08/18] fix: cmake magic --- src/guppy_nav/CMakeLists.txt | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/guppy_nav/CMakeLists.txt b/src/guppy_nav/CMakeLists.txt index 3dacedc..9b5bbcf 100644 --- a/src/guppy_nav/CMakeLists.txt +++ b/src/guppy_nav/CMakeLists.txt @@ -38,7 +38,10 @@ rclcpp_components_register_node(action_server PLUGIN "NavigateActionServer" EXEC install(TARGETS action_server - DESTINATION lib/${PROJECT_NAME} + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + #DESTINATION lib/${PROJECT_NAME} ) install(DIRECTORY From d9d01fbdc45d0bf2c21a0749961f6bc3a3a980fb Mon Sep 17 00:00:00 2001 From: Ryden Handsaker Date: Sun, 26 Apr 2026 05:40:31 +0000 Subject: [PATCH 09/18] fix: send zero cmd vel and PID param changes --- src/guppy_control/config/controller_params.yaml | 8 ++++---- src/guppy_nav/src/navigate_action_server.cpp | 12 +++++++++++- src/guppy_tests/package.xml | 17 +++++++++++++++++ 3 files changed, 32 insertions(+), 5 deletions(-) create mode 100644 src/guppy_tests/package.xml diff --git a/src/guppy_control/config/controller_params.yaml b/src/guppy_control/config/controller_params.yaml index 814c929..38809a2 100644 --- a/src/guppy_control/config/controller_params.yaml +++ b/src/guppy_control/config/controller_params.yaml @@ -25,14 +25,14 @@ control_chassis: pid_gains_vel_linear: [ 1000.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: [ 70.0, 0.0, 40.0 ] + #pid_gains_pose_linear: [ 90.0, 0.0, 30.0 ] + #pid_gains_pose_angular: [ 70.0, 0.0, 40.0 ] # pid_gains_vel_linear: [ 0.0, 0.0, 0.0 ] # pid_gains_vel_angular: [ 0.0, 0.0, 0.0 ] - # pid_gains_pose_linear: [ 0.0, 0.0, 0.0 ] - # pid_gains_pose_angular: [ 0.0, 0.0, 0.0 ] + pid_gains_pose_linear: [ 0.0, 0.0, 0.0 ] + pid_gains_pose_angular: [ 0.0, 0.0, 0.0 ] pose_lock_deadband: [ 0.1, 0.1, 0.1, 0.1, 0.1, 0.1 ] diff --git a/src/guppy_nav/src/navigate_action_server.cpp b/src/guppy_nav/src/navigate_action_server.cpp index 8221d91..1361255 100644 --- a/src/guppy_nav/src/navigate_action_server.cpp +++ b/src/guppy_nav/src/navigate_action_server.cpp @@ -211,6 +211,9 @@ class NavigateActionServer : public rclcpp::Node { result->target_reached = false; goalHandle->canceled(result); + geometry_msgs::msg::Twist zeroTwist(); // publish zero twist + _commandVelocityPublisher->publich(zeroTwist); + return; } @@ -222,7 +225,11 @@ class NavigateActionServer : public rclcpp::Node { auto state = getKinematicState(); auto commandVelocity = computeCommandVelocity(trajectory, orientationSolver, elapsed, delta, state, initialPosition); - feedback->progress = state.pose; + Eigen::Vector3d currentPosition; + currentPosition << state.pose.position.x, state.pose.position.y, state.pose.position.z; + auto relativeCurrentPosition = currentPosition - initalPosition; + + feedback->progress.position.x = currentPosition.x(), feedback->progress.position.x = currentPosition.y(), feedback->progress.position.x = currentPosition.z(); _commandVelocityPublisher->publish(commandVelocity); goalHandle->publish_feedback(feedback); @@ -232,6 +239,9 @@ class NavigateActionServer : public rclcpp::Node { rate.sleep(); } + geometry_msgs::msg::Twist zeroTwist(); // publish zero twist + _commandVelocityPublisher->publich(zeroTwist); + if (rclcpp::ok()) { result->pose = getKinematicState().pose; result->target_reached = true; // shouldn't assume true, fix that by calcing the hypotenuse diff --git a/src/guppy_tests/package.xml b/src/guppy_tests/package.xml new file mode 100644 index 0000000..21e263f --- /dev/null +++ b/src/guppy_tests/package.xml @@ -0,0 +1,17 @@ + + + + guppy_teleop + 0.0.0 + Remote/human operation code package for the 2026 Guppy AUV. + Palouse RoboSub + Apache-2.0 + + ament_copyright + ament_flake8 + ament_pep257 + + + ament_python + + From 14bd6c4de37a64ad1acfb8ac07eb6825d7bcbca2 Mon Sep 17 00:00:00 2001 From: Ryden Handsaker Date: Sun, 26 Apr 2026 05:42:20 +0000 Subject: [PATCH 10/18] feat: added tests, wip --- src/guppy_tests/README.md | 0 src/guppy_tests/guppy_tests/translator.py | 114 ++++++++++++++++++++++ src/guppy_tests/package.xml | 2 +- src/guppy_tests/resource/guppy_teleop | 0 src/guppy_tests/setup.cfg | 4 + src/guppy_tests/setup.py | 75 ++++++++++++++ src/guppy_tests/test/test_copyright.py | 25 +++++ src/guppy_tests/test/test_flake8.py | 25 +++++ src/guppy_tests/test/test_pep257.py | 23 +++++ 9 files changed, 267 insertions(+), 1 deletion(-) create mode 100644 src/guppy_tests/README.md create mode 100644 src/guppy_tests/guppy_tests/translator.py create mode 100644 src/guppy_tests/resource/guppy_teleop create mode 100644 src/guppy_tests/setup.cfg create mode 100644 src/guppy_tests/setup.py create mode 100644 src/guppy_tests/test/test_copyright.py create mode 100644 src/guppy_tests/test/test_flake8.py create mode 100644 src/guppy_tests/test/test_pep257.py diff --git a/src/guppy_tests/README.md b/src/guppy_tests/README.md new file mode 100644 index 0000000..e69de29 diff --git a/src/guppy_tests/guppy_tests/translator.py b/src/guppy_tests/guppy_tests/translator.py new file mode 100644 index 0000000..fd41233 --- /dev/null +++ b/src/guppy_tests/guppy_tests/translator.py @@ -0,0 +1,114 @@ +import rclpy +from geometry_msgs.msg import Twist +from rclpy.node import Node +from std_msgs.msg import Float32MultiArray, Int32MultiArray, String + +class Translator(Node): + def __init__(self): + super().__init__("translator") + + self.dpad_subscriber = self.create_subscription( + Int32MultiArray, "dpad", self.dpad_callback, 10 + ) + self.axes_subscriber = self.create_subscription( + Float32MultiArray, "axes", self.axes_callback, 10 + ) + self.button_subscriber = self.create_subscription( + Int32MultiArray, "buttons", self.button_callback, 10 + ) + self.name_subscriber = self.create_subscription( + String, "gamepad_name", self.name_callback, 10 + ) + + self.publisher = self.create_publisher(Twist, "/cmd_vel/teleop", 10) + + self.controller_state = {"dpad": None, "axes": None, "buttons": None} + self.controller_name = None + + def dpad_callback(self, msg): + self.get_logger().debug("Received dpad message: %s" % msg.data) + self.controller_state["dpad"] = msg.data + self.update_controller_state() + + def axes_callback(self, msg): + self.get_logger().debug("Received axes message: %s" % msg.data) + self.controller_state["axes"] = msg.data + self.update_controller_state() + + def button_callback(self, msg): + self.get_logger().debug("Received button message: %s" % msg.data) + self.controller_state["buttons"] = msg.data + self.update_controller_state() + + def name_callback(self, msg): + self.controller_name = msg.data + + def update_controller_state(self): + if ( + self.controller_state["dpad"] is not None + and self.controller_state["axes"] is not None + and self.controller_state["buttons"] is not None + and self.controller_name is not None + ): + self.publish_twist() + self.controller_state = {"dpad": None, "axes": None, "buttons": None} + + def publish_twist(self): + if ( + self.controller_state["dpad"] is not None + and self.controller_state["axes"] is not None + and self.controller_state["buttons"] is not None + and self.controller_name is not None + ): + twist = None + if self.controller_name == "Logitech Gamepad F310": + twist = logitech_twist(self.controller_state) + elif self.controller_name == "Xbox Series X Controller": + twist = series_x_twist(self.controller_state) + + if twist is not None: + self.publisher.publish(twist) + +def logitech_twist(controller_state): + twist = Twist() + + twist.linear.x = -controller_state["axes"][4] if abs(controller_state["axes"][4]) > 0.05 else 0.0 # right stick vertical + twist.linear.y = -controller_state["axes"][3] if abs(controller_state["axes"][3]) > 0.05 else 0.0 # right stick horizontal + twist.linear.z = -controller_state["axes"][1] if abs(controller_state["axes"][1]) > 0.05 else 0.0 # left stick vertical + twist.angular.y = -float(controller_state["dpad"][1]) # pitch + twist.angular.x = -float(controller_state["dpad"][0]) # roll + yaw_r = controller_state["axes"][5] # right trigger + yaw_l = controller_state["axes"][2] # left trigger + yaw_r = (yaw_r + 1) / 2 + yaw_l = (yaw_l + 1) / 2 * (-1) + twist.angular.z = (yaw_r + yaw_l) + + return twist + +def series_x_twist(controller_state): + twist = Twist() + twist.linear.x = controller_state["axes"][4] # right stick vertical + twist.linear.y = controller_state["axes"][3] # right stick horizontal + twist.linear.z = -controller_state["axes"][1] # left stick + twist.angular.y = float(controller_state["dpad"][1]) # pitch + twist.angular.x = -float(controller_state["dpad"][0]) # roll + yaw_r = controller_state["axes"][5] # right trigger + yaw_l = controller_state["axes"][2] # left trigger + yaw_r = (yaw_r + 1) / 2 + yaw_l = (yaw_l + 1) / 2 * (-1) + twist.angular.z = -(yaw_r + yaw_l) + return twist + +def main(Args=None): + rclpy.init(args=Args) + + translator = Translator() + + rclpy.spin(translator) + + translator.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/guppy_tests/package.xml b/src/guppy_tests/package.xml index 21e263f..fd283d1 100644 --- a/src/guppy_tests/package.xml +++ b/src/guppy_tests/package.xml @@ -1,7 +1,7 @@ - guppy_teleop + guppy_tests 0.0.0 Remote/human operation code package for the 2026 Guppy AUV. Palouse RoboSub diff --git a/src/guppy_tests/resource/guppy_teleop b/src/guppy_tests/resource/guppy_teleop new file mode 100644 index 0000000..e69de29 diff --git a/src/guppy_tests/setup.cfg b/src/guppy_tests/setup.cfg new file mode 100644 index 0000000..de0ec2b --- /dev/null +++ b/src/guppy_tests/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/guppy_tests +[install] +install_scripts=$base/lib/guppy_tests \ No newline at end of file diff --git a/src/guppy_tests/setup.py b/src/guppy_tests/setup.py new file mode 100644 index 0000000..e1c47cc --- /dev/null +++ b/src/guppy_tests/setup.py @@ -0,0 +1,75 @@ +import os, sys, subprocess +from glob import glob +from setuptools import find_packages, setup +from setuptools.command.build_py import build_py +from pathlib import Path + +package_name = 'guppy_teleop' + +# builds qt assets +class build_qt(build_py): + def run(self): + qrc_src = Path("guppy_teleop/frontend/assets.qrc") + asset_dest = Path(self.build_lib) / "guppy_teleop/frontend/rc_assets.py" + + print(f"Compiling Qt resources... {qrc_src} -> {asset_dest}") + + asset_dest.parent.mkdir(parents=True, exist_ok=True) + + result = subprocess.run(["pyside6-rcc", str(qrc_src), "-o", str(asset_dest)]) + + if (x := result.returncode) != 0: + sys.exit(x) + + super().run() + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + include_package_data=True, + package_data={ + 'guppy_teleop.frontend' : ['assets.qrc'], + 'guppy_teleop.frontend.ui' : ['Main.qml', 'qmldir', 'Theme.qml'], + 'guppy_teleop.frontend.ui.canvas' : ['*.qml'], + 'guppy_teleop.frontend.ui.sidebar' : ['*.qml'], + 'guppy_teleop.frontend.ui.widgets' : ['*.qml'], + 'guppy_teleop.frontend.ui.widgets.state' : ['*.qml'], + 'guppy_teleop.frontend.ui.widgets.parameter' : ['*.qml'], + 'guppy_teleop.frontend.ui.widgets.input' : ['*.qml'], + 'guppy_teleop.frontend.ui.toastify' : ['*.qml'], + 'guppy_teleop.frontend.workspaces' : ['*.json'], + 'guppy_teleop.frontend.icons' : ['*.svg'], + 'guppy_teleop.frontend.icons.toastify' : ['*.svg'], + 'guppy_teleop.frontend.workspaces' : ['*.json'] + }, + data_files=[ + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*.[pxy][yma]*'))), + ], + install_requires=['setuptools', 'evdev', 'PySide6', 'pygame'], + zip_safe=True, + maintainer='robosub', + maintainer_email='robosub@todo.todo', + description='TODO: Package description', + license='Apache-2.0', + extras_require={ + 'test': [ + 'pytest', + ], + }, + cmdclass={'build_py': build_qt}, + entry_points={ + 'console_scripts': [ + 'input = guppy_teleop.input_handler:main', + 'controller = guppy_teleop.input_handler:controller', + 'keyboard = guppy_teleop.input_handler:keyboard', + 'terminal = guppy_teleop.frontend.terminal:main', + 'ksed = guppy_teleop.ksed_controller:main', + + 'trans = guppy_teleop.translator:main', + 'raw = guppy_teleop.raw_controller:main', + ], + }, +) diff --git a/src/guppy_tests/test/test_copyright.py b/src/guppy_tests/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/src/guppy_tests/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/guppy_tests/test/test_flake8.py b/src/guppy_tests/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/src/guppy_tests/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/src/guppy_tests/test/test_pep257.py b/src/guppy_tests/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/src/guppy_tests/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' From ab536b32b68a5001ebece5f51cbc895ba15d991b Mon Sep 17 00:00:00 2001 From: Ryden Handsaker Date: Sun, 26 Apr 2026 05:43:27 +0000 Subject: [PATCH 11/18] fix: publich --- src/guppy_nav/src/navigate_action_server.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/guppy_nav/src/navigate_action_server.cpp b/src/guppy_nav/src/navigate_action_server.cpp index 1361255..756658b 100644 --- a/src/guppy_nav/src/navigate_action_server.cpp +++ b/src/guppy_nav/src/navigate_action_server.cpp @@ -212,7 +212,7 @@ class NavigateActionServer : public rclcpp::Node { goalHandle->canceled(result); geometry_msgs::msg::Twist zeroTwist(); // publish zero twist - _commandVelocityPublisher->publich(zeroTwist); + _commandVelocityPublisher->publish(zeroTwist); return; } @@ -240,7 +240,7 @@ class NavigateActionServer : public rclcpp::Node { } geometry_msgs::msg::Twist zeroTwist(); // publish zero twist - _commandVelocityPublisher->publich(zeroTwist); + _commandVelocityPublisher->publish(zeroTwist); if (rclcpp::ok()) { result->pose = getKinematicState().pose; From c99b5ad703bdf9eea96c399acdfa41a2f4d00f0b Mon Sep 17 00:00:00 2001 From: Ryden Handsaker Date: Sun, 26 Apr 2026 05:45:39 +0000 Subject: [PATCH 12/18] fix: more typos --- src/guppy_nav/src/navigate_action_server.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/guppy_nav/src/navigate_action_server.cpp b/src/guppy_nav/src/navigate_action_server.cpp index 756658b..56b8084 100644 --- a/src/guppy_nav/src/navigate_action_server.cpp +++ b/src/guppy_nav/src/navigate_action_server.cpp @@ -227,7 +227,7 @@ class NavigateActionServer : public rclcpp::Node { Eigen::Vector3d currentPosition; currentPosition << state.pose.position.x, state.pose.position.y, state.pose.position.z; - auto relativeCurrentPosition = currentPosition - initalPosition; + auto relativeCurrentPosition = currentPosition - initialPosition; feedback->progress.position.x = currentPosition.x(), feedback->progress.position.x = currentPosition.y(), feedback->progress.position.x = currentPosition.z(); @@ -239,7 +239,7 @@ class NavigateActionServer : public rclcpp::Node { rate.sleep(); } - geometry_msgs::msg::Twist zeroTwist(); // publish zero twist + geometry_msgs::msg::Twist zeroTwist; // publish zero twist _commandVelocityPublisher->publish(zeroTwist); if (rclcpp::ok()) { From 959de25fed110f3b1595c42a69e30580864b3f3a Mon Sep 17 00:00:00 2001 From: Ryden Handsaker Date: Sun, 26 Apr 2026 05:46:41 +0000 Subject: [PATCH 13/18] fix: tpyo AGAIN AGAIN --- src/guppy_nav/src/navigate_action_server.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/guppy_nav/src/navigate_action_server.cpp b/src/guppy_nav/src/navigate_action_server.cpp index 56b8084..cbe2e38 100644 --- a/src/guppy_nav/src/navigate_action_server.cpp +++ b/src/guppy_nav/src/navigate_action_server.cpp @@ -211,7 +211,7 @@ class NavigateActionServer : public rclcpp::Node { result->target_reached = false; goalHandle->canceled(result); - geometry_msgs::msg::Twist zeroTwist(); // publish zero twist + geometry_msgs::msg::Twist zeroTwist; // publish zero twist _commandVelocityPublisher->publish(zeroTwist); return; From a25ffb4615d2dd43acc95a45b57c280a45930a37 Mon Sep 17 00:00:00 2001 From: "Cole (via LattePanda)" Date: Sun, 26 Apr 2026 00:28:51 -0700 Subject: [PATCH 14/18] fix: std::thread --- src/guppy_nav/src/navigate_action_server.cpp | 45 ++++++++++++------- .../guppy_vision/apriltag_detector.py | 6 +-- 2 files changed, 33 insertions(+), 18 deletions(-) diff --git a/src/guppy_nav/src/navigate_action_server.cpp b/src/guppy_nav/src/navigate_action_server.cpp index cbe2e38..b44d3c8 100644 --- a/src/guppy_nav/src/navigate_action_server.cpp +++ b/src/guppy_nav/src/navigate_action_server.cpp @@ -1,5 +1,6 @@ #include #include +#include #include #include @@ -29,6 +30,9 @@ class NavigateActionServer : public rclcpp::Node { public: explicit NavigateActionServer(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) : Node("navigate_action_server", options) { + // action_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + // sub_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto handleGoal = [this](const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal) { RCLCPP_INFO(this->get_logger(), "goal request with pose and relative set to %d", goal->relative); //goal->pose @@ -46,7 +50,9 @@ class NavigateActionServer : public rclcpp::Node { }; auto handleAccepted = [this](const std::shared_ptr> goalHandle) { - execute(goalHandle); + std::thread{[this, goalHandle]() { + execute(goalHandle); + }}.detach(); }; _actionServer = rclcpp_action::create_server( @@ -54,26 +60,35 @@ class NavigateActionServer : public rclcpp::Node { "/navigate", handleGoal, handleCancel, - handleAccepted + handleAccepted, + rcl_action_server_get_default_options()//, + // action_cb_group_ ); + rclcpp::SubscriptionOptions odom_options; + odom_options.callback_group = sub_cb_group_; + _commandVelocityPublisher = create_publisher("cmd_vel/nav", 10); - _odomSubscription = create_subscription("/odometry/filtered",10,std::bind(&NavigateActionServer::odometryCallback, this, std::placeholders::_1)); - - _xPid.set_gains(1.5, 0.0, 0.1, std::numeric_limits::infinity(), -std::numeric_limits::infinity(), antiWindupStrategy); - _yPid.set_gains(1.5, 0.0, 0.1, std::numeric_limits::infinity(), -std::numeric_limits::infinity(), antiWindupStrategy); - _zPid.set_gains(1.5, 0.0, 0.1, std::numeric_limits::infinity(), -std::numeric_limits::infinity(), antiWindupStrategy); - _yawPid.set_gains(1.5, 0.0, 0.1, std::numeric_limits::infinity(), -std::numeric_limits::infinity(), antiWindupStrategy); - _pitchPid.set_gains(1.5, 0.0, 0.1, std::numeric_limits::infinity(), -std::numeric_limits::infinity(), antiWindupStrategy); - _rollPid.set_gains(1.5, 0.0, 0.1, std::numeric_limits::infinity(), -std::numeric_limits::infinity(), antiWindupStrategy); + _odomSubscription = create_subscription("/odometry/filtered",10,std::bind(&NavigateActionServer::odometryCallback, this, std::placeholders::_1), odom_options); + + _xPid.set_gains(0.0, 0.0, 0.0, std::numeric_limits::infinity(), -std::numeric_limits::infinity(), antiWindupStrategy); + _yPid.set_gains(0.0, 0.0, 0.0, std::numeric_limits::infinity(), -std::numeric_limits::infinity(), antiWindupStrategy); + _zPid.set_gains(0.0, 0.0, 0.0, std::numeric_limits::infinity(), -std::numeric_limits::infinity(), antiWindupStrategy); + _yawPid.set_gains(0.0, 0.0, 0.0, std::numeric_limits::infinity(), -std::numeric_limits::infinity(), antiWindupStrategy); + _pitchPid.set_gains(0.0, 0.0, 0.0, std::numeric_limits::infinity(), -std::numeric_limits::infinity(), antiWindupStrategy); + _rollPid.set_gains(0.0, 0.0, 0.0, std::numeric_limits::infinity(), -std::numeric_limits::infinity(), antiWindupStrategy); } void odometryCallback(nav_msgs::msg::Odometry::SharedPtr msg) { - std::lock_guard lock(_kinematiStateMutex); + // std::lock_guard lock(_kinematiStateMutex); + RCLCPP_INFO(this->get_logger(), "new thingy"); _kinematicState.pose = msg->pose.pose; _kinematicState.twist = msg->twist.twist; } private: + rclcpp::CallbackGroup::SharedPtr action_cb_group_; + rclcpp::CallbackGroup::SharedPtr sub_cb_group_; + rclcpp_action::Server::SharedPtr _actionServer; rclcpp::Publisher::SharedPtr _commandVelocityPublisher; @@ -85,7 +100,7 @@ class NavigateActionServer : public rclcpp::Node { }; KinematicState _kinematicState; - std::mutex _kinematiStateMutex; + // std::mutex _kinematiStateMutex; control_toolbox::Pid _xPid, _yPid, _zPid, _yawPid, _pitchPid, _rollPid; @@ -132,7 +147,7 @@ class NavigateActionServer : public rclcpp::Node { }(); KinematicState getKinematicState() { - std::lock_guard lock(_kinematiStateMutex); + // std::lock_guard lock(_kinematiStateMutex); return _kinematicState; } @@ -148,7 +163,7 @@ class NavigateActionServer : public rclcpp::Node { currentOrientation.normalize(); const auto localError = currentOrientation.inverse() * worldError; - const auto targetVelocityLocal = currentOrientation.inverse() * targetVelocityWorld; + const auto targetVelocityLocal = currentOrientation.inverse() * targetVelocityWorld * 10.0; const auto angularErrorLocal = orientationSolver.error(elapsed, currentOrientation); geometry_msgs::msg::Twist commandVelocity; @@ -229,7 +244,7 @@ class NavigateActionServer : public rclcpp::Node { currentPosition << state.pose.position.x, state.pose.position.y, state.pose.position.z; auto relativeCurrentPosition = currentPosition - initialPosition; - feedback->progress.position.x = currentPosition.x(), feedback->progress.position.x = currentPosition.y(), feedback->progress.position.x = currentPosition.z(); + feedback->progress.position.x = currentPosition.x(), feedback->progress.position.y = currentPosition.y(), feedback->progress.position.z = currentPosition.z(); _commandVelocityPublisher->publish(commandVelocity); goalHandle->publish_feedback(feedback); diff --git a/src/guppy_vision/guppy_vision/apriltag_detector.py b/src/guppy_vision/guppy_vision/apriltag_detector.py index 64ffee2..380416d 100644 --- a/src/guppy_vision/guppy_vision/apriltag_detector.py +++ b/src/guppy_vision/guppy_vision/apriltag_detector.py @@ -25,9 +25,9 @@ def __init__(self): def callback(self, image): img_o = bridge.imgmsg_to_cv2(image) - img_o[:,:,0] *= 0 - img_o[:,:,1] *= 0 - img_o[:,:,2] *= 3 + img_o[:,:,0] *= 1 + img_o[:,:,1] *= 1 + img_o[:,:,2] *= 1 # img_o = cv2.merge([img_o[:,:,0]*0.01, img_o[:,:,1]*0.01, img_o[:,:,2]*1]).astype(np.uint8) img = cv2.cvtColor(img_o, cv2.COLOR_BGR2GRAY) results = detector.detect(img) From 030b3f8843d8ab9ba539da0639e9239cc2261578 Mon Sep 17 00:00:00 2001 From: Ryden Handsaker Date: Sun, 26 Apr 2026 09:06:37 +0000 Subject: [PATCH 15/18] fix: changed navigate action function --- src/guppy_msgs/action/Navigate.action | 4 +- src/guppy_nav/src/navigate_action_server.cpp | 86 ++++++++++---------- 2 files changed, 47 insertions(+), 43 deletions(-) diff --git a/src/guppy_msgs/action/Navigate.action b/src/guppy_msgs/action/Navigate.action index 40ce756..3ef0d21 100644 --- a/src/guppy_msgs/action/Navigate.action +++ b/src/guppy_msgs/action/Navigate.action @@ -1,6 +1,6 @@ geometry_msgs/Pose pose -bool relative true #use global pose or relative pose -float64 time +bool local true +float64 duration --- geometry_msgs/Pose pose #final relative position bool target_reached #if guppy made it to the target pose diff --git a/src/guppy_nav/src/navigate_action_server.cpp b/src/guppy_nav/src/navigate_action_server.cpp index b44d3c8..57fe3fd 100644 --- a/src/guppy_nav/src/navigate_action_server.cpp +++ b/src/guppy_nav/src/navigate_action_server.cpp @@ -27,6 +27,8 @@ #define TARGET_RATE_MS 10 +#define PI 3.1415 + class NavigateActionServer : public rclcpp::Node { public: explicit NavigateActionServer(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) : Node("navigate_action_server", options) { @@ -122,13 +124,15 @@ class NavigateActionServer : public rclcpp::Node { }; struct OrientationSolver { - Eigen::Quaterniond initialOrientation, finalOrientation; + Eigen::Quaterniond initialOrientation, relativeFinalOrientation; // relative to initial, i.e. delta orientation double totalTime; - explicit OrientationSolver(const Eigen::Quaterniond& initialOrientation, const Eigen::Quaterniond& finalOrientation, double totalTime) : - initialOrientation(initialOrientation), finalOrientation(finalOrientation), totalTime(totalTime) {} + explicit OrientationSolver(const Eigen::Quaterniond& initialOrientation /*world frame*/, const Eigen::Quaterniond& finalOrientation /*world frame*/, double totalTime) : + initialOrientation(initialOrientation), finalOrientation(finalOrientation), totalTime(totalTime) { + if (initialOrientation.dot(finalOrientation) < 0.0) finalOrientation.coeffs() *= -1.0; + } - Eigen::Vector3d error(double elapsed, const Eigen::Quaterniond& currentOrientation) const { + Eigen::Vector3d error(double elapsed, const Eigen::Quaterniond& currentOrientation /*world frame*/) const { double alpha = std::clamp(elapsed / totalTime, 0.0, 1.0); Eigen::Quaterniond targetOrientation = initialOrientation.slerp(alpha, finalOrientation); @@ -136,7 +140,10 @@ class NavigateActionServer : public rclcpp::Node { Eigen::Quaterniond orientationError = currentOrientation.inverse() * targetOrientation; Eigen::AngleAxisd angleAxisError(orientationError); - return angleAxisError.axis() * angleAxisError.angle(); + double angle = angleAxisError.angle(); + if (angle > PI) angle -= 2.0 * PI; + + return angleAxisError.axis() * angle; } }; @@ -147,23 +154,20 @@ class NavigateActionServer : public rclcpp::Node { }(); KinematicState getKinematicState() { - // std::lock_guard lock(_kinematiStateMutex); return _kinematicState; } geometry_msgs::msg::Twist computeCommandVelocity(const Trajectory3& trajectory, const OrientationSolver& orientationSolver, double elapsed, double delta, const KinematicState& state, const Eigen::Vector3d& initialPosition) { - const auto targetPositionWorld = trajectory.position(elapsed); // world - const auto targetVelocityWorld = trajectory.velocity(elapsed); // world - - const Eigen::Vector3d currentPosition(state.pose.position.x, state.pose.position.y, state.pose.position.z); // world! - const auto relativePosition = currentPosition - initialPosition; // still world - const auto worldError = targetPositionWorld - relativePosition; + const auto relativeTargetPosition = trajectory.position(elapsed); // relative to guppy starting position, world frame + const auto targetVelocity = trajectory.velocity(elapsed); // world frame - Eigen::Quaterniond currentOrientation(state.pose.orientation.w, state.pose.orientation.x, state.pose.orientation.y, state.pose.orientation.z); // world! - currentOrientation.normalize(); + const Eigen::Vector3d currentPosition(state.pose.position.x, state.pose.position.y, state.pose.position.z); // world + const auto relativePosition = currentPosition - initialPosition; // world, relative to the initial position, as if guppy's initial position were (0, 0, 0) + const auto error = relativeTargetPosition - relativePosition; // end - start; target - current; gives vector from current to target, i.e. error - const auto localError = currentOrientation.inverse() * worldError; - const auto targetVelocityLocal = currentOrientation.inverse() * targetVelocityWorld * 10.0; + Eigen::Quaterniond currentOrientation(state.pose.orientation.w, state.pose.orientation.x, state.pose.orientation.y, state.pose.orientation.z); // world frame + const auto localTargetVelocity = currentOrientation.inverse() * targetVelocity; + const auto localError = currentOrientation.inverse() * error; const auto angularErrorLocal = orientationSolver.error(elapsed, currentOrientation); geometry_msgs::msg::Twist commandVelocity; @@ -172,9 +176,9 @@ class NavigateActionServer : public rclcpp::Node { commandVelocity.angular.y = _pitchPid.compute_command(angularErrorLocal.y(), rclcpp::Duration::from_seconds(delta)); commandVelocity.angular.x = _rollPid.compute_command(angularErrorLocal.x(), rclcpp::Duration::from_seconds(delta)); - commandVelocity.linear.x = targetVelocityLocal.x() + _xPid.compute_command(localError.x(), rclcpp::Duration::from_seconds(delta)); - commandVelocity.linear.y = targetVelocityLocal.y() + _yPid.compute_command(localError.y(), rclcpp::Duration::from_seconds(delta)); - commandVelocity.linear.z = targetVelocityLocal.z() + _zPid.compute_command(localError.z(), rclcpp::Duration::from_seconds(delta)); + commandVelocity.linear.x = localTargetVelocity.x() + _xPid.compute_command(localError.x(), rclcpp::Duration::from_seconds(delta)); + commandVelocity.linear.y = localTargetVelocity.y() + _yPid.compute_command(localError.y(), rclcpp::Duration::from_seconds(delta)); + commandVelocity.linear.z = localTargetVelocity.z() + _zPid.compute_command(localError.z(), rclcpp::Duration::from_seconds(delta)); return commandVelocity; }; @@ -182,28 +186,28 @@ class NavigateActionServer : public rclcpp::Node { void execute(const std::shared_ptr> goalHandle) { const auto& goal = goalHandle->get_goal(); - const auto initialState = getKinematicState(); - - Eigen::Vector3d initialPosition(initialState.pose.position.x, initialState.pose.position.y, initialState.pose.position.z); //world - Eigen::Quaterniond initialOrientation(initialState.pose.orientation.w, initialState.pose.orientation.x, initialState.pose.orientation.y, initialState.pose.orientation.z); //world - initialOrientation.normalize(); - - Eigen::Vector3d relativeFinalPosition; // still world, just makes origin (0, 0, 0) (may have to be converted if user submitted local coordinates) - - const auto& finalPose = goal->pose; - Eigen::Quaterniond finalOrientation(finalPose.orientation.w, finalPose.orientation.x, finalPose.orientation.y, finalPose.orientation.z); // will end up in world - finalOrientation.normalize(); - - if (goal->relative) { - relativeFinalPosition << finalPose.position.x, finalPose.position.y, finalPose.position.z; - finalOrientation = initialOrientation * finalOrientation; - } else relativeFinalPosition << finalPose.position.x - initialPosition.x(), finalPose.position.y - initialPosition.y(), finalPose.position.z - initialPosition.z(); - - Eigen::Vector3d startVelocity(initialState.twist.linear.x, initialState.twist.linear.y, initialState.twist.linear.z); // world + const auto initialState = getKinematicState(); // world frame + const Eigen::Vector3d initialPosition(initialState.pose.position.x, initialState.pose.position.y, initialState.pose.position.z); + const Eigen::Quaterniond initialOrientation(initialState.pose.orientation.w, initialState.pose.orientation.x, initialState.pose.orientation.y, initialState.pose.orientation.z); + + const Eigen::Vector3d goalPosition(goal->pose.position.x, goal->pose.position.y, goal->pose.position.z); // if local == true, relative to guppy; if local == false it is a world position (not relative to guppy) + const Eigen::Quaterniond goalOrientation(goal->pose.orientation.w, goal->pose.orientation.x, goal->pose.orientation.y, goal->pose.orientation.z); + + Eigen::Vector3d finalPosition; // world + Eigen::Quaterniond finalOrientation; // world + if (goal->local) { + finalPosition = initialPosition + initialOrientation.inverse() * goalPosition; // get final position in world based on guppy's position/orientation + finalOrientation = initialOrientation * goalOrientation; + } else { + finalPosition = goalPosition; + finalOrientation = goalOrientation; + } - Trajectory3 trajectory(startVelocity, Eigen::Vector3d::Zero(), ATTACK, DECAY, TOTAL_TIME, relativeFinalPosition); + const Eigen::Vector3d initialVelocity(initialState.twist.linear.x, initialState.twist.linear.y, initialState.twist.linear.z); // world frame + const auto relativeFinalPosition = finalPosition - initialPosition; // vector between guppy's initial position (world) and target final position (world), position as if guppy were (0, 0, 0) - OrientationSolver orientationSolver(initialOrientation, finalOrientation, TOTAL_TIME); + Trajectory3 trajectory(initialVelocity, Eigen::Vector3d::Zero(), ATTACK, DECAY, goal->duration, relativeFinalPosition); // world frame velocities (will output target velocities in world frame) + OrientationSolver orientationSolver(initialOrientation, finalOrientation, goal->duration); // will output target angular velocities rclcpp::Rate rate(1000.0 / TARGET_RATE_MS); @@ -237,8 +241,8 @@ class NavigateActionServer : public rclcpp::Node { double delta = std::clamp((now - last).seconds(), 1e-4, 0.05); last = now; - auto state = getKinematicState(); - auto commandVelocity = computeCommandVelocity(trajectory, orientationSolver, elapsed, delta, state, initialPosition); + auto state = getKinematicState(); // world + auto commandVelocity = computeCommandVelocity(trajectory, orientationSolver, elapsed, delta, state /*world*/, initialPosition /*world*/); Eigen::Vector3d currentPosition; currentPosition << state.pose.position.x, state.pose.position.y, state.pose.position.z; From 58bba67f4ff0e9b29192a4a951c955ce3fe939aa Mon Sep 17 00:00:00 2001 From: Ryden Handsaker Date: Sun, 26 Apr 2026 09:34:08 +0000 Subject: [PATCH 16/18] fix: fixed error --- src/guppy_nav/src/navigate_action_server.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/guppy_nav/src/navigate_action_server.cpp b/src/guppy_nav/src/navigate_action_server.cpp index 57fe3fd..5602f51 100644 --- a/src/guppy_nav/src/navigate_action_server.cpp +++ b/src/guppy_nav/src/navigate_action_server.cpp @@ -36,7 +36,7 @@ class NavigateActionServer : public rclcpp::Node { // sub_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); auto handleGoal = [this](const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal) { - RCLCPP_INFO(this->get_logger(), "goal request with pose and relative set to %d", goal->relative); //goal->pose + RCLCPP_INFO(this->get_logger(), "goal request with pose and relative set to %d", goal->local); //goal->pose (void)uuid; // silence unused parameter warning @@ -124,7 +124,7 @@ class NavigateActionServer : public rclcpp::Node { }; struct OrientationSolver { - Eigen::Quaterniond initialOrientation, relativeFinalOrientation; // relative to initial, i.e. delta orientation + Eigen::Quaterniond initialOrientation, FinalOrientation; // relative to initial, i.e. delta orientation double totalTime; explicit OrientationSolver(const Eigen::Quaterniond& initialOrientation /*world frame*/, const Eigen::Quaterniond& finalOrientation /*world frame*/, double totalTime) : From efaefaaf44bb2edbda1a90aebc5821ee391148a8 Mon Sep 17 00:00:00 2001 From: Ryden Handsaker Date: Sun, 26 Apr 2026 09:35:28 +0000 Subject: [PATCH 17/18] fix: F --- src/guppy_nav/src/navigate_action_server.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/guppy_nav/src/navigate_action_server.cpp b/src/guppy_nav/src/navigate_action_server.cpp index 5602f51..32e854f 100644 --- a/src/guppy_nav/src/navigate_action_server.cpp +++ b/src/guppy_nav/src/navigate_action_server.cpp @@ -124,7 +124,7 @@ class NavigateActionServer : public rclcpp::Node { }; struct OrientationSolver { - Eigen::Quaterniond initialOrientation, FinalOrientation; // relative to initial, i.e. delta orientation + Eigen::Quaterniond initialOrientation, finalOrientation; // relative to initial, i.e. delta orientation double totalTime; explicit OrientationSolver(const Eigen::Quaterniond& initialOrientation /*world frame*/, const Eigen::Quaterniond& finalOrientation /*world frame*/, double totalTime) : From 4b4d76dad1511097b355e138dc33a32db4f4670e Mon Sep 17 00:00:00 2001 From: "Cole (via LattePanda)" Date: Sun, 26 Apr 2026 03:26:31 -0700 Subject: [PATCH 18/18] feat: somewhat working nav --- .../config/controller_params.yaml | 8 +++---- src/guppy_nav/src/navigate_action_server.cpp | 23 +++++++++++-------- util/T.sh | 6 +++++ 3 files changed, 23 insertions(+), 14 deletions(-) create mode 100644 util/T.sh diff --git a/src/guppy_control/config/controller_params.yaml b/src/guppy_control/config/controller_params.yaml index 38809a2..e62c2e0 100644 --- a/src/guppy_control/config/controller_params.yaml +++ b/src/guppy_control/config/controller_params.yaml @@ -25,14 +25,14 @@ control_chassis: pid_gains_vel_linear: [ 1000.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: [ 70.0, 0.0, 40.0 ] + pid_gains_pose_linear: [ 90.0, 0.0, 30.0 ] + pid_gains_pose_angular: [ 50.0, 0.0, 50.0 ] # pid_gains_vel_linear: [ 0.0, 0.0, 0.0 ] # pid_gains_vel_angular: [ 0.0, 0.0, 0.0 ] - pid_gains_pose_linear: [ 0.0, 0.0, 0.0 ] - pid_gains_pose_angular: [ 0.0, 0.0, 0.0 ] + # pid_gains_pose_linear: [ 0.0, 0.0, 0.0 ] + # pid_gains_pose_angular: [ 0.0, 0.0, 0.0 ] pose_lock_deadband: [ 0.1, 0.1, 0.1, 0.1, 0.1, 0.1 ] diff --git a/src/guppy_nav/src/navigate_action_server.cpp b/src/guppy_nav/src/navigate_action_server.cpp index 32e854f..894ecb3 100644 --- a/src/guppy_nav/src/navigate_action_server.cpp +++ b/src/guppy_nav/src/navigate_action_server.cpp @@ -76,9 +76,9 @@ class NavigateActionServer : public rclcpp::Node { _xPid.set_gains(0.0, 0.0, 0.0, std::numeric_limits::infinity(), -std::numeric_limits::infinity(), antiWindupStrategy); _yPid.set_gains(0.0, 0.0, 0.0, std::numeric_limits::infinity(), -std::numeric_limits::infinity(), antiWindupStrategy); _zPid.set_gains(0.0, 0.0, 0.0, std::numeric_limits::infinity(), -std::numeric_limits::infinity(), antiWindupStrategy); - _yawPid.set_gains(0.0, 0.0, 0.0, std::numeric_limits::infinity(), -std::numeric_limits::infinity(), antiWindupStrategy); - _pitchPid.set_gains(0.0, 0.0, 0.0, std::numeric_limits::infinity(), -std::numeric_limits::infinity(), antiWindupStrategy); - _rollPid.set_gains(0.0, 0.0, 0.0, std::numeric_limits::infinity(), -std::numeric_limits::infinity(), antiWindupStrategy); + _yawPid.set_gains(0.5, 0.0, 0.0, std::numeric_limits::infinity(), -std::numeric_limits::infinity(), antiWindupStrategy); + _pitchPid.set_gains(0.5, 0.0, 0.0, std::numeric_limits::infinity(), -std::numeric_limits::infinity(), antiWindupStrategy); + _rollPid.set_gains(0.5, 0.0, 0.0, std::numeric_limits::infinity(), -std::numeric_limits::infinity(), antiWindupStrategy); } void odometryCallback(nav_msgs::msg::Odometry::SharedPtr msg) { @@ -127,9 +127,12 @@ class NavigateActionServer : public rclcpp::Node { Eigen::Quaterniond initialOrientation, finalOrientation; // relative to initial, i.e. delta orientation double totalTime; - explicit OrientationSolver(const Eigen::Quaterniond& initialOrientation /*world frame*/, const Eigen::Quaterniond& finalOrientation /*world frame*/, double totalTime) : + explicit OrientationSolver(const Eigen::Quaterniond& initialOrientation /*world frame*/, Eigen::Quaterniond& finalOrientation /*world frame*/, double totalTime) : initialOrientation(initialOrientation), finalOrientation(finalOrientation), totalTime(totalTime) { - if (initialOrientation.dot(finalOrientation) < 0.0) finalOrientation.coeffs() *= -1.0; + if (initialOrientation.dot(finalOrientation) < 0.0) { + Eigen::Quaterniond second(-finalOrientation.w(), -finalOrientation.x(), -finalOrientation.y(), -finalOrientation.z()); + finalOrientation = second; + } } Eigen::Vector3d error(double elapsed, const Eigen::Quaterniond& currentOrientation /*world frame*/) const { @@ -138,12 +141,12 @@ class NavigateActionServer : public rclcpp::Node { Eigen::Quaterniond targetOrientation = initialOrientation.slerp(alpha, finalOrientation); Eigen::Quaterniond orientationError = currentOrientation.inverse() * targetOrientation; - Eigen::AngleAxisd angleAxisError(orientationError); - double angle = angleAxisError.angle(); - if (angle > PI) angle -= 2.0 * PI; + // Eigen::AngleAxisd angleAxisError(orientationError); + // double angle = angleAxisError.angle(); + // if (angle > PI) angle -= 2.0 * PI; - return angleAxisError.axis() * angle; + return orientationError.vec(); } }; @@ -253,7 +256,7 @@ class NavigateActionServer : public rclcpp::Node { _commandVelocityPublisher->publish(commandVelocity); goalHandle->publish_feedback(feedback); - if (elapsed >= TOTAL_TIME) break; + if (elapsed >= goal->duration) break; rate.sleep(); } diff --git a/util/T.sh b/util/T.sh new file mode 100644 index 0000000..4cb46db --- /dev/null +++ b/util/T.sh @@ -0,0 +1,6 @@ +source install/setup.bash +ros2 action send_goal --feedback /navigate guppy_msgs/action/Navigate "{pose: {position: {x: 1, y: 0.0, z: 0.0}, orientation: {w: 1.0, x: 0.0, y: 0.0, z: 0.0}}, local: false, duration: 5}" +ros2 action send_goal --feedback /navigate guppy_msgs/action/Navigate "{pose: {position: {x: 1, y: 0.5, z: 0.0}, orientation: {w: 1.0, x: 0.0, y: 0.0, z: 0.0}}, local: false, duration: 5}" +ros2 action send_goal --feedback /navigate guppy_msgs/action/Navigate "{pose: {position: {x: 1, y: -0.5, z: 0.0}, orientation: {w: 1.0, x: 0.0, y: 0.0, z: 0.0}}, local: false, duration: 5}" +ros2 action send_goal --feedback /navigate guppy_msgs/action/Navigate "{pose: {position: {x: 1, y: 0.0, z: 0.0}, orientation: {w: 1.0, x: 0.0, y: 0.0, z: 0.0}}, local: false, duration: 5}" +ros2 action send_goal --feedback /navigate guppy_msgs/action/Navigate "{pose: {position: {x: 0, y: 0.0, z: 0.0}, orientation: {w: 1.0, x: 0.0, y: 0.0, z: 0.0}}, local: false, duration: 5}"