Merge branch 'main' into user/Jay/rpi-workspace

Author: FireBoyAJ24

.devcontainer/devcontainer.json

@@ -3,7 +3,7 @@
 	"name": "Sailbot Workspace",
 	"dockerComposeFile": [
 		"docker-compose.yml",
-		"website/docker-compose.website.yml", // website
+		// "website/docker-compose.website.yml", // website
 
 		// Uncomment the files containing the programs you need
 		// "docs/docker-compose.docs.yml", // docs

src/local_pathfinding/local_pathfinding/node_mock_gps.py

@@ -10,7 +10,7 @@
 from geopy.distance import great_circle
 from rclpy.node import Node
 
-from local_pathfinding.objectives import get_true_wind
+from local_pathfinding.ompl_objectives import get_true_wind
 
 MEAN_SPEED = ci.HelperSpeed(speed=15.0)  # mean boat speed in kmph
 

src/local_pathfinding/local_pathfinding/objectives.py renamed to src/local_pathfinding/local_pathfinding/ompl_objectives.py

@@ -21,7 +21,7 @@
 
 import local_pathfinding.coord_systems as cs
 import local_pathfinding.obstacles as ob
-from local_pathfinding.objectives import get_sailing_objective
+from local_pathfinding.ompl_objectives import get_sailing_objective
 
 if TYPE_CHECKING:
     from local_pathfinding.local_path import LocalPathState

src/local_pathfinding/local_pathfinding/ompl_path.py

@@ -27,7 +27,7 @@
 from shapely.geometry import Polygon
 
 import local_pathfinding.coord_systems as cs
-from local_pathfinding.objectives import get_true_wind
+from local_pathfinding.ompl_objectives import get_true_wind
 
 app = dash.Dash(__name__)
 

src/local_pathfinding/local_pathfinding/visualizer.py

@@ -5,10 +5,10 @@
 from rclpy.impl.rcutils_logger import RcutilsLogger
 
 import local_pathfinding.coord_systems as coord_systems
-import local_pathfinding.objectives as objectives
+import local_pathfinding.ompl_objectives as objectives
 import local_pathfinding.ompl_path as ompl_path
 from local_pathfinding.local_path import LocalPathState
-from local_pathfinding.objectives import get_true_wind
+from local_pathfinding.ompl_objectives import get_true_wind
 
 # Upwind downwind cost multipliers
 UPWIND_MULTIPLIER = 3000.0

src/local_pathfinding/test/test_objectives.py renamed to src/local_pathfinding/test/test_ompl_objectives.py

@@ -67,8 +67,8 @@ constexpr float LON_LBND = -180.0;
 constexpr float LON_UBND = 180.0;
 
 /***** Bounds for Speed ******/
-constexpr float SPEED_LBND = -10.0;  // Placeholder number
-constexpr float SPEED_UBND = 10.0;   // Placeholder number
+constexpr float SPEED_LBND = -10.0;  // Placeholder number (kmph)
+constexpr float SPEED_UBND = 10.0;   // Placeholder number (kmph)
 
 /***** Bounds for Heading ******/
 constexpr float HEADING_LBND = 0.0;
@@ -102,9 +102,9 @@ constexpr float PH_LBND = -1.6;  // lbnd of sensor being used
 constexpr float PH_UBND = 15.6;  // ubnd of sensor being used
 
 /***** Bounds for Salinity Sensor ******/
-constexpr float SALINITY_LBND = 0;       // lbnd of sensor being used is 0.07
+constexpr float SALINITY_LBND = 0;        // lbnd of sensor being used is 0.07
 constexpr float SALINITY_UBND = 1000000;  // ubnd of sensor being used is 500000+
 
 /***** Bounds for Pressure Sensor ******/
-constexpr float PRESSURE_LBND = -100;    // lowest lbnd of pressure sensors under consideration is -14.5 psi
+constexpr float PRESSURE_LBND = -100;  // lowest lbnd of pressure sensors under consideration is -14.5 psi
 constexpr float PRESSURE_UBND = 1000;  // highest lbnd of pressure sensors is 8702 psi

src/network_systems/lib/cmn_hdrs/shared_constants.h

@@ -275,7 +275,7 @@ std::string WindSensor::debugStr() const
 {
     std::stringstream ss;
     ss << BaseFrame::debugStr() << "\n"
-       << "Wind speed (m/s): " << wind_speed_ << "\n"
+       << "Wind speed (km/h): " << wind_speed_ << "\n"
        << "Wind angle (degrees): " << wind_angle_;
     return ss.str();
 }

src/network_systems/projects/can_transceiver/src/can_frame_parser.cpp

@@ -1,6 +1,7 @@
 #include "can_transceiver.h"
 
 #include <errno.h>
+#include <fcntl.h>
 #include <linux/can.h>
 #include <net/if.h>
 #include <sys/ioctl.h>
@@ -108,11 +109,21 @@ CanTransceiver::~CanTransceiver()
 
 void CanTransceiver::receive()
 {
+    int flags = fcntl(sock_desc_, F_GETFL, 0);
+    if (flags == -1) {
+        std::cerr << "failed to get flags for CAN socket fd" << std::endl;
+    }
+    // make read() non-blocking so mutex gets released
+    flags |= O_NONBLOCK;
+    if (fcntl(sock_desc_, F_SETFL, flags) == -1) {
+        std::cerr << "failed to set flags for CAN socket fd" << std::endl;
+    }
     while (!shutdown_flag_) {
         // make sure the lock is acquired and released INSIDE the loop, otherwise send() will never get the lock
-        std::lock_guard<std::mutex> lock(can_mtx_);
-        CanFrame                    frame;
-        ssize_t                     bytes_read = read(sock_desc_, &frame, sizeof(CanFrame));
+        CanFrame                     frame;
+        std::unique_lock<std::mutex> lock(can_mtx_);
+        ssize_t                      bytes_read = read(sock_desc_, &frame, sizeof(CanFrame));
+        lock.unlock();
         if (bytes_read > 0) {
             if (bytes_read != sizeof(CanFrame)) {
                 std::cerr << "CAN read error: read " << bytes_read << "B but CAN frames are expected to be "
@@ -121,8 +132,12 @@ void CanTransceiver::receive()
                 onNewCanData(frame);
             }
         } else if (bytes_read < 0) {
-            std::cerr << "CAN read error: " << errno << "(" << strerror(errno)  // NOLINT(concurrency-mt-unsafe)
-                      << ")" << std::endl;
+            if (errno != EAGAIN && errno != EWOULDBLOCK) {
+                std::cerr << "CAN read error: " << errno << "(" << strerror(errno)  // NOLINT(concurrency-mt-unsafe)
+                          << ")" << std::endl;
+            } else {
+                std::this_thread::sleep_for(std::chrono::milliseconds(1));
+            }
         }
     }
 }

src/network_systems/projects/can_transceiver/src/can_transceiver.cpp

@@ -90,6 +90,9 @@ class CanTransceiverIntf : public NetNode
               std::make_pair(CanId::SAIL_WIND, std::function<void(const CanFrame &)>([this](const CanFrame & frame) {
                                  publishWindSensor(frame);
                              })),
+              std::make_pair(CanId::DATA_WIND, std::function<void(const CanFrame &)>([this](const CanFrame & frame) {
+                                 publishWindSensor(frame);
+                             })),
               std::make_pair(
                 CanId::GENERIC_SENSOR_START,
                 std::function<void(const CanFrame &)>([this](const CanFrame & frame) { publishGeneric(frame); })),
@@ -533,7 +536,7 @@ class CanTransceiverIntf : public NetNode
     {
         desired_heading_ = desired_heading;
         try {
-            CAN_FP::DesiredHeading desired_heading_frame(desired_heading, CanId::MAIN_TR_TAB);
+            auto desired_heading_frame = CAN_FP::DesiredHeading(desired_heading_, CanId::MAIN_HEADING);
             can_trns_->send(desired_heading_frame.toLinuxCan());
             RCLCPP_INFO(
               this->get_logger(), "%s %s", getCurrentTimeString().c_str(), desired_heading_frame.toString().c_str());