Nav!

src/guppy/launch/hw.xml

@@ -11,4 +11,5 @@
     <include file="$(find-pkg-share guppy_description)/launch/core.xml" />
     <include file="$(find-pkg-share guppy_state)/launch/core.xml" />
     <include file="$(find-pkg-share guppy_vision)/launch/core.xml" />
+    <include file="$(find-pkg-share guppy_nav)/launch/core.xml" />
 </launch>

src/guppy/launch/sim.xml

@@ -10,4 +10,6 @@
     <include file="$(find-pkg-share guppy_localization)/launch/sim.xml" />
     <include file="$(find-pkg-share guppy_description)/launch/sim.xml" />
     <include file="$(find-pkg-share guppy_state)/launch/sim.xml" />
+    <include file="$(find-pkg-share guppy_vision)/launch/sim.xml" />
+    <include file="$(find-pkg-share guppy_nav)/launch/sim.xml" />
 </launch>

src/guppy/package.xml

@@ -15,7 +15,7 @@
   <exec_depend>guppy_localization</exec_depend>
   <exec_depend>guppy_moveit</exec_depend>
   <exec_depend>guppy_msgs</exec_depend>
-  <exec_depend>guppy_nav2</exec_depend>
+  <exec_depend>guppy_nav</exec_depend>
   <exec_depend>guppy_safety</exec_depend>
   <exec_depend>guppy_state</exec_depend>
   <exec_depend>guppy_tasks</exec_depend>

src/guppy_control/config/controller_params.yaml

@@ -26,7 +26,7 @@ control_chassis:
     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_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 ]

src/guppy_msgs/CMakeLists.txt

@@ -11,18 +11,25 @@ find_package(ament_cmake REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 find_package(builtin_interfaces REQUIRED)
 find_package(vision_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/ChangeState.srv"
-  "srv/SendCan.srv"
+  "msg/State.msg"
 
   "msg/CanFrame.msg"
-  "msg/State.msg"
+  "srv/SendCan.srv"
+
   "msg/Toast.msg"
+
   "msg/CornerDetection.msg"
   "msg/CornerDetectionList.msg"
   "msg/TransformList.msg"
+
+  "action/Navigate.action"
 
+  DEPENDENCIES builtin_interfaces
+  DEPENDENCIES geometry_msgs
   DEPENDENCIES builtin_interfaces vision_msgs
 )
 

src/guppy_msgs/action/Navigate.action

@@ -0,0 +1,8 @@
+geometry_msgs/Pose pose
+bool local true
+float64 duration
+---
+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

src/guppy_msgs/package.xml

@@ -7,6 +7,7 @@
   <maintainer email="colewilson.main@gmail.com">Palouse RoboSub</maintainer>
   <license>MIT</license>
 
+  <depend>geometry_msgs</depend>
   <depend>builtin_interfaces</depend>
   <depend>vision_msgs</depend>
   <buildtool_depend>rosidl_default_generators</buildtool_depend>

src/guppy_nav/CMakeLists.txt

@@ -0,0 +1,53 @@
+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(guppy_msgs REQUIRED)
+
+find_package(nav_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+
+find_package(eigen3_cmake_module REQUIRED)
+find_package(Eigen3)
+
+find_package(control_toolbox REQUIRED)
+
+add_library(action_server SHARED src/navigate_action_server.cpp)
+
+#target_include_directories(action_server PRIVATE
+#  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+#  $<INSTALL_INTERFACE:include>
+#)
+
+include_directories(include)
+
+#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
+  action_server
+
+  LIBRARY DESTINATION lib
+  ARCHIVE DESTINATION lib
+  RUNTIME DESTINATION bin
+  #DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY
+  launch
+
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+ament_package()

src/guppy_nav/include/guppy_nav/trajectory.hpp

@@ -0,0 +1,51 @@
+#pragma once
+
+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;
+    }
+};

src/guppy_nav/launch/core.xml

@@ -0,0 +1,4 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+    <node pkg="guppy_nav" exec="navigate_action_server"/>
+</launch>

src/guppy_nav/launch/sim.xml

@@ -0,0 +1,4 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+    <include file="$(find-pkg-share guppy_nav)/launch/core.xml" />
+</launch>

src/guppy_nav2/package.xml → src/guppy_nav/package.xml

@@ -1,12 +1,19 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>guppy_nav2</name>
+  <name>guppy_nav</name>
   <version>0.0.0</version>
-  <description>Nav2 configuration files and behavior trees for the 2026 Guppy AUV. This includes paths, and task ordering/strategy.</description>
+  <description>Navigation configuration files and behavior trees for the 2026 Guppy AUV. This includes paths, and task ordering/strategy.</description>
   <maintainer email="colewilson.main@gmail.com">Palouse RoboSub</maintainer>
   <license>MIT</license>
 
+  <exec_depend>guppy_msgs</exec_depend>
+
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>nav_msgs</exec_depend>
+
+  <exec_depend>control_toolbox</exec_depend>
+
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <test_depend>ament_lint_auto</test_depend>