Simulation Interface Draft Stubs

CMakeLists.txt

@@ -15,36 +15,54 @@ endif()
 set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
 # Configure build flags
 if(CMAKE_BUILD_TYPE STREQUAL "Debug")
-    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS_DEBUG} -fprofile-arcs \
-        -ftest-coverage -fprofile-instr-generate -fcoverage-mapping")
-    add_definitions(-DDEBUG)
+    set(coverage_flags "-fprofile-arcs -ftest-coverage -fprofile-instr-generate -fcoverage-mapping")
+    set(CMAKE_CXX_FLAGS
+        "${CMAKE_CXX_FLAGS_DEBUG}"
+        # ${coverage_flags} Needs to be implemented
+    )
 elseif(CMAKE_BUILD_TYPE STREQUAL "RelWithDebInfo")
     set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS_RELWITHDEBINFO}")
 endif()
 # Enable all warnings as errors except unused parameter because auto-generated protobuf files trigger it,
 # and clang-tidy can cover it without false-flagging the protobuf files.
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++20 -Wall -Wextra -Wpedantic -Werror -Wno-unused-parameter -pthread")
-message("Building Network Systems with build type '${CMAKE_BUILD_TYPE}' "
+message(WARNING "Building Network Systems with build type '${CMAKE_BUILD_TYPE}' "
         "and flags: '${CMAKE_CXX_FLAGS}'")
 
 # ROS dependencies
+set(ROS_DEPS
+    rclcpp
+    std_msgs
+    custom_interfaces
+)
 find_package(ament_cmake REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(std_msgs REQUIRED)
+foreach(dep IN LISTS ROS_DEPS)
+    find_package(${dep} REQUIRED)
+endforeach()
+
 
 # Boost
 find_package(Boost 1.74.0 COMPONENTS REQUIRED
     # Insert desired libraries below (see /usr/include/boost/ for available options)
     serialization
+    thread
 )
 
+# MongoDB
+find_package(mongocxx REQUIRED)
+find_package(bsoncxx REQUIRED)
+
 # Protobuf
 find_package(Protobuf REQUIRED)
+set(PROTOBUF_LINK_LIBS
+    ${Protobuf_LIBRARIES}
+    protofiles
+)
 
 # Googletest (installed by sailbot_workspace Docker config)
 if(UNIT_TEST)
-    execute_process(COMMAND arch OUTPUT_VARIABLE ARCH OUTPUT_STRIP_TRAILING_WHITESPACE)
-    set(GTEST_LINK_LIBS "/usr/lib/${ARCH}-linux-gnu/libgtest.a" "/usr/lib/${ARCH}-linux-gnu/libgtest_main.a")
+    find_package(GTest CONFIG REQUIRED)
+    set(GTEST_LINK_LIBS gtest gtest_main)
 endif()
 
 # Add src directories

functions.cmake

@@ -1,6 +1,7 @@
 # Create module library
-function(make_lib module srcs link_libs inc_dirs)
+function(make_lib module srcs link_libs inc_dirs compile_defs)
     add_library(${module} ${srcs})
+    target_compile_definitions(${module} PUBLIC ${compile_defs})
     target_link_libraries(${module} PUBLIC ${link_libs})
     target_include_directories(
         ${module} PUBLIC
@@ -12,10 +13,11 @@ function(make_lib module srcs link_libs inc_dirs)
 endfunction()
 
 # Create module ROS executable
-function(make_ros_exe module srcs link_libs inc_dirs)
+function(make_exe module srcs link_libs inc_dirs ${compile_defs})
     set(bin_module bin_${module})
     add_executable(${bin_module} ${srcs})
-    ament_target_dependencies(${bin_module} PUBLIC rclcpp std_msgs)
+    target_compile_definitions(${bin_module} PUBLIC ${compile_defs})
+    ament_target_dependencies(${bin_module} PUBLIC ${ROS_DEPS})
     target_link_libraries(${bin_module} PUBLIC ${link_libs})
     target_include_directories(
         ${bin_module} PUBLIC
@@ -30,10 +32,11 @@ function(make_ros_exe module srcs link_libs inc_dirs)
 endfunction()
 
 # Create unit test
-function(make_unit_test module srcs link_libs inc_dirs)
+function(make_unit_test module srcs link_libs inc_dirs compile_defs)
     if(UNIT_TEST)
         set(test_module test_${module})
         add_executable(${test_module} ${srcs})
+        target_compile_definitions(${test_module} PUBLIC ${compile_defs})
         target_include_directories(
             ${test_module} PRIVATE
             ${CMAKE_CURRENT_LIST_DIR}/inc

package.xml

@@ -11,6 +11,7 @@
  <buildtool_depend>ament_cmake</buildtool_depend>
  <depend>rclcpp</depend>
  <depend>std_msgs</depend>
+ <depend>custom_interfaces</depend>
  <exec_depend>ros2launch</exec_depend>
 
  <export>

projects/can_transceiver/CMakeLists.txt

@@ -6,6 +6,9 @@ set(link_libs
 set(inc_dirs
 )
 
+set(compile_defs
+)
+
 set(srcs
     ${CMAKE_CURRENT_LIST_DIR}/src/can_transceiver.cpp
     ${CMAKE_CURRENT_LIST_DIR}/src/can_frame_parser.cpp
@@ -16,4 +19,4 @@ set(bin_srcs
     ${srcs}
     ${CMAKE_CURRENT_LIST_DIR}/src/can_transceiver_ros_intf.cpp
 )
-make_ros_exe(${module} "${bin_srcs}" "${link_libs}" "${inc_dirs}")
+make_exe(${module} "${bin_srcs}" "${link_libs}" "${inc_dirs}" "${compile_defs}")

projects/can_transceiver/inc/can_transceiver.h

@@ -2,6 +2,7 @@
 
 #include <linux/can.h>
 
+#include <std_msgs/msg/detail/string__struct.hpp>
 #include <string>
 
 #include "can_frame_parser.h"
@@ -99,12 +100,3 @@ class CanbusIntf : public CanTransceiver
      */
     void send(const CanFrame & frame) const;
 };
-
-/**
- * Implementation of CAN Transceiver that interfaces with the simulator
- * 
- */
-class CanSimIntf : public CanTransceiver, public rclcpp::Node
-{
-    void receive();
-};

projects/can_transceiver/src/can_transceiver.cpp

@@ -5,22 +5,31 @@
 #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;
 using SockAddrCan = struct sockaddr_can;
 
+#define QUEUE_SIZE 10
 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 +123,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,38 @@
+#include <cstdio>
+#include <custom_interfaces/msg/detail/batteries__struct.hpp>
+#include <custom_interfaces/msg/detail/generic_sensors__struct.hpp>
+#include <custom_interfaces/msg/detail/gps__struct.hpp>
+#include <custom_interfaces/msg/detail/wind_sensor__struct.hpp>
+#include <custom_interfaces/msg/detail/wind_sensors__struct.hpp>
+#include <memory>
+#include <chrono>
+#include <functional>
+#include <string>
+#include <thread>
+
 #include "can_frame_parser.h"
 #include "can_transceiver.h"
+
+#include "custom_interfaces/msg/gps.hpp"
+#include "custom_interfaces/msg/wind_sensors.hpp"
+#include "custom_interfaces/msg/batteries.hpp"
+#include "custom_interfaces/msg/generic_sensors.hpp"
+#include "custom_interfaces/msg/helper_generic_sensor.hpp"
+#include "custom_interfaces/msg/wind_sensor.hpp"
+
 #include "rclcpp/publisher.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp/subscription.hpp"
 #include "rclcpp/timer.hpp"
 #include "ros_info.h"
 #include "std_msgs/msg/string.hpp"
 
+using namespace std::chrono_literals;
+
+#define QUEUE_SIZE 10
+#define WIND_SENSOR_BOUND 2
+#define PLACEHOLDER_VALUE 42 // Placeholder value for debugging or testing
+
 class CanTransceiverIntf : public rclcpp::Node
 {
 public:
@@ -43,13 +69,131 @@ 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", 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", QUEUE_SIZE, std::bind(&CanSimIntf::wind_callback, this, std::placeholders::_1));
+
+    // Publisher
+        publisherWindSensors_ = this->create_publisher<custom_interfaces::msg::WindSensors>("wind_sensors", QUEUE_SIZE);
+        publisherWindSensor_ = this->create_publisher<custom_interfaces::msg::WindSensor>("filtered_wind_sensor", QUEUE_SIZE);
+        publisherGPS_ = this->create_publisher<custom_interfaces::msg::GPS>("gps", QUEUE_SIZE);
+        publisherGenericSensors_ = this->create_publisher<custom_interfaces::msg::GenericSensors>("data_sensors", QUEUE_SIZE);
+        publisherBatteries_ = this->create_publisher<custom_interfaces::msg::Batteries>("batteries", 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 = custom_interfaces::msg::GPS();
+        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 = custom_interfaces::msg::WindSensor();
+        WSensorData.speed.speed = PLACEHOLDER_VALUE;
+        WSensorData.direction = PLACEHOLDER_VALUE;
+        // Publishes msg
+        publisherWindSensor_->publish(WSensorData);
+
+        // ** Wind Sensors (WSensors)**
+        custom_interfaces::msg::WindSensors WSensorsData = custom_interfaces::msg::WindSensors();
+        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 = custom_interfaces::msg::Batteries();
+        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::HelperGenericSensor();
+        custom_interfaces::msg::GenericSensors GenSensorData = custom_interfaces::msg::GenericSensors();
+        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_;
+
+    // Variable to count number of msgs published
+    //size_t                                              count_;
+
+    // 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;
 }

projects/example/CMakeLists.txt

@@ -7,22 +7,25 @@ set(link_libs
 set(inc_dirs
 )
 
+set(compile_defs
+)
+
 # Create module library
 set(srcs
     ${CMAKE_CURRENT_LIST_DIR}/src/cached_fib.cpp
 )
-make_lib(${module} "${srcs}" "${link_libs}" "${inc_dirs}")
+make_lib(${module} "${srcs}" "${link_libs}" "${inc_dirs}" "${compile_defs}")
 
 # Create module ROS executable
 set(bin_srcs
     ${srcs}
     ${CMAKE_CURRENT_LIST_DIR}/src/cached_fib_subscriber.cpp
 )
-make_ros_exe(${module} "${bin_srcs}" "${link_libs}" "${inc_dirs}")
+make_exe(${module} "${bin_srcs}" "${link_libs}" "${inc_dirs}" "${compile_defs}")
 
 # Create unit test
 set(test_srcs
     ${srcs}
     ${CMAKE_CURRENT_LIST_DIR}/test/test_cached_fib.cpp
 )
-make_unit_test(${module} "${test_srcs}" "${link_libs}" "${inc_dirs}")
+make_unit_test(${module} "${test_srcs}" "${link_libs}" "${inc_dirs}" "${compile_defs}")