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/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_control/config/controller_params.yaml b/src/guppy_control/config/controller_params.yaml index 814c929..e62c2e0 100644 --- a/src/guppy_control/config/controller_params.yaml +++ b/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 ] diff --git a/src/guppy_msgs/CMakeLists.txt b/src/guppy_msgs/CMakeLists.txt index dc30d8d..1fd5e69 100644 --- a/src/guppy_msgs/CMakeLists.txt +++ b/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 ) diff --git a/src/guppy_msgs/action/Navigate.action b/src/guppy_msgs/action/Navigate.action new file mode 100644 index 0000000..3ef0d21 --- /dev/null +++ b/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 diff --git a/src/guppy_msgs/package.xml b/src/guppy_msgs/package.xml index cc87643..a9c6d78 100644 --- a/src/guppy_msgs/package.xml +++ b/src/guppy_msgs/package.xml @@ -7,6 +7,7 @@ Palouse RoboSub MIT + geometry_msgs builtin_interfaces vision_msgs rosidl_default_generators diff --git a/src/guppy_nav/CMakeLists.txt b/src/guppy_nav/CMakeLists.txt new file mode 100644 index 0000000..9b5bbcf --- /dev/null +++ b/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 +# $ +# $ +#) + +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() 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..2bf084f --- /dev/null +++ b/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; + } +}; \ No newline at end of file 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_nav2/package.xml b/src/guppy_nav/package.xml similarity index 59% rename from src/guppy_nav2/package.xml rename to src/guppy_nav/package.xml index a2cb939..a33cb6c 100644 --- a/src/guppy_nav2/package.xml +++ b/src/guppy_nav/package.xml @@ -1,12 +1,19 @@ - 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 + guppy_msgs + + geometry_msgs + nav_msgs + + control_toolbox + ament_cmake ament_lint_auto 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..894ecb3 --- /dev/null +++ b/src/guppy_nav/src/navigate_action_server.cpp @@ -0,0 +1,293 @@ +#include +#include +#include + +#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 + +#define PI 3.1415 + +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->local); //goal->pose + + (void)uuid; // silence unused parameter warning + + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + auto handleCancel = [this](const std::shared_ptr> goalHandle) { + RCLCPP_INFO(this->get_logger(), "request to cancel goal"); + + (void)goalHandle; // silence unused parameter warning + + return rclcpp_action::CancelResponse::ACCEPT; + }; + + auto handleAccepted = [this](const std::shared_ptr> goalHandle) { + std::thread{[this, goalHandle]() { + execute(goalHandle); + }}.detach(); + }; + + _actionServer = rclcpp_action::create_server( + this, + "/navigate", + handleGoal, + handleCancel, + 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), 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.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) { + // 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; + rclcpp::Subscription::SharedPtr _odomSubscription; + + struct KinematicState { + geometry_msgs::msg::Pose pose; + geometry_msgs::msg::Twist twist; + }; + + KinematicState _kinematicState; + // std::mutex _kinematiStateMutex; + + control_toolbox::Pid _xPid, _yPid, _zPid, _yawPid, _pitchPid, _rollPid; + + struct Trajectory3 { + Trajectory x, y, 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())) {} + + Eigen::Vector3d velocity(double elapsed) const { + return Eigen::Vector3d(x.getTargetVelocity(elapsed), y.getTargetVelocity(elapsed), z.getTargetVelocity(elapsed)); + } + + Eigen::Vector3d position(double elapsed) const { + return Eigen::Vector3d(x.getTargetPosition(elapsed), y.getTargetPosition(elapsed), z.getTargetPosition(elapsed)); + } + }; + + struct OrientationSolver { + Eigen::Quaterniond initialOrientation, finalOrientation; // relative to initial, i.e. delta orientation + 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) { + 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 { + 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); + // double angle = angleAxisError.angle(); + // if (angle > PI) angle -= 2.0 * PI; + + return orientationError.vec(); + } + }; + + inline static const control_toolbox::AntiWindupStrategy antiWindupStrategy = []{ + control_toolbox::AntiWindupStrategy strategy; + strategy.set_type("back_calculation"); + return strategy; + }(); + + KinematicState getKinematicState() { + 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 relativeTargetPosition = trajectory.position(elapsed); // relative to guppy starting position, world frame + const auto targetVelocity = trajectory.velocity(elapsed); // world frame + + 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 + + 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; + + 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 = 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; + }; + + void execute(const std::shared_ptr> goalHandle) { + const auto& goal = goalHandle->get_goal(); + + 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; + } + + 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) + + 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); + + _xPid.reset(); + _yPid.reset(); + _zPid.reset(); + _yawPid.reset(); + _pitchPid.reset(); + _rollPid.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); + + geometry_msgs::msg::Twist zeroTwist; // publish zero twist + _commandVelocityPublisher->publish(zeroTwist); + + return; + } + + 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(); // 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; + auto relativeCurrentPosition = currentPosition - initialPosition; + + 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); + + if (elapsed >= goal->duration) break; + + rate.sleep(); + } + + geometry_msgs::msg::Twist zeroTwist; // publish zero twist + _commandVelocityPublisher->publish(zeroTwist); + + if (rclcpp::ok()) { + result->pose = getKinematicState().pose; + result->target_reached = true; // shouldn't assume true, fix that by calcing the hypotenuse + + goalHandle->succeed(result); + + RCLCPP_INFO(this->get_logger(), "goal succeeded"); + } + } +}; + +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 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() 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 new file mode 100644 index 0000000..fd283d1 --- /dev/null +++ b/src/guppy_tests/package.xml @@ -0,0 +1,17 @@ + + + + guppy_tests + 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 + + 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' 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) 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}"