Simulation Interface Draft Stubs

projects/can_transceiver/inc/can_frame_parser.h

@@ -16,7 +16,7 @@ static std::array<std::string, CanId::CAN_ID_MAX> CAN_DEVICE_NAMES = {"Placehold
 
 /**
  * Custom exception for when an attempt is made to construct a CAN object with a mismatched ID
- * 
+ *
  */
 class CanIdMismatchException : public std::runtime_error
 {
@@ -33,7 +33,7 @@ class CanIdMismatchException : public std::runtime_error
 
 /**
  * Placeholder CAN device
- * 
+ *
  */
 struct Placeholder0
 {
@@ -58,21 +58,21 @@ struct Placeholder0
 
     /**
      * @brief Construct the object from a CAN frame
-     * 
+     *
      * @param frame CAN frame
      */
     explicit Placeholder0(const CanFrame & frame);
 
     /**
      * @brief Construct the object from a ROS msg
-     * 
+     *
      */
     Placeholder0(/* Placeholder0 ROS msg */);
 };
 
 /**
  * Placeholder CAN device
- * 
+ *
  */
 struct Placeholder1
 {
@@ -93,14 +93,14 @@ struct Placeholder1
 
     /**
      * @brief Construct the object from a CAN frame
-     * 
+     *
      * @param frame CAN frame
      */
     explicit Placeholder1(const CanFrame & frame);
 
     /**
      * @brief Construct the object from a ROS msg
-     * 
+     *
      */
     Placeholder1(/* Placeholder1 ROS msg */);
 
@@ -115,7 +115,7 @@ struct Placeholder1
 
 /**
  * Rudder Command Frame
- * 
+ *
  */
 struct RudderCmd
 {
@@ -140,7 +140,7 @@ struct RudderCmd
 
     /**
      * @brief Convert this object into a standard Linux CAN frame and return it
-     * 
+     *
      * @return Rudder command as a standard Linux CAN frame object
      */
     CanFrame toLinuxCan();

projects/can_transceiver/inc/can_transceiver.h

@@ -10,27 +10,27 @@
 /**
  * Abstract CAN Transceiver Class
  * Handles transmission and reception of data to and from the hardware/simulator
- * 
+ *
  */
 class CanTransceiver
 {
 public:
     /**
     * @brief Destroy the Can Transceiver object
-    * 
+    *
     */
     virtual ~CanTransceiver() = 0;
 
     /**
      * @brief Call when a new command (ex. rudder command) needs to be executed
      *        Passes the command down to the hardware/simulator
-     * 
+     *
      */
     void onNewCmd(CanId id /*, other data fields... */);
 
     /**
      * @brief Retrieve the most recent set of sensors data
-     * 
+     *
      * @return Data in some format - string is just a placeholder; DO NOT USE STRING IN ACTUAL IMPLEMENTATION
      */
     std::string getRecentSensors();
@@ -40,42 +40,42 @@ class CanTransceiver
     void * sensor_buf_;
     /**
      * @brief Retrieve latest incoming data from hardware/simulator and process it
-     * 
+     *
      */
     virtual void receive() = 0;
 
     /**
      * @brief Send a command to the hardware/simulator
-     * 
+     *
      * @param frame Command frame to send
      */
     virtual void send(const CanFrame & frame) const = 0;
 
     /**
      * @brief Call on receiving a new CAN data frame from hardware/simulator
-     * 
+     *
      * @param frame received CAN data frame
      */
     void onNewCanData(const CanFrame & frame);
 };
 
 /**
  * Implementation of CAN Transceiver that interfaces with CAN hardware
- * 
+ *
  */
 class CanbusIntf : public CanTransceiver
 {
 public:
     /**
     * @brief Construct a new Canbus Intf object and start listening for incoming data on a new thread
-    * 
-    * @param can_inst 
+    *
+    * @param can_inst
     */
     explicit CanbusIntf(const std::string & can_inst);
 
     /**
      * @brief Destroy the Canbus Intf object
-     * 
+     *
      */
     ~CanbusIntf();
 
@@ -88,23 +88,23 @@ class CanbusIntf : public CanTransceiver
 
     /**
      * @brief Retrieve latest incoming CAN frame from hardware and process it
-     * 
+     *
      */
     void receive();
 
     /**
      * @brief Send a command to hardware
-     * 
+     *
      * @param frame command frame to send
      */
     void send(const CanFrame & frame) const;
 };
 
 /**
  * Implementation of CAN Transceiver that interfaces with the simulator
- * 
+ *
  */
-class CanSimIntf : public CanTransceiver, public rclcpp::Node
+class CanSimTransceiver : public CanTransceiver
 {
     void receive();
 };

projects/can_transceiver/src/can_frame_parser.cpp

@@ -6,7 +6,7 @@ namespace
 {
 /**
  * @brief Verify that a given CAN frame to construct a device has a valid ID assigned to it
- * 
+ *
  * @param actual_can_id   ID of the given CAN frame
  * @param expected_can_id ID of the device object that is attempting to be constructed
  */
@@ -20,7 +20,7 @@ void checkId(const canid_t & actual_can_id, const CanId & expected_can_id)
 /**
  * @brief Default CAN device object constructor for when construction is a 1:1 mapping between raw data and data fields
  *        Copies data from source CAN frame to a given buffer
- * 
+ *
  * @param frame           Source CAN frame
  * @param expected_can_id Device ID of CAN Frame that is attempting to be constructed
  * @param buf             Output buffer for data to be copied into

projects/can_transceiver/src/can_transceiver.cpp

@@ -5,11 +5,19 @@
 #include <sys/ioctl.h>
 #include <sys/socket.h>
 
+#include <chrono>
 #include <cstring>
+#include <functional>
+#include <memory>
 #include <stdexcept>
+#include <string>
 #include <thread>
 
 #include "can_frame_parser.h"
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/string.hpp"
+
+using namespace std::chrono_literals;
 
 using IFreq       = struct ifreq;
 using SockAddr    = struct sockaddr;
@@ -20,7 +28,7 @@ namespace
 
 /**
  * @brief Format a CAN frame into a string for debugging and logging
- * 
+ *
  * @param frame CAN frame to format
  * @return String representation of frame
  */
@@ -114,5 +122,3 @@ void CanbusIntf::send(const CanFrame & frame) const
         std::cerr << fmtCanFrameDbgStr(frame) << std::endl;
     }
 }
-
-void CanSimIntf::receive() {}

projects/can_transceiver/src/can_transceiver_ros_intf.cpp

@@ -1,12 +1,29 @@
+#include <chrono>
+#include <functional>
+#include <memory>
+#include <string>
+#include <thread>
+
 #include "can_frame_parser.h"
 #include "can_transceiver.h"
 #include "cmn_hdrs/ros_info.h"
+#include "custom_interfaces/msg/batteries.hpp"
+#include "custom_interfaces/msg/generic_sensors.hpp"
+#include "custom_interfaces/msg/gps.hpp"
+#include "custom_interfaces/msg/helper_generic_sensor.hpp"
+#include "custom_interfaces/msg/wind_sensor.hpp"
+#include "custom_interfaces/msg/wind_sensors.hpp"
 #include "rclcpp/publisher.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp/subscription.hpp"
 #include "rclcpp/timer.hpp"
 #include "std_msgs/msg/string.hpp"
 
+using namespace std::chrono_literals;
+
+constexpr int QUEUE_SIZE        = 10;
+constexpr int PLACEHOLDER_VALUE = 42;  // Placeholder value for debugging or testing
+
 class CanTransceiverIntf : public rclcpp::Node
 {
 public:
@@ -44,12 +61,120 @@ class CanTransceiverIntf : public rclcpp::Node
     /**
      * @brief Callback function to subscribe to the onboard ROS network
      *
+     *
      */
     void sub_cb(std_msgs::msg::String /* placeholder */) { can_trns_->onNewCmd(Placeholder0); }
 };
 
+//===========================================================================================
+// Simulation Interface
+//===========================================================================================
+/* Refer to https://ubcsailbot.atlassian.net/wiki/spaces/prjt22/pages/1768849494/Simulation+Interface#Interfaces
+ * Handles publishing and subscribing to certain ROS topics
+ */
+class CanSimIntf : public rclcpp::Node
+{
+public:
+    CanSimIntf()  //Our node which subs to topics
+    : Node("CanSimIntf")
+    {
+        // Subscriber
+        // There is no timer because subscriber will respond to whatever data is published to the topic /Simulator
+        // Topic: mock_gps
+        subscriptionGPS_ = this->create_subscription<custom_interfaces::msg::GPS>(
+          MOCK_GPS_TOPIC, QUEUE_SIZE, std::bind(&CanSimIntf::gps_callback, this, std::placeholders::_1));
+
+        // Topic: mock_wind_sensors
+        subscriptionWindSensor_ = this->create_subscription<custom_interfaces::msg::WindSensors>(
+          MOCK_WIND_SENSORS_TOPIC, QUEUE_SIZE, std::bind(&CanSimIntf::wind_callback, this, std::placeholders::_1));
+
+        // Publisher
+        publisherWindSensors_ =
+          this->create_publisher<custom_interfaces::msg::WindSensors>(WIND_SENSORS_TOPIC, QUEUE_SIZE);
+        publisherWindSensor_ =
+          this->create_publisher<custom_interfaces::msg::WindSensor>(FILTERED_WIND_SENSOR_TOPIC, QUEUE_SIZE);
+        publisherGPS_ = this->create_publisher<custom_interfaces::msg::GPS>(GPS_TOPIC, QUEUE_SIZE);
+        publisherGenericSensors_ =
+          this->create_publisher<custom_interfaces::msg::GenericSensors>(DATA_SENSORS_TOPIC, QUEUE_SIZE);
+        publisherBatteries_ = this->create_publisher<custom_interfaces::msg::Batteries>(BATTERIES_TOPIC, QUEUE_SIZE);
+        // Timer with 500ms delay
+        timer_ = this->create_wall_timer(500ms, std::bind(&CanSimIntf::timer_callback, this));
+    }
+
+private:
+    // Subscriber
+    void gps_callback(const custom_interfaces::msg::GPS::SharedPtr msg) const {}
+    void wind_callback(const custom_interfaces::msg::WindSensors::SharedPtr msg) const {}
+
+    // Publisher
+    void timer_callback()
+    {
+        // ** GPS **
+        custom_interfaces::msg::GPS gpsPubData;
+        gpsPubData.heading.heading   = PLACEHOLDER_VALUE;
+        gpsPubData.speed.speed       = PLACEHOLDER_VALUE;
+        gpsPubData.lat_lon.latitude  = PLACEHOLDER_VALUE;
+        gpsPubData.lat_lon.longitude = PLACEHOLDER_VALUE;
+        // Publishes msg
+        publisherGPS_->publish(gpsPubData);
+
+        // ** Wind Sensor (WSensor)**
+        custom_interfaces::msg::WindSensor WSensorData;
+        WSensorData.speed.speed = PLACEHOLDER_VALUE;
+        WSensorData.direction   = PLACEHOLDER_VALUE;
+        // Publishes msg
+        publisherWindSensor_->publish(WSensorData);
+
+        // ** Wind Sensors (WSensors)**
+        custom_interfaces::msg::WindSensors WSensorsData;
+        WSensorsData.wind_sensors[0].direction   = PLACEHOLDER_VALUE;
+        WSensorsData.wind_sensors[0].speed.speed = PLACEHOLDER_VALUE;
+        // Publishes msg
+        publisherWindSensors_->publish(WSensorsData);
+
+        // ** Batteries **
+        custom_interfaces::msg::Batteries WBatteriesData;
+        WBatteriesData.batteries[0].voltage = PLACEHOLDER_VALUE;
+        WBatteriesData.batteries[0].current = PLACEHOLDER_VALUE;
+        // Publishes msg
+        publisherBatteries_->publish(WBatteriesData);
+
+        // ** Generic Sensors **
+        custom_interfaces::msg::HelperGenericSensor HelperGenSensorData;
+        custom_interfaces::msg::GenericSensors      GenSensorData;
+        HelperGenSensorData.id   = 0x0;  //uint8
+        HelperGenSensorData.data = 0x0;  //uint64
+        // Publishes msg
+        GenSensorData.generic_sensors.push_back(HelperGenSensorData);
+        publisherGenericSensors_->publish(GenSensorData);
+    }
+
+    // Field Operations
+
+    // Publisher Field Declarations
+    rclcpp::Publisher<custom_interfaces::msg::WindSensors>::SharedPtr    publisherWindSensors_;
+    rclcpp::Publisher<custom_interfaces::msg::WindSensor>::SharedPtr     publisherWindSensor_;
+    rclcpp::Publisher<custom_interfaces::msg::GPS>::SharedPtr            publisherGPS_;
+    rclcpp::Publisher<custom_interfaces::msg::GenericSensors>::SharedPtr publisherGenericSensors_;
+    rclcpp::Publisher<custom_interfaces::msg::Batteries>::SharedPtr      publisherBatteries_;
+
+    // Subscriber Field Declarations
+    rclcpp::Subscription<custom_interfaces::msg::GPS>::SharedPtr         subscriptionGPS_;
+    rclcpp::Subscription<custom_interfaces::msg::WindSensors>::SharedPtr subscriptionWindSensor_;
+
+    // Timer object to allow our CamSimIntfFeedback node to perform action at x rate
+    rclcpp::TimerBase::SharedPtr timer_;
+};
+
+//===========================================================================================
+// Main
+//===========================================================================================
+// Run with:
+//$ ros2 run network_systems can_transceiver
 int main(int argc, char * argv[])
 {
     rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<CanSimIntf>());
+    rclcpp::shutdown();
     return 0;
 }