adds digital sensor interface

01_Libraries/ghost_v5_interfaces/CMakeLists.txt

@@ -61,6 +61,7 @@ set(UTIL_LIBS
   load_motor_device_config_yaml
   load_rotation_sensor_device_config_yaml
   load_inertial_sensor_device_config_yaml
+  load_digital_device_config_yaml
 )
 
 foreach(UTIL ${UTIL_LIBS})
@@ -137,6 +138,7 @@ set(DEVICE_TESTS
   test_rotation_sensor_device_interface
   test_inertial_sensor_device_interface
   test_joystick_device_interface
+  test_digital_device_interface
 )
 foreach(TEST ${DEVICE_TESTS})
   ament_add_gtest(${TEST} test/devices/${TEST}.cpp)
@@ -152,9 +154,7 @@ foreach(TEST ${UTIL_LIBS})
   ament_target_dependencies(test_${TEST} ${DEPENDENCIES})
   target_link_libraries(test_${TEST}
     gtest_main
-    load_motor_device_config_yaml
-    load_rotation_sensor_device_config_yaml
-    load_inertial_sensor_device_config_yaml
+    ${UTIL_LIBS}
     device_config_factory_utils
     yaml-cpp
   )
@@ -170,12 +170,20 @@ target_link_libraries(test_device_config_factory_utils
 )
 
 # Hardware Interface Tests
+ament_add_gtest(test_rhi_serialization test/test_rhi_serialization.cpp)
+ament_target_dependencies(test_rhi_serialization ${DEPENDENCIES})
+target_link_libraries(test_rhi_serialization
+ gtest_main
+ device_config_factory_utils
+ robot_hardware_interface
+)
+
 ament_add_gtest(test_robot_hardware_interface test/test_robot_hardware_interface.cpp)
 ament_target_dependencies(test_robot_hardware_interface ${DEPENDENCIES})
 target_link_libraries(test_robot_hardware_interface
-  gtest_main
-  device_config_factory_utils
-  robot_hardware_interface
+ gtest_main
+ device_config_factory_utils
+ robot_hardware_interface
 )
 
 ament_package()

01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/device_interfaces.hpp

@@ -1,5 +1,5 @@
 /*
- *   Copyright (c) 2024 Maxx Wilson
+ *   Copyright (c) 2024 Maxx Wilson, Xander Wilson
  *   All rights reserved.
 
  *   Permission is hereby granted, free of charge, to any person obtaining a copy
@@ -46,6 +46,8 @@ enum device_type_e
   GPS_SENSOR,            // Unsupported
   RADIO,                 // Unsupported
   JOYSTICK,
+  DIGITAL_INPUT,
+  DIGITAL_OUTPUT,
   INVALID
 };
 
@@ -164,7 +166,7 @@ class DeviceData : public DeviceBase
   /**
    * @brief Converts device data to byte stream.
    *
-   * @param to_v5	set to true when the data is going from coprocessor -> V5 Brain
+   * @param hardware_type current hardware type
    * @return std::vector<unsigned char> byte stream
    */
   virtual std::vector<unsigned char> serialize(hardware_type_e hardware_type) const = 0;
@@ -173,7 +175,7 @@ class DeviceData : public DeviceBase
    * @brief Updates device data from byte stream.
    *
    * @param data byte stream as unsigned char vector
-   * @param from_coprocessor set to true when the data is going from coprocesspr -> V5 Brain
+   * @param hardware_type current hardware type
    */
   virtual void deserialize(
     const std::vector<unsigned char> & data,

01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_device_interface.hpp

@@ -0,0 +1,161 @@
+/*
+ *   Copyright (c) 2024 Maxx Wilson, Xander Wilson
+ *   All rights reserved.
+
+ *   Permission is hereby granted, free of charge, to any person obtaining a copy
+ *   of this software and associated documentation files (the "Software"), to deal
+ *   in the Software without restriction, including without limitation the rights
+ *   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ *   copies of the Software, and to permit persons to whom the Software is
+ *   furnished to do so, subject to the following conditions:
+
+ *   The above copyright notice and this permission notice shall be included in all
+ *   copies or substantial portions of the Software.
+
+ *   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ *   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ *   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ *   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ *   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ *   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ *   SOFTWARE.
+ */
+
+#pragma once
+
+#include <cstring>
+#include "ghost_v5_interfaces/devices/device_interfaces.hpp"
+
+namespace ghost_v5_interfaces
+{
+
+namespace devices
+{
+
+class DigitalInputDeviceData : public DeviceData
+{
+  public:
+  DigitalInputDeviceData(std::string name)
+  : DeviceData(name, DIGITAL_INPUT)
+  {
+  }
+
+  int getActuatorPacketSize() const override
+  {
+    return 0;
+  }
+
+  int getSensorPacketSize() const override
+  {
+    return 1;
+  }
+
+  std::shared_ptr<DeviceBase> clone() const override
+  {
+    return std::make_shared<DigitalInputDeviceData>(*this);
+  }
+
+  std::vector<unsigned char> serialize(hardware_type_e hardware_type) const override
+  {
+    std::vector<unsigned char> msg(1);
+    if (hardware_type == V5_BRAIN) {
+        msg.push_back((unsigned char) value);
+    }
+    return msg;
+  }
+
+  void deserialize(const std::vector<unsigned char> & msg, hardware_type_e hardware_type) override
+  {
+    if (hardware_type == COPROCESSOR) {
+        value = ((bool) msg.data()[0]);
+    }
+  }
+
+  void update(std::shared_ptr<DeviceData> data_ptr) override
+  {
+    auto digital_data_ptr = data_ptr->as<DigitalInputDeviceData>();
+    value = digital_data_ptr->value;
+  }
+
+  bool operator==(const DeviceBase & rhs) const override
+  {
+    const DigitalInputDeviceData * d_rhs = dynamic_cast<const DigitalInputDeviceData *>(&rhs);
+    return (d_rhs != nullptr) && (name == d_rhs->name) && (type == d_rhs->type) && (value == d_rhs->value);
+  }
+
+  bool value = false;
+};
+
+class DigitalOutputDeviceData : public DeviceData
+{
+  public:
+  DigitalOutputDeviceData(std::string name)
+  : DeviceData(name, DIGITAL_OUTPUT)
+  {
+  }
+
+  int getActuatorPacketSize() const override
+  {
+    return 1;
+  }
+
+  int getSensorPacketSize() const override
+  {
+    return 0;
+  }
+
+  std::shared_ptr<DeviceBase> clone() const override
+  {
+    return std::make_shared<DigitalOutputDeviceData>(*this);
+  }
+
+  std::vector<unsigned char> serialize(hardware_type_e hardware_type) const override
+  {
+    std::vector<unsigned char> msg;
+    if (hardware_type == COPROCESSOR) {
+        msg.push_back((unsigned char) value);
+    }
+    return msg;
+  }
+
+  void deserialize(const std::vector<unsigned char> & msg, hardware_type_e hardware_type) override
+  {
+    if (hardware_type == V5_BRAIN) {
+        value = ((bool) msg.data()[0]);
+    }
+  }
+
+  void update(std::shared_ptr<DeviceData> data_ptr) override
+  {
+    auto digital_data_ptr = data_ptr->as<DigitalOutputDeviceData>();
+    value = digital_data_ptr->value;
+  }
+
+  bool operator==(const DeviceBase & rhs) const override
+  {
+    const DigitalOutputDeviceData * d_rhs = dynamic_cast<const DigitalOutputDeviceData *>(&rhs);
+    return (d_rhs != nullptr) && (name == d_rhs->name) && (type == d_rhs->type) && (value == d_rhs->value);
+  }
+
+  bool value = false;
+};
+
+class DigitalDeviceConfig : public DeviceConfig
+{
+public:
+  std::shared_ptr<DeviceBase> clone() const override
+  {
+    return std::make_shared<DigitalDeviceConfig>(*this);
+  }
+
+  bool operator==(const DeviceBase & rhs) const override
+  {
+    const DigitalDeviceConfig * d_rhs = dynamic_cast<const DigitalDeviceConfig *>(&rhs);
+    return (d_rhs != nullptr) && (port == d_rhs->port) && (name == d_rhs->name) &&
+           (type == d_rhs->type);
+  }
+};
+
+} // namespace devices
+
+} // namespace ghost_v5_interfaces

01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/robot_hardware_interface.hpp

@@ -27,11 +27,13 @@
 #include <memory>
 #include <mutex>
 #include <unordered_map>
+#include "ghost_util/byte_utils.hpp"
 #include "ghost_v5_interfaces/devices/device_config_map.hpp"
 #include "ghost_v5_interfaces/devices/inertial_sensor_device_interface.hpp"
 #include "ghost_v5_interfaces/devices/joystick_device_interface.hpp"
 #include "ghost_v5_interfaces/devices/motor_device_interface.hpp"
 #include "ghost_v5_interfaces/devices/rotation_sensor_device_interface.hpp"
+#include "ghost_v5_interfaces/devices/digital_device_interface.hpp"
 
 
 #if GHOST_DEVICE == GHOST_JETSON
@@ -214,19 +216,19 @@ class RobotHardwareInterface
   float getInertialSensorZAccel(const std::string & sensor_name);
   float getInertialSensorHeading(const std::string & sensor_name);
 
+  /////////////////////////////////////////////////////////////
+  ///////////////// Digital Device Interfaces /////////////////
+  /////////////////////////////////////////////////////////////
+  bool getDigitalDeviceValue(const std::string & device_name);
+  void setDigitalOutputValue(const std::string & device_name, bool value);
+  void setDigitalInputValue(const std::string & device_name, bool value);
+
   //////////////////////////////////////////////////////////////
   ///////////////// Joystick Device Interfaces /////////////////
   //////////////////////////////////////////////////////////////
   std::shared_ptr<devices::JoystickDeviceData> getMainJoystickData();
   std::shared_ptr<devices::JoystickDeviceData> getPartnerJoystickData();
 
-  //////////////////////////////////////////////////////////////
-  ///////////////////////// Digital IO /////////////////////////
-  //////////////////////////////////////////////////////////////
-
-  void setDigitalIO(const std::vector<bool> & digital_io);
-  const std::vector<bool> & getDigitalIO() const;
-
   /////////////////////////////////////////////////////////
   /////////////////// Device Interfaces ///////////////////
   /////////////////////////////////////////////////////////
@@ -397,9 +399,6 @@ class RobotHardwareInterface
   bool is_autonomous_ = false;
   bool is_connected_ = false;
 
-  // Digital IO
-  std::vector<bool> digital_io_;
-
   // Serialization
   int msg_id_ = 0;
   int sensor_update_msg_length_;

01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/test/device_test_utils.hpp

@@ -1,5 +1,5 @@
 /*
- *   Copyright (c) 2024 Maxx Wilson
+ *   Copyright (c) 2024 Maxx Wilson, Xander Wilson
  *   All rights reserved.
 
  *   Permission is hereby granted, free of charge, to any person obtaining a copy
@@ -26,6 +26,7 @@
 #include <memory>
 #include <ghost_util/test_util.hpp>
 #include "ghost_v5_interfaces/devices/inertial_sensor_device_interface.hpp"
+#include "ghost_v5_interfaces/devices/digital_device_interface.hpp"
 #include "ghost_v5_interfaces/devices/joystick_device_interface.hpp"
 #include "ghost_v5_interfaces/devices/motor_device_interface.hpp"
 #include "ghost_v5_interfaces/devices/rotation_sensor_device_interface.hpp"
@@ -183,6 +184,18 @@ std::shared_ptr<devices::InertialSensorDeviceData> getRandomInertialSensorData(
   return inertial_sensor_ptr;
 }
 
+
+std::shared_ptr<devices::DigitalOutputDeviceData> getRandomDigitalOutputDeviceData(){
+  auto digital_output_ptr = std::make_shared<devices::DigitalOutputDeviceData>("test");
+  digital_output_ptr->value = ghost_util::getRandomBool();
+  return digital_output_ptr;
+}
+
+std::shared_ptr<devices::DigitalInputDeviceData> getRandomDigitalInputDeviceData(){
+  auto digital_input_ptr = std::make_shared<devices::DigitalInputDeviceData>("test");
+  digital_input_ptr->value = ghost_util::getRandomBool();
+  return digital_input_ptr;
+}
 }
 
 }

01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/util/device_config_factory_utils.hpp

@@ -42,6 +42,10 @@ namespace util
  *
  * port_configuration:
  *              use_partner_joystick = false/true
+ *      adi:
+ *          limit_switch_1:
+ *              port: A
+ *              type: DIGITAL_INPUT
  *      devices:
  *          my_motor_name_here:
  *              port: 1

01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/util/device_type_helpers.hpp

@@ -42,6 +42,8 @@ const std::unordered_map<std::string, devices::device_type_e> STRING_TO_DEVICE_T
   {"VISION_SENSOR", devices::device_type_e::VISION_SENSOR},
   {"GPS_SENSOR", devices::device_type_e::GPS_SENSOR},
   {"RADIO", devices::device_type_e::RADIO},
+  {"DIGITAL_INPUT", devices::device_type_e::DIGITAL_INPUT},
+  {"DIGITAL_OUTPUT", devices::device_type_e::DIGITAL_OUTPUT},
   {"INVALID", devices::device_type_e::INVALID},
 };
 const std::unordered_map<devices::device_type_e, std::string> DEVICE_TYPE_TO_STRING_MAP{
@@ -54,6 +56,8 @@ const std::unordered_map<devices::device_type_e, std::string> DEVICE_TYPE_TO_STR
   {devices::device_type_e::VISION_SENSOR, "VISION_SENSOR"},
   {devices::device_type_e::GPS_SENSOR, "GPS_SENSOR"},
   {devices::device_type_e::RADIO, "RADIO"},
+  {devices::device_type_e::DIGITAL_INPUT, "DIGITAL_INPUT"},
+  {devices::device_type_e::DIGITAL_OUTPUT, "DIGITAL_OUTPUT"},
   {devices::device_type_e::INVALID, "INVALID"}
 };