pull main into local robot code

.clangd

@@ -1,4 +1,5 @@
 CompileFlags:
+  CompilationDatabase: build/
   Add:
     - -std=gnu++17
     - -I/opt/ros/jazzy/include

.envrc

@@ -0,0 +1,2 @@
+use flake
+source install/setup.bash

.gitignore

@@ -216,7 +216,6 @@ activemq-data/
 
 # Environments
 .env
-.envrc
 .venv
 env/
 venv/
@@ -322,4 +321,7 @@ __marimo__/
 
 # debug information files
 *.dwo
-compile_commands.json
+compile_commands.json
+
+# NixOS stuff
+.direnv

README.md

@@ -48,6 +48,34 @@ The included `Dockerfile` and `devcontainer.json` allow the guppy repo to be ope
 
 To interact with the Docker contaner graphically, you can visit [localhost:6080/vnc.html](https://localhost:6080/vnc.html), or connect with a VCN client to [localhost:5901](vnc://localhost:5091)
 
+### Nix & NixOS
+
+The included `flake.nix` allows for development, sim, and operation to run on any x86 machine with Nix or NixOS installed.
+
+After cloning, simply run:
+```bash
+nix develop
+```
+Or if using `direnv`, run:
+```bash
+direnv allow
+```
+
+This is made possible using the [nix-ros-overlay](https://github.com/lopsided98/nix-ros-overlay)
+
+#### Nix Networking
+
+If you encounter issues, make sure to allow the UDP ports required by ROS in your `configuration.nix`:
+```nix
+networking.firewall = {
+  allowedUDPPorts = [ 7400 7401 ];
+  allowedUDPPortRanges = [
+    { from = 7410; to = 7500; }
+  ];
+};
+```
+
+
 ## Organization
 The code is broken up into several ROS 2 packages, in the [`src/`](./src/) directory:
 - [**`guppy`**](./src/guppy/#readme): A metapackage that contains dependencies of all other packages, as well as bringup and launch scripts.

flake.nix

@@ -0,0 +1,87 @@
+{
+  inputs = {
+    nix-ros-overlay.url = "github:lopsided98/nix-ros-overlay/master";
+    nixpkgs.follows = "nix-ros-overlay/nixpkgs";  # IMPORTANT!!!
+  };
+  outputs = { self, nix-ros-overlay, nixpkgs }:
+    nix-ros-overlay.inputs.flake-utils.lib.eachDefaultSystem (system:
+      let
+        pkgs = import nixpkgs {
+          inherit system;
+          overlays = [ nix-ros-overlay.overlays.default ];
+        };
+      in {
+        devShells.default = pkgs.mkShell {
+          name = "guppy_ros";
+          packages = [
+            # non ros
+
+            # build
+            pkgs.colcon
+            pkgs.cmake
+            pkgs.clang-tools
+
+            # deps
+            pkgs.proxsuite
+            pkgs.python3Packages.pygame
+            pkgs.python3Packages.pip
+
+            # extra
+            pkgs.fastfetch
+            pkgs.can-utils
+
+            (with pkgs.rosPackages.jazzy; buildEnv {
+              # ros packages
+              paths = [
+                # ros base
+                ros-core
+                ros-base
+                rclcpp
+                rclpy
+
+                # ros msgs
+                std-msgs
+                geometry-msgs
+                sensor-msgs
+                nav-msgs
+
+                # rqt
+                rqt
+                rqt-common-plugins
+
+                # build
+                ament-cmake
+                ament-cmake-core # vectornav_msgs
+                ament-cmake-python
+                ament-lint-auto
+
+                # launch
+                launch
+                launch-ros
+                launch-xml
+
+                # rviz
+                rviz2
+
+                # deps
+                ros2-control
+                control-toolbox
+                ros-gz-interfaces # gncea_autonomy
+                python-cmake-module # vecornav_msgs
+                marine-acoustic-msgs # waterlinked_dvl_driver
+                ros-gz-sim # gncea_description
+                ros-gz-bridge # sim
+                joint-state-publisher # sim
+              ];
+            })
+          ];
+          shellHook = ''
+            fastfetch -l ./.github/guppy.txt
+          '';
+        };
+      });
+  nixConfig = {
+    extra-substituters = [ "https://ros.cachix.org" ];
+    extra-trusted-public-keys = [ "ros.cachix.org-1:dSyZxI8geDCJrwgvCOHDoAfOm5sV1wCPjBkKL+38Rvo=" ];
+  };
+}

src/guppy_control/CMakeLists.txt

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

src/guppy_control/include/guppy_control/t200_interface.hpp

@@ -27,6 +27,9 @@ class T200Interface {
 
     /* the can interface (like can0 or vcan0) */
     std::string can_interface_;
+
+    /* are thrusters enabled */
+    bool enabled_;
 
     /*
         @brief sets up the CAN interface and socket
@@ -76,6 +79,11 @@ class T200Interface {
         @return whether or not the operation was sucessful for all values
     */
     bool shutdown();
+
+    /*
+        @brief sets thrusters to be enabled/disabled
+    */
+    void set_enabled(bool enabled);
 };
 }
 

src/guppy_control/package.xml

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

src/guppy_control/src/control_chassis_node.cpp

@@ -1,9 +1,11 @@
+#include <rclcpp/subscription.hpp>
 #include "rclcpp/rclcpp.hpp"
 #include "guppy_control/chassis_controller.hpp"
 #include "guppy_control/t200_interface.hpp"
 
 #include "std_msgs/msg/float32.hpp"
 #include "std_msgs/msg/float64.hpp"
+#include "guppy_msgs/msg/state.hpp"
 
 using namespace std::chrono_literals;
 using namespace t200_interface;
@@ -38,7 +40,7 @@ class ControlChassis : public rclcpp::Node {
     load_params_(&parameters); // load parameters from configuration file
 
     this->param_subscriber_ = std::make_shared<rclcpp::ParameterEventHandler>(this); // subscribe to parameter change event
-
+    
     // parameter callback to update parameters on event
 
     auto param_callback = [this](const rcl_interfaces::msg::ParameterEvent & parameter_event) {
@@ -105,6 +107,7 @@ class ControlChassis : public rclcpp::Node {
     // setup subscriptions
     odom_subscription_ = this->create_subscription<nav_msgs::msg::Odometry>("/waterlinked_dvl_driver/odom",10,std::bind(&ControlChassis::odom_callback, this, std::placeholders::_1));
     cmd_vel_subscription_ = this->create_subscription<geometry_msgs::msg::Twist>("/cmd_vel",10,std::bind(&ControlChassis::cmdvel_callback, this, std::placeholders::_1));
+    state_subscription_ = this->create_subscription<guppy_msgs::msg::State>("/state", 10, std::bind(&ControlChassis::state_callback, this, std::placeholders::_1));
 
     auto timer_callback = [this]() -> void {
       auto thrusts = controller->get_motor_thrusts();
@@ -139,11 +142,20 @@ class ControlChassis : public rclcpp::Node {
   void cmdvel_callback(geometry_msgs::msg::Twist::SharedPtr msg) {
     controller->update_desired_state(msg);
   }
+
+  void state_callback(guppy_msgs::msg::State::SharedPtr msg) {
+      if (msg->state == guppy_msgs::msg::State::DISABLED) {
+          this->thruster_interface->set_enabled(false);
+      } else {
+          this->thruster_interface->set_enabled(true);
+      }
+  }
 
 private:
   rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr sim_motor_publishers_[8];
   rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_subscription_;
   rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_subscription_;
+  rclcpp::Subscription<guppy_msgs::msg::State>::SharedPtr state_subscription_;
   rclcpp::TimerBase::SharedPtr timer_;
 
   std::shared_ptr<rclcpp::ParameterEventHandler> param_subscriber_;

src/guppy_control/src/t200_interface.cpp

@@ -36,8 +36,11 @@ bool T200Interface::write(Eigen::VectorXd throttles) {
 
     bool okay = true;
     for (size_t i=0; i < n; i++) {
-        okay = okay && send_to_can(can_ids[i], (float)throttles[i]);
-        // std::cout << can_ids[i] << "\t" << (float)throttles[i] << "\t" << okay << std::endl;
+        if (this->enabled_)
+            okay = okay && send_to_can(can_ids[i], throttles[i]);
+        else 
+            okay = okay && send_to_can(can_ids[i], 0);
+        // std::cout << can_ids[i] << "\t" << throttles[i] << "\t" << okay << std::endl;
     }
 
     return okay;
@@ -63,4 +66,8 @@ bool T200Interface::shutdown() {
     return okay;
 }
 
+void T200Interface::set_enabled(bool enabled) {
+    this->enabled_ = enabled;
+}
+
 }

util/guppy_can/install_can_service.sh

@@ -0,0 +1,16 @@
+#!/bin/bash
+
+# Check if script is run as root
+if [[ "$EUID" -ne 0 ]]; then
+  echo "Error: This script must be run as sudo or as root."
+  echo "Usage: sudo $0"
+  exit 1
+fi
+
+set -e
+
+cp setup_can.sh /etc/setup_can.sh
+
+cp setup_can.service /etc/systemd/system/guppy_can.service
+
+systemctl enable --now guppy_can.service

util/guppy_can/setup_can.service

@@ -0,0 +1,11 @@
+[Unit]
+Description=Sets up can interface can0
+After=network.target
+
+[Service]
+Type=oneshot
+ExecStart=/etc/setup_can.sh
+RemainAfterExit=yes
+
+[Install]
+WantedBy=multi-user.target