From b686a0546246465d87fc552c24f2d331c7d1eaf5 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Sun, 2 Jun 2024 17:48:44 -0500 Subject: [PATCH 01/97] updates copyright for bag recorder and timer nodes --- .../src/bag_recorder/bag_recorder_service.cpp | 2 +- .../src/bag_recorder/start_bag_recorder_client.cpp | 2 +- .../src/bag_recorder/stop_bag_recorder_client.cpp | 2 +- 03_ROS/ghost_ros_interfaces/src/timer/check_timer_client.cpp | 2 +- 03_ROS/ghost_ros_interfaces/src/timer/start_timer_client.cpp | 2 +- 03_ROS/ghost_ros_interfaces/src/timer/timer_service.cpp | 2 +- 6 files changed, 6 insertions(+), 6 deletions(-) diff --git a/03_ROS/ghost_ros_interfaces/src/bag_recorder/bag_recorder_service.cpp b/03_ROS/ghost_ros_interfaces/src/bag_recorder/bag_recorder_service.cpp index f1760f8f..38521e4d 100644 --- a/03_ROS/ghost_ros_interfaces/src/bag_recorder/bag_recorder_service.cpp +++ b/03_ROS/ghost_ros_interfaces/src/bag_recorder/bag_recorder_service.cpp @@ -1,5 +1,5 @@ /* - * Copyright (c) 2024 Maxx Wilson + * Copyright (c) 2024 Xander Wilson * All rights reserved. * Permission is hereby granted, free of charge, to any person obtaining a copy diff --git a/03_ROS/ghost_ros_interfaces/src/bag_recorder/start_bag_recorder_client.cpp b/03_ROS/ghost_ros_interfaces/src/bag_recorder/start_bag_recorder_client.cpp index fa8daa67..e88dd0d9 100644 --- a/03_ROS/ghost_ros_interfaces/src/bag_recorder/start_bag_recorder_client.cpp +++ b/03_ROS/ghost_ros_interfaces/src/bag_recorder/start_bag_recorder_client.cpp @@ -1,5 +1,5 @@ /* - * Copyright (c) 2024 Maxx Wilson + * Copyright (c) 2024 Xander Wilson * All rights reserved. * Permission is hereby granted, free of charge, to any person obtaining a copy diff --git a/03_ROS/ghost_ros_interfaces/src/bag_recorder/stop_bag_recorder_client.cpp b/03_ROS/ghost_ros_interfaces/src/bag_recorder/stop_bag_recorder_client.cpp index 5b0a1d37..89cd2e9e 100644 --- a/03_ROS/ghost_ros_interfaces/src/bag_recorder/stop_bag_recorder_client.cpp +++ b/03_ROS/ghost_ros_interfaces/src/bag_recorder/stop_bag_recorder_client.cpp @@ -1,5 +1,5 @@ /* - * Copyright (c) 2024 Maxx Wilson + * Copyright (c) 2024 Xander Wilson * All rights reserved. * Permission is hereby granted, free of charge, to any person obtaining a copy diff --git a/03_ROS/ghost_ros_interfaces/src/timer/check_timer_client.cpp b/03_ROS/ghost_ros_interfaces/src/timer/check_timer_client.cpp index 6ff621db..778063d4 100644 --- a/03_ROS/ghost_ros_interfaces/src/timer/check_timer_client.cpp +++ b/03_ROS/ghost_ros_interfaces/src/timer/check_timer_client.cpp @@ -1,5 +1,5 @@ /* - * Copyright (c) 2024 Maxx Wilson + * Copyright (c) 2024 Xander Wilson * All rights reserved. * Permission is hereby granted, free of charge, to any person obtaining a copy diff --git a/03_ROS/ghost_ros_interfaces/src/timer/start_timer_client.cpp b/03_ROS/ghost_ros_interfaces/src/timer/start_timer_client.cpp index e73ed03a..b2e7fd0a 100644 --- a/03_ROS/ghost_ros_interfaces/src/timer/start_timer_client.cpp +++ b/03_ROS/ghost_ros_interfaces/src/timer/start_timer_client.cpp @@ -1,5 +1,5 @@ /* - * Copyright (c) 2024 Maxx Wilson + * Copyright (c) 2024 Xander Wilson * All rights reserved. * Permission is hereby granted, free of charge, to any person obtaining a copy diff --git a/03_ROS/ghost_ros_interfaces/src/timer/timer_service.cpp b/03_ROS/ghost_ros_interfaces/src/timer/timer_service.cpp index b20dafc6..410dbdf5 100644 --- a/03_ROS/ghost_ros_interfaces/src/timer/timer_service.cpp +++ b/03_ROS/ghost_ros_interfaces/src/timer/timer_service.cpp @@ -1,5 +1,5 @@ /* - * Copyright (c) 2024 Maxx Wilson + * Copyright (c) 2024 Xander Wilson * All rights reserved. * Permission is hereby granted, free of charge, to any person obtaining a copy From 430ec172ff62cf72ca833ecb0f4070ad8be5251a Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Sun, 2 Jun 2024 18:55:39 -0500 Subject: [PATCH 02/97] adds digital IO to device interfaces --- .../include/ghost_v5_interfaces/devices/device_interfaces.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/device_interfaces.hpp b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/device_interfaces.hpp index 858912f9..be0855f0 100644 --- a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/device_interfaces.hpp +++ b/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,7 @@ enum device_type_e GPS_SENSOR, // Unsupported RADIO, // Unsupported JOYSTICK, + DIGITAL_IO, INVALID }; From f8615023b9a8dc111534486c4cb8a90c4a493cd6 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Sun, 2 Jun 2024 19:12:08 -0500 Subject: [PATCH 03/97] copies joystick interface into new digital io interface --- .../devices/digital_io_interface.hpp | 219 ++++++++++++++++++ 1 file changed, 219 insertions(+) create mode 100644 01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_io_interface.hpp diff --git a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_io_interface.hpp b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_io_interface.hpp new file mode 100644 index 00000000..db896f6b --- /dev/null +++ b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_io_interface.hpp @@ -0,0 +1,219 @@ +/* + * Copyright (c) 2024 Maxx 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 +#include "ghost_util/byte_utils.hpp" +#include "ghost_v5_interfaces/devices/device_interfaces.hpp" + +using ghost_util::packByte; +using ghost_util::unpackByte; + +namespace ghost_v5_interfaces +{ + +namespace devices +{ + +const std::string MAIN_JOYSTICK_NAME = "joy_master"; +const std::string PARTNER_JOYSTICK_NAME = "joy_partner"; + +class JoystickDeviceData : public DeviceData +{ +public: + JoystickDeviceData(std::string name) + : DeviceData(name, device_type_e::JOYSTICK) + { + } + + int getActuatorPacketSize() const override + { + return 0; + } + + int getSensorPacketSize() const override + { + return 4 * 4 + 2; + } + + // Joystick State + float left_x = 0.0; + float left_y = 0.0; + float right_x = 0.0; + float right_y = 0.0; + bool btn_a = false; + bool btn_b = false; + bool btn_x = false; + bool btn_y = false; + bool btn_r1 = false; + bool btn_r2 = false; + bool btn_l1 = false; + bool btn_l2 = false; + bool btn_u = false; + bool btn_l = false; + bool btn_r = false; + bool btn_d = false; + + void update(std::shared_ptr data_ptr) override + { + auto joy_data_ptr = data_ptr->as(); + left_x = joy_data_ptr->left_x; + left_y = joy_data_ptr->left_y; + right_x = joy_data_ptr->right_x; + right_y = joy_data_ptr->right_y; + btn_a = joy_data_ptr->btn_a; + btn_b = joy_data_ptr->btn_b; + btn_x = joy_data_ptr->btn_x; + btn_y = joy_data_ptr->btn_y; + btn_u = joy_data_ptr->btn_u; + btn_l = joy_data_ptr->btn_l; + btn_r = joy_data_ptr->btn_r; + btn_d = joy_data_ptr->btn_d; + btn_r1 = joy_data_ptr->btn_r1; + btn_r2 = joy_data_ptr->btn_r2; + btn_l1 = joy_data_ptr->btn_l1; + btn_l2 = joy_data_ptr->btn_l2; + } + + std::shared_ptr clone() const override + { + return std::make_shared(*this); + } + + bool operator==(const DeviceBase & rhs) const override + { + const JoystickDeviceData * d_rhs = dynamic_cast(&rhs); + return (d_rhs != nullptr) && (name == d_rhs->name) && (type == d_rhs->type) && + (left_x == d_rhs->left_x) && (left_y == d_rhs->left_y) && (right_x == d_rhs->right_x) && + (right_y == d_rhs->right_y) && (btn_a == d_rhs->btn_a) && (btn_b == d_rhs->btn_b) && + (btn_x == d_rhs->btn_x) && (btn_y == d_rhs->btn_y) && (btn_r1 == d_rhs->btn_r1) && + (btn_r2 == d_rhs->btn_r2) && (btn_l1 == d_rhs->btn_l1) && (btn_l2 == d_rhs->btn_l2) && + (btn_u == d_rhs->btn_u) && (btn_l == d_rhs->btn_l) && (btn_r == d_rhs->btn_r) && + (btn_d == d_rhs->btn_d); + } + + std::vector serialize(hardware_type_e hardware_type) const override + { + std::vector msg; + int byte_offset = 0; + if ((hardware_type == hardware_type_e::V5_BRAIN)) { + msg.resize(getSensorPacketSize(), 0); + auto msg_data = msg.data(); + memcpy(msg_data + byte_offset, &left_x, 4); + byte_offset += 4; + memcpy(msg_data + byte_offset, &left_y, 4); + byte_offset += 4; + memcpy(msg_data + byte_offset, &right_x, 4); + byte_offset += 4; + memcpy(msg_data + byte_offset, &right_y, 4); + byte_offset += 4; + + auto byte_pack_1 = packByte( + std::vector{ + btn_a, btn_b, btn_x, btn_y, btn_u, btn_l, btn_r, btn_d + }); + memcpy(msg_data + byte_offset, &byte_pack_1, 1); + byte_offset += 1; + + auto byte_pack_2 = packByte( + std::vector{ + btn_r1, btn_r2, btn_l1, btn_l2, 0, 0, 0, 0 + }); + memcpy(msg_data + byte_offset, &byte_pack_2, 1); + byte_offset += 1; + } + + int msg_size = + (hardware_type == + hardware_type_e::V5_BRAIN) ? getSensorPacketSize() : getActuatorPacketSize(); + checkMsgSize(msg, msg_size); + return msg; + } + + void deserialize(const std::vector & msg, hardware_type_e hardware_type) override + { + int msg_size = + (hardware_type == + hardware_type_e::V5_BRAIN) ? getActuatorPacketSize() : getSensorPacketSize(); + checkMsgSize(msg, msg_size); + + auto msg_data = msg.data(); + int byte_offset = 0; + if (hardware_type == hardware_type_e::COPROCESSOR) { + memcpy(&left_x, msg_data + byte_offset, 4); + byte_offset += 4; + memcpy(&left_y, msg_data + byte_offset, 4); + byte_offset += 4; + memcpy(&right_x, msg_data + byte_offset, 4); + byte_offset += 4; + memcpy(&right_y, msg_data + byte_offset, 4); + byte_offset += 4; + + unsigned char byte_pack_1, byte_pack_2; + memcpy(&byte_pack_1, msg_data + byte_offset, 1); + byte_offset += 1; + memcpy(&byte_pack_2, msg_data + byte_offset, 1); + byte_offset += 1; + + auto byte_vector_1 = unpackByte(byte_pack_1); + auto byte_vector_2 = unpackByte(byte_pack_2); + + btn_a = byte_vector_1[0]; + btn_b = byte_vector_1[1]; + btn_x = byte_vector_1[2]; + btn_y = byte_vector_1[3]; + btn_u = byte_vector_1[4]; + btn_l = byte_vector_1[5]; + btn_r = byte_vector_1[6]; + btn_d = byte_vector_1[7]; + btn_r1 = byte_vector_2[0]; + btn_r2 = byte_vector_2[1]; + btn_l1 = byte_vector_2[2]; + btn_l2 = byte_vector_2[3]; + } + } +}; + +class JoystickDeviceConfig : public DeviceConfig +{ +public: + bool is_partner = false; + + std::shared_ptr clone() const override + { + return std::make_shared(*this); + } + + bool operator==(const DeviceBase & rhs) const override + { + const JoystickDeviceConfig * d_rhs = dynamic_cast(&rhs); + return (d_rhs != nullptr) && (port == d_rhs->port) && (name == d_rhs->name) && + (type == d_rhs->type) && + (is_partner == d_rhs->is_partner); + } +}; + +} // namespace devices + +} // namespace ghost_v5_interfaces From 63a5392ae81ed84477c124eaaaf8c7223a2343cf Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Sun, 2 Jun 2024 21:10:41 -0500 Subject: [PATCH 04/97] adds digital IO interface header --- .../devices/digital_io_interface.hpp | 176 +++++------------- 1 file changed, 49 insertions(+), 127 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_io_interface.hpp b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_io_interface.hpp index db896f6b..8ad795ba 100644 --- a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_io_interface.hpp +++ b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_io_interface.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 @@ -36,181 +36,103 @@ namespace ghost_v5_interfaces namespace devices { -const std::string MAIN_JOYSTICK_NAME = "joy_master"; -const std::string PARTNER_JOYSTICK_NAME = "joy_partner"; - -class JoystickDeviceData : public DeviceData +class DigitalIODeviceData : public DeviceData { public: - JoystickDeviceData(std::string name) - : DeviceData(name, device_type_e::JOYSTICK) + DigitalIODeviceData(std::string name) + : DeviceData(name, device_type_e::DIGITAL_IO) { } int getActuatorPacketSize() const override { - return 0; + return 1; } int getSensorPacketSize() const override { - return 4 * 4 + 2; + return 1; } - // Joystick State - float left_x = 0.0; - float left_y = 0.0; - float right_x = 0.0; - float right_y = 0.0; - bool btn_a = false; - bool btn_b = false; - bool btn_x = false; - bool btn_y = false; - bool btn_r1 = false; - bool btn_r2 = false; - bool btn_l1 = false; - bool btn_l2 = false; - bool btn_u = false; - bool btn_l = false; - bool btn_r = false; - bool btn_d = false; + // Digital IO State + std::vector ports; + std::vector is_actuator; void update(std::shared_ptr data_ptr) override { - auto joy_data_ptr = data_ptr->as(); - left_x = joy_data_ptr->left_x; - left_y = joy_data_ptr->left_y; - right_x = joy_data_ptr->right_x; - right_y = joy_data_ptr->right_y; - btn_a = joy_data_ptr->btn_a; - btn_b = joy_data_ptr->btn_b; - btn_x = joy_data_ptr->btn_x; - btn_y = joy_data_ptr->btn_y; - btn_u = joy_data_ptr->btn_u; - btn_l = joy_data_ptr->btn_l; - btn_r = joy_data_ptr->btn_r; - btn_d = joy_data_ptr->btn_d; - btn_r1 = joy_data_ptr->btn_r1; - btn_r2 = joy_data_ptr->btn_r2; - btn_l1 = joy_data_ptr->btn_l1; - btn_l2 = joy_data_ptr->btn_l2; + auto digital_data_ptr = data_ptr->as(); + ports = digital_data_ptr->ports; } std::shared_ptr clone() const override { - return std::make_shared(*this); + return std::make_shared(*this); } bool operator==(const DeviceBase & rhs) const override { - const JoystickDeviceData * d_rhs = dynamic_cast(&rhs); - return (d_rhs != nullptr) && (name == d_rhs->name) && (type == d_rhs->type) && - (left_x == d_rhs->left_x) && (left_y == d_rhs->left_y) && (right_x == d_rhs->right_x) && - (right_y == d_rhs->right_y) && (btn_a == d_rhs->btn_a) && (btn_b == d_rhs->btn_b) && - (btn_x == d_rhs->btn_x) && (btn_y == d_rhs->btn_y) && (btn_r1 == d_rhs->btn_r1) && - (btn_r2 == d_rhs->btn_r2) && (btn_l1 == d_rhs->btn_l1) && (btn_l2 == d_rhs->btn_l2) && - (btn_u == d_rhs->btn_u) && (btn_l == d_rhs->btn_l) && (btn_r == d_rhs->btn_r) && - (btn_d == d_rhs->btn_d); + const DigitalIODeviceData * d_rhs = dynamic_cast(&rhs); + if (d_rhs == nullptr || ports.size() != d_rhs->ports.size()) return false; + for (int i = 0; i < ports.size(); i++) { + if (ports[i] != d_rhs->ports[i]) return false; + if (is_actuator[i] != d_rhs->is_actuator[i]) return false; + } + return true; } std::vector serialize(hardware_type_e hardware_type) const override { - std::vector msg; - int byte_offset = 0; - if ((hardware_type == hardware_type_e::V5_BRAIN)) { - msg.resize(getSensorPacketSize(), 0); - auto msg_data = msg.data(); - memcpy(msg_data + byte_offset, &left_x, 4); - byte_offset += 4; - memcpy(msg_data + byte_offset, &left_y, 4); - byte_offset += 4; - memcpy(msg_data + byte_offset, &right_x, 4); - byte_offset += 4; - memcpy(msg_data + byte_offset, &right_y, 4); - byte_offset += 4; - - auto byte_pack_1 = packByte( - std::vector{ - btn_a, btn_b, btn_x, btn_y, btn_u, btn_l, btn_r, btn_d - }); - memcpy(msg_data + byte_offset, &byte_pack_1, 1); - byte_offset += 1; - - auto byte_pack_2 = packByte( - std::vector{ - btn_r1, btn_r2, btn_l1, btn_l2, 0, 0, 0, 0 - }); - memcpy(msg_data + byte_offset, &byte_pack_2, 1); - byte_offset += 1; + std::vector msg(getActuatorPacketSize()); + unsigned char byte_pack = packByte(ports); + unsigned char write_mask; + + //TODO(xander): change packByte(is_actuator) and ~packByte(is_actuator) into field variables + if (hardware_type == hardware_type_e::V5_BRAIN) { + write_mask = ~packByte(is_actuator); } + else if (hardware_type == hardware_type_e::COPROCESSOR) { + write_mask = packByte(is_actuator); + } + + *(msg.data()) = byte_pack & write_mask; - int msg_size = - (hardware_type == - hardware_type_e::V5_BRAIN) ? getSensorPacketSize() : getActuatorPacketSize(); - checkMsgSize(msg, msg_size); + checkMsgSize(msg, getActuatorPacketSize()); return msg; } void deserialize(const std::vector & msg, hardware_type_e hardware_type) override { - int msg_size = - (hardware_type == - hardware_type_e::V5_BRAIN) ? getActuatorPacketSize() : getSensorPacketSize(); - checkMsgSize(msg, msg_size); - + checkMsgSize(msg, getActuatorPacketSize()); auto msg_data = msg.data(); - int byte_offset = 0; - if (hardware_type == hardware_type_e::COPROCESSOR) { - memcpy(&left_x, msg_data + byte_offset, 4); - byte_offset += 4; - memcpy(&left_y, msg_data + byte_offset, 4); - byte_offset += 4; - memcpy(&right_x, msg_data + byte_offset, 4); - byte_offset += 4; - memcpy(&right_y, msg_data + byte_offset, 4); - byte_offset += 4; - - unsigned char byte_pack_1, byte_pack_2; - memcpy(&byte_pack_1, msg_data + byte_offset, 1); - byte_offset += 1; - memcpy(&byte_pack_2, msg_data + byte_offset, 1); - byte_offset += 1; - - auto byte_vector_1 = unpackByte(byte_pack_1); - auto byte_vector_2 = unpackByte(byte_pack_2); - - btn_a = byte_vector_1[0]; - btn_b = byte_vector_1[1]; - btn_x = byte_vector_1[2]; - btn_y = byte_vector_1[3]; - btn_u = byte_vector_1[4]; - btn_l = byte_vector_1[5]; - btn_r = byte_vector_1[6]; - btn_d = byte_vector_1[7]; - btn_r1 = byte_vector_2[0]; - btn_r2 = byte_vector_2[1]; - btn_l1 = byte_vector_2[2]; - btn_l2 = byte_vector_2[3]; + unsigned char byte_pack; + unsigned char read_mask; + + //TODO(xander): change packByte(is_actuator) and ~packByte(is_actuator) into field variables + if (hardware_type == hardware_type_e::V5_BRAIN) { + read_mask = packByte(is_actuator); + } + else if (hardware_type == hardware_type_e::COPROCESSOR) { + read_mask = ~packByte(is_actuator); } + + memcpy(&byte_pack, msg_data, 1); + ports = unpackByte(byte_pack & read_mask); } }; -class JoystickDeviceConfig : public DeviceConfig +class DigitalIODeviceConfig : public DeviceConfig { public: - bool is_partner = false; - std::shared_ptr clone() const override { - return std::make_shared(*this); + return std::make_shared(*this); } bool operator==(const DeviceBase & rhs) const override { - const JoystickDeviceConfig * d_rhs = dynamic_cast(&rhs); + const DigitalIODeviceConfig * d_rhs = dynamic_cast(&rhs); return (d_rhs != nullptr) && (port == d_rhs->port) && (name == d_rhs->name) && - (type == d_rhs->type) && - (is_partner == d_rhs->is_partner); + (type == d_rhs->type); } }; From 7d0c74360461823b9f5b02e8c734b477c56955f2 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Sun, 2 Jun 2024 22:05:54 -0500 Subject: [PATCH 05/97] fixes deserialize write behavior --- .../ghost_v5_interfaces/devices/digital_io_interface.hpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_io_interface.hpp b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_io_interface.hpp index 8ad795ba..1dc9fe99 100644 --- a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_io_interface.hpp +++ b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_io_interface.hpp @@ -116,7 +116,11 @@ class DigitalIODeviceData : public DeviceData } memcpy(&byte_pack, msg_data, 1); - ports = unpackByte(byte_pack & read_mask); + auto byte_vector = unpackByte(byte_pack & read_mask); + + for (int i = 0; i < byte_vector.size(); i++) { + ports[i] = ports[i] || byte_vector[i]; + } } }; From f8b3bc11d4337914dfcb12adfbc8dfd40fd935f1 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Sun, 2 Jun 2024 22:18:49 -0500 Subject: [PATCH 06/97] renames digital io config --- .../ghost_v5_interfaces/devices/digital_io_interface.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_io_interface.hpp b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_io_interface.hpp index 1dc9fe99..7db9c15f 100644 --- a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_io_interface.hpp +++ b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_io_interface.hpp @@ -124,17 +124,17 @@ class DigitalIODeviceData : public DeviceData } }; -class DigitalIODeviceConfig : public DeviceConfig +class DigitalIOConfig : public DeviceConfig { public: std::shared_ptr clone() const override { - return std::make_shared(*this); + return std::make_shared(*this); } bool operator==(const DeviceBase & rhs) const override { - const DigitalIODeviceConfig * d_rhs = dynamic_cast(&rhs); + const DigitalIOConfig * d_rhs = dynamic_cast(&rhs); return (d_rhs != nullptr) && (port == d_rhs->port) && (name == d_rhs->name) && (type == d_rhs->type); } From ae75931208e830e6ff1b85e331fe6d947e5461df Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Mon, 3 Jun 2024 05:23:41 -0500 Subject: [PATCH 07/97] fixes deserialize behavior --- .../devices/digital_io_interface.hpp | 15 ++++----------- 1 file changed, 4 insertions(+), 11 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_io_interface.hpp b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_io_interface.hpp index 7db9c15f..62aa8f4b 100644 --- a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_io_interface.hpp +++ b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_io_interface.hpp @@ -105,21 +105,14 @@ class DigitalIODeviceData : public DeviceData checkMsgSize(msg, getActuatorPacketSize()); auto msg_data = msg.data(); unsigned char byte_pack; - unsigned char read_mask; - - //TODO(xander): change packByte(is_actuator) and ~packByte(is_actuator) into field variables - if (hardware_type == hardware_type_e::V5_BRAIN) { - read_mask = packByte(is_actuator); - } - else if (hardware_type == hardware_type_e::COPROCESSOR) { - read_mask = ~packByte(is_actuator); - } memcpy(&byte_pack, msg_data, 1); - auto byte_vector = unpackByte(byte_pack & read_mask); + auto byte_vector = unpackByte(byte_pack); for (int i = 0; i < byte_vector.size(); i++) { - ports[i] = ports[i] || byte_vector[i]; + if (hardware_type == hardware_type_e::V5_BRAIN && !is_actuator[i]) continue; + else if (hardware_type == hardware_type_e::COPROCESSOR && is_actuator[i]) continue; + ports[i] = byte_vector[i]; } } }; From 75366f144488a38af671c441fa0aadc92e6c63c7 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Tue, 4 Jun 2024 07:02:15 -0500 Subject: [PATCH 08/97] updates device type enum --- .../include/ghost_v5_interfaces/devices/device_interfaces.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/device_interfaces.hpp b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/device_interfaces.hpp index be0855f0..0037bd6f 100644 --- a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/device_interfaces.hpp +++ b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/device_interfaces.hpp @@ -46,7 +46,8 @@ enum device_type_e GPS_SENSOR, // Unsupported RADIO, // Unsupported JOYSTICK, - DIGITAL_IO, + DIGITAL_SENSOR, + DIGITAL_ACTUATOR, INVALID }; From b5e5235faf940d661cd4b172b7f3dec71fef96b3 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Tue, 4 Jun 2024 07:07:22 -0500 Subject: [PATCH 09/97] commits base, unfinished load libraries --- .../util/load_digital_io_config_yaml.hpp | 45 +++++++++++++ .../src/util/load_digital_io_config_yaml.cpp | 64 +++++++++++++++++++ 2 files changed, 109 insertions(+) create mode 100644 01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/util/load_digital_io_config_yaml.hpp create mode 100644 01_Libraries/ghost_v5_interfaces/src/util/load_digital_io_config_yaml.cpp diff --git a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/util/load_digital_io_config_yaml.hpp b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/util/load_digital_io_config_yaml.hpp new file mode 100644 index 00000000..1e785b80 --- /dev/null +++ b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/util/load_digital_io_config_yaml.hpp @@ -0,0 +1,45 @@ +/* + * 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 +#include +#include "ghost_v5_interfaces/devices/device_interfaces.hpp" +#include "ghost_v5_interfaces/devices/digital_io_interface.hpp" +#include "yaml-cpp/yaml.h" + +namespace ghost_v5_interfaces +{ + +namespace util +{ + +void loadDigitalIOConfigFromYAML( + YAML::Node node, + std::shared_ptr device_io_config_ptr, + bool verbose = false); + +} // namespace util + +} // namespace ghost_v5_interfaces diff --git a/01_Libraries/ghost_v5_interfaces/src/util/load_digital_io_config_yaml.cpp b/01_Libraries/ghost_v5_interfaces/src/util/load_digital_io_config_yaml.cpp new file mode 100644 index 00000000..cd283583 --- /dev/null +++ b/01_Libraries/ghost_v5_interfaces/src/util/load_digital_io_config_yaml.cpp @@ -0,0 +1,64 @@ +/* + * 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. + */ + +#include +#include + +using ghost_util::loadYAMLParam; +using namespace ghost_v5_interfaces::devices; +namespace ghost_v5_interfaces +{ + +namespace util +{ + +void loadDigitalIOConfigFromYAML( + YAML::Node node, + std::shared_ptr digital_io_config_ptr, + bool verbose) +{ + // Get ADI node + auto adi_node = node["ADI"]; + if (!adi_node || !adi_node.IsMap()) { + //TODO(xander): set digital_io_config_ptr to default values and return + } + + for (const auto &device_node : adi_node) { + //set ports and is_actuator mask + } + + // Set base attributes + digital_io_config_ptr->type = device_type_e::DIGITAL_IO; + + + // example of param loading process + if (!loadYAMLParam(adi_node, "port", digital_io_config_ptr->port, verbose)) { + throw std::runtime_error( + "[loadDigitalIOConfigFromYAML] Error: Port not specified for Rotation Sensor " + sensor_name + + "!"); + } +} + +} // namespace util + +} // namespace ghost_v5_interfaces From 3cfb57702b2a191c9dc5c846a6120d85e1a56498 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Tue, 4 Jun 2024 07:42:59 -0500 Subject: [PATCH 10/97] changes digital io into digital sensor --- ...pp => digital_sensor_device_interface.hpp} | 68 +++++-------------- 1 file changed, 16 insertions(+), 52 deletions(-) rename 01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/{digital_io_interface.hpp => digital_sensor_device_interface.hpp} (51%) diff --git a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_io_interface.hpp b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_sensor_device_interface.hpp similarity index 51% rename from 01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_io_interface.hpp rename to 01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_sensor_device_interface.hpp index 62aa8f4b..ed716ff9 100644 --- a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_io_interface.hpp +++ b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_sensor_device_interface.hpp @@ -24,29 +24,25 @@ #pragma once #include -#include "ghost_util/byte_utils.hpp" #include "ghost_v5_interfaces/devices/device_interfaces.hpp" -using ghost_util::packByte; -using ghost_util::unpackByte; - namespace ghost_v5_interfaces { namespace devices { -class DigitalIODeviceData : public DeviceData +class DigitalSensorDeviceData : public DeviceData { public: - DigitalIODeviceData(std::string name) - : DeviceData(name, device_type_e::DIGITAL_IO) + DigitalSensorDeviceData(std::string name) + : DeviceData(name, device_type_e::DIGITAL_SENSOR) { } int getActuatorPacketSize() const override { - return 1; + return 0; } int getSensorPacketSize() const override @@ -54,80 +50,48 @@ class DigitalIODeviceData : public DeviceData return 1; } - // Digital IO State - std::vector ports; - std::vector is_actuator; + bool value; void update(std::shared_ptr data_ptr) override { - auto digital_data_ptr = data_ptr->as(); - ports = digital_data_ptr->ports; + auto digital_data_ptr = data_ptr->as(); + value = digital_data_ptr->value; } std::shared_ptr clone() const override { - return std::make_shared(*this); + return std::make_shared(*this); } bool operator==(const DeviceBase & rhs) const override { - const DigitalIODeviceData * d_rhs = dynamic_cast(&rhs); - if (d_rhs == nullptr || ports.size() != d_rhs->ports.size()) return false; - for (int i = 0; i < ports.size(); i++) { - if (ports[i] != d_rhs->ports[i]) return false; - if (is_actuator[i] != d_rhs->is_actuator[i]) return false; - } - return true; + const DigitalSensorDeviceData * d_rhs = dynamic_cast(&rhs); + return (d_rhs != nullptr && value == d_rhs->value); } std::vector serialize(hardware_type_e hardware_type) const override { - std::vector msg(getActuatorPacketSize()); - unsigned char byte_pack = packByte(ports); - unsigned char write_mask; - - //TODO(xander): change packByte(is_actuator) and ~packByte(is_actuator) into field variables - if (hardware_type == hardware_type_e::V5_BRAIN) { - write_mask = ~packByte(is_actuator); - } - else if (hardware_type == hardware_type_e::COPROCESSOR) { - write_mask = packByte(is_actuator); - } - - *(msg.data()) = byte_pack & write_mask; - - checkMsgSize(msg, getActuatorPacketSize()); - return msg; + return {(unsigned char) ((hardware_type == V5_BRAIN && value) ? 0x1 : 0x0)}; } void deserialize(const std::vector & msg, hardware_type_e hardware_type) override { - checkMsgSize(msg, getActuatorPacketSize()); - auto msg_data = msg.data(); - unsigned char byte_pack; - - memcpy(&byte_pack, msg_data, 1); - auto byte_vector = unpackByte(byte_pack); - - for (int i = 0; i < byte_vector.size(); i++) { - if (hardware_type == hardware_type_e::V5_BRAIN && !is_actuator[i]) continue; - else if (hardware_type == hardware_type_e::COPROCESSOR && is_actuator[i]) continue; - ports[i] = byte_vector[i]; - } + if (hardware_type == V5_BRAIN) return; + value = (msg.data()[0] == 0x1); } }; -class DigitalIOConfig : public DeviceConfig +class DigitalSensorDeviceConfig : public DeviceConfig { public: std::shared_ptr clone() const override { - return std::make_shared(*this); + return std::make_shared(*this); } bool operator==(const DeviceBase & rhs) const override { - const DigitalIOConfig * d_rhs = dynamic_cast(&rhs); + const DigitalSensorDeviceConfig * d_rhs = dynamic_cast(&rhs); return (d_rhs != nullptr) && (port == d_rhs->port) && (name == d_rhs->name) && (type == d_rhs->type); } From 93c6fef1c89afc6988721e5fce3dd5427d4f109f Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Tue, 4 Jun 2024 07:44:50 -0500 Subject: [PATCH 11/97] adds digital actuator interface --- .../digital_actuator_device_interface.hpp | 102 ++++++++++++++++++ 1 file changed, 102 insertions(+) create mode 100644 01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_actuator_device_interface.hpp diff --git a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_actuator_device_interface.hpp b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_actuator_device_interface.hpp new file mode 100644 index 00000000..cbaed588 --- /dev/null +++ b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_actuator_device_interface.hpp @@ -0,0 +1,102 @@ +/* + * 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 +#include "ghost_v5_interfaces/devices/device_interfaces.hpp" + +namespace ghost_v5_interfaces +{ + +namespace devices +{ + +class DigitalActuatorDeviceData : public DeviceData +{ +public: + DigitalActuatorDeviceData(std::string name) + : DeviceData(name, device_type_e::DIGITAL_ACTUATOR) + { + } + + int getActuatorPacketSize() const override + { + return 1; + } + + int getSensorPacketSize() const override + { + return 0; + } + + bool value; + + void update(std::shared_ptr data_ptr) override + { + auto digital_data_ptr = data_ptr->as(); + value = digital_data_ptr->value; + } + + std::shared_ptr clone() const override + { + return std::make_shared(*this); + } + + bool operator==(const DeviceBase & rhs) const override + { + const DigitalActuatorDeviceData * d_rhs = dynamic_cast(&rhs); + return (d_rhs != nullptr && value == d_rhs->value); + } + + std::vector serialize(hardware_type_e hardware_type) const override + { + return {(unsigned char) ((hardware_type == COPROCESSOR && value) ? 0x1 : 0x0)}; + } + + void deserialize(const std::vector & msg, hardware_type_e hardware_type) override + { + if (hardware_type == COPROCESSOR) return; + value = (msg.data()[0] == 0x1); + } +}; + +class DigitalActuatorDeviceConfig : public DeviceConfig +{ +public: + std::shared_ptr clone() const override + { + return std::make_shared(*this); + } + + bool operator==(const DeviceBase & rhs) const override + { + const DigitalActuatorDeviceConfig * d_rhs = dynamic_cast(&rhs); + return (d_rhs != nullptr) && (port == d_rhs->port) && (name == d_rhs->name) && + (type == d_rhs->type); + } +}; + +} // namespace devices + +} // namespace ghost_v5_interfaces From 4147a202107bc27b1753dd3cbeb02435dd955d30 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Tue, 4 Jun 2024 07:55:58 -0500 Subject: [PATCH 12/97] recombines digital device types --- .../include/ghost_v5_interfaces/devices/device_interfaces.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/device_interfaces.hpp b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/device_interfaces.hpp index 0037bd6f..f7ba7335 100644 --- a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/device_interfaces.hpp +++ b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/device_interfaces.hpp @@ -46,8 +46,7 @@ enum device_type_e GPS_SENSOR, // Unsupported RADIO, // Unsupported JOYSTICK, - DIGITAL_SENSOR, - DIGITAL_ACTUATOR, + DIGITAL, INVALID }; From 8a37ecf5c89f9cd1a552ea3b3682c50f654a2c85 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Tue, 4 Jun 2024 08:18:56 -0500 Subject: [PATCH 13/97] adds digital_io_type enum --- .../ghost_v5_interfaces/devices/device_interfaces.hpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/device_interfaces.hpp b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/device_interfaces.hpp index f7ba7335..cab9bf31 100644 --- a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/device_interfaces.hpp +++ b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/device_interfaces.hpp @@ -50,6 +50,11 @@ enum device_type_e INVALID }; +enum digital_io_type_e { + ACTUATOR, + SENSOR +}; + enum hardware_type_e { COPROCESSOR, From efa5e5663c58a43a702b80aeb20bc9d1b0a7a985 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Tue, 4 Jun 2024 08:20:09 -0500 Subject: [PATCH 14/97] recombines digital device interface headers --- ...rface.hpp => digital_device_interface.hpp} | 41 ++++--- .../digital_sensor_device_interface.hpp | 102 ------------------ 2 files changed, 27 insertions(+), 116 deletions(-) rename 01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/{digital_actuator_device_interface.hpp => digital_device_interface.hpp} (66%) delete mode 100644 01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_sensor_device_interface.hpp diff --git a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_actuator_device_interface.hpp b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_device_interface.hpp similarity index 66% rename from 01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_actuator_device_interface.hpp rename to 01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_device_interface.hpp index cbaed588..7ef11efe 100644 --- a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_actuator_device_interface.hpp +++ b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_device_interface.hpp @@ -32,69 +32,82 @@ namespace ghost_v5_interfaces namespace devices { -class DigitalActuatorDeviceData : public DeviceData +class DigitalDeviceData : public DeviceData { public: - DigitalActuatorDeviceData(std::string name) - : DeviceData(name, device_type_e::DIGITAL_ACTUATOR) + DigitalDeviceData(std::string name) + : DeviceData(name, device_type_e::DIGITAL) { } int getActuatorPacketSize() const override { - return 1; + return (io_type == ACTUATOR) ? 1 : 0; } int getSensorPacketSize() const override { - return 0; + return (io_type == SENSOR) ? 1 : 0; } + digital_io_type_e io_type; bool value; void update(std::shared_ptr data_ptr) override { - auto digital_data_ptr = data_ptr->as(); + auto digital_data_ptr = data_ptr->as(); value = digital_data_ptr->value; } std::shared_ptr clone() const override { - return std::make_shared(*this); + return std::make_shared(*this); } bool operator==(const DeviceBase & rhs) const override { - const DigitalActuatorDeviceData * d_rhs = dynamic_cast(&rhs); + const DigitalDeviceData * d_rhs = dynamic_cast(&rhs); return (d_rhs != nullptr && value == d_rhs->value); } std::vector serialize(hardware_type_e hardware_type) const override { - return {(unsigned char) ((hardware_type == COPROCESSOR && value) ? 0x1 : 0x0)}; + std::vector msg(1); + bool valid = hardware_type == V5_BRAIN && io_type == SENSOR + || hardware_type == COPROCESSOR && io_type == ACTUATOR; + if (valid) { + msg.push_back((unsigned char) value); + } + return msg; } void deserialize(const std::vector & msg, hardware_type_e hardware_type) override { - if (hardware_type == COPROCESSOR) return; - value = (msg.data()[0] == 0x1); + bool valid = hardware_type == V5_BRAIN && io_type == SENSOR + || hardware_type == COPROCESSOR && io_type == ACTUATOR; + if (!valid) { + return; + } + value = ((bool) msg.data()[0]); } }; -class DigitalActuatorDeviceConfig : public DeviceConfig +class DigitalDeviceConfig : public DeviceConfig { public: std::shared_ptr clone() const override { - return std::make_shared(*this); + return std::make_shared(*this); } bool operator==(const DeviceBase & rhs) const override { - const DigitalActuatorDeviceConfig * d_rhs = dynamic_cast(&rhs); + const DigitalDeviceConfig * d_rhs = dynamic_cast(&rhs); return (d_rhs != nullptr) && (port == d_rhs->port) && (name == d_rhs->name) && (type == d_rhs->type); } + + digital_io_type_e io_type; }; } // namespace devices diff --git a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_sensor_device_interface.hpp b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_sensor_device_interface.hpp deleted file mode 100644 index ed716ff9..00000000 --- a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_sensor_device_interface.hpp +++ /dev/null @@ -1,102 +0,0 @@ -/* - * 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 -#include "ghost_v5_interfaces/devices/device_interfaces.hpp" - -namespace ghost_v5_interfaces -{ - -namespace devices -{ - -class DigitalSensorDeviceData : public DeviceData -{ -public: - DigitalSensorDeviceData(std::string name) - : DeviceData(name, device_type_e::DIGITAL_SENSOR) - { - } - - int getActuatorPacketSize() const override - { - return 0; - } - - int getSensorPacketSize() const override - { - return 1; - } - - bool value; - - void update(std::shared_ptr data_ptr) override - { - auto digital_data_ptr = data_ptr->as(); - value = digital_data_ptr->value; - } - - std::shared_ptr clone() const override - { - return std::make_shared(*this); - } - - bool operator==(const DeviceBase & rhs) const override - { - const DigitalSensorDeviceData * d_rhs = dynamic_cast(&rhs); - return (d_rhs != nullptr && value == d_rhs->value); - } - - std::vector serialize(hardware_type_e hardware_type) const override - { - return {(unsigned char) ((hardware_type == V5_BRAIN && value) ? 0x1 : 0x0)}; - } - - void deserialize(const std::vector & msg, hardware_type_e hardware_type) override - { - if (hardware_type == V5_BRAIN) return; - value = (msg.data()[0] == 0x1); - } -}; - -class DigitalSensorDeviceConfig : public DeviceConfig -{ -public: - std::shared_ptr clone() const override - { - return std::make_shared(*this); - } - - bool operator==(const DeviceBase & rhs) const override - { - const DigitalSensorDeviceConfig * d_rhs = dynamic_cast(&rhs); - return (d_rhs != nullptr) && (port == d_rhs->port) && (name == d_rhs->name) && - (type == d_rhs->type); - } -}; - -} // namespace devices - -} // namespace ghost_v5_interfaces From 3102c59dc13c682fe41018bc5d6e567161b7fd9b Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Tue, 4 Jun 2024 08:24:58 -0500 Subject: [PATCH 15/97] renames yaml loading helper --- ...nfig_yaml.hpp => load_digital_device_config_yaml.hpp} | 7 ++++--- ...nfig_yaml.cpp => load_digital_device_config_yaml.cpp} | 9 +++++---- 2 files changed, 9 insertions(+), 7 deletions(-) rename 01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/util/{load_digital_io_config_yaml.hpp => load_digital_device_config_yaml.hpp} (88%) rename 01_Libraries/ghost_v5_interfaces/src/util/{load_digital_io_config_yaml.cpp => load_digital_device_config_yaml.cpp} (86%) diff --git a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/util/load_digital_io_config_yaml.hpp b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/util/load_digital_device_config_yaml.hpp similarity index 88% rename from 01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/util/load_digital_io_config_yaml.hpp rename to 01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/util/load_digital_device_config_yaml.hpp index 1e785b80..4de83e98 100644 --- a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/util/load_digital_io_config_yaml.hpp +++ b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/util/load_digital_device_config_yaml.hpp @@ -26,7 +26,7 @@ #include #include #include "ghost_v5_interfaces/devices/device_interfaces.hpp" -#include "ghost_v5_interfaces/devices/digital_io_interface.hpp" +#include "ghost_v5_interfaces/devices/digital_device_interface.hpp" #include "yaml-cpp/yaml.h" namespace ghost_v5_interfaces @@ -35,9 +35,10 @@ namespace ghost_v5_interfaces namespace util { -void loadDigitalIOConfigFromYAML( +void loadDigitalDeviceConfigFromYAML( YAML::Node node, - std::shared_ptr device_io_config_ptr, + std::string device_name, + std::shared_ptr device_config_ptr, bool verbose = false); } // namespace util diff --git a/01_Libraries/ghost_v5_interfaces/src/util/load_digital_io_config_yaml.cpp b/01_Libraries/ghost_v5_interfaces/src/util/load_digital_device_config_yaml.cpp similarity index 86% rename from 01_Libraries/ghost_v5_interfaces/src/util/load_digital_io_config_yaml.cpp rename to 01_Libraries/ghost_v5_interfaces/src/util/load_digital_device_config_yaml.cpp index cd283583..e28dd415 100644 --- a/01_Libraries/ghost_v5_interfaces/src/util/load_digital_io_config_yaml.cpp +++ b/01_Libraries/ghost_v5_interfaces/src/util/load_digital_device_config_yaml.cpp @@ -22,7 +22,7 @@ */ #include -#include +#include using ghost_util::loadYAMLParam; using namespace ghost_v5_interfaces::devices; @@ -32,9 +32,10 @@ namespace ghost_v5_interfaces namespace util { -void loadDigitalIOConfigFromYAML( +void loadDigitalDeviceConfigFromYAML( YAML::Node node, - std::shared_ptr digital_io_config_ptr, + std::string device_name, + std::shared_ptr device_config_ptr, bool verbose) { // Get ADI node @@ -54,7 +55,7 @@ void loadDigitalIOConfigFromYAML( // example of param loading process if (!loadYAMLParam(adi_node, "port", digital_io_config_ptr->port, verbose)) { throw std::runtime_error( - "[loadDigitalIOConfigFromYAML] Error: Port not specified for Rotation Sensor " + sensor_name + + "[loadDigitalDeviceConfigFromYAML] Error: Port not specified for Rotation Sensor " + sensor_name + "!"); } } From 5ce2712c061cfd7f6b886374b0243cce605cca40 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Tue, 4 Jun 2024 09:40:57 -0500 Subject: [PATCH 16/97] adds serial config to digital device --- .../devices/digital_device_interface.hpp | 32 +++++++++++++------ 1 file changed, 23 insertions(+), 9 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_device_interface.hpp b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_device_interface.hpp index 7ef11efe..c1a1bf45 100644 --- a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_device_interface.hpp +++ b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_device_interface.hpp @@ -35,22 +35,34 @@ namespace devices class DigitalDeviceData : public DeviceData { public: - DigitalDeviceData(std::string name) + struct SerialConfig + { + SerialConfig() + { + } + + bool operator== (const SerialConfig &rhs) const { + return io_type == rhs.io_type; + } + + digital_io_type_e io_type; + }; + + DigitalDeviceData(std::string name, SerialConfig serial_config = SerialConfig()) : DeviceData(name, device_type_e::DIGITAL) { } int getActuatorPacketSize() const override { - return (io_type == ACTUATOR) ? 1 : 0; + return 1; } int getSensorPacketSize() const override { - return (io_type == SENSOR) ? 1 : 0; + return 1; } - digital_io_type_e io_type; bool value; void update(std::shared_ptr data_ptr) override @@ -73,8 +85,8 @@ class DigitalDeviceData : public DeviceData std::vector serialize(hardware_type_e hardware_type) const override { std::vector msg(1); - bool valid = hardware_type == V5_BRAIN && io_type == SENSOR - || hardware_type == COPROCESSOR && io_type == ACTUATOR; + bool valid = hardware_type == V5_BRAIN && serial_config_.io_type == SENSOR + || hardware_type == COPROCESSOR && serial_config_.io_type == ACTUATOR; if (valid) { msg.push_back((unsigned char) value); } @@ -83,13 +95,15 @@ class DigitalDeviceData : public DeviceData void deserialize(const std::vector & msg, hardware_type_e hardware_type) override { - bool valid = hardware_type == V5_BRAIN && io_type == SENSOR - || hardware_type == COPROCESSOR && io_type == ACTUATOR; + bool valid = hardware_type == V5_BRAIN && serial_config_.io_type == SENSOR + || hardware_type == COPROCESSOR && serial_config_.io_type == ACTUATOR; if (!valid) { return; } value = ((bool) msg.data()[0]); } + + SerialConfig serial_config_; }; class DigitalDeviceConfig : public DeviceConfig @@ -107,7 +121,7 @@ class DigitalDeviceConfig : public DeviceConfig (type == d_rhs->type); } - digital_io_type_e io_type; + DigitalDeviceData::SerialConfig serial_config; }; } // namespace devices From b59dcb97108d940781064d23eff0c8170b9ffd8e Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Tue, 4 Jun 2024 09:48:32 -0500 Subject: [PATCH 17/97] adds yaml loading --- .../util/load_digital_device_config_yaml.cpp | 56 +++++++++++++++---- 1 file changed, 44 insertions(+), 12 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/src/util/load_digital_device_config_yaml.cpp b/01_Libraries/ghost_v5_interfaces/src/util/load_digital_device_config_yaml.cpp index e28dd415..41797402 100644 --- a/01_Libraries/ghost_v5_interfaces/src/util/load_digital_device_config_yaml.cpp +++ b/01_Libraries/ghost_v5_interfaces/src/util/load_digital_device_config_yaml.cpp @@ -38,26 +38,58 @@ void loadDigitalDeviceConfigFromYAML( std::shared_ptr device_config_ptr, bool verbose) { - // Get ADI node - auto adi_node = node["ADI"]; - if (!adi_node || !adi_node.IsMap()) { - //TODO(xander): set digital_io_config_ptr to default values and return + // Get device node + auto device_node = node["devices"][device_name]; + if (!device_node) { + throw std::runtime_error( + "[loadDigitalDeviceConfigFromYAML] Error: Device Sensor " + device_name + + " not found!"); } - for (const auto &device_node : adi_node) { - //set ports and is_actuator mask + // Set base attributes + device_config_ptr->name = device_name; + device_config_ptr->type = device_type_e::DIGITAL; + + // Get port + char port; + if (!loadYAMLParam(device_node, "port", port, verbose)) { + throw std::runtime_error( + "[loadDigitalDeviceConfigFromYAML] Error: Port not specified for Digital Device " + + device_name + "!"); } - // Set base attributes - digital_io_config_ptr->type = device_type_e::DIGITAL_IO; + // Validate port + const std::vector valid_ports = {'A', 'B', 'C', 'D', 'E', 'F', 'G', 'H'}; + if (std::count(valid_ports.begin(), valid_ports.end(), port) == 0) { + throw std::runtime_error( + "[loadDigitalDeviceConfigFromYAML] Error: Invalid port specified for Digital Device " + + device_name + "!"); + } + + // Convert port from A-H to 21-28 (for compatibility with existing interfaces) + port -= 'A'; + port += 21; + + // Set port in device config + device_config_ptr->port = (int) port; + // Load IO type + std::string type; + if (!loadYAMLParam(device_node, "type", type, verbose)) { + throw std::runtime_error( + "[loadDigitalDeviceConfigFromYAML] Error: Type not specified for Digital Device " + + device_name + "!"); + } - // example of param loading process - if (!loadYAMLParam(adi_node, "port", digital_io_config_ptr->port, verbose)) { + // Validate IO type + if (type != "SENSOR" && type != "ACTUATOR") { throw std::runtime_error( - "[loadDigitalDeviceConfigFromYAML] Error: Port not specified for Rotation Sensor " + sensor_name + - "!"); + "[loadDigitalDeviceConfigFromYAML] Error: Invalid type specified for Digital Device " + + device_name + "!"); } + + // Set IO type in device config + device_config_ptr->serial_config.io_type = (type == "SENSOR") ? SENSOR : ACTUATOR; } } // namespace util From fcaf7c3849e16891aa349072ded70fc3416c5b9e Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Tue, 4 Jun 2024 10:13:53 -0500 Subject: [PATCH 18/97] changes io type name in YAML loading --- .../src/util/load_digital_device_config_yaml.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/01_Libraries/ghost_v5_interfaces/src/util/load_digital_device_config_yaml.cpp b/01_Libraries/ghost_v5_interfaces/src/util/load_digital_device_config_yaml.cpp index 41797402..85bed806 100644 --- a/01_Libraries/ghost_v5_interfaces/src/util/load_digital_device_config_yaml.cpp +++ b/01_Libraries/ghost_v5_interfaces/src/util/load_digital_device_config_yaml.cpp @@ -75,7 +75,7 @@ void loadDigitalDeviceConfigFromYAML( // Load IO type std::string type; - if (!loadYAMLParam(device_node, "type", type, verbose)) { + if (!loadYAMLParam(device_node, "io_type", type, verbose)) { throw std::runtime_error( "[loadDigitalDeviceConfigFromYAML] Error: Type not specified for Digital Device " + device_name + "!"); From cb7021d2ba1a5bf981c02261b5926d0045bb8800 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Tue, 4 Jun 2024 10:17:57 -0500 Subject: [PATCH 19/97] adds example to comment doc --- .../ghost_v5_interfaces/util/device_config_factory_utils.hpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/util/device_config_factory_utils.hpp b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/util/device_config_factory_utils.hpp index d665905a..9af9d31a 100644 --- a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/util/device_config_factory_utils.hpp +++ b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/util/device_config_factory_utils.hpp @@ -53,6 +53,10 @@ namespace util * type: ROTATION_SENSOR * reversed: true/false * data_rate: 5 + * limit_switch_1: + * port: A + * type: DIGITAL + * io_type: SENSOR * ... * device_configurations: * my_motor_config_name: From 27d9fe9e5291c3ef10f68f07ae50aa1cf316b8ee Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Tue, 4 Jun 2024 10:32:38 -0500 Subject: [PATCH 20/97] fixes method comments in device interfaces header --- .../include/ghost_v5_interfaces/devices/device_interfaces.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/device_interfaces.hpp b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/device_interfaces.hpp index cab9bf31..7939acd9 100644 --- a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/device_interfaces.hpp +++ b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/device_interfaces.hpp @@ -170,7 +170,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 byte stream */ virtual std::vector serialize(hardware_type_e hardware_type) const = 0; @@ -179,7 +179,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 & data, From ed725e7d2d8948953e8bc38e272131698a52a2e1 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Wed, 5 Jun 2024 15:44:43 -0500 Subject: [PATCH 21/97] changes digital device ports to start at 22 --- .../src/util/load_digital_device_config_yaml.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/src/util/load_digital_device_config_yaml.cpp b/01_Libraries/ghost_v5_interfaces/src/util/load_digital_device_config_yaml.cpp index 85bed806..da161509 100644 --- a/01_Libraries/ghost_v5_interfaces/src/util/load_digital_device_config_yaml.cpp +++ b/01_Libraries/ghost_v5_interfaces/src/util/load_digital_device_config_yaml.cpp @@ -66,9 +66,8 @@ void loadDigitalDeviceConfigFromYAML( device_name + "!"); } - // Convert port from A-H to 21-28 (for compatibility with existing interfaces) - port -= 'A'; - port += 21; + // Convert port from A-H to 22-28 + port += 22 - 'A'; // Set port in device config device_config_ptr->port = (int) port; From 16a47062c97f7e8800036767c5fb2fd9f59f2736 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Wed, 5 Jun 2024 16:58:31 -0500 Subject: [PATCH 22/97] adds digital device support to RHI --- .../robot_hardware_interface.hpp | 2 + .../src/robot_hardware_interface.cpp | 64 +++++++++++-------- 2 files changed, 38 insertions(+), 28 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/robot_hardware_interface.hpp b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/robot_hardware_interface.hpp index 94f46f10..7ba302fd 100644 --- a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/robot_hardware_interface.hpp +++ b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/robot_hardware_interface.hpp @@ -27,11 +27,13 @@ #include #include #include +#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 diff --git a/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp b/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp index fbc8c0e2..5a8d3021 100644 --- a/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp +++ b/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp @@ -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 @@ -57,6 +57,10 @@ RobotHardwareInterface::RobotHardwareInterface( } else if (pair.config_ptr->type == device_type_e::JOYSTICK) { pair.data_ptr = std::make_shared( val->name); + } else if (pair.config_ptr->type == device_type_e::DIGITAL) { + pair.data_ptr = std::make_shared( + val->name, + pair.config_ptr->as()->serial_config); } else { throw std::runtime_error( "[RobotHardwareInterface::RobotHardwareInterface()] Device type " + std::to_string( @@ -67,21 +71,20 @@ RobotHardwareInterface::RobotHardwareInterface( device_pair_port_map_[pair.config_ptr->port] = pair; port_to_device_name_map_.emplace(pair.config_ptr->port, val->name); - // Update msg lengths based on each device - sensor_update_msg_length_ += pair.data_ptr->getSensorPacketSize(); - actuator_command_msg_length_ += pair.data_ptr->getActuatorPacketSize(); + // Update msg lengths based on each non-digital device + if (pair.config_ptr->type != device_type_e::DIGITAL) { + sensor_update_msg_length_ += pair.data_ptr->getSensorPacketSize(); + actuator_command_msg_length_ += pair.data_ptr->getActuatorPacketSize(); + } } for (const auto & [key, val] : port_to_device_name_map_) { device_names_ordered_by_port_.emplace_back(val); } - // Add Digital IO to actuator command msg - actuator_command_msg_length_ += 1; - digital_io_ = std::vector(8, false); - - // Add Competition State to sensor update msg - sensor_update_msg_length_ += 1; + // Add Competiton State and Digital IO to each command msg + actuator_command_msg_length_ += 2; + sensor_update_msg_length_ += 2; } std::vector RobotHardwareInterface::serialize() const @@ -89,7 +92,6 @@ std::vector RobotHardwareInterface::serialize() const std::vector serial_data; std::unique_lock update_lock(update_mutex_); - // Only send competition state and joystick info from V5 Brain to Coprocessor if (hardware_type_ == hardware_type_e::V5_BRAIN) { serial_data.push_back( packByte( @@ -97,13 +99,20 @@ std::vector RobotHardwareInterface::serialize() const is_disabled_, is_autonomous_, is_connected_, 0, 0, 0, 0, 0 })); } else if (hardware_type_ == hardware_type_e::COPROCESSOR) { - // Send state of all Digital IO Ports - serial_data.push_back(packByte(digital_io_)); + serial_data.push_back((unsigned char) 0); } + // Reserve empty byte for all Digital IO Ports + serial_data.push_back((unsigned char) 0); + for (const auto & [key, val] : device_pair_port_map_) { auto device_serial_msg = val.data_ptr->serialize(hardware_type_); - serial_data.insert(serial_data.end(), device_serial_msg.begin(), device_serial_msg.end()); + if (val.config_ptr->type == device_type_e::DIGITAL) { + setBit(serial_data[1], (val.config_ptr->port - 22), (device_serial_msg[0] == 1)); + } + else { + serial_data.insert(serial_data.end(), device_serial_msg.begin(), device_serial_msg.end()); + } } // Error Checking @@ -143,24 +152,18 @@ int RobotHardwareInterface::deserialize(const std::vector & msg) } // Deserialize - int byte_offset = 0; std::unique_lock update_lock(update_mutex_); - if (hardware_type_ == hardware_type_e::V5_BRAIN) { - // Unpack Digital IO - digital_io_ = unpackByte(msg[byte_offset]); - byte_offset++; - } - if (hardware_type_ == hardware_type_e::COPROCESSOR) { // Unpack competition state - auto packet_start_byte = unpackByte(msg[byte_offset]); + auto packet_start_byte = unpackByte(msg[0]); is_disabled_ = packet_start_byte[0]; is_autonomous_ = packet_start_byte[1]; is_connected_ = packet_start_byte[2]; - byte_offset++; } + int byte_offset = 2; + // Unpack each device in device tree for (const auto & [key, val] : device_pair_port_map_) { int msg_len; @@ -173,12 +176,17 @@ int RobotHardwareInterface::deserialize(const std::vector & msg) "[RobotHardwareInterface::deserialize] Error: Attempted to deserialize with unsupported hardware type."); } - auto start_itr = msg.begin() + byte_offset; - val.data_ptr->deserialize( + if (val.config_ptr->type == device_type_e::DIGITAL) { + val.data_ptr->deserialize({getBit(msg[1], (val.config_ptr->port - 22))}, hardware_type_); + } + else { + auto start_itr = msg.begin() + byte_offset; + val.data_ptr->deserialize( std::vector( - start_itr, - start_itr + msg_len), hardware_type_); - byte_offset += msg_len; + start_itr, + start_itr + msg_len), hardware_type_); + byte_offset += msg_len; + } } return byte_offset; } From b53f4f144fb1f6deb073f4dfde57b179b76f89d0 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Wed, 5 Jun 2024 17:22:39 -0500 Subject: [PATCH 23/97] removes (now) unused digital_io_ vector from RHI --- .../src/robot_hardware_interface.cpp | 14 -------------- 1 file changed, 14 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp b/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp index 5a8d3021..de48b715 100644 --- a/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp +++ b/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp @@ -203,9 +203,6 @@ bool RobotHardwareInterface::isDataEqual(const RobotHardwareInterface & rhs) con eq &= (actuator_command_msg_length_ == rhs.actuator_command_msg_length_); eq &= (sensor_update_msg_length_ == rhs.sensor_update_msg_length_); - // ADI Ports - eq &= (digital_io_ == rhs.digital_io_); - for (const auto & [key, val] : device_pair_name_map_) { if (rhs.device_pair_name_map_.count(key) == 0) { return false; @@ -458,17 +455,6 @@ float RobotHardwareInterface::getInertialSensorHeading(const std::string & senso } } -void RobotHardwareInterface::setDigitalIO(const std::vector & digital_io) -{ - std::unique_lock update_lock(update_mutex_); - digital_io_ = digital_io; -} - -const std::vector & RobotHardwareInterface::getDigitalIO() const -{ - return digital_io_; -} - std::shared_ptr RobotHardwareInterface::getMainJoystickData() { return getDeviceData(MAIN_JOYSTICK_NAME); From 8628c0cee28a5e8313bad99ce4808f1fe82c1373 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Wed, 5 Jun 2024 17:25:05 -0500 Subject: [PATCH 24/97] adds whitespace --- .../src/robot_hardware_interface.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp b/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp index de48b715..9687f4f0 100644 --- a/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp +++ b/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp @@ -249,14 +249,17 @@ float RobotHardwareInterface::getMotorPosition(const std::string & motor_name) { return getDeviceData(motor_name)->curr_position; } + float RobotHardwareInterface::getMotorCurrentMA(const std::string & motor_name) { return getDeviceData(motor_name)->curr_current_ma; } + float RobotHardwareInterface::getMotorVelocityRPM(const std::string & motor_name) { return getDeviceData(motor_name)->curr_velocity_rpm; } + void RobotHardwareInterface::setMotorPositionCommand( const std::string & motor_name, float position_cmd) @@ -267,6 +270,7 @@ void RobotHardwareInterface::setMotorPositionCommand( motor_data_ptr->position_control = true; setDeviceDataNoLock(motor_data_ptr); } + void RobotHardwareInterface::setMotorVelocityCommandRPM( const std::string & motor_name, float velocity_cmd) @@ -277,6 +281,7 @@ void RobotHardwareInterface::setMotorVelocityCommandRPM( motor_data_ptr->velocity_control = true; setDeviceDataNoLock(motor_data_ptr); } + void RobotHardwareInterface::setMotorVoltageCommandPercent( const std::string & motor_name, float voltage_cmd) @@ -287,6 +292,7 @@ void RobotHardwareInterface::setMotorVoltageCommandPercent( motor_data_ptr->voltage_control = true; setDeviceDataNoLock(motor_data_ptr); } + void RobotHardwareInterface::setMotorTorqueCommandPercent( const std::string & motor_name, float torque_cmd) @@ -297,6 +303,7 @@ void RobotHardwareInterface::setMotorTorqueCommandPercent( motor_data_ptr->torque_control = true; setDeviceDataNoLock(motor_data_ptr); } + void RobotHardwareInterface::setMotorCommand( const std::string & motor_name, float position_cmd, @@ -312,6 +319,7 @@ void RobotHardwareInterface::setMotorCommand( motor_data_ptr->torque_command = torque_cmd; setDeviceDataNoLock(motor_data_ptr); } + void RobotHardwareInterface::setMotorControlMode( const std::string & motor_name, bool position_control, @@ -327,6 +335,7 @@ void RobotHardwareInterface::setMotorControlMode( motor_data_ptr->torque_control = torque_control; setDeviceDataNoLock(motor_data_ptr); } + void RobotHardwareInterface::setMotorCurrentLimitMilliAmps( const std::string & motor_name, int32_t current_limit_ma) @@ -384,6 +393,7 @@ float RobotHardwareInterface::getInertialSensorXRate(const std::string & sensor_ " is not configured to send gyro data!"); } } + float RobotHardwareInterface::getInertialSensorYRate(const std::string & sensor_name) { if (getDeviceConfig(sensor_name)->serial_config.send_gyro_data) { @@ -395,6 +405,7 @@ float RobotHardwareInterface::getInertialSensorYRate(const std::string & sensor_ " is not configured to send gyro data!"); } } + float RobotHardwareInterface::getInertialSensorZRate(const std::string & sensor_name) { if (getDeviceConfig(sensor_name)->serial_config.send_gyro_data) { From 1bea32939443bf260122184a4e2f6b0ead7a9d80 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Wed, 5 Jun 2024 18:31:18 -0500 Subject: [PATCH 25/97] adds digital device helpers to RHI --- .../robot_hardware_interface.hpp | 14 ++++++------- .../src/robot_hardware_interface.cpp | 20 +++++++++++++++++++ 2 files changed, 27 insertions(+), 7 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/robot_hardware_interface.hpp b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/robot_hardware_interface.hpp index 7ba302fd..2fbae755 100644 --- a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/robot_hardware_interface.hpp +++ b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/robot_hardware_interface.hpp @@ -216,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 & sensor_name); + bool setDigitalDeviceValue(const std::string & sensor_name, bool value); + + ////////////////////////////////////////////////////////////// ///////////////// Joystick Device Interfaces ///////////////// ////////////////////////////////////////////////////////////// std::shared_ptr getMainJoystickData(); std::shared_ptr getPartnerJoystickData(); - ////////////////////////////////////////////////////////////// - ///////////////////////// Digital IO ///////////////////////// - ////////////////////////////////////////////////////////////// - - void setDigitalIO(const std::vector & digital_io); - const std::vector & getDigitalIO() const; - ///////////////////////////////////////////////////////// /////////////////// Device Interfaces /////////////////// ///////////////////////////////////////////////////////// diff --git a/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp b/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp index 9687f4f0..164b10af 100644 --- a/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp +++ b/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp @@ -466,6 +466,26 @@ float RobotHardwareInterface::getInertialSensorHeading(const std::string & senso } } +bool RobotHardwareInterface::getDigitalDeviceValue(const std::string & sensor_name){ + return getDeviceData(sensor_name)->value; +} + +bool RobotHardwareInterface::setDigitalDeviceValue(const std::string & device_name, bool value){ + + auto io_type = getDeviceConfig(device_name)->serial_config.io_type; + if (io_type == SENSOR && hardware_type_ != V5_BRAIN) { + throw std::runtime_error("[RobotHardwareInterface::setDigitalDeviceValue] Error: Attempted to set Digital Sensor value from Coprocessor."); + } + if (io_type == ACTUATOR && hardware_type_ != COPROCESSOR) { + throw std::runtime_error("[RobotHardwareInterface::setDigitalDeviceValue] Error: Attempted to set Digital Actuator value from V5 Brain."); + } + + std::unique_lock update_lock(update_mutex_); + auto device_data_ptr = getDeviceData(device_name); + device_data_ptr->value = value; + setDeviceDataNoLock(device_data_ptr); +} + std::shared_ptr RobotHardwareInterface::getMainJoystickData() { return getDeviceData(MAIN_JOYSTICK_NAME); From 939d690029368dc2d0535eb30e7d9b193818e29c Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Wed, 5 Jun 2024 19:04:20 -0500 Subject: [PATCH 26/97] removes digital io vector from RHI header --- .../include/ghost_v5_interfaces/robot_hardware_interface.hpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/robot_hardware_interface.hpp b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/robot_hardware_interface.hpp index 2fbae755..9c5733bf 100644 --- a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/robot_hardware_interface.hpp +++ b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/robot_hardware_interface.hpp @@ -399,9 +399,6 @@ class RobotHardwareInterface bool is_autonomous_ = false; bool is_connected_ = false; - // Digital IO - std::vector digital_io_; - // Serialization int msg_id_ = 0; int sensor_update_msg_length_; From 6fcdbd6585dd89843b0829c3ae61203cd1e4a0eb Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Wed, 5 Jun 2024 19:07:48 -0500 Subject: [PATCH 27/97] adds digital device to conversion helpers --- .../include/ghost_v5_interfaces/util/device_type_helpers.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/util/device_type_helpers.hpp b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/util/device_type_helpers.hpp index dde1403c..b3549019 100644 --- a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/util/device_type_helpers.hpp +++ b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/util/device_type_helpers.hpp @@ -42,6 +42,7 @@ const std::unordered_map 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", devices::device_type_e::DIGITAL}, {"INVALID", devices::device_type_e::INVALID}, }; const std::unordered_map DEVICE_TYPE_TO_STRING_MAP{ @@ -54,6 +55,7 @@ const std::unordered_map 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, "DIGITAL"}, {devices::device_type_e::INVALID, "INVALID"} }; From 321bb47b82754d4f6f4511e78aa929c6dc80b3c5 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Wed, 5 Jun 2024 19:08:52 -0500 Subject: [PATCH 28/97] adds whitespace --- 03_ROS/ghost_ros_interfaces/src/msg_helpers/msg_helpers.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/03_ROS/ghost_ros_interfaces/src/msg_helpers/msg_helpers.cpp b/03_ROS/ghost_ros_interfaces/src/msg_helpers/msg_helpers.cpp index 0a03a78e..0ae79d83 100644 --- a/03_ROS/ghost_ros_interfaces/src/msg_helpers/msg_helpers.cpp +++ b/03_ROS/ghost_ros_interfaces/src/msg_helpers/msg_helpers.cpp @@ -430,6 +430,7 @@ void fromROSMsg( labeled_vector_map[entry.label] = entry.data_array; } } + void toROSMsg( const std::unordered_map> & labeled_vector_map, ghost_msgs::msg::LabeledVectorMap & msg) From 40b2375604f2830c706b81d31b6dac5707543e89 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Wed, 5 Jun 2024 19:27:42 -0500 Subject: [PATCH 29/97] adds digital device ros msg --- .../msg/device_msgs/states/V5DigitalDeviceState.msg | 4 ++++ 1 file changed, 4 insertions(+) create mode 100644 03_ROS/ghost_msgs/msg/device_msgs/states/V5DigitalDeviceState.msg diff --git a/03_ROS/ghost_msgs/msg/device_msgs/states/V5DigitalDeviceState.msg b/03_ROS/ghost_msgs/msg/device_msgs/states/V5DigitalDeviceState.msg new file mode 100644 index 00000000..244ca339 --- /dev/null +++ b/03_ROS/ghost_msgs/msg/device_msgs/states/V5DigitalDeviceState.msg @@ -0,0 +1,4 @@ +ghost_msgs/V5DeviceHeader device_header + +bool value +string io_type From ba6db693fb883e10979d04232fa9c5639dba1ddb Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Wed, 5 Jun 2024 19:30:10 -0500 Subject: [PATCH 30/97] adds digital device msg helpers --- .../msg_helpers/msg_helpers.hpp | 24 +++++++++++++++++++ .../src/msg_helpers/msg_helpers.cpp | 17 +++++++++++++ 2 files changed, 41 insertions(+) diff --git a/03_ROS/ghost_ros_interfaces/include/ghost_ros_interfaces/msg_helpers/msg_helpers.hpp b/03_ROS/ghost_ros_interfaces/include/ghost_ros_interfaces/msg_helpers/msg_helpers.hpp index c8883121..46ce408d 100644 --- a/03_ROS/ghost_ros_interfaces/include/ghost_ros_interfaces/msg_helpers/msg_helpers.hpp +++ b/03_ROS/ghost_ros_interfaces/include/ghost_ros_interfaces/msg_helpers/msg_helpers.hpp @@ -23,6 +23,7 @@ #include #include +#include #include #include #include @@ -35,6 +36,7 @@ #include #include #include +#include #include #include #include @@ -181,6 +183,28 @@ void fromROSMsg( ghost_v5_interfaces::devices::InertialSensorDeviceData & inertial_sensor_data, const ghost_msgs::msg::V5InertialSensorState & inertial_sensor_msg); +// Digital Device + +/** + * @brief Copies digital device state data from a DigitalDeviceData object into a V5DigitalDeviceState msg. + * + * @param digital_device_data + * @param digital_device_msg + */ +void toROSMsg( + const ghost_v5_interfaces::devices::DigitalDeviceData & digital_device_data, + ghost_msgs::msg::V5DigitalDeviceState & digital_device_msg); + +/** + * @brief Copies digital device state data from a V5DigitalDeviceState msg into a DigitalDeviceData object. + * + * @param digital_device_data + * @param digital_device_msg + */ +void fromROSMsg( + const ghost_v5_interfaces::devices::DigitalDeviceData & digital_device_data, + ghost_msgs::msg::V5DigitalDeviceState & digital_device_msg); + // Aggregate Actuator Command /** diff --git a/03_ROS/ghost_ros_interfaces/src/msg_helpers/msg_helpers.cpp b/03_ROS/ghost_ros_interfaces/src/msg_helpers/msg_helpers.cpp index 0ae79d83..c05b1484 100644 --- a/03_ROS/ghost_ros_interfaces/src/msg_helpers/msg_helpers.cpp +++ b/03_ROS/ghost_ros_interfaces/src/msg_helpers/msg_helpers.cpp @@ -212,6 +212,23 @@ void fromROSMsg( inertial_sensor_data.heading = inertial_sensor_msg.heading; } +void toROSMsg( + const ghost_v5_interfaces::devices::DigitalDeviceData & digital_device_data, + ghost_msgs::msg::V5DigitalDeviceState & digital_device_msg) +{ + toROSMsg(digital_device_data, digital_device_msg.device_header); // Set device header + digital_device_msg.value = digital_device_data.value; + digital_device_msg.value = digital_device_data.serial_config.io_type; +} + +void fromROSMsg( + const ghost_v5_interfaces::devices::DigitalDeviceData & digital_device_data, + ghost_msgs::msg::V5DigitalDeviceState & digital_device_msg) +{ + fromROSMsg(digital_device_data, digital_device_msg.device_header); // Set base attributes + digital_device_data.value = digital_device_msg.value; + digital_device_data.serial_config.io_type = digital_device_msg.value; +} void toROSMsg( const RobotHardwareInterface & hardware_interface, From a0cebcccde46c74460b4e4762e9fe2c23c5b576c Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Wed, 5 Jun 2024 20:15:43 -0500 Subject: [PATCH 31/97] adds digital device msg to cmake --- 03_ROS/ghost_msgs/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/03_ROS/ghost_msgs/CMakeLists.txt b/03_ROS/ghost_msgs/CMakeLists.txt index 46cbd2a8..a4d53f17 100644 --- a/03_ROS/ghost_msgs/CMakeLists.txt +++ b/03_ROS/ghost_msgs/CMakeLists.txt @@ -24,6 +24,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/device_msgs/states/V5MotorState.msg" "msg/device_msgs/states/V5RotationSensorState.msg" "msg/device_msgs/states/V5InertialSensorState.msg" + "msg/device_msgs/states/V5DigitalDeviceState.msg" "msg/V5ActuatorCommand.msg" "msg/V5SensorUpdate.msg" "msg/DrivetrainCommand.msg" From b96061f4d442c7641229bced814cf917512291a2 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Wed, 5 Jun 2024 20:38:00 -0500 Subject: [PATCH 32/97] adds conversion maps for digital io type --- .../ghost_v5_interfaces/util/device_type_helpers.hpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/util/device_type_helpers.hpp b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/util/device_type_helpers.hpp index b3549019..c6dd8354 100644 --- a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/util/device_type_helpers.hpp +++ b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/util/device_type_helpers.hpp @@ -58,6 +58,14 @@ const std::unordered_map DEVICE_TYPE_TO_STR {devices::device_type_e::DIGITAL, "DIGITAL"}, {devices::device_type_e::INVALID, "INVALID"} }; +const std::unordered_map DIGITAL_IO_TYPE_TO_STRING_MAP{ + {devices::digital_io_type_e::SENSOR, "SENSOR"}, + {devices::digital_io_type_e::ACTUATOR, "ACTUATOR"} +}; +const std::unordered_map STRING_TO_DIGITAL_IO_TYPE_MAP{ + {"SENSOR", devices::digital_io_type_e::SENSOR}, + {"ACTUATOR", devices::digital_io_type_e::ACTUATOR} +}; } // namespace ghost_v5_interfaces From 38ecd8389f46d76cc7893df43bc5f0d21e58b1b6 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Wed, 5 Jun 2024 20:39:12 -0500 Subject: [PATCH 33/97] (TEMP) disables digital io tests --- .../test/test_robot_hardware_interface.cpp | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/test/test_robot_hardware_interface.cpp b/01_Libraries/ghost_v5_interfaces/test/test_robot_hardware_interface.cpp index cbe9e74c..4c063225 100644 --- a/01_Libraries/ghost_v5_interfaces/test/test_robot_hardware_interface.cpp +++ b/01_Libraries/ghost_v5_interfaces/test/test_robot_hardware_interface.cpp @@ -130,22 +130,22 @@ TEST_F(RobotHardwareInterfaceTestFixture, testSetAndRetrieveCompetitionStatus) { EXPECT_EQ(hw_interface.isConnected(), true); } -TEST_F(RobotHardwareInterfaceTestFixture, testSetAndRetrieveDigitalIO) { - RobotHardwareInterface hw_interface(device_config_map_ptr_dual_joy_, - hardware_type_e::COPROCESSOR); +// TEST_F(RobotHardwareInterfaceTestFixture, testSetAndRetrieveDigitalIO) { +// RobotHardwareInterface hw_interface(device_config_map_ptr_dual_joy_, +// hardware_type_e::COPROCESSOR); - // Default values - EXPECT_EQ(hw_interface.getDigitalIO(), std::vector(8, false)); +// // Default values +// EXPECT_EQ(hw_interface.getDigitalIO(), std::vector(8, false)); - auto test_io = std::vector{ - getRandomBool(), getRandomBool(), getRandomBool(), getRandomBool(), - getRandomBool(), getRandomBool(), getRandomBool(), getRandomBool() - }; +// auto test_io = std::vector{ +// getRandomBool(), getRandomBool(), getRandomBool(), getRandomBool(), +// getRandomBool(), getRandomBool(), getRandomBool(), getRandomBool() +// }; - hw_interface.setDigitalIO(test_io); +// hw_interface.setDigitalIO(test_io); - EXPECT_EQ(hw_interface.getDigitalIO(), test_io); -} +// EXPECT_EQ(hw_interface.getDigitalIO(), test_io); +// } TEST_F(RobotHardwareInterfaceTestFixture, testGetDevicePair) { RobotHardwareInterface hw_interface(device_config_map_ptr_dual_joy_, From f8a6ba850c00f7c20878cd9ba439f60ca3390d85 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Wed, 5 Jun 2024 20:48:55 -0500 Subject: [PATCH 34/97] fixes digital msg helpers, updates RHI msgs and helpers --- 03_ROS/ghost_msgs/msg/V5ActuatorCommand.msg | 4 +- 03_ROS/ghost_msgs/msg/V5SensorUpdate.msg | 5 +- .../msg_helpers/msg_helpers.hpp | 1 + .../src/msg_helpers/msg_helpers.cpp | 50 +++++++++++++------ 4 files changed, 41 insertions(+), 19 deletions(-) diff --git a/03_ROS/ghost_msgs/msg/V5ActuatorCommand.msg b/03_ROS/ghost_msgs/msg/V5ActuatorCommand.msg index 85e49a3b..4d204aa4 100644 --- a/03_ROS/ghost_msgs/msg/V5ActuatorCommand.msg +++ b/03_ROS/ghost_msgs/msg/V5ActuatorCommand.msg @@ -5,5 +5,5 @@ uint32 msg_id # Motor Commands ghost_msgs/V5MotorCommand[] motor_commands -# Digital IO -bool[] digital_io \ No newline at end of file +# Digital Devices +ghost_msgs/V5DigitalDeviceState[] digital_devices diff --git a/03_ROS/ghost_msgs/msg/V5SensorUpdate.msg b/03_ROS/ghost_msgs/msg/V5SensorUpdate.msg index 7199a8ef..f265620b 100644 --- a/03_ROS/ghost_msgs/msg/V5SensorUpdate.msg +++ b/03_ROS/ghost_msgs/msg/V5SensorUpdate.msg @@ -17,6 +17,5 @@ ghost_msgs/V5InertialSensorState[] inertial_sensors # Motor States ghost_msgs/V5MotorState[] motors -# Digital Inputs -bool[] digital_io - +# Digital Devices +ghost_msgs/V5DigitalDeviceState[] digital_devices diff --git a/03_ROS/ghost_ros_interfaces/include/ghost_ros_interfaces/msg_helpers/msg_helpers.hpp b/03_ROS/ghost_ros_interfaces/include/ghost_ros_interfaces/msg_helpers/msg_helpers.hpp index 46ce408d..2cd4983d 100644 --- a/03_ROS/ghost_ros_interfaces/include/ghost_ros_interfaces/msg_helpers/msg_helpers.hpp +++ b/03_ROS/ghost_ros_interfaces/include/ghost_ros_interfaces/msg_helpers/msg_helpers.hpp @@ -28,6 +28,7 @@ #include #include #include +#include #include #include diff --git a/03_ROS/ghost_ros_interfaces/src/msg_helpers/msg_helpers.cpp b/03_ROS/ghost_ros_interfaces/src/msg_helpers/msg_helpers.cpp index c05b1484..441be1d0 100644 --- a/03_ROS/ghost_ros_interfaces/src/msg_helpers/msg_helpers.cpp +++ b/03_ROS/ghost_ros_interfaces/src/msg_helpers/msg_helpers.cpp @@ -218,16 +218,16 @@ void toROSMsg( { toROSMsg(digital_device_data, digital_device_msg.device_header); // Set device header digital_device_msg.value = digital_device_data.value; - digital_device_msg.value = digital_device_data.serial_config.io_type; + digital_device_msg.io_type = DIGITAL_IO_TYPE_TO_STRING_MAP.at(digital_device_data.serial_config_.io_type); } void fromROSMsg( - const ghost_v5_interfaces::devices::DigitalDeviceData & digital_device_data, - ghost_msgs::msg::V5DigitalDeviceState & digital_device_msg) + ghost_v5_interfaces::devices::DigitalDeviceData & digital_device_data, + const ghost_msgs::msg::V5DigitalDeviceState & digital_device_msg) { fromROSMsg(digital_device_data, digital_device_msg.device_header); // Set base attributes digital_device_data.value = digital_device_msg.value; - digital_device_data.serial_config.io_type = digital_device_msg.value; + digital_device_data.serial_config_.io_type = STRING_TO_DIGITAL_IO_TYPE_MAP.at(digital_device_msg.io_type); } void toROSMsg( @@ -250,6 +250,13 @@ void toROSMsg( continue; } else if (device_data_ptr->type == device_type_e::INERTIAL_SENSOR) { continue; + } else if (device_data_ptr->type == device_type_e::DIGITAL) { + auto digital_device_data_ptr = device_data_ptr->as(); + if (digital_device_data_ptr->serial_config_.io_type == digital_io_type_e::ACTUATOR) { + V5DigitalDeviceState msg{}; + toROSMsg(*digital_device_data_ptr, msg); + actuator_cmd_msg.digital_devices.push_back(msg); + } } else { std::string dev_type_str; if (DEVICE_TYPE_TO_STRING_MAP.count(device_data_ptr->type) == 1) { @@ -262,9 +269,6 @@ void toROSMsg( dev_type_str); } } - - // Digital IO - actuator_cmd_msg.digital_io = hardware_interface.getDigitalIO(); } void fromROSMsg( @@ -281,8 +285,15 @@ void fromROSMsg( hardware_interface.setDeviceData(motor_data_ptr); } - // Digital IO - hardware_interface.setDigitalIO(actuator_cmd_msg.digital_io); + // Digital Devices + for (const auto & digital_device_msg : actuator_cmd_msg.digital_devices) { + auto digital_device_data_ptr = hardware_interface.getDeviceData( + digital_device_msg.device_header.name); + if (digital_device_data_ptr->serial_config_.io_type == ACTUATOR) { + fromROSMsg(*digital_device_data_ptr, digital_device_msg); + hardware_interface.setDeviceData(digital_device_data_ptr); + } + } } void toROSMsg(const RobotHardwareInterface & hardware_interface, V5SensorUpdate & sensor_update_msg) @@ -317,6 +328,13 @@ void toROSMsg(const RobotHardwareInterface & hardware_interface, V5SensorUpdate auto joy_data_ptr = device_data_ptr->as(); toROSMsg(*joy_data_ptr, msg); sensor_update_msg.joysticks.push_back(msg); + } else if (device_data_ptr->type == device_type_e::DIGITAL) { + auto digital_device_data_ptr = device_data_ptr->as(); + if (digital_device_data_ptr->serial_config_.io_type == digital_io_type_e::SENSOR) { + V5DigitalDeviceState msg{}; + toROSMsg(*digital_device_data_ptr, msg); + sensor_update_msg.digital_devices.push_back(msg); + } } else { std::string dev_type_str; if (DEVICE_TYPE_TO_STRING_MAP.count(device_data_ptr->type) == 1) { @@ -329,9 +347,6 @@ void toROSMsg(const RobotHardwareInterface & hardware_interface, V5SensorUpdate dev_type_str); } } - - // Digital IO - sensor_update_msg.digital_io = hardware_interface.getDigitalIO(); } void fromROSMsg( @@ -377,8 +392,15 @@ void fromROSMsg( hardware_interface.setDeviceData(inertial_sensor_data_ptr); } - // Digital IO - hardware_interface.setDigitalIO(sensor_update_msg.digital_io); + // Digital Devices + for (const auto & digital_device_msg : sensor_update_msg.digital_devices) { + auto digital_device_data_ptr = hardware_interface.getDeviceData( + digital_device_msg.device_header.name); + if (digital_device_data_ptr->serial_config_.io_type == SENSOR) { + fromROSMsg(*digital_device_data_ptr, digital_device_msg); + hardware_interface.setDeviceData(digital_device_data_ptr); + } + } } void fromROSMsg( From 89611aaf8ab5c36d343c5973ff59cd373aa3cdfa Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Wed, 5 Jun 2024 20:52:46 -0500 Subject: [PATCH 35/97] moves digital device base node in yaml loader --- .../src/util/load_digital_device_config_yaml.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/01_Libraries/ghost_v5_interfaces/src/util/load_digital_device_config_yaml.cpp b/01_Libraries/ghost_v5_interfaces/src/util/load_digital_device_config_yaml.cpp index da161509..38dd27b3 100644 --- a/01_Libraries/ghost_v5_interfaces/src/util/load_digital_device_config_yaml.cpp +++ b/01_Libraries/ghost_v5_interfaces/src/util/load_digital_device_config_yaml.cpp @@ -39,7 +39,7 @@ void loadDigitalDeviceConfigFromYAML( bool verbose) { // Get device node - auto device_node = node["devices"][device_name]; + auto device_node = node["adi"][device_name]; if (!device_node) { throw std::runtime_error( "[loadDigitalDeviceConfigFromYAML] Error: Device Sensor " + device_name + From 64aef43255126882b168a6d23e9e0479c4b49ce0 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Wed, 5 Jun 2024 21:03:21 -0500 Subject: [PATCH 36/97] adds adi sensors to yaml --- .../config/robot_hardware_config_worlds_24.yaml | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/11_Robots/ghost_over_under/config/robot_hardware_config_worlds_24.yaml b/11_Robots/ghost_over_under/config/robot_hardware_config_worlds_24.yaml index f5bcffbb..1f2dc8e7 100644 --- a/11_Robots/ghost_over_under/config/robot_hardware_config_worlds_24.yaml +++ b/11_Robots/ghost_over_under/config/robot_hardware_config_worlds_24.yaml @@ -1,4 +1,21 @@ port_configuration: + adi: + tail: + port: A + type: DIGITAL + io_type: ACTUATOR + claw: + port: B + type: DIGITAL + io_type: ACTUATOR + claw_inner: + port: C + type: DIGITAL + io_type: SENSOR + claw_outer: + port: D + type: DIGITAL + io_type: SENSOR devices: steering_front_left: port: 1 From 12674a8b7c365c83749f519fa494622e9333a79e Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Wed, 5 Jun 2024 21:06:32 -0500 Subject: [PATCH 37/97] replaces digital io in swerve plugin --- .../include/ghost_swerve/swerve_robot_plugin.hpp | 4 ---- 11_Robots/ghost_swerve/src/swerve_robot_plugin.cpp | 11 ++--------- 2 files changed, 2 insertions(+), 13 deletions(-) diff --git a/11_Robots/ghost_swerve/include/ghost_swerve/swerve_robot_plugin.hpp b/11_Robots/ghost_swerve/include/ghost_swerve/swerve_robot_plugin.hpp index 6b4fdc49..b19f34eb 100644 --- a/11_Robots/ghost_swerve/include/ghost_swerve/swerve_robot_plugin.hpp +++ b/11_Robots/ghost_swerve/include/ghost_swerve/swerve_robot_plugin.hpp @@ -134,10 +134,6 @@ class SwerveRobotPlugin : public ghost_ros_interfaces::V5RobotBase bool m_use_backup_estimator = false; double m_intake_setpoint = 7.0; - // Digital IO - std::vector m_digital_io; - std::unordered_map m_digital_io_name_map; - // Claw bool m_claw_btn_pressed = false; bool m_claw_open = true; diff --git a/11_Robots/ghost_swerve/src/swerve_robot_plugin.cpp b/11_Robots/ghost_swerve/src/swerve_robot_plugin.cpp index ee7ca8c4..933ea0b8 100644 --- a/11_Robots/ghost_swerve/src/swerve_robot_plugin.cpp +++ b/11_Robots/ghost_swerve/src/swerve_robot_plugin.cpp @@ -37,11 +37,6 @@ namespace ghost_swerve SwerveRobotPlugin::SwerveRobotPlugin() { - m_digital_io = std::vector(8, false); - m_digital_io_name_map = std::unordered_map{ - {"tail", 0}, - {"claw", 1} - }; } void SwerveRobotPlugin::initialize() @@ -739,10 +734,8 @@ void SwerveRobotPlugin::teleop(double current_time) rhi_ptr_->setMotorPositionCommand("tail_motor", m_stick_angle_start); } - m_digital_io[m_digital_io_name_map["tail"]] = tail_down; - m_digital_io[m_digital_io_name_map["claw"]] = !m_claw_open; - - rhi_ptr_->setDigitalIO(m_digital_io); + rhi_ptr_->setDigitalDeviceValue("tail", tail_down); + rhi_ptr_->setDigitalDeviceValue("claw", !m_claw_open); // If INTAKE_MOTOR stalling, update state and timer if ((intake_command) && From 3c688aa92ce2a1cbb758084a1bc1896e5c2e81ec Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Wed, 5 Jun 2024 21:10:35 -0500 Subject: [PATCH 38/97] replaces digital io in climb plugin --- 11_Robots/ghost_swerve/src/bt_nodes/climb.cpp | 11 +---------- 1 file changed, 1 insertion(+), 10 deletions(-) diff --git a/11_Robots/ghost_swerve/src/bt_nodes/climb.cpp b/11_Robots/ghost_swerve/src/bt_nodes/climb.cpp index 83594c5f..1f0c8b24 100644 --- a/11_Robots/ghost_swerve/src/bt_nodes/climb.cpp +++ b/11_Robots/ghost_swerve/src/bt_nodes/climb.cpp @@ -95,13 +95,6 @@ float Climb::tempPID( BT::NodeStatus Climb::onRunning() { - auto m_digital_io = std::vector(8, false); - auto m_digital_io_name_map = std::unordered_map{ - {"claw", 0}, - {"right_wing", 1}, - {"left_wing", 2}, - {"tail", 3} - }; bool claw_open; auto status = BT::NodeStatus::RUNNING; bool climbed = get_input("climbed"); @@ -125,9 +118,7 @@ BT::NodeStatus Climb::onRunning() swerve_ptr_->getConfig().lift_kP); // go to 90deg // ignore err - m_digital_io[m_digital_io_name_map.at("claw")] = claw_open; - - rhi_ptr_->setDigitalIO(m_digital_io); + rhi_ptr_->setDigitalDeviceValue("claw", claw_open); // do not return any status but RUNNING // so it keeps the state at the end of the auton From 3b3a74cdb9e91bcd26bdbd050bffd39a1a84b8aa Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Wed, 5 Jun 2024 21:17:01 -0500 Subject: [PATCH 39/97] replaces swipeTail digital io --- 11_Robots/ghost_swerve/src/bt_nodes/swipeTail.cpp | 12 ++---------- 1 file changed, 2 insertions(+), 10 deletions(-) diff --git a/11_Robots/ghost_swerve/src/bt_nodes/swipeTail.cpp b/11_Robots/ghost_swerve/src/bt_nodes/swipeTail.cpp index a6f6a9f7..7feab244 100644 --- a/11_Robots/ghost_swerve/src/bt_nodes/swipeTail.cpp +++ b/11_Robots/ghost_swerve/src/bt_nodes/swipeTail.cpp @@ -76,13 +76,6 @@ void SwipeTail::onHalted() BT::NodeStatus SwipeTail::onRunning() { - auto m_digital_io = std::vector(8, false); - auto m_digital_io_name_map = std::unordered_map{ - {"claw", 0}, - {"right_wing", 1}, - {"left_wing", 2}, - {"tail", 3} - }; auto status = BT::NodeStatus::RUNNING; int num_swipes = get_input("num_swipes"); @@ -125,12 +118,11 @@ BT::NodeStatus SwipeTail::onRunning() // } // if(status == BT::NodeStatus::SUCCESS){ - // m_digital_io[m_digital_io_name_map.at("tail")] = false; + // rhi_ptr_->setDigitalDeviceValue("tail", false); // } else { - // m_digital_io[m_digital_io_name_map.at("tail")] = true; + // rhi_ptr_->setDigitalDeviceValue("tail", true); // } - // rhi_ptr_->setDigitalIO(m_digital_io); return status; } From f4d8e3f9e208ade548e5cf2ddd0e80cc2605f378 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Wed, 5 Jun 2024 21:18:51 -0500 Subject: [PATCH 40/97] fixes example comment --- .../util/device_config_factory_utils.hpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/util/device_config_factory_utils.hpp b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/util/device_config_factory_utils.hpp index 9af9d31a..0d35ec63 100644 --- a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/util/device_config_factory_utils.hpp +++ b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/util/device_config_factory_utils.hpp @@ -42,6 +42,11 @@ namespace util * * port_configuration: * use_partner_joystick = false/true + * adi: + * limit_switch_1: + * port: A + * type: DIGITAL + * io_type: SENSOR * devices: * my_motor_name_here: * port: 1 @@ -53,10 +58,6 @@ namespace util * type: ROTATION_SENSOR * reversed: true/false * data_rate: 5 - * limit_switch_1: - * port: A - * type: DIGITAL - * io_type: SENSOR * ... * device_configurations: * my_motor_config_name: From 4d68735bf3ad20daf5b85828458d3d047bd571be Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Wed, 5 Jun 2024 22:19:53 -0500 Subject: [PATCH 41/97] removes old digital io test --- .../test/test_robot_hardware_interface.cpp | 17 ----------------- 1 file changed, 17 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/test/test_robot_hardware_interface.cpp b/01_Libraries/ghost_v5_interfaces/test/test_robot_hardware_interface.cpp index 4c063225..4d65c4d2 100644 --- a/01_Libraries/ghost_v5_interfaces/test/test_robot_hardware_interface.cpp +++ b/01_Libraries/ghost_v5_interfaces/test/test_robot_hardware_interface.cpp @@ -130,23 +130,6 @@ TEST_F(RobotHardwareInterfaceTestFixture, testSetAndRetrieveCompetitionStatus) { EXPECT_EQ(hw_interface.isConnected(), true); } -// TEST_F(RobotHardwareInterfaceTestFixture, testSetAndRetrieveDigitalIO) { -// RobotHardwareInterface hw_interface(device_config_map_ptr_dual_joy_, -// hardware_type_e::COPROCESSOR); - -// // Default values -// EXPECT_EQ(hw_interface.getDigitalIO(), std::vector(8, false)); - -// auto test_io = std::vector{ -// getRandomBool(), getRandomBool(), getRandomBool(), getRandomBool(), -// getRandomBool(), getRandomBool(), getRandomBool(), getRandomBool() -// }; - -// hw_interface.setDigitalIO(test_io); - -// EXPECT_EQ(hw_interface.getDigitalIO(), test_io); -// } - TEST_F(RobotHardwareInterfaceTestFixture, testGetDevicePair) { RobotHardwareInterface hw_interface(device_config_map_ptr_dual_joy_, hardware_type_e::COPROCESSOR); From 069300469565ce0cda7c48272261992fbb22a39a Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Wed, 5 Jun 2024 22:40:12 -0500 Subject: [PATCH 42/97] adds digital test devices --- .../ghost_v5_interfaces/test/config/example_robot_2.yaml | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/01_Libraries/ghost_v5_interfaces/test/config/example_robot_2.yaml b/01_Libraries/ghost_v5_interfaces/test/config/example_robot_2.yaml index daf23687..ba98e99b 100644 --- a/01_Libraries/ghost_v5_interfaces/test/config/example_robot_2.yaml +++ b/01_Libraries/ghost_v5_interfaces/test/config/example_robot_2.yaml @@ -1,5 +1,14 @@ port_configuration: use_partner_joystick: false + digital: + limit_switch_1: + port: A + type: DIGITAL + io_type: SENSOR + arm_solenoid: + port: B + type: DIGITAL + io_type: ACTUATOR devices: rotation_sensor_1: port: 4 From 071ec22a7979896692cc1a87615fc6eb15881b64 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Wed, 5 Jun 2024 22:44:33 -0500 Subject: [PATCH 43/97] fixes digital device == --- .../ghost_v5_interfaces/devices/digital_device_interface.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_device_interface.hpp b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_device_interface.hpp index c1a1bf45..f2fcf601 100644 --- a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_device_interface.hpp +++ b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_device_interface.hpp @@ -79,7 +79,7 @@ class DigitalDeviceData : public DeviceData bool operator==(const DeviceBase & rhs) const override { const DigitalDeviceData * d_rhs = dynamic_cast(&rhs); - return (d_rhs != nullptr && value == d_rhs->value); + return (d_rhs != nullptr && value == d_rhs->value && serial_config_ == d_rhs->serial_config_); } std::vector serialize(hardware_type_e hardware_type) const override From a413e8b4c400887bb6eeec5512803ef4f8784e85 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Wed, 5 Jun 2024 22:53:02 -0500 Subject: [PATCH 44/97] adds digital device test helpers --- .../test/device_test_utils.hpp | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/test/device_test_utils.hpp b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/test/device_test_utils.hpp index 90918b5c..2f0d84c4 100644 --- a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/test/device_test_utils.hpp +++ b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/test/device_test_utils.hpp @@ -26,6 +26,7 @@ #include #include #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" @@ -69,6 +70,13 @@ devices::InertialSensorDeviceData::SerialConfig getRandomInertialSensorSerialCon return config; } +devices::DigitalDeviceData::SerialConfig getRandomDigitalDeviceSerialConfig() +{ + devices::DigitalDeviceData::SerialConfig config; + config.io_type = ghost_util::getRandomBool() ? SENSOR : ACTUATOR; + return config; +} + std::shared_ptr getRandomJoystickData() { auto joy_ptr = std::make_shared("joy_" + std::to_string(rand() % 2)); @@ -183,6 +191,17 @@ std::shared_ptr getRandomInertialSensorData( return inertial_sensor_ptr; } +std::shared_ptr getRandomDigitalDeviceData( + devices::DigitalDeviceData::SerialConfig serial_config = devices::DigitalDeviceData::SerialConfig()) +{ + auto digital_device_ptr = std::make_shared( + "test", + serial_config); + digital_device_ptr->value = ghost_util::getRandomBool(); + return digital_device_ptr; +} + + } } From 7c0a32e38ac09a3140519e756eaca3bc3941cfe9 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Wed, 5 Jun 2024 23:02:49 -0500 Subject: [PATCH 45/97] fixes digital config == --- .../ghost_v5_interfaces/devices/digital_device_interface.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_device_interface.hpp b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_device_interface.hpp index f2fcf601..991e9254 100644 --- a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_device_interface.hpp +++ b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_device_interface.hpp @@ -118,7 +118,7 @@ class DigitalDeviceConfig : public DeviceConfig { const DigitalDeviceConfig * d_rhs = dynamic_cast(&rhs); return (d_rhs != nullptr) && (port == d_rhs->port) && (name == d_rhs->name) && - (type == d_rhs->type); + (type == d_rhs->type) && (serial_config == d_rhs->serial_config); } DigitalDeviceData::SerialConfig serial_config; From 3720c8bb7b0dd40de757293136b3da6a5d98624a Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Wed, 5 Jun 2024 23:06:48 -0500 Subject: [PATCH 46/97] adds io_type to digital test helper --- .../include/ghost_v5_interfaces/test/device_test_utils.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/test/device_test_utils.hpp b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/test/device_test_utils.hpp index 2f0d84c4..ba819b4b 100644 --- a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/test/device_test_utils.hpp +++ b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/test/device_test_utils.hpp @@ -192,12 +192,14 @@ std::shared_ptr getRandomInertialSensorData( } std::shared_ptr getRandomDigitalDeviceData( + devices::digital_io_type_e io_type, devices::DigitalDeviceData::SerialConfig serial_config = devices::DigitalDeviceData::SerialConfig()) { auto digital_device_ptr = std::make_shared( "test", serial_config); digital_device_ptr->value = ghost_util::getRandomBool(); + digital_device_ptr->serial_config_.io_type = io_type; return digital_device_ptr; } From cd82add7976e62ff5cdeca000e2f3845b4f3f62a Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Wed, 5 Jun 2024 23:45:06 -0500 Subject: [PATCH 47/97] adds digital devices to existing tests --- .../test/test_robot_hardware_interface.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/01_Libraries/ghost_v5_interfaces/test/test_robot_hardware_interface.cpp b/01_Libraries/ghost_v5_interfaces/test/test_robot_hardware_interface.cpp index 4d65c4d2..b6906ef5 100644 --- a/01_Libraries/ghost_v5_interfaces/test/test_robot_hardware_interface.cpp +++ b/01_Libraries/ghost_v5_interfaces/test/test_robot_hardware_interface.cpp @@ -261,6 +261,8 @@ TEST_F(RobotHardwareInterfaceTestFixture, testSerializationPipelineCoprocessorTo auto motor_data_3 = getRandomMotorData(true); motor_data_3->name = "default_motor"; hw_interface.setDeviceData(motor_data_3); + auto digital_data_1 = getRandomDigitalDeviceData(ACTUATOR); + hw_interface.setDeviceData(digital_data_1); RobotHardwareInterface hw_interface_copy(device_config_map_ptr_single_joy_, hardware_type_e::V5_BRAIN); @@ -298,6 +300,11 @@ TEST_F(RobotHardwareInterfaceTestFixture, testSerializationPipelineV5ToCoprocess inertial_sensor_1->name = "inertial_sensor_1"; hw_interface.setDeviceData(inertial_sensor_1); + // Update Digital Sensor + auto digital_sensor_1 = getRandomDigitalDeviceData(SENSOR); + digital_sensor_1->name = "digital_sensor_1"; + hw_interface.setDeviceData(digital_sensor_1); + // Update Competition State hw_interface.setDisabledStatus(getRandomBool()); hw_interface.setAutonomousStatus(getRandomBool()); @@ -343,6 +350,11 @@ TEST_F(RobotHardwareInterfaceTestFixture, testSerializationPipelineV5ToCoprocess inertial_sensor_1->name = "inertial_sensor_1"; hw_interface.setDeviceData(inertial_sensor_1); + // Update Digital Sensor + auto digital_sensor_1 = getRandomDigitalDeviceData(SENSOR); + digital_sensor_1->name = "digital_sensor_1"; + hw_interface.setDeviceData(digital_sensor_1); + // Update Competition State hw_interface.setDisabledStatus(getRandomBool()); hw_interface.setAutonomousStatus(getRandomBool()); @@ -442,6 +454,8 @@ TEST_F(RobotHardwareInterfaceTestFixture, testInertialSensorStateGetters) { EXPECT_EQ(hw_interface.getInertialSensorHeading("inertial_sensor_1"), sensor_data_ptr->heading); } +//TODO(xander): testDigitalDeviceStateGetters + TEST_F(RobotHardwareInterfaceTestFixture, testSetMotorPositionCommand) { RobotHardwareInterface hw_interface(device_config_map_ptr_dual_joy_, hardware_type_e::COPROCESSOR); From 6643ad4f792f0bcc1281f4afaae1a70b25ac8d5b Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Thu, 6 Jun 2024 00:38:22 -0500 Subject: [PATCH 48/97] replaces previous digital io setup with new vectors in headers --- .../include/ghost_v5/globals/v5_globals.hpp | 5 ++--- 02_V5/ghost_pros/include/main.h | 15 ++------------- 2 files changed, 4 insertions(+), 16 deletions(-) diff --git a/02_V5/ghost_pros/include/ghost_v5/globals/v5_globals.hpp b/02_V5/ghost_pros/include/ghost_v5/globals/v5_globals.hpp index adbb69e2..ce709232 100644 --- a/02_V5/ghost_pros/include/ghost_v5/globals/v5_globals.hpp +++ b/02_V5/ghost_pros/include/ghost_v5/globals/v5_globals.hpp @@ -56,13 +56,12 @@ extern std::shared_ptr robot_hardwa extern std::unordered_map > motor_interfaces; extern std::unordered_map > encoders; extern std::unordered_map > imus; +std::unordered_map > digital_sensors; +std::unordered_map > digital_actuators; extern const pros::controller_analog_e_t joy_channels[4]; extern const pros::controller_digital_e_t joy_btns[12]; -extern pros::ADIDigitalOut adi_ports[8]; -extern std::vector digital_out_cmds; - // Serial Port extern std::shared_ptr serial_node_ptr; diff --git a/02_V5/ghost_pros/include/main.h b/02_V5/ghost_pros/include/main.h index 646c888e..f765c135 100644 --- a/02_V5/ghost_pros/include/main.h +++ b/02_V5/ghost_pros/include/main.h @@ -50,6 +50,8 @@ std::shared_ptr robot_hardware_inte std::unordered_map > motor_interfaces; std::unordered_map > encoders; std::unordered_map > imus; +std::unordered_map > digital_sensors; +std::unordered_map > digital_actuators; const pros::controller_analog_e_t joy_channels[4] = { ANALOG_LEFT_X, @@ -72,19 +74,6 @@ const pros::controller_digital_e_t joy_btns[12] = { DIGITAL_R2, }; -pros::ADIDigitalOut adi_ports[8] = { - pros::ADIDigitalOut('A', false), - pros::ADIDigitalOut('B', false), - pros::ADIDigitalOut('C', false), - pros::ADIDigitalOut('D', false), - pros::ADIDigitalOut('E', false), - pros::ADIDigitalOut('F', false), - pros::ADIDigitalOut('G', false), - pros::ADIDigitalOut('H', false), -}; - -std::vector digital_out_cmds(8, false); - // Serial Port std::shared_ptr serial_node_ptr; From 53ad581c37210f3fd029eefdc0cb06d7361fc4ba Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Thu, 6 Jun 2024 01:01:28 -0500 Subject: [PATCH 49/97] adds mutex for digital actuators just in case --- 02_V5/ghost_pros/include/ghost_v5/globals/v5_globals.hpp | 1 + 02_V5/ghost_pros/include/main.h | 1 + 2 files changed, 2 insertions(+) diff --git a/02_V5/ghost_pros/include/ghost_v5/globals/v5_globals.hpp b/02_V5/ghost_pros/include/ghost_v5/globals/v5_globals.hpp index ce709232..1d4f572b 100644 --- a/02_V5/ghost_pros/include/ghost_v5/globals/v5_globals.hpp +++ b/02_V5/ghost_pros/include/ghost_v5/globals/v5_globals.hpp @@ -49,6 +49,7 @@ extern pros::Controller controller_main; extern pros::Controller controller_partner; extern pros::Mutex actuator_update_lock; +extern pros::Mutex digital_actuator_update_lock; extern std::shared_ptr robot_device_config_map_ptr; extern std::shared_ptr robot_hardware_interface_ptr; diff --git a/02_V5/ghost_pros/include/main.h b/02_V5/ghost_pros/include/main.h index f765c135..2de58ce4 100644 --- a/02_V5/ghost_pros/include/main.h +++ b/02_V5/ghost_pros/include/main.h @@ -40,6 +40,7 @@ uint32_t loop_frequency = 10; std::atomic run = true; std::string error_str; pros::Mutex actuator_update_lock; +pros::Mutex digital_actuator_update_lock; pros::Controller controller_main(pros::E_CONTROLLER_MASTER); pros::Controller controller_partner(pros::E_CONTROLLER_PARTNER); From 0ebc19342bbbed95463cc7bc68181f2580a5cf7d Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Thu, 6 Jun 2024 04:54:24 -0500 Subject: [PATCH 50/97] makes interface class for digital actuators --- .../digital/v5_digital_actuator_interface.hpp | 59 +++++++++++++++++++ .../digital/v5_digital_actuator_interface.cpp | 39 ++++++++++++ 2 files changed, 98 insertions(+) create mode 100644 02_V5/ghost_pros/include/ghost_v5/digital/v5_digital_actuator_interface.hpp create mode 100644 02_V5/ghost_pros/src/ghost_v5/digital/v5_digital_actuator_interface.cpp diff --git a/02_V5/ghost_pros/include/ghost_v5/digital/v5_digital_actuator_interface.hpp b/02_V5/ghost_pros/include/ghost_v5/digital/v5_digital_actuator_interface.hpp new file mode 100644 index 00000000..cea67cd5 --- /dev/null +++ b/02_V5/ghost_pros/include/ghost_v5/digital/v5_digital_actuator_interface.hpp @@ -0,0 +1,59 @@ +/* + * 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 "ghost_v5_interfaces/devices/digital_device_interface.hpp" +#include "pros/apix.h" +#include "pros/error.h" +#include "pros/motors.hpp" + +#include + +namespace ghost_v5 { + +class V5DigitalActuatorInterface { +public: + V5DigitalActuatorInterface(std::shared_ptr); + + void updateInterface(); + + bool getDeviceIsConnected(){ + return device_connected_; + } + + void setValue(bool value){ + value_ = value; + } + + std::shared_ptr getDigitalActuatorPtr(){ + return digital_actuator_ptr_; + } + +private: + bool value_; + std::shared_ptr digital_actuator_ptr_; + bool device_connected_; + std::shared_ptr config_ptr_; +}; + +} // namespace ghost_v5 \ No newline at end of file diff --git a/02_V5/ghost_pros/src/ghost_v5/digital/v5_digital_actuator_interface.cpp b/02_V5/ghost_pros/src/ghost_v5/digital/v5_digital_actuator_interface.cpp new file mode 100644 index 00000000..a0eb1e7f --- /dev/null +++ b/02_V5/ghost_pros/src/ghost_v5/digital/v5_digital_actuator_interface.cpp @@ -0,0 +1,39 @@ +/* + * 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. + */ + +#include "ghost_v5/digital/v5_digital_actuator_interface.hpp" + +using namespace ghost_v5_interfaces::devices; + +namespace ghost_v5 { + +V5DigitalActuatorInterface::V5DigitalActuatorInterface(std::shared_ptr config_ptr){ + config_ptr_ = config_ptr->clone()->as(); + digital_actuator_ptr_ = std::make_shared(config_ptr->port - 21); +} + +void V5DigitalActuatorInterface::updateInterface(){ + digital_actuator_ptr_->set_value(value_); +} + +} // namespace ghost_v5 \ No newline at end of file From eae7a50cbe14bb993aa2fe9b98c351e2da8e85ee Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Thu, 6 Jun 2024 04:57:17 -0500 Subject: [PATCH 51/97] switches digital actuator vector to new interface --- 02_V5/ghost_pros/include/ghost_v5/globals/v5_globals.hpp | 3 ++- 02_V5/ghost_pros/include/main.h | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/02_V5/ghost_pros/include/ghost_v5/globals/v5_globals.hpp b/02_V5/ghost_pros/include/ghost_v5/globals/v5_globals.hpp index 1d4f572b..8828340b 100644 --- a/02_V5/ghost_pros/include/ghost_v5/globals/v5_globals.hpp +++ b/02_V5/ghost_pros/include/ghost_v5/globals/v5_globals.hpp @@ -32,6 +32,7 @@ #include "ghost_v5_interfaces/devices/device_config_map.hpp" #include "ghost_v5_interfaces/robot_hardware_interface.hpp" +#include "ghost_v5/digital/v5_digital_actuator_interface.hpp" #include "ghost_v5/motor/v5_motor_interface.hpp" #include "ghost_v5/screen/screen_interface.hpp" #include "ghost_v5/serial/v5_serial_node.hpp" @@ -58,7 +59,7 @@ extern std::unordered_map > encoders; extern std::unordered_map > imus; std::unordered_map > digital_sensors; -std::unordered_map > digital_actuators; +std::unordered_map > digital_actuators; extern const pros::controller_analog_e_t joy_channels[4]; extern const pros::controller_digital_e_t joy_btns[12]; diff --git a/02_V5/ghost_pros/include/main.h b/02_V5/ghost_pros/include/main.h index 2de58ce4..5f4492dc 100644 --- a/02_V5/ghost_pros/include/main.h +++ b/02_V5/ghost_pros/include/main.h @@ -27,6 +27,7 @@ #include "robot_config.hpp" #include "ghost_v5/motor/v5_motor_interface.hpp" +#include "ghost_v5/digital/v5_digital_actuator_interface.hpp" #include "ghost_v5_interfaces/devices/device_config_map.hpp" #include @@ -52,7 +53,7 @@ std::unordered_map > mo std::unordered_map > encoders; std::unordered_map > imus; std::unordered_map > digital_sensors; -std::unordered_map > digital_actuators; +std::unordered_map > digital_actuators; const pros::controller_analog_e_t joy_channels[4] = { ANALOG_LEFT_X, From 5c51071dd9653c252e0058b6d3b97db81ea2fe1b Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Thu, 6 Jun 2024 05:02:25 -0500 Subject: [PATCH 52/97] removes useless digital actuator mutex --- 02_V5/ghost_pros/include/ghost_v5/globals/v5_globals.hpp | 1 - 02_V5/ghost_pros/include/main.h | 1 - 2 files changed, 2 deletions(-) diff --git a/02_V5/ghost_pros/include/ghost_v5/globals/v5_globals.hpp b/02_V5/ghost_pros/include/ghost_v5/globals/v5_globals.hpp index 8828340b..8b1275a3 100644 --- a/02_V5/ghost_pros/include/ghost_v5/globals/v5_globals.hpp +++ b/02_V5/ghost_pros/include/ghost_v5/globals/v5_globals.hpp @@ -50,7 +50,6 @@ extern pros::Controller controller_main; extern pros::Controller controller_partner; extern pros::Mutex actuator_update_lock; -extern pros::Mutex digital_actuator_update_lock; extern std::shared_ptr robot_device_config_map_ptr; extern std::shared_ptr robot_hardware_interface_ptr; diff --git a/02_V5/ghost_pros/include/main.h b/02_V5/ghost_pros/include/main.h index 5f4492dc..eb02bfab 100644 --- a/02_V5/ghost_pros/include/main.h +++ b/02_V5/ghost_pros/include/main.h @@ -41,7 +41,6 @@ uint32_t loop_frequency = 10; std::atomic run = true; std::string error_str; pros::Mutex actuator_update_lock; -pros::Mutex digital_actuator_update_lock; pros::Controller controller_main(pros::E_CONTROLLER_MASTER); pros::Controller controller_partner(pros::E_CONTROLLER_PARTNER); From 5985a0c5cd8b73d604eef0a1dcc6c91c491b8bd9 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Thu, 6 Jun 2024 05:13:27 -0500 Subject: [PATCH 53/97] changes digital io in v5 code --- .../src/ghost_v5/serial/v5_serial_node.cpp | 18 ++++++++++--- 02_V5/ghost_pros/src/main.cpp | 27 +++++++++++++------ 2 files changed, 34 insertions(+), 11 deletions(-) diff --git a/02_V5/ghost_pros/src/ghost_v5/serial/v5_serial_node.cpp b/02_V5/ghost_pros/src/ghost_v5/serial/v5_serial_node.cpp index 0e94bcdb..f3b30c46 100644 --- a/02_V5/ghost_pros/src/ghost_v5/serial/v5_serial_node.cpp +++ b/02_V5/ghost_pros/src/ghost_v5/serial/v5_serial_node.cpp @@ -84,8 +84,6 @@ bool V5SerialNode::readV5ActuatorUpdate(){ void V5SerialNode::updateActuatorCommands(std::vector& buffer){ hardware_interface_ptr_->deserialize(buffer); - v5_globals::digital_out_cmds = hardware_interface_ptr_->getDigitalIO(); - for(const auto& name : *hardware_interface_ptr_){ auto device_data_ptr = hardware_interface_ptr_->getDeviceData(name); @@ -121,11 +119,19 @@ void V5SerialNode::updateActuatorCommands(std::vector& buffer){ } break; + case device_type_e::DIGITAL: + { + auto digital_device_data_ptr = device_data_ptr->as(); + if (digital_device_data_ptr->serial_config_.io_type == ACTUATOR) { + v5_globals::digital_actuators.at(name)->setValue(digital_device_data_ptr->value); + } + } + break; + case device_type_e::JOYSTICK: { continue; } - break; case device_type_e::INVALID: @@ -235,6 +241,12 @@ void V5SerialNode::writeV5StateUpdate(){ hardware_interface_ptr_->setDeviceData(inertial_sensor_data_ptr); } + // Digital Sensors + for (const auto& [name, device] : v5_globals::digital_sensors){ + auto digital_device_data_ptr = hardware_interface_ptr_->getDeviceData(name); + digital_device_data_ptr->value = device->get_value(); + } + serial_base_interface_->writeMsgToSerial(hardware_interface_ptr_->serialize().data(), sensor_update_msg_len_); } diff --git a/02_V5/ghost_pros/src/main.cpp b/02_V5/ghost_pros/src/main.cpp index e3bdb457..49288cfc 100644 --- a/02_V5/ghost_pros/src/main.cpp +++ b/02_V5/ghost_pros/src/main.cpp @@ -12,6 +12,7 @@ #include "ghost_v5_interfaces/util/device_type_helpers.hpp" +#include "ghost_v5/digital/v5_digital_actuator_interface.hpp" #include "ghost_v5/motor/v5_motor_interface.hpp" #include "ghost_v5/screen/screen_interface.hpp" #include "ghost_v5/serial/v5_serial_node.hpp" @@ -70,10 +71,6 @@ void zero_actuators(){ m.second->setMotorCommand(0.0, 0.0, 0.0, 0.0); } - // // Zero Pneumatics - // for(int i = 0; i < 8; i++){ - // v5_globals::adi_ports[i].set_value(false); - // } actuator_lock.unlock(); } @@ -85,10 +82,11 @@ void update_actuators(){ m.second->updateInterface(); } - // Update Pneumatics - for(int i = 0; i < 8; i++){ - v5_globals::adi_ports[i].set_value(v5_globals::digital_out_cmds[i]); + // Update digital actuators + for(auto & a : v5_globals::digital_actuators){ + a.second->updateInterface(); } + actuator_lock.unlock(); } @@ -142,7 +140,7 @@ void ghost_main_loop(){ // Send robot state over serial to coprocessor v5_globals::serial_node_ptr->writeV5StateUpdate(); - // Zero All Motors if disableds + // Zero All Motors if disabled if(pros::competition::is_disabled()){ zero_actuators(); } @@ -201,6 +199,19 @@ void initialize(){ } break; + case device_type_e::DIGITAL: + { + auto digital_device_config_ptr = config_ptr->as(); + auto io_type = digital_device_config_ptr->serial_config.io_type; + if (io_type == SENSOR) { + v5_globals::digital_sensors[device_name] = std::make_shared(digital_device_config_ptr->port - 21); + } + else if (io_type == ACTUATOR) { + v5_globals::digital_actuators[device_name] = std::make_shared(digital_device_config_ptr); + } + } + break; + case device_type_e::JOYSTICK: // Do nothing, these are initialized already break; From f3ed0e183528346b6b9ffe19d6ae3e76d38b8c6a Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Thu, 6 Jun 2024 05:30:44 -0500 Subject: [PATCH 54/97] adds digital device codegen --- .../src/util/device_config_factory_utils.cpp | 77 ++++++++++++++++++- 1 file changed, 76 insertions(+), 1 deletion(-) diff --git a/01_Libraries/ghost_v5_interfaces/src/util/device_config_factory_utils.cpp b/01_Libraries/ghost_v5_interfaces/src/util/device_config_factory_utils.cpp index 33a1781b..93c76820 100644 --- a/01_Libraries/ghost_v5_interfaces/src/util/device_config_factory_utils.cpp +++ b/01_Libraries/ghost_v5_interfaces/src/util/device_config_factory_utils.cpp @@ -22,11 +22,13 @@ */ #include +#include #include #include #include #include #include +#include #include #include #include @@ -68,7 +70,7 @@ std::shared_ptr loadRobotConfigFromYAML(YAML::Node node, bool v device_config_map_ptr->addDeviceConfig(joy_partner); } - // Iterate through each device defined in the YAML file + // Iterate through each smart device defined in the YAML file for (auto it = node["port_configuration"]["devices"].begin(); it != node["port_configuration"]["devices"].end(); it++) { @@ -149,6 +151,61 @@ std::shared_ptr loadRobotConfigFromYAML(YAML::Node node, bool v device_config_map_ptr->addDeviceConfig(device_config_base_ptr); } + + // Iterate through each ADI device defined in the YAML file + for (auto it = node["port_configuration"]["adi"].begin(); + it != node["port_configuration"]["adi"].end(); it++) + { + // Unpack Device Name and associated YAML Node + std::string device_name = it->first.as(); + YAML::Node device_yaml_node = it->second; + + // Load device type and device_config (if it exists) + std::string device_type; + loadYAMLParam(device_yaml_node, "type", device_type, verbose); + + std::shared_ptr device_config_base_ptr; + // Custom initialization based on device type + switch (STRING_TO_DEVICE_TYPE_MAP.at(device_type)) { + case device_type_e::DIGITAL: + { + // Load digital device config + auto digital_device_config_ptr = std::make_shared(); + std::string device_config_name; + if (loadYAMLParam(device_yaml_node, "config", device_config_name, verbose)) { + loadDigitalDeviceConfigFromYAML( + node["port_configuration"], device_name, + digital_device_config_ptr); + } + device_config_base_ptr = digital_device_config_ptr; + } + break; + + case device_type_e::INVALID: + { + throw std::runtime_error( + "[loadDeviceInterfaceMapFromYAML] Error: Device name " + device_name + + " has invalid type."); + } + break; + + default: + { + throw std::runtime_error( + "[loadDeviceInterfaceMapFromYAML] Error: Device type " + device_type + + " is not currently supported."); + } + break; + } + + // Set device base attributes + device_config_base_ptr->name = device_name; + loadYAMLParam(device_yaml_node, "port", device_config_base_ptr->port, verbose); + device_config_base_ptr->type = STRING_TO_DEVICE_TYPE_MAP.at(device_type); + + device_config_map_ptr->addDeviceConfig(device_config_base_ptr); + } + return device_config_map_ptr; } @@ -368,6 +425,24 @@ void generateCodeFromRobotConfig( config_ptr->serial_config.send_heading_data) + ";\n"; output_file << "\trobot_config->addDeviceConfig(" + sensor_name + ");\n"; output_file << "\n"; + } else if (val->type == device_type_e::DIGITAL) { + auto config_ptr = val->as(); + std::string device_name = config_ptr->name; + + output_file << + "\tstd::shared_ptr " + + device_name + + " = std::make_shared();\n"; + output_file << + "\t" + device_name + "->" + "port = " + std::to_string(config_ptr->port) + ";\n"; + output_file << "\t" + device_name + "->" + "name = \"" + device_name + "\";\n"; + output_file << + "\t" + device_name + "->" + + "type = ghost_v5_interfaces::devices::device_type_e::DIGITAL;\n"; + output_file << + "\t" + device_name + "->" + "serial_config.send_heading_data = " + ((config_ptr->serial_config.io_type == SENSOR) ? "SENSOR" : "ACTUATOR") + ";\n"; + output_file << "\trobot_config->addDeviceConfig(" + device_name + ");\n"; + output_file << "\n"; } else if (val->type == device_type_e::JOYSTICK) { auto config_ptr = val->as(); std::string joy_name = config_ptr->name; From 9675f29bb10d73c00a494c8073b0a5bcdfb4ba91 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Thu, 6 Jun 2024 07:32:11 -0500 Subject: [PATCH 55/97] fixes digital test util --- .../include/ghost_v5_interfaces/test/device_test_utils.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/test/device_test_utils.hpp b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/test/device_test_utils.hpp index ba819b4b..90eb2d8f 100644 --- a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/test/device_test_utils.hpp +++ b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/test/device_test_utils.hpp @@ -73,7 +73,7 @@ devices::InertialSensorDeviceData::SerialConfig getRandomInertialSensorSerialCon devices::DigitalDeviceData::SerialConfig getRandomDigitalDeviceSerialConfig() { devices::DigitalDeviceData::SerialConfig config; - config.io_type = ghost_util::getRandomBool() ? SENSOR : ACTUATOR; + config.io_type = ghost_util::getRandomBool() ? devices::digital_io_type_e::SENSOR : devices::digital_io_type_e::ACTUATOR; return config; } From d04b25a5a3d91e49a32a6a260c2c6c2fa5332029 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Thu, 6 Jun 2024 07:32:35 -0500 Subject: [PATCH 56/97] adds digital device yaml loader to cmake --- .../ghost_v5_interfaces/CMakeLists.txt | 1 + .../devices/test_digital_device_interface.cpp | 73 ++++++ .../test_load_digital_device_config_yaml.cpp | 219 ++++++++++++++++++ 3 files changed, 293 insertions(+) create mode 100644 01_Libraries/ghost_v5_interfaces/test/devices/test_digital_device_interface.cpp create mode 100644 01_Libraries/ghost_v5_interfaces/test/util/test_load_digital_device_config_yaml.cpp diff --git a/01_Libraries/ghost_v5_interfaces/CMakeLists.txt b/01_Libraries/ghost_v5_interfaces/CMakeLists.txt index 74699575..ba15f803 100644 --- a/01_Libraries/ghost_v5_interfaces/CMakeLists.txt +++ b/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}) diff --git a/01_Libraries/ghost_v5_interfaces/test/devices/test_digital_device_interface.cpp b/01_Libraries/ghost_v5_interfaces/test/devices/test_digital_device_interface.cpp new file mode 100644 index 00000000..c8d3880c --- /dev/null +++ b/01_Libraries/ghost_v5_interfaces/test/devices/test_digital_device_interface.cpp @@ -0,0 +1,73 @@ +// /* +// * 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. +// */ + + +// #include "ghost_v5_interfaces/devices/digital_device_interface.hpp" +// #include "ghost_v5_interfaces/test/device_test_utils.hpp" + +// #include +// #include "gtest/gtest.h" + +// using ghost_v5_interfaces::devices::hardware_type_e; +// using ghost_v5_interfaces::devices::MotorDeviceConfig; +// using ghost_v5_interfaces::devices::MotorDeviceData; +// using ghost_v5_interfaces::test_util::getRandomMotorData; +// using ghost_v5_interfaces::test_util::getRandomMotorSerialConfig; + +// const int NUM_TESTS = 50; +// TEST(TestMotorDeviceInterface, testSerializationV5ToCoprocessor) { +// for (int i = 0; i < NUM_TESTS; i++) { +// auto serial_config = getRandomMotorSerialConfig(); +// auto data_1 = getRandomMotorData(false, serial_config); +// auto data_2 = std::make_shared("test", serial_config); +// auto serial_stream_2 = data_1->serialize(hardware_type_e::V5_BRAIN); +// data_2->deserialize(serial_stream_2, hardware_type_e::COPROCESSOR); + +// EXPECT_EQ(*data_1, *data_2); +// } +// } + +// TEST(TestMotorDeviceInterface, testSerializationCoprocessorToV5) { +// for (int i = 0; i < NUM_TESTS; i++) { +// auto serial_config = getRandomMotorSerialConfig(); +// auto data_1 = getRandomMotorData(true, serial_config); +// auto data_2 = std::make_shared("test", serial_config); +// auto serial_stream_1 = data_1->serialize(hardware_type_e::COPROCESSOR); +// data_2->deserialize(serial_stream_1, hardware_type_e::V5_BRAIN); + +// EXPECT_EQ(*data_1, *data_2); +// } +// } + +// TEST(TestMotorDeviceInterface, testSerialMsgLengths) { +// for (int i = 0; i < NUM_TESTS; i++) { +// auto serial_config = getRandomMotorSerialConfig(); +// auto m1 = getRandomMotorData(false, serial_config); +// auto m2 = getRandomMotorData(true, serial_config); + +// EXPECT_EQ(m1->serialize(hardware_type_e::COPROCESSOR).size(), m1->getActuatorPacketSize()); +// EXPECT_EQ(m1->serialize(hardware_type_e::V5_BRAIN).size(), m1->getSensorPacketSize()); +// EXPECT_EQ(m2->serialize(hardware_type_e::COPROCESSOR).size(), m1->getActuatorPacketSize()); +// EXPECT_EQ(m2->serialize(hardware_type_e::V5_BRAIN).size(), m1->getSensorPacketSize()); +// } +// } diff --git a/01_Libraries/ghost_v5_interfaces/test/util/test_load_digital_device_config_yaml.cpp b/01_Libraries/ghost_v5_interfaces/test/util/test_load_digital_device_config_yaml.cpp new file mode 100644 index 00000000..fba8b206 --- /dev/null +++ b/01_Libraries/ghost_v5_interfaces/test/util/test_load_digital_device_config_yaml.cpp @@ -0,0 +1,219 @@ +// /* +// * 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. +// */ + + +// #include +// #include "gtest/gtest.h" +// #include "yaml-cpp/yaml.h" + +// using namespace ghost_v5_interfaces::devices; +// using namespace ghost_v5_interfaces::util; +// using namespace ghost_v5_interfaces; + +// class TestLoadMotorDeviceConfigYAML : public ::testing::Test +// { +// protected: +// void SetUp() override +// { +// std::string config_path = std::string(getenv("VEXU_HOME")) + +// "/01_Libraries/ghost_v5_interfaces/test/config/test_load_motor_config.yaml"; +// config_yaml_ = YAML::LoadFile(config_path); + +// // Expected motor configurations +// motor_empty_config_ = std::make_shared(); +// motor_empty_config_->port = 0; +// motor_empty_config_->name = "motor_empty_config"; +// motor_empty_config_->type = device_type_e::MOTOR; + +// left_drive_motor_config_ = std::make_shared(); +// left_drive_motor_config_->gearset = ghost_gearset::GEARSET_600; +// left_drive_motor_config_->encoder_units = ghost_encoder_unit::ENCODER_DEGREES; +// left_drive_motor_config_->controller_config.pos_gain = 7.5; +// left_drive_motor_config_->port = 1; +// left_drive_motor_config_->name = "left_drive_motor"; +// left_drive_motor_config_->type = device_type_e::MOTOR; +// left_drive_motor_config_->serial_config.send_torque_command = true; +// left_drive_motor_config_->serial_config.send_temp_data = true; + +// right_drive_motor_config_ = std::make_shared(); +// right_drive_motor_config_->gearset = ghost_gearset::GEARSET_600; +// right_drive_motor_config_->encoder_units = ghost_encoder_unit::ENCODER_DEGREES; +// right_drive_motor_config_->controller_config.pos_gain = 7.5; +// right_drive_motor_config_->port = 2; +// right_drive_motor_config_->name = "right_drive_motor"; +// right_drive_motor_config_->reversed = true; +// right_drive_motor_config_->type = device_type_e::MOTOR; +// right_drive_motor_config_->serial_config.send_torque_command = true; +// right_drive_motor_config_->serial_config.send_temp_data = true; +// } + +// std::shared_ptr motor_empty_config_; +// std::shared_ptr motor_no_config_; +// std::shared_ptr left_drive_motor_config_; +// std::shared_ptr right_drive_motor_config_; +// std::shared_ptr test_motor_config_; + +// YAML::Node config_yaml_; +// }; + +// /** +// * @brief Test that a DCMotorModel::Config struct can be properly loaded from YAML +// */ +// TEST_F(TestLoadMotorDeviceConfigYAML, testLoadMotorModelConfig) { +// DCMotorModel::Config test_model_config{}; +// test_model_config.free_speed = 1104.0; +// test_model_config.stall_torque = 52.25; +// test_model_config.free_current = 0.9090; +// test_model_config.stall_current = 21.58; +// test_model_config.nominal_voltage = 1200.0; +// test_model_config.gear_ratio = 2915.0; + +// DCMotorModel::Config loaded_model_config; +// EXPECT_NO_THROW( +// loaded_model_config = +// loadMotorModelConfigFromYAML( +// config_yaml_["port_configuration"]["device_configurations"][ +// "test_motor_config"]["model"])); +// EXPECT_EQ(test_model_config, loaded_model_config); +// } + +// /** +// * @brief Test that a MotorController::Config struct can be properly loaded from YAML +// */ +// TEST_F(TestLoadMotorDeviceConfigYAML, testLoadMotorControllerConfig) { +// MotorController::Config test_controller_config{}; +// test_controller_config.pos_gain = 8000.0; +// test_controller_config.vel_gain = 118.0; +// test_controller_config.ff_vel_gain = 400.0; +// test_controller_config.ff_torque_gain = 2587.0; +// test_controller_config.cmd_duration = 4; + +// MotorController::Config loaded_controller_config; +// EXPECT_NO_THROW( +// loaded_controller_config = +// loadMotorControllerConfigFromYAML( +// config_yaml_["port_configuration"]["device_configurations"][ +// "test_motor_config"]["controller"])); +// EXPECT_EQ(test_controller_config, loaded_controller_config); +// } + +// /** +// * @brief Test that a SecondOrderLowPassFilter::Config struct can be properly loaded from YAML +// */ +// TEST_F(TestLoadMotorDeviceConfigYAML, testLoadLowPassFilterConfig) { +// SecondOrderLowPassFilter::Config test_filter_config{}; +// test_filter_config.cutoff_frequency = 62.0; +// test_filter_config.damping_ratio = 0.10; +// test_filter_config.timestep = 0.21; + +// SecondOrderLowPassFilter::Config loaded_filter_config; +// EXPECT_NO_THROW( +// loaded_filter_config = +// loadLowPassFilterConfigFromYAML( +// config_yaml_["port_configuration"]["device_configurations"][ +// "test_motor_config"]["filter"])); +// EXPECT_EQ(test_filter_config, loaded_filter_config); +// } + +// /** +// * @brief Test that a MotorDeviceData::SerialConfig struct can be properly loaded from YAML +// */ +// TEST_F(TestLoadMotorDeviceConfigYAML, testLoadMotorSerialConfig) { +// MotorDeviceData::SerialConfig test_serial_config{}; +// test_serial_config.send_position_command = true; +// test_serial_config.send_velocity_command = true; +// test_serial_config.send_voltage_command = false; +// test_serial_config.send_torque_command = true; +// test_serial_config.send_current_data = true; +// test_serial_config.send_power_data = false; +// test_serial_config.send_temp_data = true; + +// MotorDeviceData::SerialConfig loaded_serial_config; +// EXPECT_NO_THROW( +// loaded_serial_config = +// loadMotorSerialConfigFromYAML( +// config_yaml_["port_configuration"]["device_configurations"][ +// "test_motor_config"]["serial"])); +// EXPECT_EQ(test_serial_config, loaded_serial_config); +// } + +// /** +// * @brief Test that all fields are properly set when loading the MotorDeviceConfig class +// */ +// TEST_F(TestLoadMotorDeviceConfigYAML, testLoadCustomMotorConfigFromYAML) { +// auto config_node = config_yaml_["port_configuration"]; +// std::string motor_name = "right_drive_motor"; +// auto config_ptr = std::make_shared(); +// loadMotorDeviceConfigFromYAML(config_node, motor_name, config_ptr); +// EXPECT_EQ(*config_ptr, *right_drive_motor_config_); +// } + +// /** +// * @brief Test that missing fields will be populated with the default args for the MotorDeviceConfig class +// */ +// TEST_F(TestLoadMotorDeviceConfigYAML, testLoadDefaultMotorConfigFromYAML) { +// auto config_node = config_yaml_["port_configuration"]; +// std::string motor_name = "motor_empty_config"; +// auto config_ptr = std::make_shared(); +// loadMotorDeviceConfigFromYAML(config_node, motor_name, config_ptr); +// EXPECT_EQ(*config_ptr, *motor_empty_config_); +// } + +// /** +// * @brief Test that if the motor config is not found, it throws. +// */ +// TEST_F(TestLoadMotorDeviceConfigYAML, testMissingConfigThrows) { +// auto config_node = config_yaml_["port_configuration"]; +// std::string motor_name = "motor_no_config"; +// auto config_ptr = std::make_shared(); +// EXPECT_THROW( +// loadMotorDeviceConfigFromYAML( +// config_node, motor_name, +// config_ptr), std::runtime_error); +// } + +// /** +// * @brief Test that if the device config is missing the port attribute, it throws. +// */ +// TEST_F(TestLoadMotorDeviceConfigYAML, testMissingPortThrowsError) { +// auto config_node = config_yaml_["port_configuration"]; +// std::string motor_name = "motor_no_port"; +// auto config_ptr = std::make_shared(); +// EXPECT_THROW( +// loadMotorDeviceConfigFromYAML( +// config_node, motor_name, +// config_ptr), std::runtime_error); +// } + +// /** +// * @brief Test that if the device config is missing the type attribute, it throws +// */ +// TEST_F(TestLoadMotorDeviceConfigYAML, testMissingTypeThrowsError) { +// auto config_node = config_yaml_["port_configuration"]; +// std::string motor_name = "motor_no_type"; +// auto config_ptr = std::make_shared(); +// EXPECT_THROW( +// loadMotorDeviceConfigFromYAML( +// config_node, motor_name, +// config_ptr), std::runtime_error); +// } From 45874a3ad45cca08408d5d8702cc9b6ff3c381ae Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Thu, 6 Jun 2024 13:18:52 -0500 Subject: [PATCH 57/97] fixes yaml conversions --- .../src/util/load_digital_device_config_yaml.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/src/util/load_digital_device_config_yaml.cpp b/01_Libraries/ghost_v5_interfaces/src/util/load_digital_device_config_yaml.cpp index 38dd27b3..e3c38560 100644 --- a/01_Libraries/ghost_v5_interfaces/src/util/load_digital_device_config_yaml.cpp +++ b/01_Libraries/ghost_v5_interfaces/src/util/load_digital_device_config_yaml.cpp @@ -51,12 +51,14 @@ void loadDigitalDeviceConfigFromYAML( device_config_ptr->type = device_type_e::DIGITAL; // Get port - char port; - if (!loadYAMLParam(device_node, "port", port, verbose)) { + std::string port_string; + if (!loadYAMLParam(device_node, "port", port_string, verbose)) { throw std::runtime_error( "[loadDigitalDeviceConfigFromYAML] Error: Port not specified for Digital Device " + device_name + "!"); } + char port = port_string[0]; + // Validate port const std::vector valid_ports = {'A', 'B', 'C', 'D', 'E', 'F', 'G', 'H'}; From 6ccd49162eef8351ca6d659d1e63055d313239f6 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Thu, 6 Jun 2024 14:09:48 -0500 Subject: [PATCH 58/97] changes io_type to device types --- .../devices/device_interfaces.hpp | 8 ++--- .../devices/digital_device_interface.hpp | 36 ++++++------------- .../util/device_config_factory_utils.hpp | 3 +- .../util/device_type_helpers.hpp | 14 +++----- .../test/config/example_robot_2.yaml | 6 ++-- 5 files changed, 19 insertions(+), 48 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/device_interfaces.hpp b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/device_interfaces.hpp index 7939acd9..6bba0a5f 100644 --- a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/device_interfaces.hpp +++ b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/device_interfaces.hpp @@ -46,15 +46,11 @@ enum device_type_e GPS_SENSOR, // Unsupported RADIO, // Unsupported JOYSTICK, - DIGITAL, + DIGITAL_INPUT, + DIGITAL_OUTPUT, INVALID }; -enum digital_io_type_e { - ACTUATOR, - SENSOR -}; - enum hardware_type_e { COPROCESSOR, diff --git a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_device_interface.hpp b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_device_interface.hpp index 991e9254..643fae54 100644 --- a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_device_interface.hpp +++ b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_device_interface.hpp @@ -35,32 +35,20 @@ namespace devices class DigitalDeviceData : public DeviceData { public: - struct SerialConfig - { - SerialConfig() - { - } - - bool operator== (const SerialConfig &rhs) const { - return io_type == rhs.io_type; - } - digital_io_type_e io_type; - }; - - DigitalDeviceData(std::string name, SerialConfig serial_config = SerialConfig()) - : DeviceData(name, device_type_e::DIGITAL) + DigitalDeviceData(std::string name, device_type_e type) + : DeviceData(name, type) { } int getActuatorPacketSize() const override { - return 1; + return 0; } int getSensorPacketSize() const override { - return 1; + return 0; } bool value; @@ -79,14 +67,14 @@ class DigitalDeviceData : public DeviceData bool operator==(const DeviceBase & rhs) const override { const DigitalDeviceData * d_rhs = dynamic_cast(&rhs); - return (d_rhs != nullptr && value == d_rhs->value && serial_config_ == d_rhs->serial_config_); + return (d_rhs != nullptr && value == d_rhs->value); } std::vector serialize(hardware_type_e hardware_type) const override { std::vector msg(1); - bool valid = hardware_type == V5_BRAIN && serial_config_.io_type == SENSOR - || hardware_type == COPROCESSOR && serial_config_.io_type == ACTUATOR; + bool valid = hardware_type == V5_BRAIN && type == DIGITAL_INPUT + || hardware_type == COPROCESSOR && type == DIGITAL_OUTPUT; if (valid) { msg.push_back((unsigned char) value); } @@ -95,15 +83,13 @@ class DigitalDeviceData : public DeviceData void deserialize(const std::vector & msg, hardware_type_e hardware_type) override { - bool valid = hardware_type == V5_BRAIN && serial_config_.io_type == SENSOR - || hardware_type == COPROCESSOR && serial_config_.io_type == ACTUATOR; + bool valid = hardware_type == V5_BRAIN && type == DIGITAL_INPUT + || hardware_type == COPROCESSOR && type == DIGITAL_OUTPUT; if (!valid) { return; } value = ((bool) msg.data()[0]); } - - SerialConfig serial_config_; }; class DigitalDeviceConfig : public DeviceConfig @@ -118,10 +104,8 @@ class DigitalDeviceConfig : public DeviceConfig { const DigitalDeviceConfig * d_rhs = dynamic_cast(&rhs); return (d_rhs != nullptr) && (port == d_rhs->port) && (name == d_rhs->name) && - (type == d_rhs->type) && (serial_config == d_rhs->serial_config); + (type == d_rhs->type); } - - DigitalDeviceData::SerialConfig serial_config; }; } // namespace devices diff --git a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/util/device_config_factory_utils.hpp b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/util/device_config_factory_utils.hpp index 0d35ec63..456facd5 100644 --- a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/util/device_config_factory_utils.hpp +++ b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/util/device_config_factory_utils.hpp @@ -45,8 +45,7 @@ namespace util * adi: * limit_switch_1: * port: A - * type: DIGITAL - * io_type: SENSOR + * type: DIGITAL_INPUT * devices: * my_motor_name_here: * port: 1 diff --git a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/util/device_type_helpers.hpp b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/util/device_type_helpers.hpp index c6dd8354..2620d804 100644 --- a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/util/device_type_helpers.hpp +++ b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/util/device_type_helpers.hpp @@ -42,7 +42,8 @@ const std::unordered_map 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", devices::device_type_e::DIGITAL}, + {"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 DEVICE_TYPE_TO_STRING_MAP{ @@ -55,17 +56,10 @@ const std::unordered_map 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, "DIGITAL"}, + {devices::device_type_e::DIGITAL_INPUT, "DIGITAL_INPUT"}, + {devices::device_type_e::DIGITAL_OUTPUT, "DIGITAL_OUTPUT"}, {devices::device_type_e::INVALID, "INVALID"} }; -const std::unordered_map DIGITAL_IO_TYPE_TO_STRING_MAP{ - {devices::digital_io_type_e::SENSOR, "SENSOR"}, - {devices::digital_io_type_e::ACTUATOR, "ACTUATOR"} -}; -const std::unordered_map STRING_TO_DIGITAL_IO_TYPE_MAP{ - {"SENSOR", devices::digital_io_type_e::SENSOR}, - {"ACTUATOR", devices::digital_io_type_e::ACTUATOR} -}; } // namespace ghost_v5_interfaces diff --git a/01_Libraries/ghost_v5_interfaces/test/config/example_robot_2.yaml b/01_Libraries/ghost_v5_interfaces/test/config/example_robot_2.yaml index ba98e99b..4ffdd0ef 100644 --- a/01_Libraries/ghost_v5_interfaces/test/config/example_robot_2.yaml +++ b/01_Libraries/ghost_v5_interfaces/test/config/example_robot_2.yaml @@ -3,12 +3,10 @@ port_configuration: digital: limit_switch_1: port: A - type: DIGITAL - io_type: SENSOR + type: DIGITAL_INPUT arm_solenoid: port: B - type: DIGITAL - io_type: ACTUATOR + type: DIGITAL_OUTPUT devices: rotation_sensor_1: port: 4 From 635476d487c1740448dd1d15b0486d54473717f6 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Thu, 6 Jun 2024 14:35:10 -0500 Subject: [PATCH 59/97] adds digital child classes --- .../devices/digital_device_interface.hpp | 45 +++++++++++++++---- 1 file changed, 37 insertions(+), 8 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_device_interface.hpp b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_device_interface.hpp index 643fae54..5b1482ba 100644 --- a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_device_interface.hpp +++ b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_device_interface.hpp @@ -69,13 +69,45 @@ class DigitalDeviceData : public DeviceData const DigitalDeviceData * d_rhs = dynamic_cast(&rhs); return (d_rhs != nullptr && value == d_rhs->value); } +}; + +class DigitalInputDeviceData : public DigitalDeviceData +{ + public: + DigitalInputDeviceData(std::string name) + : DigitalDeviceData(name, DIGITAL_INPUT) + { + } + + std::vector serialize(hardware_type_e hardware_type) const override + { + std::vector msg(1); + if (hardware_type == V5_BRAIN) { + msg.push_back((unsigned char) value); + } + return msg; + } + + void deserialize(const std::vector & msg, hardware_type_e hardware_type) override + { + if (hardware_type == COPROCESSOR) { + value = ((bool) msg.data()[0]); + } + } +}; + +class DigitalOutputDeviceData : public DigitalDeviceData +{ + public: + DigitalOutputDeviceData(std::string name) + : DigitalDeviceData(name, DIGITAL_OUTPUT) + { + } std::vector serialize(hardware_type_e hardware_type) const override { std::vector msg(1); - bool valid = hardware_type == V5_BRAIN && type == DIGITAL_INPUT - || hardware_type == COPROCESSOR && type == DIGITAL_OUTPUT; - if (valid) { + if (hardware_type == COPROCESSOR) { msg.push_back((unsigned char) value); } return msg; @@ -83,12 +115,9 @@ class DigitalDeviceData : public DeviceData void deserialize(const std::vector & msg, hardware_type_e hardware_type) override { - bool valid = hardware_type == V5_BRAIN && type == DIGITAL_INPUT - || hardware_type == COPROCESSOR && type == DIGITAL_OUTPUT; - if (!valid) { - return; + if (hardware_type == V5_BRAIN) { + value = ((bool) msg.data()[0]); } - value = ((bool) msg.data()[0]); } }; From 3ae0c82b916bb6a1f223692e18aae66153c292c2 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Thu, 6 Jun 2024 14:49:50 -0500 Subject: [PATCH 60/97] removes digital device base class --- .../devices/digital_device_interface.hpp | 85 ++++++++++++------- 1 file changed, 52 insertions(+), 33 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_device_interface.hpp b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_device_interface.hpp index 5b1482ba..6bf6c2b7 100644 --- a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_device_interface.hpp +++ b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_device_interface.hpp @@ -32,12 +32,11 @@ namespace ghost_v5_interfaces namespace devices { -class DigitalDeviceData : public DeviceData +class DigitalInputDeviceData : public DeviceData { -public: - - DigitalDeviceData(std::string name, device_type_e type) - : DeviceData(name, type) + public: + DigitalInputDeviceData(std::string name) + : DeviceData(name, DIGITAL_INPUT) { } @@ -45,38 +44,15 @@ class DigitalDeviceData : public DeviceData { return 0; } - + int getSensorPacketSize() const override { - return 0; - } - - bool value; - - void update(std::shared_ptr data_ptr) override - { - auto digital_data_ptr = data_ptr->as(); - value = digital_data_ptr->value; + return 1; } std::shared_ptr clone() const override { - return std::make_shared(*this); - } - - bool operator==(const DeviceBase & rhs) const override - { - const DigitalDeviceData * d_rhs = dynamic_cast(&rhs); - return (d_rhs != nullptr && value == d_rhs->value); - } -}; - -class DigitalInputDeviceData : public DigitalDeviceData -{ - public: - DigitalInputDeviceData(std::string name) - : DigitalDeviceData(name, DIGITAL_INPUT) - { + return std::make_shared(*this); } std::vector serialize(hardware_type_e hardware_type) const override @@ -94,16 +70,45 @@ class DigitalInputDeviceData : public DigitalDeviceData value = ((bool) msg.data()[0]); } } + + void update(std::shared_ptr data_ptr) override + { + auto digital_data_ptr = data_ptr->as(); + value = digital_data_ptr->value; + } + + bool operator==(const DeviceBase & rhs) const override + { + const DigitalInputDeviceData * d_rhs = dynamic_cast(&rhs); + return (d_rhs != nullptr) && (value == d_rhs->value); + } + + bool value; }; -class DigitalOutputDeviceData : public DigitalDeviceData +class DigitalOutputDeviceData : public DeviceData { public: DigitalOutputDeviceData(std::string name) - : DigitalDeviceData(name, DIGITAL_OUTPUT) + : DeviceData(name, DIGITAL_OUTPUT) { } + int getActuatorPacketSize() const override + { + return 1; + } + + int getSensorPacketSize() const override + { + return 0; + } + + std::shared_ptr clone() const override + { + return std::make_shared(*this); + } + std::vector serialize(hardware_type_e hardware_type) const override { std::vector msg(1); @@ -119,6 +124,20 @@ class DigitalOutputDeviceData : public DigitalDeviceData value = ((bool) msg.data()[0]); } } + + void update(std::shared_ptr data_ptr) override + { + auto digital_data_ptr = data_ptr->as(); + value = digital_data_ptr->value; + } + + bool operator==(const DeviceBase & rhs) const override + { + const DigitalOutputDeviceData * d_rhs = dynamic_cast(&rhs); + return (d_rhs != nullptr) && (value == d_rhs->value); + } + + bool value; }; class DigitalDeviceConfig : public DeviceConfig From f448004e92ab17053765d377f64d21631eccddb5 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Thu, 6 Jun 2024 14:50:13 -0500 Subject: [PATCH 61/97] updates digital test helpers --- .../test/device_test_utils.hpp | 30 +++++++------------ 1 file changed, 11 insertions(+), 19 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/test/device_test_utils.hpp b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/test/device_test_utils.hpp index 90eb2d8f..0ade0051 100644 --- a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/test/device_test_utils.hpp +++ b/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 @@ -70,13 +70,6 @@ devices::InertialSensorDeviceData::SerialConfig getRandomInertialSensorSerialCon return config; } -devices::DigitalDeviceData::SerialConfig getRandomDigitalDeviceSerialConfig() -{ - devices::DigitalDeviceData::SerialConfig config; - config.io_type = ghost_util::getRandomBool() ? devices::digital_io_type_e::SENSOR : devices::digital_io_type_e::ACTUATOR; - return config; -} - std::shared_ptr getRandomJoystickData() { auto joy_ptr = std::make_shared("joy_" + std::to_string(rand() % 2)); @@ -191,19 +184,18 @@ std::shared_ptr getRandomInertialSensorData( return inertial_sensor_ptr; } -std::shared_ptr getRandomDigitalDeviceData( - devices::digital_io_type_e io_type, - devices::DigitalDeviceData::SerialConfig serial_config = devices::DigitalDeviceData::SerialConfig()) -{ - auto digital_device_ptr = std::make_shared( - "test", - serial_config); - digital_device_ptr->value = ghost_util::getRandomBool(); - digital_device_ptr->serial_config_.io_type = io_type; - return digital_device_ptr; -} +std::shared_ptr getRandomDigitalOutputDeviceData(){ + auto digital_output_ptr = std::make_shared("test"); + digital_output_ptr->value = ghost_util::getRandomBool(); + return digital_output_ptr; +} +std::shared_ptr getRandomDigitalInputDeviceData(){ + auto digital_input_ptr = std::make_shared("test"); + digital_input_ptr->value = ghost_util::getRandomBool(); + return digital_input_ptr; +} } } From adec68b853e1ca81292140f96bb57a00fc134932 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Thu, 6 Jun 2024 14:51:47 -0500 Subject: [PATCH 62/97] updates robot config yaml --- .../config/robot_hardware_config_worlds_24.yaml | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/11_Robots/ghost_over_under/config/robot_hardware_config_worlds_24.yaml b/11_Robots/ghost_over_under/config/robot_hardware_config_worlds_24.yaml index 1f2dc8e7..48641404 100644 --- a/11_Robots/ghost_over_under/config/robot_hardware_config_worlds_24.yaml +++ b/11_Robots/ghost_over_under/config/robot_hardware_config_worlds_24.yaml @@ -2,20 +2,16 @@ port_configuration: adi: tail: port: A - type: DIGITAL - io_type: ACTUATOR + type: DIGITAL_OUTPUT claw: port: B - type: DIGITAL - io_type: ACTUATOR + type: DIGITAL_OUTPUT claw_inner: port: C - type: DIGITAL - io_type: SENSOR + type: DIGITAL_INPUT claw_outer: port: D - type: DIGITAL - io_type: SENSOR + type: DIGITAL_INPUT devices: steering_front_left: port: 1 From 52088069fe226454e1db7f5d8ec5ba7a5b3b3f44 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Thu, 6 Jun 2024 14:58:13 -0500 Subject: [PATCH 63/97] updates device config factory --- .../src/util/device_config_factory_utils.cpp | 91 +++++++------------ 1 file changed, 33 insertions(+), 58 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/src/util/device_config_factory_utils.cpp b/01_Libraries/ghost_v5_interfaces/src/util/device_config_factory_utils.cpp index 93c76820..fa6aed15 100644 --- a/01_Libraries/ghost_v5_interfaces/src/util/device_config_factory_utils.cpp +++ b/01_Libraries/ghost_v5_interfaces/src/util/device_config_factory_utils.cpp @@ -43,7 +43,6 @@ using namespace ghost_v5_interfaces::devices; namespace ghost_v5_interfaces { - namespace util { @@ -70,6 +69,22 @@ std::shared_ptr loadRobotConfigFromYAML(YAML::Node node, bool v device_config_map_ptr->addDeviceConfig(joy_partner); } + // Iterate through Digital IO Ports + if (node["digital"]) { + + + for (auto it = node["port_configuration"]["digital"].begin(); + it != node["port_configuration"]["digital"].end(); it++) + { + std::string device_name = it->first.as(); + YAML::Node device_yaml_node = it->second; + + auto device_config_base_ptr = std::make_shared(); + loadDigitalDeviceConfigFromYAML(device_yaml_node, device_name, device_config_base_ptr, verbose); + device_config_map_ptr->addDeviceConfig(device_config_base_ptr); + } + } + // Iterate through each smart device defined in the YAML file for (auto it = node["port_configuration"]["devices"].begin(); it != node["port_configuration"]["devices"].end(); it++) @@ -152,60 +167,6 @@ std::shared_ptr loadRobotConfigFromYAML(YAML::Node node, bool v device_config_map_ptr->addDeviceConfig(device_config_base_ptr); } - // Iterate through each ADI device defined in the YAML file - for (auto it = node["port_configuration"]["adi"].begin(); - it != node["port_configuration"]["adi"].end(); it++) - { - // Unpack Device Name and associated YAML Node - std::string device_name = it->first.as(); - YAML::Node device_yaml_node = it->second; - - // Load device type and device_config (if it exists) - std::string device_type; - loadYAMLParam(device_yaml_node, "type", device_type, verbose); - - std::shared_ptr device_config_base_ptr; - // Custom initialization based on device type - switch (STRING_TO_DEVICE_TYPE_MAP.at(device_type)) { - case device_type_e::DIGITAL: - { - // Load digital device config - auto digital_device_config_ptr = std::make_shared(); - std::string device_config_name; - if (loadYAMLParam(device_yaml_node, "config", device_config_name, verbose)) { - loadDigitalDeviceConfigFromYAML( - node["port_configuration"], device_name, - digital_device_config_ptr); - } - device_config_base_ptr = digital_device_config_ptr; - } - break; - - case device_type_e::INVALID: - { - throw std::runtime_error( - "[loadDeviceInterfaceMapFromYAML] Error: Device name " + device_name + - " has invalid type."); - } - break; - - default: - { - throw std::runtime_error( - "[loadDeviceInterfaceMapFromYAML] Error: Device type " + device_type + - " is not currently supported."); - } - break; - } - - // Set device base attributes - device_config_base_ptr->name = device_name; - loadYAMLParam(device_yaml_node, "port", device_config_base_ptr->port, verbose); - device_config_base_ptr->type = STRING_TO_DEVICE_TYPE_MAP.at(device_type); - - device_config_map_ptr->addDeviceConfig(device_config_base_ptr); - } - return device_config_map_ptr; } @@ -425,7 +386,7 @@ void generateCodeFromRobotConfig( config_ptr->serial_config.send_heading_data) + ";\n"; output_file << "\trobot_config->addDeviceConfig(" + sensor_name + ");\n"; output_file << "\n"; - } else if (val->type == device_type_e::DIGITAL) { + } else if (val->type == device_type_e::DIGITAL_INPUT) { auto config_ptr = val->as(); std::string device_name = config_ptr->name; @@ -438,9 +399,23 @@ void generateCodeFromRobotConfig( output_file << "\t" + device_name + "->" + "name = \"" + device_name + "\";\n"; output_file << "\t" + device_name + "->" + - "type = ghost_v5_interfaces::devices::device_type_e::DIGITAL;\n"; + "type = ghost_v5_interfaces::devices::device_type_e::DIGITAL_INPUT;\n"; + output_file << "\trobot_config->addDeviceConfig(" + device_name + ");\n"; + output_file << "\n"; + } else if (val->type == device_type_e::DIGITAL_INPUT) { + auto config_ptr = val->as(); + std::string device_name = config_ptr->name; + output_file << - "\t" + device_name + "->" + "serial_config.send_heading_data = " + ((config_ptr->serial_config.io_type == SENSOR) ? "SENSOR" : "ACTUATOR") + ";\n"; + "\tstd::shared_ptr " + + device_name + + " = std::make_shared();\n"; + output_file << + "\t" + device_name + "->" + "port = " + std::to_string(config_ptr->port) + ";\n"; + output_file << "\t" + device_name + "->" + "name = \"" + device_name + "\";\n"; + output_file << + "\t" + device_name + "->" + + "type = ghost_v5_interfaces::devices::device_type_e::DIGITAL_OUTPUT;\n"; output_file << "\trobot_config->addDeviceConfig(" + device_name + ");\n"; output_file << "\n"; } else if (val->type == device_type_e::JOYSTICK) { From 80806c3b9b63fd9f6541c4da64c0a9add6dda3c3 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Thu, 6 Jun 2024 15:04:46 -0500 Subject: [PATCH 64/97] updates v5 serial node --- .../src/ghost_v5/serial/v5_serial_node.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/02_V5/ghost_pros/src/ghost_v5/serial/v5_serial_node.cpp b/02_V5/ghost_pros/src/ghost_v5/serial/v5_serial_node.cpp index f3b30c46..455d1e1a 100644 --- a/02_V5/ghost_pros/src/ghost_v5/serial/v5_serial_node.cpp +++ b/02_V5/ghost_pros/src/ghost_v5/serial/v5_serial_node.cpp @@ -119,12 +119,16 @@ void V5SerialNode::updateActuatorCommands(std::vector& buffer){ } break; - case device_type_e::DIGITAL: + case device_type_e::DIGITAL_INPUT: { - auto digital_device_data_ptr = device_data_ptr->as(); - if (digital_device_data_ptr->serial_config_.io_type == ACTUATOR) { - v5_globals::digital_actuators.at(name)->setValue(digital_device_data_ptr->value); - } + continue; + } + break; + + case device_type_e::DIGITAL_OUTPUT: + { + auto digital_device_data_ptr = device_data_ptr->as(); + v5_globals::digital_actuators.at(name)->setValue(digital_device_data_ptr->value); } break; @@ -243,7 +247,7 @@ void V5SerialNode::writeV5StateUpdate(){ // Digital Sensors for (const auto& [name, device] : v5_globals::digital_sensors){ - auto digital_device_data_ptr = hardware_interface_ptr_->getDeviceData(name); + auto digital_device_data_ptr = hardware_interface_ptr_->getDeviceData(name); digital_device_data_ptr->value = device->get_value(); } From 2252c6da25ac4c7d16dd8bcbbb2f9b4c86851fc7 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Thu, 6 Jun 2024 15:06:55 -0500 Subject: [PATCH 65/97] updates v5 main --- 02_V5/ghost_pros/src/main.cpp | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/02_V5/ghost_pros/src/main.cpp b/02_V5/ghost_pros/src/main.cpp index 49288cfc..47f8037a 100644 --- a/02_V5/ghost_pros/src/main.cpp +++ b/02_V5/ghost_pros/src/main.cpp @@ -199,16 +199,17 @@ void initialize(){ } break; - case device_type_e::DIGITAL: + case device_type_e::DIGITAL_INPUT: { auto digital_device_config_ptr = config_ptr->as(); - auto io_type = digital_device_config_ptr->serial_config.io_type; - if (io_type == SENSOR) { - v5_globals::digital_sensors[device_name] = std::make_shared(digital_device_config_ptr->port - 21); - } - else if (io_type == ACTUATOR) { - v5_globals::digital_actuators[device_name] = std::make_shared(digital_device_config_ptr); - } + v5_globals::digital_sensors[device_name] = std::make_shared(digital_device_config_ptr->port - 21); + } + break; + + case device_type_e::DIGITAL_OUTPUT: + { + auto digital_device_config_ptr = config_ptr->as(); + v5_globals::digital_actuators[device_name] = std::make_shared(digital_device_config_ptr); } break; From 3b6fa22341effb6258a6a9e28597fbebaa09ffe0 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Thu, 6 Jun 2024 15:10:56 -0500 Subject: [PATCH 66/97] updates RHI digital helpers --- .../robot_hardware_interface.hpp | 6 +-- .../src/robot_hardware_interface.cpp | 46 +++++++++++++------ 2 files changed, 34 insertions(+), 18 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/robot_hardware_interface.hpp b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/robot_hardware_interface.hpp index 9c5733bf..34173bf2 100644 --- a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/robot_hardware_interface.hpp +++ b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/robot_hardware_interface.hpp @@ -219,9 +219,9 @@ class RobotHardwareInterface ///////////////////////////////////////////////////////////// ///////////////// Digital Device Interfaces ///////////////// ///////////////////////////////////////////////////////////// - bool getDigitalDeviceValue(const std::string & sensor_name); - bool setDigitalDeviceValue(const std::string & sensor_name, bool value); - + 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 ///////////////// diff --git a/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp b/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp index 164b10af..b8b1bc01 100644 --- a/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp +++ b/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp @@ -57,11 +57,11 @@ RobotHardwareInterface::RobotHardwareInterface( } else if (pair.config_ptr->type == device_type_e::JOYSTICK) { pair.data_ptr = std::make_shared( val->name); - } else if (pair.config_ptr->type == device_type_e::DIGITAL) { - pair.data_ptr = std::make_shared( - val->name, - pair.config_ptr->as()->serial_config); - } else { + } else if (pair.config_ptr->type == device_type_e::DIGITAL_INPUT) { + pair.data_ptr = std::make_shared(val->name); + } else if (pair.config_ptr->type == device_type_e::DIGITAL_OUTPUT) { + pair.data_ptr = std::make_shared(val->name); + }else { throw std::runtime_error( "[RobotHardwareInterface::RobotHardwareInterface()] Device type " + std::to_string( pair.config_ptr->type) + " is unsupported!"); @@ -72,7 +72,7 @@ RobotHardwareInterface::RobotHardwareInterface( port_to_device_name_map_.emplace(pair.config_ptr->port, val->name); // Update msg lengths based on each non-digital device - if (pair.config_ptr->type != device_type_e::DIGITAL) { + if (pair.config_ptr->type != device_type_e::DIGITAL_INPUT|| pair.config_ptr->type != device_type_e::DIGITAL_OUTPUT) { sensor_update_msg_length_ += pair.data_ptr->getSensorPacketSize(); actuator_command_msg_length_ += pair.data_ptr->getActuatorPacketSize(); } @@ -107,7 +107,7 @@ std::vector RobotHardwareInterface::serialize() const for (const auto & [key, val] : device_pair_port_map_) { auto device_serial_msg = val.data_ptr->serialize(hardware_type_); - if (val.config_ptr->type == device_type_e::DIGITAL) { + if (val.config_ptr->type == device_type_e::DIGITAL_INPUT || val.config_ptr->type == device_type_e::DIGITAL_OUTPUT) { setBit(serial_data[1], (val.config_ptr->port - 22), (device_serial_msg[0] == 1)); } else { @@ -176,7 +176,7 @@ int RobotHardwareInterface::deserialize(const std::vector & msg) "[RobotHardwareInterface::deserialize] Error: Attempted to deserialize with unsupported hardware type."); } - if (val.config_ptr->type == device_type_e::DIGITAL) { + if (val.config_ptr->type == device_type_e::DIGITAL_INPUT || val.config_ptr->type == device_type_e::DIGITAL_OUTPUT) { val.data_ptr->deserialize({getBit(msg[1], (val.config_ptr->port - 22))}, hardware_type_); } else { @@ -466,22 +466,38 @@ float RobotHardwareInterface::getInertialSensorHeading(const std::string & senso } } -bool RobotHardwareInterface::getDigitalDeviceValue(const std::string & sensor_name){ - return getDeviceData(sensor_name)->value; +bool RobotHardwareInterface::getDigitalDeviceValue(const std::string & device_name){ + if(getDeviceData(device_name)->type == DIGITAL_INPUT){ + return getDeviceData(device_name)->value; + } + else if(getDeviceData(device_name)->type == DIGITAL_OUTPUT){ + return getDeviceData(device_name)->value; + } + else{ + throw std::runtime_error(std::string("[RobotHardwareInterface::getDigitalDeviceValue] Error: ") + device_name + " is not a digital device!"); + } } -bool RobotHardwareInterface::setDigitalDeviceValue(const std::string & device_name, bool value){ +void RobotHardwareInterface::setDigitalInputValue(const std::string & device_name, bool value){ - auto io_type = getDeviceConfig(device_name)->serial_config.io_type; - if (io_type == SENSOR && hardware_type_ != V5_BRAIN) { + if (hardware_type_ != V5_BRAIN) { throw std::runtime_error("[RobotHardwareInterface::setDigitalDeviceValue] Error: Attempted to set Digital Sensor value from Coprocessor."); } - if (io_type == ACTUATOR && hardware_type_ != COPROCESSOR) { + + std::unique_lock update_lock(update_mutex_); + auto device_data_ptr = getDeviceData(device_name); + device_data_ptr->value = value; + setDeviceDataNoLock(device_data_ptr); +} + +void RobotHardwareInterface::setDigitalOutputValue(const std::string & device_name, bool value){ + + if (hardware_type_ != COPROCESSOR) { throw std::runtime_error("[RobotHardwareInterface::setDigitalDeviceValue] Error: Attempted to set Digital Actuator value from V5 Brain."); } std::unique_lock update_lock(update_mutex_); - auto device_data_ptr = getDeviceData(device_name); + auto device_data_ptr = getDeviceData(device_name); device_data_ptr->value = value; setDeviceDataNoLock(device_data_ptr); } From 002e02f90735c670319a5a2f76e6fd23c546daf4 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Thu, 6 Jun 2024 15:16:48 -0500 Subject: [PATCH 67/97] fixes example yaml --- .../ghost_v5_interfaces/test/config/example_robot_2.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/test/config/example_robot_2.yaml b/01_Libraries/ghost_v5_interfaces/test/config/example_robot_2.yaml index 4ffdd0ef..d7fe7ac0 100644 --- a/01_Libraries/ghost_v5_interfaces/test/config/example_robot_2.yaml +++ b/01_Libraries/ghost_v5_interfaces/test/config/example_robot_2.yaml @@ -1,10 +1,10 @@ port_configuration: use_partner_joystick: false - digital: - limit_switch_1: + adi: + digital_in: port: A type: DIGITAL_INPUT - arm_solenoid: + digital_out: port: B type: DIGITAL_OUTPUT devices: From 83adcb42d590a6c483da31b5c1d2a801b26e8c20 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Thu, 6 Jun 2024 15:18:19 -0500 Subject: [PATCH 68/97] updates yaml loader --- .../util/load_digital_device_config_yaml.cpp | 39 +++++++++---------- 1 file changed, 19 insertions(+), 20 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/src/util/load_digital_device_config_yaml.cpp b/01_Libraries/ghost_v5_interfaces/src/util/load_digital_device_config_yaml.cpp index e3c38560..f3b7e52f 100644 --- a/01_Libraries/ghost_v5_interfaces/src/util/load_digital_device_config_yaml.cpp +++ b/01_Libraries/ghost_v5_interfaces/src/util/load_digital_device_config_yaml.cpp @@ -46,9 +46,23 @@ void loadDigitalDeviceConfigFromYAML( " not found!"); } - // Set base attributes - device_config_ptr->name = device_name; - device_config_ptr->type = device_type_e::DIGITAL; + // Load IO type + std::string type; + if (!loadYAMLParam(device_node, "type", type, verbose)) { + throw std::runtime_error( + "[loadDigitalDeviceConfigFromYAML] Error: Type not specified for Digital Device " + + device_name + "!"); + } + + if(type == "DIGITAL_INPUT"){ + device_config_ptr->type = devices::device_type_e::DIGITAL_INPUT; + } + else if(type == "DIGITAL_OUTPUT"){ + device_config_ptr->type = devices::device_type_e::DIGITAL_OUTPUT; + } + else{ + throw std::runtime_error(std::string("[loadDigitalDeviceConfigFromYAML] Error: Unsupported digital device ") + type + "!"); + } // Get port std::string port_string; @@ -74,23 +88,8 @@ void loadDigitalDeviceConfigFromYAML( // Set port in device config device_config_ptr->port = (int) port; - // Load IO type - std::string type; - if (!loadYAMLParam(device_node, "io_type", type, verbose)) { - throw std::runtime_error( - "[loadDigitalDeviceConfigFromYAML] Error: Type not specified for Digital Device " + - device_name + "!"); - } - - // Validate IO type - if (type != "SENSOR" && type != "ACTUATOR") { - throw std::runtime_error( - "[loadDigitalDeviceConfigFromYAML] Error: Invalid type specified for Digital Device " + - device_name + "!"); - } - - // Set IO type in device config - device_config_ptr->serial_config.io_type = (type == "SENSOR") ? SENSOR : ACTUATOR; + // Set base attributes + device_config_ptr->name = device_name; } } // namespace util From 613818bd0cf33dc4b0af51333fda88f7ac33fa62 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Thu, 6 Jun 2024 15:21:30 -0500 Subject: [PATCH 69/97] updates device config factory --- .../src/util/device_config_factory_utils.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/src/util/device_config_factory_utils.cpp b/01_Libraries/ghost_v5_interfaces/src/util/device_config_factory_utils.cpp index fa6aed15..54ca5505 100644 --- a/01_Libraries/ghost_v5_interfaces/src/util/device_config_factory_utils.cpp +++ b/01_Libraries/ghost_v5_interfaces/src/util/device_config_factory_utils.cpp @@ -70,17 +70,15 @@ std::shared_ptr loadRobotConfigFromYAML(YAML::Node node, bool v } // Iterate through Digital IO Ports - if (node["digital"]) { - - - for (auto it = node["port_configuration"]["digital"].begin(); - it != node["port_configuration"]["digital"].end(); it++) + if (node["adi"]) { + for (auto it = node["port_configuration"]["adi"].begin(); + it != node["port_configuration"]["adi"].end(); it++) { std::string device_name = it->first.as(); YAML::Node device_yaml_node = it->second; auto device_config_base_ptr = std::make_shared(); - loadDigitalDeviceConfigFromYAML(device_yaml_node, device_name, device_config_base_ptr, verbose); + loadDigitalDeviceConfigFromYAML(node["port_configuration"], device_name, device_config_base_ptr, verbose); device_config_map_ptr->addDeviceConfig(device_config_base_ptr); } } From 5f505774df61431880c7a21a0a391255a4307e6b Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Thu, 6 Jun 2024 15:45:30 -0500 Subject: [PATCH 70/97] updates digital msg helpers --- .../states/V5DigitalDeviceState.msg | 1 - .../src/msg_helpers/msg_helpers.cpp | 64 +++++++++++-------- 2 files changed, 37 insertions(+), 28 deletions(-) diff --git a/03_ROS/ghost_msgs/msg/device_msgs/states/V5DigitalDeviceState.msg b/03_ROS/ghost_msgs/msg/device_msgs/states/V5DigitalDeviceState.msg index 244ca339..0be84a57 100644 --- a/03_ROS/ghost_msgs/msg/device_msgs/states/V5DigitalDeviceState.msg +++ b/03_ROS/ghost_msgs/msg/device_msgs/states/V5DigitalDeviceState.msg @@ -1,4 +1,3 @@ ghost_msgs/V5DeviceHeader device_header bool value -string io_type diff --git a/03_ROS/ghost_ros_interfaces/src/msg_helpers/msg_helpers.cpp b/03_ROS/ghost_ros_interfaces/src/msg_helpers/msg_helpers.cpp index 441be1d0..e0c1e6b2 100644 --- a/03_ROS/ghost_ros_interfaces/src/msg_helpers/msg_helpers.cpp +++ b/03_ROS/ghost_ros_interfaces/src/msg_helpers/msg_helpers.cpp @@ -213,21 +213,35 @@ void fromROSMsg( } void toROSMsg( - const ghost_v5_interfaces::devices::DigitalDeviceData & digital_device_data, + const ghost_v5_interfaces::devices::DigitalInputDeviceData & digital_device_data, ghost_msgs::msg::V5DigitalDeviceState & digital_device_msg) { toROSMsg(digital_device_data, digital_device_msg.device_header); // Set device header digital_device_msg.value = digital_device_data.value; - digital_device_msg.io_type = DIGITAL_IO_TYPE_TO_STRING_MAP.at(digital_device_data.serial_config_.io_type); } void fromROSMsg( - ghost_v5_interfaces::devices::DigitalDeviceData & digital_device_data, + ghost_v5_interfaces::devices::DigitalInputDeviceData & digital_device_data, + const ghost_msgs::msg::V5DigitalDeviceState & digital_device_msg) +{ + fromROSMsg(digital_device_data, digital_device_msg.device_header); // Set base attributes + digital_device_data.value = digital_device_msg.value; +} + +void toROSMsg( + const ghost_v5_interfaces::devices::DigitalOutputDeviceData & digital_device_data, + ghost_msgs::msg::V5DigitalDeviceState & digital_device_msg) +{ + toROSMsg(digital_device_data, digital_device_msg.device_header); // Set device header + digital_device_msg.value = digital_device_data.value; +} + +void fromROSMsg( + ghost_v5_interfaces::devices::DigitalOutputDeviceData & digital_device_data, const ghost_msgs::msg::V5DigitalDeviceState & digital_device_msg) { fromROSMsg(digital_device_data, digital_device_msg.device_header); // Set base attributes digital_device_data.value = digital_device_msg.value; - digital_device_data.serial_config_.io_type = STRING_TO_DIGITAL_IO_TYPE_MAP.at(digital_device_msg.io_type); } void toROSMsg( @@ -250,13 +264,13 @@ void toROSMsg( continue; } else if (device_data_ptr->type == device_type_e::INERTIAL_SENSOR) { continue; - } else if (device_data_ptr->type == device_type_e::DIGITAL) { - auto digital_device_data_ptr = device_data_ptr->as(); - if (digital_device_data_ptr->serial_config_.io_type == digital_io_type_e::ACTUATOR) { - V5DigitalDeviceState msg{}; - toROSMsg(*digital_device_data_ptr, msg); - actuator_cmd_msg.digital_devices.push_back(msg); - } + } else if (device_data_ptr->type == device_type_e::DIGITAL_INPUT) { + continue; + } else if (device_data_ptr->type == device_type_e::DIGITAL_OUTPUT) { + auto digital_device_data_ptr = device_data_ptr->as(); + V5DigitalDeviceState msg{}; + toROSMsg(*digital_device_data_ptr, msg); + actuator_cmd_msg.digital_devices.push_back(msg); } else { std::string dev_type_str; if (DEVICE_TYPE_TO_STRING_MAP.count(device_data_ptr->type) == 1) { @@ -287,12 +301,10 @@ void fromROSMsg( // Digital Devices for (const auto & digital_device_msg : actuator_cmd_msg.digital_devices) { - auto digital_device_data_ptr = hardware_interface.getDeviceData( + auto digital_device_data_ptr = hardware_interface.getDeviceData( digital_device_msg.device_header.name); - if (digital_device_data_ptr->serial_config_.io_type == ACTUATOR) { fromROSMsg(*digital_device_data_ptr, digital_device_msg); hardware_interface.setDeviceData(digital_device_data_ptr); - } } } @@ -328,13 +340,13 @@ void toROSMsg(const RobotHardwareInterface & hardware_interface, V5SensorUpdate auto joy_data_ptr = device_data_ptr->as(); toROSMsg(*joy_data_ptr, msg); sensor_update_msg.joysticks.push_back(msg); - } else if (device_data_ptr->type == device_type_e::DIGITAL) { - auto digital_device_data_ptr = device_data_ptr->as(); - if (digital_device_data_ptr->serial_config_.io_type == digital_io_type_e::SENSOR) { - V5DigitalDeviceState msg{}; - toROSMsg(*digital_device_data_ptr, msg); - sensor_update_msg.digital_devices.push_back(msg); - } + } else if (device_data_ptr->type == device_type_e::DIGITAL_INPUT) { + auto digital_device_data_ptr = device_data_ptr->as(); + V5DigitalDeviceState msg{}; + toROSMsg(*digital_device_data_ptr, msg); + sensor_update_msg.digital_devices.push_back(msg); + } else if (device_data_ptr->type == device_type_e::DIGITAL_OUTPUT) { + continue; } else { std::string dev_type_str; if (DEVICE_TYPE_TO_STRING_MAP.count(device_data_ptr->type) == 1) { @@ -394,12 +406,10 @@ void fromROSMsg( // Digital Devices for (const auto & digital_device_msg : sensor_update_msg.digital_devices) { - auto digital_device_data_ptr = hardware_interface.getDeviceData( - digital_device_msg.device_header.name); - if (digital_device_data_ptr->serial_config_.io_type == SENSOR) { - fromROSMsg(*digital_device_data_ptr, digital_device_msg); - hardware_interface.setDeviceData(digital_device_data_ptr); - } + auto digital_device_data_ptr = hardware_interface.getDeviceData( + digital_device_msg.device_header.name); + fromROSMsg(*digital_device_data_ptr, digital_device_msg); + hardware_interface.setDeviceData(digital_device_data_ptr); } } From 25fad9a38a59ff75f5aead0fc9856c86a4fefca7 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Thu, 6 Jun 2024 15:46:02 -0500 Subject: [PATCH 71/97] adds more test yamls --- .../test/config/example_robot.yaml | 8 ++++++++ .../test/config/test_load_digital_device_config.yaml | 11 +++++++++++ 2 files changed, 19 insertions(+) create mode 100644 01_Libraries/ghost_v5_interfaces/test/config/test_load_digital_device_config.yaml diff --git a/01_Libraries/ghost_v5_interfaces/test/config/example_robot.yaml b/01_Libraries/ghost_v5_interfaces/test/config/example_robot.yaml index 8c89699b..4eddf127 100644 --- a/01_Libraries/ghost_v5_interfaces/test/config/example_robot.yaml +++ b/01_Libraries/ghost_v5_interfaces/test/config/example_robot.yaml @@ -1,5 +1,13 @@ port_configuration: use_partner_joystick: false + adi: + digital_in: + port: A + type: DIGITAL_INPUT + digital_out: + port: B + type: DIGITAL_OUTPUT + devices: rotation_sensor_1: port: 4 diff --git a/01_Libraries/ghost_v5_interfaces/test/config/test_load_digital_device_config.yaml b/01_Libraries/ghost_v5_interfaces/test/config/test_load_digital_device_config.yaml new file mode 100644 index 00000000..749722b3 --- /dev/null +++ b/01_Libraries/ghost_v5_interfaces/test/config/test_load_digital_device_config.yaml @@ -0,0 +1,11 @@ +port_configuration: + adi: + in: + port: A + type: DIGITAL_INPUT + out: + port: B + type: DIGITAL_OUTPUT + devices: + + device_configurations: From cb14b6c45435e0bb7affc64b1c9d3d98dc86637b Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Thu, 6 Jun 2024 15:57:44 -0500 Subject: [PATCH 72/97] adds interface test Co-authored-by: Maxx Wilson --- .../test_load_digital_device_config_yaml.cpp | 290 +++++------------- 1 file changed, 71 insertions(+), 219 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/test/util/test_load_digital_device_config_yaml.cpp b/01_Libraries/ghost_v5_interfaces/test/util/test_load_digital_device_config_yaml.cpp index fba8b206..b86ae0e3 100644 --- a/01_Libraries/ghost_v5_interfaces/test/util/test_load_digital_device_config_yaml.cpp +++ b/01_Libraries/ghost_v5_interfaces/test/util/test_load_digital_device_config_yaml.cpp @@ -1,219 +1,71 @@ -// /* -// * 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. -// */ - - -// #include -// #include "gtest/gtest.h" -// #include "yaml-cpp/yaml.h" - -// using namespace ghost_v5_interfaces::devices; -// using namespace ghost_v5_interfaces::util; -// using namespace ghost_v5_interfaces; - -// class TestLoadMotorDeviceConfigYAML : public ::testing::Test -// { -// protected: -// void SetUp() override -// { -// std::string config_path = std::string(getenv("VEXU_HOME")) + -// "/01_Libraries/ghost_v5_interfaces/test/config/test_load_motor_config.yaml"; -// config_yaml_ = YAML::LoadFile(config_path); - -// // Expected motor configurations -// motor_empty_config_ = std::make_shared(); -// motor_empty_config_->port = 0; -// motor_empty_config_->name = "motor_empty_config"; -// motor_empty_config_->type = device_type_e::MOTOR; - -// left_drive_motor_config_ = std::make_shared(); -// left_drive_motor_config_->gearset = ghost_gearset::GEARSET_600; -// left_drive_motor_config_->encoder_units = ghost_encoder_unit::ENCODER_DEGREES; -// left_drive_motor_config_->controller_config.pos_gain = 7.5; -// left_drive_motor_config_->port = 1; -// left_drive_motor_config_->name = "left_drive_motor"; -// left_drive_motor_config_->type = device_type_e::MOTOR; -// left_drive_motor_config_->serial_config.send_torque_command = true; -// left_drive_motor_config_->serial_config.send_temp_data = true; - -// right_drive_motor_config_ = std::make_shared(); -// right_drive_motor_config_->gearset = ghost_gearset::GEARSET_600; -// right_drive_motor_config_->encoder_units = ghost_encoder_unit::ENCODER_DEGREES; -// right_drive_motor_config_->controller_config.pos_gain = 7.5; -// right_drive_motor_config_->port = 2; -// right_drive_motor_config_->name = "right_drive_motor"; -// right_drive_motor_config_->reversed = true; -// right_drive_motor_config_->type = device_type_e::MOTOR; -// right_drive_motor_config_->serial_config.send_torque_command = true; -// right_drive_motor_config_->serial_config.send_temp_data = true; -// } - -// std::shared_ptr motor_empty_config_; -// std::shared_ptr motor_no_config_; -// std::shared_ptr left_drive_motor_config_; -// std::shared_ptr right_drive_motor_config_; -// std::shared_ptr test_motor_config_; - -// YAML::Node config_yaml_; -// }; - -// /** -// * @brief Test that a DCMotorModel::Config struct can be properly loaded from YAML -// */ -// TEST_F(TestLoadMotorDeviceConfigYAML, testLoadMotorModelConfig) { -// DCMotorModel::Config test_model_config{}; -// test_model_config.free_speed = 1104.0; -// test_model_config.stall_torque = 52.25; -// test_model_config.free_current = 0.9090; -// test_model_config.stall_current = 21.58; -// test_model_config.nominal_voltage = 1200.0; -// test_model_config.gear_ratio = 2915.0; - -// DCMotorModel::Config loaded_model_config; -// EXPECT_NO_THROW( -// loaded_model_config = -// loadMotorModelConfigFromYAML( -// config_yaml_["port_configuration"]["device_configurations"][ -// "test_motor_config"]["model"])); -// EXPECT_EQ(test_model_config, loaded_model_config); -// } - -// /** -// * @brief Test that a MotorController::Config struct can be properly loaded from YAML -// */ -// TEST_F(TestLoadMotorDeviceConfigYAML, testLoadMotorControllerConfig) { -// MotorController::Config test_controller_config{}; -// test_controller_config.pos_gain = 8000.0; -// test_controller_config.vel_gain = 118.0; -// test_controller_config.ff_vel_gain = 400.0; -// test_controller_config.ff_torque_gain = 2587.0; -// test_controller_config.cmd_duration = 4; - -// MotorController::Config loaded_controller_config; -// EXPECT_NO_THROW( -// loaded_controller_config = -// loadMotorControllerConfigFromYAML( -// config_yaml_["port_configuration"]["device_configurations"][ -// "test_motor_config"]["controller"])); -// EXPECT_EQ(test_controller_config, loaded_controller_config); -// } - -// /** -// * @brief Test that a SecondOrderLowPassFilter::Config struct can be properly loaded from YAML -// */ -// TEST_F(TestLoadMotorDeviceConfigYAML, testLoadLowPassFilterConfig) { -// SecondOrderLowPassFilter::Config test_filter_config{}; -// test_filter_config.cutoff_frequency = 62.0; -// test_filter_config.damping_ratio = 0.10; -// test_filter_config.timestep = 0.21; - -// SecondOrderLowPassFilter::Config loaded_filter_config; -// EXPECT_NO_THROW( -// loaded_filter_config = -// loadLowPassFilterConfigFromYAML( -// config_yaml_["port_configuration"]["device_configurations"][ -// "test_motor_config"]["filter"])); -// EXPECT_EQ(test_filter_config, loaded_filter_config); -// } - -// /** -// * @brief Test that a MotorDeviceData::SerialConfig struct can be properly loaded from YAML -// */ -// TEST_F(TestLoadMotorDeviceConfigYAML, testLoadMotorSerialConfig) { -// MotorDeviceData::SerialConfig test_serial_config{}; -// test_serial_config.send_position_command = true; -// test_serial_config.send_velocity_command = true; -// test_serial_config.send_voltage_command = false; -// test_serial_config.send_torque_command = true; -// test_serial_config.send_current_data = true; -// test_serial_config.send_power_data = false; -// test_serial_config.send_temp_data = true; - -// MotorDeviceData::SerialConfig loaded_serial_config; -// EXPECT_NO_THROW( -// loaded_serial_config = -// loadMotorSerialConfigFromYAML( -// config_yaml_["port_configuration"]["device_configurations"][ -// "test_motor_config"]["serial"])); -// EXPECT_EQ(test_serial_config, loaded_serial_config); -// } - -// /** -// * @brief Test that all fields are properly set when loading the MotorDeviceConfig class -// */ -// TEST_F(TestLoadMotorDeviceConfigYAML, testLoadCustomMotorConfigFromYAML) { -// auto config_node = config_yaml_["port_configuration"]; -// std::string motor_name = "right_drive_motor"; -// auto config_ptr = std::make_shared(); -// loadMotorDeviceConfigFromYAML(config_node, motor_name, config_ptr); -// EXPECT_EQ(*config_ptr, *right_drive_motor_config_); -// } - -// /** -// * @brief Test that missing fields will be populated with the default args for the MotorDeviceConfig class -// */ -// TEST_F(TestLoadMotorDeviceConfigYAML, testLoadDefaultMotorConfigFromYAML) { -// auto config_node = config_yaml_["port_configuration"]; -// std::string motor_name = "motor_empty_config"; -// auto config_ptr = std::make_shared(); -// loadMotorDeviceConfigFromYAML(config_node, motor_name, config_ptr); -// EXPECT_EQ(*config_ptr, *motor_empty_config_); -// } - -// /** -// * @brief Test that if the motor config is not found, it throws. -// */ -// TEST_F(TestLoadMotorDeviceConfigYAML, testMissingConfigThrows) { -// auto config_node = config_yaml_["port_configuration"]; -// std::string motor_name = "motor_no_config"; -// auto config_ptr = std::make_shared(); -// EXPECT_THROW( -// loadMotorDeviceConfigFromYAML( -// config_node, motor_name, -// config_ptr), std::runtime_error); -// } - -// /** -// * @brief Test that if the device config is missing the port attribute, it throws. -// */ -// TEST_F(TestLoadMotorDeviceConfigYAML, testMissingPortThrowsError) { -// auto config_node = config_yaml_["port_configuration"]; -// std::string motor_name = "motor_no_port"; -// auto config_ptr = std::make_shared(); -// EXPECT_THROW( -// loadMotorDeviceConfigFromYAML( -// config_node, motor_name, -// config_ptr), std::runtime_error); -// } - -// /** -// * @brief Test that if the device config is missing the type attribute, it throws -// */ -// TEST_F(TestLoadMotorDeviceConfigYAML, testMissingTypeThrowsError) { -// auto config_node = config_yaml_["port_configuration"]; -// std::string motor_name = "motor_no_type"; -// auto config_ptr = std::make_shared(); -// EXPECT_THROW( -// loadMotorDeviceConfigFromYAML( -// config_node, motor_name, -// config_ptr), std::runtime_error); -// } +/* + * 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. + */ + + +#include +#include "gtest/gtest.h" +#include "yaml-cpp/yaml.h" + +using namespace ghost_v5_interfaces::devices; +using namespace ghost_v5_interfaces::util; +using namespace ghost_v5_interfaces; + +class TestLoadDigitalDeviceConfigYAML : public ::testing::Test +{ +protected: + void SetUp() override + { + std::string config_path = std::string(getenv("VEXU_HOME")) + + "/01_Libraries/ghost_v5_interfaces/test/config/test_load_digital_device_config.yaml"; + config_yaml_ = YAML::LoadFile(config_path); + + // Expected motor configurations + digital_input_config_ = std::make_shared(); + digital_input_config_->port = 22; + digital_input_config_->name = "in"; + digital_input_config_->type = device_type_e::DIGITAL_INPUT; + + digital_output_config_ = std::make_shared(); + digital_output_config_->port = 23; + digital_output_config_->name = "out"; + digital_output_config_->type = device_type_e::DIGITAL_OUTPUT; + + } + + std::shared_ptr digital_input_config_; + std::shared_ptr digital_output_config_; + + YAML::Node config_yaml_; +}; + +TEST_F(TestLoadDigitalDeviceConfigYAML, testLoadDigitalIn) { + std::shared_ptr input_ptr = std::make_shared(); + loadDigitalDeviceConfigFromYAML(config_yaml_["port_configuration"], "in", input_ptr, false); + EXPECT_EQ(*input_ptr, *digital_input_config_); +} + +TEST_F(TestLoadDigitalDeviceConfigYAML, testLoadDigitalOut) { + std::shared_ptr output_ptr = std::make_shared(); + loadDigitalDeviceConfigFromYAML(config_yaml_["port_configuration"], "out", output_ptr, false); + EXPECT_EQ(*output_ptr, *digital_output_config_); +} \ No newline at end of file From 897d5ebaa9cfb96ad6f42116040faf3c5e86a261 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Thu, 6 Jun 2024 15:59:02 -0500 Subject: [PATCH 73/97] fixes config factory --- .../src/util/device_config_factory_utils.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/src/util/device_config_factory_utils.cpp b/01_Libraries/ghost_v5_interfaces/src/util/device_config_factory_utils.cpp index 54ca5505..a569bb31 100644 --- a/01_Libraries/ghost_v5_interfaces/src/util/device_config_factory_utils.cpp +++ b/01_Libraries/ghost_v5_interfaces/src/util/device_config_factory_utils.cpp @@ -70,7 +70,7 @@ std::shared_ptr loadRobotConfigFromYAML(YAML::Node node, bool v } // Iterate through Digital IO Ports - if (node["adi"]) { + if (node["port_configuration"]["adi"]) { for (auto it = node["port_configuration"]["adi"].begin(); it != node["port_configuration"]["adi"].end(); it++) { @@ -238,6 +238,7 @@ void generateCodeFromRobotConfig( output_file << "#include \"ghost_v5_interfaces/devices/motor_device_interface.hpp\"\n"; output_file << "#include \"ghost_v5_interfaces/devices/rotation_sensor_device_interface.hpp\"\n"; output_file << "#include \"ghost_v5_interfaces/devices/joystick_device_interface.hpp\"\n"; + output_file << "#include \"ghost_v5_interfaces/devices/digital_device_interface.hpp\"\n"; output_file << "\n"; output_file << "// This is externed as raw C code so we can resolve the symbols in the shared object easily for unit testing.\n"; @@ -400,7 +401,7 @@ void generateCodeFromRobotConfig( "type = ghost_v5_interfaces::devices::device_type_e::DIGITAL_INPUT;\n"; output_file << "\trobot_config->addDeviceConfig(" + device_name + ");\n"; output_file << "\n"; - } else if (val->type == device_type_e::DIGITAL_INPUT) { + } else if (val->type == device_type_e::DIGITAL_OUTPUT) { auto config_ptr = val->as(); std::string device_name = config_ptr->name; From 8c1b2760a252736d65d2a3dc4eb435e3aabdaf2d Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Thu, 6 Jun 2024 17:08:25 -0500 Subject: [PATCH 74/97] fixes digital == and serialize Co-authored-by: Maxx Wilson --- .../devices/digital_device_interface.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_device_interface.hpp b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_device_interface.hpp index 6bf6c2b7..669a5cf9 100644 --- a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_device_interface.hpp +++ b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_device_interface.hpp @@ -80,7 +80,7 @@ class DigitalInputDeviceData : public DeviceData bool operator==(const DeviceBase & rhs) const override { const DigitalInputDeviceData * d_rhs = dynamic_cast(&rhs); - return (d_rhs != nullptr) && (value == d_rhs->value); + return (d_rhs != nullptr) && (name == d_rhs->name) && (type == d_rhs->type) && (value == d_rhs->value); } bool value; @@ -111,7 +111,7 @@ class DigitalOutputDeviceData : public DeviceData std::vector serialize(hardware_type_e hardware_type) const override { - std::vector msg(1); + std::vector msg; if (hardware_type == COPROCESSOR) { msg.push_back((unsigned char) value); } @@ -134,7 +134,7 @@ class DigitalOutputDeviceData : public DeviceData bool operator==(const DeviceBase & rhs) const override { const DigitalOutputDeviceData * d_rhs = dynamic_cast(&rhs); - return (d_rhs != nullptr) && (value == d_rhs->value); + return (d_rhs != nullptr) && (name == d_rhs->name) && (type == d_rhs->type) && (value == d_rhs->value); } bool value; From ee93d5699811446f3ec05420d008eaaf7358a0df Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Thu, 6 Jun 2024 17:11:35 -0500 Subject: [PATCH 75/97] fixes digital data fetching in RHI --- .../src/robot_hardware_interface.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp b/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp index b8b1bc01..8d3fbed4 100644 --- a/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp +++ b/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp @@ -72,7 +72,7 @@ RobotHardwareInterface::RobotHardwareInterface( port_to_device_name_map_.emplace(pair.config_ptr->port, val->name); // Update msg lengths based on each non-digital device - if (pair.config_ptr->type != device_type_e::DIGITAL_INPUT|| pair.config_ptr->type != device_type_e::DIGITAL_OUTPUT) { + if (pair.config_ptr->type != device_type_e::DIGITAL_INPUT && pair.config_ptr->type != device_type_e::DIGITAL_OUTPUT) { sensor_update_msg_length_ += pair.data_ptr->getSensorPacketSize(); actuator_command_msg_length_ += pair.data_ptr->getActuatorPacketSize(); } @@ -106,11 +106,14 @@ std::vector RobotHardwareInterface::serialize() const serial_data.push_back((unsigned char) 0); for (const auto & [key, val] : device_pair_port_map_) { - auto device_serial_msg = val.data_ptr->serialize(hardware_type_); - if (val.config_ptr->type == device_type_e::DIGITAL_INPUT || val.config_ptr->type == device_type_e::DIGITAL_OUTPUT) { - setBit(serial_data[1], (val.config_ptr->port - 22), (device_serial_msg[0] == 1)); + if (val.config_ptr->type == device_type_e::DIGITAL_INPUT) { + setBit(serial_data[1], (val.config_ptr->port - 22), val.data_ptr->as()->value); + } + else if (val.config_ptr->type == device_type_e::DIGITAL_OUTPUT) { + setBit(serial_data[1], (val.config_ptr->port - 22), val.data_ptr->as()->value); } else { + auto device_serial_msg = val.data_ptr->serialize(hardware_type_); serial_data.insert(serial_data.end(), device_serial_msg.begin(), device_serial_msg.end()); } } From 3159b72c3efb1ffb0c26782f5bef77ce5b8e2733 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Thu, 6 Jun 2024 17:19:34 -0500 Subject: [PATCH 76/97] initializes digital value --- .../ghost_v5_interfaces/devices/digital_device_interface.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_device_interface.hpp b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_device_interface.hpp index 669a5cf9..b8255bc9 100644 --- a/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_device_interface.hpp +++ b/01_Libraries/ghost_v5_interfaces/include/ghost_v5_interfaces/devices/digital_device_interface.hpp @@ -83,7 +83,7 @@ class DigitalInputDeviceData : public DeviceData return (d_rhs != nullptr) && (name == d_rhs->name) && (type == d_rhs->type) && (value == d_rhs->value); } - bool value; + bool value = false; }; class DigitalOutputDeviceData : public DeviceData @@ -137,7 +137,7 @@ class DigitalOutputDeviceData : public DeviceData return (d_rhs != nullptr) && (name == d_rhs->name) && (type == d_rhs->type) && (value == d_rhs->value); } - bool value; + bool value = false; }; class DigitalDeviceConfig : public DeviceConfig From 5cfc4643f60522ead0f9f61aea97b7ea607255d8 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Thu, 6 Jun 2024 17:20:42 -0500 Subject: [PATCH 77/97] adds maxx tests --- .../test/test_rhi_serialization.cpp | 215 ++++++++++++++++++ .../test/test_robot_hardware_interface.cpp | 147 ++---------- .../util/test_device_config_factory_utils.cpp | 14 ++ 3 files changed, 251 insertions(+), 125 deletions(-) create mode 100644 01_Libraries/ghost_v5_interfaces/test/test_rhi_serialization.cpp diff --git a/01_Libraries/ghost_v5_interfaces/test/test_rhi_serialization.cpp b/01_Libraries/ghost_v5_interfaces/test/test_rhi_serialization.cpp new file mode 100644 index 00000000..43dccc52 --- /dev/null +++ b/01_Libraries/ghost_v5_interfaces/test/test_rhi_serialization.cpp @@ -0,0 +1,215 @@ +/* + * Copyright (c) 2024 Maxx 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. + */ + +#include +#include +#include "ghost_util/test_util.hpp" +#include "ghost_v5_interfaces/devices/device_config_map.hpp" +#include "ghost_v5_interfaces/devices/joystick_device_interface.hpp" +#include "ghost_v5_interfaces/devices/motor_device_interface.hpp" +#include "ghost_v5_interfaces/devices/digital_device_interface.hpp" +#include "ghost_v5_interfaces/devices/rotation_sensor_device_interface.hpp" +#include "ghost_v5_interfaces/robot_hardware_interface.hpp" +#include "ghost_v5_interfaces/test/device_test_utils.hpp" +#include "ghost_v5_interfaces/util/device_config_factory_utils.hpp" +#include "yaml-cpp/yaml.h" + +#include + +using ghost_v5_interfaces::util::loadRobotConfigFromYAML; +using namespace ghost_util; +using namespace ghost_v5_interfaces::devices; +using namespace ghost_v5_interfaces::test_util; +using namespace ghost_v5_interfaces; + +class RobotHardwareInterfaceTestFixture : public ::testing::Test +{ +public: + void SetUp() override + { + std::string config_path = std::string(getenv("VEXU_HOME")) + + "/01_Libraries/ghost_v5_interfaces/test/config/example_robot_2.yaml"; + config_yaml_ = YAML::LoadFile(config_path); + + device_config_map_ptr_single_joy_ = loadRobotConfigFromYAML(config_yaml_, false); + + config_yaml_["port_configuration"]["use_partner_joystick"] = true; + device_config_map_ptr_dual_joy_ = loadRobotConfigFromYAML(config_yaml_, false); + } + + std::shared_ptr device_config_map_ptr_single_joy_; + std::shared_ptr device_config_map_ptr_dual_joy_; + YAML::Node config_yaml_; +}; + +TEST_F(RobotHardwareInterfaceTestFixture, testSerializationPipelineCoprocessorToV5) { + RobotHardwareInterface hw_interface(device_config_map_ptr_single_joy_, + hardware_type_e::COPROCESSOR); + + // Update all motors in the default robot config + auto motor_data_1 = getRandomMotorData(true); + motor_data_1->name = "left_drive_motor"; + hw_interface.setDeviceData(motor_data_1); + auto motor_data_2 = getRandomMotorData(true); + motor_data_2->name = "test_motor"; + hw_interface.setDeviceData(motor_data_2); + auto motor_data_3 = getRandomMotorData(true); + motor_data_3->name = "default_motor"; + hw_interface.setDeviceData(motor_data_3); + + // Update Digital In + auto digital_in = getRandomDigitalInputDeviceData(); + digital_in->name = "digital_in"; + digital_in->value = false; + hw_interface.setDeviceData(digital_in); + + // Update Digital Out + auto digital_out = getRandomDigitalOutputDeviceData(); + digital_out->name = "digital_out"; + hw_interface.setDeviceData(digital_out); + + RobotHardwareInterface hw_interface_copy(device_config_map_ptr_single_joy_, + hardware_type_e::V5_BRAIN); + std::vector serial_data = hw_interface.serialize(); + hw_interface_copy.deserialize(serial_data); + + EXPECT_TRUE(hw_interface.isDataEqual(hw_interface_copy)); +} + + +TEST_F(RobotHardwareInterfaceTestFixture, testSerializationPipelineV5ToCoprocessor) { + RobotHardwareInterface hw_interface(device_config_map_ptr_single_joy_, hardware_type_e::V5_BRAIN); + + // Update Motors + auto motor_data_1 = getRandomMotorData(false); + motor_data_1->name = "left_drive_motor"; + hw_interface.setDeviceData(motor_data_1); + auto motor_data_2 = getRandomMotorData(false); + motor_data_2->name = "test_motor"; + hw_interface.setDeviceData(motor_data_2); + auto motor_data_3 = getRandomMotorData(false); + motor_data_3->name = "default_motor"; + hw_interface.setDeviceData(motor_data_3); + + // Update Rotation Sensors + auto rotation_sensor_1 = getRandomRotationSensorData(); + rotation_sensor_1->name = "rotation_sensor_1"; + hw_interface.setDeviceData(rotation_sensor_1); + auto rotation_sensor_2 = getRandomRotationSensorData(); + rotation_sensor_2->name = "rotation_sensor_2"; + hw_interface.setDeviceData(rotation_sensor_2); + + // Update Inertial Sensor + auto inertial_sensor_1 = getRandomInertialSensorData(); + inertial_sensor_1->name = "inertial_sensor_1"; + hw_interface.setDeviceData(inertial_sensor_1); + + // Update Digital In + auto digital_in = getRandomDigitalInputDeviceData(); + digital_in->name = "digital_in"; + digital_in->value = false; + hw_interface.setDeviceData(digital_in); + + // Update Digital Out + auto digital_out = getRandomDigitalOutputDeviceData(); + digital_out->name = "digital_out"; + digital_out->value = false; + hw_interface.setDeviceData(digital_out); + + // Update Competition State + hw_interface.setDisabledStatus(getRandomBool()); + hw_interface.setAutonomousStatus(getRandomBool()); + hw_interface.setConnectedStatus(getRandomBool()); + + // Update Joystick + auto joy = getRandomJoystickData(); + joy->name = MAIN_JOYSTICK_NAME; + hw_interface.setDeviceData(joy); + + RobotHardwareInterface hw_interface_copy(device_config_map_ptr_single_joy_, + hardware_type_e::COPROCESSOR); + std::vector serial_data = hw_interface.serialize(); + hw_interface_copy.deserialize(serial_data); + + EXPECT_TRUE(hw_interface.isDataEqual(hw_interface_copy)); +} + +TEST_F(RobotHardwareInterfaceTestFixture, testSerializationPipelineV5ToCoprocessorDualJoystick) { + RobotHardwareInterface hw_interface(device_config_map_ptr_dual_joy_, hardware_type_e::V5_BRAIN); + + // Update Motors + auto motor_data_1 = getRandomMotorData(false); + motor_data_1->name = "left_drive_motor"; + hw_interface.setDeviceData(motor_data_1); + auto motor_data_2 = getRandomMotorData(false); + motor_data_2->name = "test_motor"; + hw_interface.setDeviceData(motor_data_2); + auto motor_data_3 = getRandomMotorData(false); + motor_data_3->name = "default_motor"; + hw_interface.setDeviceData(motor_data_3); + + // Update Rotation Sensors + auto rotation_sensor_1 = getRandomRotationSensorData(); + rotation_sensor_1->name = "rotation_sensor_1"; + hw_interface.setDeviceData(rotation_sensor_1); + auto rotation_sensor_2 = getRandomRotationSensorData(); + rotation_sensor_2->name = "rotation_sensor_2"; + hw_interface.setDeviceData(rotation_sensor_2); + + // Update Inertial Sensor + auto inertial_sensor_1 = getRandomInertialSensorData(); + inertial_sensor_1->name = "inertial_sensor_1"; + hw_interface.setDeviceData(inertial_sensor_1); + + // Update Digital In + auto digital_in = getRandomDigitalInputDeviceData(); + digital_in->name = "digital_in"; + hw_interface.setDeviceData(digital_in); + + // Update Digital Out + auto digital_out = getRandomDigitalOutputDeviceData(); + digital_out->name = "digital_out"; + digital_out->value = false; + hw_interface.setDeviceData(digital_out); + + // Update Competition State + hw_interface.setDisabledStatus(getRandomBool()); + hw_interface.setAutonomousStatus(getRandomBool()); + hw_interface.setConnectedStatus(getRandomBool()); + + // Update Joysticks + auto joy = getRandomJoystickData(); + joy->name = MAIN_JOYSTICK_NAME; + hw_interface.setDeviceData(joy); + + auto joy_2 = getRandomJoystickData(); + joy_2->name = PARTNER_JOYSTICK_NAME; + hw_interface.setDeviceData(joy_2); + + RobotHardwareInterface hw_interface_copy(device_config_map_ptr_dual_joy_, + hardware_type_e::COPROCESSOR); + std::vector serial_data = hw_interface.serialize(); + hw_interface_copy.deserialize(serial_data); + + EXPECT_TRUE(hw_interface.isDataEqual(hw_interface_copy)); +} diff --git a/01_Libraries/ghost_v5_interfaces/test/test_robot_hardware_interface.cpp b/01_Libraries/ghost_v5_interfaces/test/test_robot_hardware_interface.cpp index b6906ef5..0ca190aa 100644 --- a/01_Libraries/ghost_v5_interfaces/test/test_robot_hardware_interface.cpp +++ b/01_Libraries/ghost_v5_interfaces/test/test_robot_hardware_interface.cpp @@ -26,6 +26,7 @@ #include "ghost_v5_interfaces/devices/device_config_map.hpp" #include "ghost_v5_interfaces/devices/joystick_device_interface.hpp" #include "ghost_v5_interfaces/devices/motor_device_interface.hpp" +#include "ghost_v5_interfaces/devices/digital_device_interface.hpp" #include "ghost_v5_interfaces/devices/rotation_sensor_device_interface.hpp" #include "ghost_v5_interfaces/robot_hardware_interface.hpp" #include "ghost_v5_interfaces/test/device_test_utils.hpp" @@ -247,134 +248,32 @@ TEST_F(RobotHardwareInterfaceTestFixture, testIteratorIsOrderedByPort) { EXPECT_EQ(ports, ports_sorted); } -TEST_F(RobotHardwareInterfaceTestFixture, testSerializationPipelineCoprocessorToV5) { - RobotHardwareInterface hw_interface(device_config_map_ptr_single_joy_, - hardware_type_e::COPROCESSOR); - - // Update all motors in the default robot config - auto motor_data_1 = getRandomMotorData(true); - motor_data_1->name = "left_drive_motor"; - hw_interface.setDeviceData(motor_data_1); - auto motor_data_2 = getRandomMotorData(true); - motor_data_2->name = "test_motor"; - hw_interface.setDeviceData(motor_data_2); - auto motor_data_3 = getRandomMotorData(true); - motor_data_3->name = "default_motor"; - hw_interface.setDeviceData(motor_data_3); - auto digital_data_1 = getRandomDigitalDeviceData(ACTUATOR); - hw_interface.setDeviceData(digital_data_1); - - RobotHardwareInterface hw_interface_copy(device_config_map_ptr_single_joy_, - hardware_type_e::V5_BRAIN); - std::vector serial_data = hw_interface.serialize(); - hw_interface_copy.deserialize(serial_data); - - EXPECT_TRUE(hw_interface.isDataEqual(hw_interface_copy)); -} - - -TEST_F(RobotHardwareInterfaceTestFixture, testSerializationPipelineV5ToCoprocessor) { - RobotHardwareInterface hw_interface(device_config_map_ptr_single_joy_, hardware_type_e::V5_BRAIN); - - // Update Motors - auto motor_data_1 = getRandomMotorData(false); - motor_data_1->name = "left_drive_motor"; - hw_interface.setDeviceData(motor_data_1); - auto motor_data_2 = getRandomMotorData(false); - motor_data_2->name = "test_motor"; - hw_interface.setDeviceData(motor_data_2); - auto motor_data_3 = getRandomMotorData(false); - motor_data_3->name = "default_motor"; - hw_interface.setDeviceData(motor_data_3); - - // Update Rotation Sensors - auto rotation_sensor_1 = getRandomRotationSensorData(); - rotation_sensor_1->name = "rotation_sensor_1"; - hw_interface.setDeviceData(rotation_sensor_1); - auto rotation_sensor_2 = getRandomRotationSensorData(); - rotation_sensor_2->name = "rotation_sensor_2"; - hw_interface.setDeviceData(rotation_sensor_2); - - // Update Inertial Sensor - auto inertial_sensor_1 = getRandomInertialSensorData(); - inertial_sensor_1->name = "inertial_sensor_1"; - hw_interface.setDeviceData(inertial_sensor_1); - - // Update Digital Sensor - auto digital_sensor_1 = getRandomDigitalDeviceData(SENSOR); - digital_sensor_1->name = "digital_sensor_1"; - hw_interface.setDeviceData(digital_sensor_1); - - // Update Competition State - hw_interface.setDisabledStatus(getRandomBool()); - hw_interface.setAutonomousStatus(getRandomBool()); - hw_interface.setConnectedStatus(getRandomBool()); - - // Update Joystick - auto joy = getRandomJoystickData(); - joy->name = MAIN_JOYSTICK_NAME; - hw_interface.setDeviceData(joy); - - RobotHardwareInterface hw_interface_copy(device_config_map_ptr_single_joy_, +TEST_F(RobotHardwareInterfaceTestFixture, testDigitalOutSetterGetter) { + RobotHardwareInterface hw_interface(device_config_map_ptr_dual_joy_, hardware_type_e::COPROCESSOR); - std::vector serial_data = hw_interface.serialize(); - hw_interface_copy.deserialize(serial_data); - EXPECT_TRUE(hw_interface.isDataEqual(hw_interface_copy)); + // Default + EXPECT_FALSE(hw_interface.getDigitalDeviceValue("digital_in")); + EXPECT_FALSE(hw_interface.getDigitalDeviceValue("digital_out")); + EXPECT_THROW(hw_interface.setDigitalInputValue("digital_in", true), std::runtime_error); + EXPECT_NO_THROW(hw_interface.setDigitalOutputValue("digital_out", true)); + EXPECT_EQ(hw_interface.getDigitalDeviceValue("digital_out"), true); + EXPECT_NO_THROW(hw_interface.setDigitalOutputValue("digital_out", false)); + EXPECT_EQ(hw_interface.getDigitalDeviceValue("digital_out"), false); } -TEST_F(RobotHardwareInterfaceTestFixture, testSerializationPipelineV5ToCoprocessorDualJoystick) { - RobotHardwareInterface hw_interface(device_config_map_ptr_dual_joy_, hardware_type_e::V5_BRAIN); - - // Update Motors - auto motor_data_1 = getRandomMotorData(false); - motor_data_1->name = "left_drive_motor"; - hw_interface.setDeviceData(motor_data_1); - auto motor_data_2 = getRandomMotorData(false); - motor_data_2->name = "test_motor"; - hw_interface.setDeviceData(motor_data_2); - auto motor_data_3 = getRandomMotorData(false); - motor_data_3->name = "default_motor"; - hw_interface.setDeviceData(motor_data_3); - - // Update Rotation Sensors - auto rotation_sensor_1 = getRandomRotationSensorData(); - rotation_sensor_1->name = "rotation_sensor_1"; - hw_interface.setDeviceData(rotation_sensor_1); - auto rotation_sensor_2 = getRandomRotationSensorData(); - rotation_sensor_2->name = "rotation_sensor_2"; - hw_interface.setDeviceData(rotation_sensor_2); - - // Update Inertial Sensor - auto inertial_sensor_1 = getRandomInertialSensorData(); - inertial_sensor_1->name = "inertial_sensor_1"; - hw_interface.setDeviceData(inertial_sensor_1); - - // Update Digital Sensor - auto digital_sensor_1 = getRandomDigitalDeviceData(SENSOR); - digital_sensor_1->name = "digital_sensor_1"; - hw_interface.setDeviceData(digital_sensor_1); - - // Update Competition State - hw_interface.setDisabledStatus(getRandomBool()); - hw_interface.setAutonomousStatus(getRandomBool()); - hw_interface.setConnectedStatus(getRandomBool()); - - // Update Joysticks - auto joy = getRandomJoystickData(); - joy->name = MAIN_JOYSTICK_NAME; - hw_interface.setDeviceData(joy); - - auto joy_2 = getRandomJoystickData(); - joy_2->name = PARTNER_JOYSTICK_NAME; - hw_interface.setDeviceData(joy_2); - - RobotHardwareInterface hw_interface_copy(device_config_map_ptr_dual_joy_, - hardware_type_e::COPROCESSOR); - std::vector serial_data = hw_interface.serialize(); - hw_interface_copy.deserialize(serial_data); +TEST_F(RobotHardwareInterfaceTestFixture, testDigitalInSetterGetter) { + RobotHardwareInterface hw_interface(device_config_map_ptr_dual_joy_, + hardware_type_e::V5_BRAIN); - EXPECT_TRUE(hw_interface.isDataEqual(hw_interface_copy)); + // Default + EXPECT_FALSE(hw_interface.getDigitalDeviceValue("digital_out")); + EXPECT_FALSE(hw_interface.getDigitalDeviceValue("digital_in")); + EXPECT_THROW(hw_interface.setDigitalOutputValue("digital_out", true), std::runtime_error); + EXPECT_NO_THROW(hw_interface.setDigitalInputValue("digital_in", true)); + EXPECT_EQ(hw_interface.getDigitalDeviceValue("digital_in"), true); + EXPECT_NO_THROW(hw_interface.setDigitalInputValue("digital_in", false)); + EXPECT_EQ(hw_interface.getDigitalDeviceValue("digital_in"), false); } TEST_F(RobotHardwareInterfaceTestFixture, testMotorStateGetters) { @@ -454,8 +353,6 @@ TEST_F(RobotHardwareInterfaceTestFixture, testInertialSensorStateGetters) { EXPECT_EQ(hw_interface.getInertialSensorHeading("inertial_sensor_1"), sensor_data_ptr->heading); } -//TODO(xander): testDigitalDeviceStateGetters - TEST_F(RobotHardwareInterfaceTestFixture, testSetMotorPositionCommand) { RobotHardwareInterface hw_interface(device_config_map_ptr_dual_joy_, hardware_type_e::COPROCESSOR); diff --git a/01_Libraries/ghost_v5_interfaces/test/util/test_device_config_factory_utils.cpp b/01_Libraries/ghost_v5_interfaces/test/util/test_device_config_factory_utils.cpp index 20cdc645..4cefb5b2 100644 --- a/01_Libraries/ghost_v5_interfaces/test/util/test_device_config_factory_utils.cpp +++ b/01_Libraries/ghost_v5_interfaces/test/util/test_device_config_factory_utils.cpp @@ -129,6 +129,20 @@ class DeviceConfigMapTestFixture : public ::testing::Test rotation_sensor_2->type = device_type_e::ROTATION_SENSOR; robot_config_ptr->addDeviceConfig(rotation_sensor_2); + // Default (minimal required params) + auto digital_in = std::make_shared(); + digital_in->port = 22; + digital_in->name = "digital_in"; + digital_in->type = device_type_e::DIGITAL_INPUT; + robot_config_ptr->addDeviceConfig(digital_in); + + // Default (minimal required params) + auto digital_out = std::make_shared(); + digital_out->port = 23; + digital_out->name = "digital_out"; + digital_out->type = device_type_e::DIGITAL_OUTPUT; + robot_config_ptr->addDeviceConfig(digital_out); + return robot_config_ptr; } }; From e50f1ab0d13a98bc6ac4936a9b4ab1dd34559cde Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Thu, 6 Jun 2024 17:20:49 -0500 Subject: [PATCH 78/97] updates cmake --- 01_Libraries/ghost_v5_interfaces/CMakeLists.txt | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/CMakeLists.txt b/01_Libraries/ghost_v5_interfaces/CMakeLists.txt index ba15f803..609a8dda 100644 --- a/01_Libraries/ghost_v5_interfaces/CMakeLists.txt +++ b/01_Libraries/ghost_v5_interfaces/CMakeLists.txt @@ -156,6 +156,7 @@ foreach(TEST ${UTIL_LIBS}) load_motor_device_config_yaml load_rotation_sensor_device_config_yaml load_inertial_sensor_device_config_yaml + load_digital_device_config_yaml device_config_factory_utils yaml-cpp ) @@ -171,12 +172,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() From e65bc77a2689d8a09de4a5c167086bb0333fe7dc Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Thu, 6 Jun 2024 17:34:06 -0500 Subject: [PATCH 79/97] adds digital device msg tests --- .../test/test_msg_helpers.cpp | 22 +++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/03_ROS/ghost_ros_interfaces/test/test_msg_helpers.cpp b/03_ROS/ghost_ros_interfaces/test/test_msg_helpers.cpp index 86833d21..71e96b05 100644 --- a/03_ROS/ghost_ros_interfaces/test/test_msg_helpers.cpp +++ b/03_ROS/ghost_ros_interfaces/test/test_msg_helpers.cpp @@ -90,6 +90,28 @@ TEST(TestMsgHelpers, testJoystickStateMsg) { EXPECT_EQ(*joy_input, *joy_output); } +TEST(TestMsgHelpers, testDigitalInputDeviceStateMsg) { + auto digital_state_input = getRandomDigitalInputDeviceData(); + auto msg = std::make_shared(); + auto digital_state_output = std::make_shared(); + + toROSMsg(*digital_state_input, *msg); + fromROSMsg(*digital_state_output, *msg); + + EXPECT_EQ(*digital_state_input, *digital_state_output); +} + +TEST(TestMsgHelpers, testDigitalOutputDeviceStateMsg) { + auto digital_state_input = getRandomDigitalOutputDeviceData(); + auto msg = std::make_shared(); + auto digital_state_output = std::make_shared(); + + toROSMsg(*digital_state_input, *msg); + fromROSMsg(*digital_state_output, *msg); + + EXPECT_EQ(*digital_state_input, *digital_state_output); +} + TEST(TestDeviceInterfaces, testRobotTrajectoryMsg) { auto rt_input = std::make_shared(); auto mt_input = std::make_shared(); From 64300bf83dfa9adc466970899172579c22cfcfbf Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Thu, 6 Jun 2024 17:58:18 -0500 Subject: [PATCH 80/97] updates digital msg helpers --- .../msg_helpers/msg_helpers.hpp | 28 ++++++++++++++++--- 1 file changed, 24 insertions(+), 4 deletions(-) diff --git a/03_ROS/ghost_ros_interfaces/include/ghost_ros_interfaces/msg_helpers/msg_helpers.hpp b/03_ROS/ghost_ros_interfaces/include/ghost_ros_interfaces/msg_helpers/msg_helpers.hpp index 2cd4983d..3410c3e8 100644 --- a/03_ROS/ghost_ros_interfaces/include/ghost_ros_interfaces/msg_helpers/msg_helpers.hpp +++ b/03_ROS/ghost_ros_interfaces/include/ghost_ros_interfaces/msg_helpers/msg_helpers.hpp @@ -187,25 +187,45 @@ void fromROSMsg( // Digital Device /** - * @brief Copies digital device state data from a DigitalDeviceData object into a V5DigitalDeviceState msg. + * @brief Copies digital device state data from a DigitalInputDeviceData object into a V5DigitalDeviceState msg. * * @param digital_device_data * @param digital_device_msg */ void toROSMsg( - const ghost_v5_interfaces::devices::DigitalDeviceData & digital_device_data, + const ghost_v5_interfaces::devices::DigitalInputDeviceData & digital_device_data, ghost_msgs::msg::V5DigitalDeviceState & digital_device_msg); /** - * @brief Copies digital device state data from a V5DigitalDeviceState msg into a DigitalDeviceData object. + * @brief Copies digital device state data from a V5DigitalDeviceState msg into a DigitalInputDeviceData object. * * @param digital_device_data * @param digital_device_msg */ void fromROSMsg( - const ghost_v5_interfaces::devices::DigitalDeviceData & digital_device_data, + ghost_v5_interfaces::devices::DigitalInputDeviceData & digital_device_data, + const ghost_msgs::msg::V5DigitalDeviceState & digital_device_msg); + +/** + * @brief Copies digital device state data from a DigitalOutputDeviceData object into a V5DigitalDeviceState msg. + * + * @param digital_device_data + * @param digital_device_msg + */ +void toROSMsg( + const ghost_v5_interfaces::devices::DigitalOutputDeviceData & digital_device_data, ghost_msgs::msg::V5DigitalDeviceState & digital_device_msg); +/** + * @brief Copies digital device state data from a V5DigitalDeviceState msg into a DigitalOutputDeviceData object. + * + * @param digital_device_data + * @param digital_device_msg + */ +void fromROSMsg( + ghost_v5_interfaces::devices::DigitalOutputDeviceData & digital_device_data, + const ghost_msgs::msg::V5DigitalDeviceState & digital_device_msg); + // Aggregate Actuator Command /** From 880e58aebf0e51b9f4521f73ed9786daeb0adb8e Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Thu, 6 Jun 2024 17:58:27 -0500 Subject: [PATCH 81/97] adds msg helper tests --- 03_ROS/ghost_ros_interfaces/test/test_msg_helpers.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/03_ROS/ghost_ros_interfaces/test/test_msg_helpers.cpp b/03_ROS/ghost_ros_interfaces/test/test_msg_helpers.cpp index 71e96b05..5f6a0a96 100644 --- a/03_ROS/ghost_ros_interfaces/test/test_msg_helpers.cpp +++ b/03_ROS/ghost_ros_interfaces/test/test_msg_helpers.cpp @@ -93,7 +93,7 @@ TEST(TestMsgHelpers, testJoystickStateMsg) { TEST(TestMsgHelpers, testDigitalInputDeviceStateMsg) { auto digital_state_input = getRandomDigitalInputDeviceData(); auto msg = std::make_shared(); - auto digital_state_output = std::make_shared(); + auto digital_state_output = std::make_shared(""); toROSMsg(*digital_state_input, *msg); fromROSMsg(*digital_state_output, *msg); @@ -104,7 +104,7 @@ TEST(TestMsgHelpers, testDigitalInputDeviceStateMsg) { TEST(TestMsgHelpers, testDigitalOutputDeviceStateMsg) { auto digital_state_input = getRandomDigitalOutputDeviceData(); auto msg = std::make_shared(); - auto digital_state_output = std::make_shared(); + auto digital_state_output = std::make_shared(""); toROSMsg(*digital_state_input, *msg); fromROSMsg(*digital_state_output, *msg); From b40503a127484d492f5b7483103e200b9d1096c4 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Thu, 6 Jun 2024 17:58:41 -0500 Subject: [PATCH 82/97] fixes error messages in RHI digital helpers --- .../ghost_v5_interfaces/src/robot_hardware_interface.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp b/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp index 8d3fbed4..4a8a980f 100644 --- a/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp +++ b/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp @@ -484,7 +484,7 @@ bool RobotHardwareInterface::getDigitalDeviceValue(const std::string & device_na void RobotHardwareInterface::setDigitalInputValue(const std::string & device_name, bool value){ if (hardware_type_ != V5_BRAIN) { - throw std::runtime_error("[RobotHardwareInterface::setDigitalDeviceValue] Error: Attempted to set Digital Sensor value from Coprocessor."); + throw std::runtime_error("[RobotHardwareInterface::setDigitalInputValue] Error: Attempted to set Digital Sensor value from Coprocessor."); } std::unique_lock update_lock(update_mutex_); @@ -496,7 +496,7 @@ void RobotHardwareInterface::setDigitalInputValue(const std::string & device_nam void RobotHardwareInterface::setDigitalOutputValue(const std::string & device_name, bool value){ if (hardware_type_ != COPROCESSOR) { - throw std::runtime_error("[RobotHardwareInterface::setDigitalDeviceValue] Error: Attempted to set Digital Actuator value from V5 Brain."); + throw std::runtime_error("[RobotHardwareInterface::setDigitalOutputValue] Error: Attempted to set Digital Actuator value from V5 Brain."); } std::unique_lock update_lock(update_mutex_); From a2af3a7102fe7a59456dbbcfde73041f25e4794b Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Thu, 6 Jun 2024 17:58:53 -0500 Subject: [PATCH 83/97] updates rhi digital helper calls in BT nodes --- 11_Robots/ghost_swerve/src/bt_nodes/climb.cpp | 2 +- 11_Robots/ghost_swerve/src/bt_nodes/swipeTail.cpp | 4 ++-- 11_Robots/ghost_swerve/src/swerve_robot_plugin.cpp | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/11_Robots/ghost_swerve/src/bt_nodes/climb.cpp b/11_Robots/ghost_swerve/src/bt_nodes/climb.cpp index 1f0c8b24..4cc4a7da 100644 --- a/11_Robots/ghost_swerve/src/bt_nodes/climb.cpp +++ b/11_Robots/ghost_swerve/src/bt_nodes/climb.cpp @@ -118,7 +118,7 @@ BT::NodeStatus Climb::onRunning() swerve_ptr_->getConfig().lift_kP); // go to 90deg // ignore err - rhi_ptr_->setDigitalDeviceValue("claw", claw_open); + rhi_ptr_->setDigitalInputValue("claw", claw_open); // do not return any status but RUNNING // so it keeps the state at the end of the auton diff --git a/11_Robots/ghost_swerve/src/bt_nodes/swipeTail.cpp b/11_Robots/ghost_swerve/src/bt_nodes/swipeTail.cpp index 7feab244..13ca7a76 100644 --- a/11_Robots/ghost_swerve/src/bt_nodes/swipeTail.cpp +++ b/11_Robots/ghost_swerve/src/bt_nodes/swipeTail.cpp @@ -118,9 +118,9 @@ BT::NodeStatus SwipeTail::onRunning() // } // if(status == BT::NodeStatus::SUCCESS){ - // rhi_ptr_->setDigitalDeviceValue("tail", false); + // rhi_ptr_->setDigitalInputValue("tail", false); // } else { - // rhi_ptr_->setDigitalDeviceValue("tail", true); + // rhi_ptr_->setDigitalInputValue("tail", true); // } return status; diff --git a/11_Robots/ghost_swerve/src/swerve_robot_plugin.cpp b/11_Robots/ghost_swerve/src/swerve_robot_plugin.cpp index 933ea0b8..d13d91eb 100644 --- a/11_Robots/ghost_swerve/src/swerve_robot_plugin.cpp +++ b/11_Robots/ghost_swerve/src/swerve_robot_plugin.cpp @@ -734,8 +734,8 @@ void SwerveRobotPlugin::teleop(double current_time) rhi_ptr_->setMotorPositionCommand("tail_motor", m_stick_angle_start); } - rhi_ptr_->setDigitalDeviceValue("tail", tail_down); - rhi_ptr_->setDigitalDeviceValue("claw", !m_claw_open); + rhi_ptr_->setDigitalInputValue("tail", tail_down); + rhi_ptr_->setDigitalInputValue("claw", !m_claw_open); // If INTAKE_MOTOR stalling, update state and timer if ((intake_command) && From 6e2a2519eb91d3474acf95f54451eed40f66bb9c Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Thu, 6 Jun 2024 18:05:33 -0500 Subject: [PATCH 84/97] updates rhi ros msg tests --- .../test_robot_hardware_interface_ros.cpp | 27 +++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/03_ROS/ghost_ros_interfaces/test/test_robot_hardware_interface_ros.cpp b/03_ROS/ghost_ros_interfaces/test/test_robot_hardware_interface_ros.cpp index a8884e40..81abbb7a 100644 --- a/03_ROS/ghost_ros_interfaces/test/test_robot_hardware_interface_ros.cpp +++ b/03_ROS/ghost_ros_interfaces/test/test_robot_hardware_interface_ros.cpp @@ -92,6 +92,10 @@ TEST_F(RobotHardwareInterfaceROSTestFixture, testRobotHardwareInterfaceSensorUpd rotation_sensor_data_ptr->name = "rotation_sensor_1"; rhi_input_ptr_->setDeviceData(rotation_sensor_data_ptr); + auto digital_in_ptr = getRandomDigitalInputDeviceData(); + digital_in_ptr->name = "digital_in"; + rhi_input_ptr_->setDeviceData(digital_in_ptr); + auto msg = std::make_shared(); // Convert to ROS Msg @@ -107,6 +111,11 @@ TEST_F(RobotHardwareInterfaceROSTestFixture, testRobotHardwareInterfaceActuatorC rhi_input_ptr_->setDeviceData(motor_data_ptr); auto msg = std::make_shared(); + auto digital_out_ptr = getRandomDigitalOutputDeviceData(); + digital_out_ptr->name = "digital_out"; + digital_out_ptr->value = false; + rhi_input_ptr_->setDeviceData(digital_out_ptr); + // Convert to ROS Msg toROSMsg(*rhi_input_ptr_, *msg); fromROSMsg(*rhi_output_ptr_, *msg); @@ -146,6 +155,15 @@ TEST_F(RobotHardwareInterfaceROSTestFixture, testRobotHardwareInterfaceFullCycle motor_data_ptr->curr_temp_c = getRandomFloat(); rhi_input_ptr_->setDeviceData(motor_data_ptr); + auto digital_in_ptr = getRandomDigitalInputDeviceData(); + digital_in_ptr->name = "digital_in"; + rhi_input_ptr_->setDeviceData(digital_in_ptr); + + auto digital_out_ptr = getRandomDigitalOutputDeviceData(); + digital_out_ptr->name = "digital_out"; + digital_out_ptr->value = false; + rhi_input_ptr_->setDeviceData(digital_out_ptr); + auto rotation_sensor_data_ptr = getRandomRotationSensorData(); rotation_sensor_data_ptr->name = "rotation_sensor_1"; rhi_input_ptr_->setDeviceData(rotation_sensor_data_ptr); @@ -194,6 +212,15 @@ TEST_F(RobotHardwareInterfaceROSTestFixture, testRobotHardwareInterfaceFullCycle motor_data_ptr->curr_temp_c = getRandomFloat(); rhi_input_ptr_->setDeviceData(motor_data_ptr); + auto digital_in_ptr = getRandomDigitalInputDeviceData(); + digital_in_ptr->name = "digital_in"; + rhi_input_ptr_->setDeviceData(digital_in_ptr); + + auto digital_out_ptr = getRandomDigitalOutputDeviceData(); + digital_out_ptr->name = "digital_out"; + digital_out_ptr->value = false; + rhi_input_ptr_->setDeviceData(digital_out_ptr); + auto rotation_sensor_data_ptr = getRandomRotationSensorData(); rotation_sensor_data_ptr->name = "rotation_sensor_1"; rhi_input_ptr_->setDeviceData(rotation_sensor_data_ptr); From 64b22033f5d637b394d31b396030475724068a03 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Sun, 17 Nov 2024 17:40:41 -0600 Subject: [PATCH 85/97] fixes whitespace --- .../ghost_v5_interfaces/src/robot_hardware_interface.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp b/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp index 4a8a980f..68020f75 100644 --- a/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp +++ b/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp @@ -61,7 +61,7 @@ RobotHardwareInterface::RobotHardwareInterface( pair.data_ptr = std::make_shared(val->name); } else if (pair.config_ptr->type == device_type_e::DIGITAL_OUTPUT) { pair.data_ptr = std::make_shared(val->name); - }else { + } else { throw std::runtime_error( "[RobotHardwareInterface::RobotHardwareInterface()] Device type " + std::to_string( pair.config_ptr->type) + " is unsupported!"); From 7c5e79a4bc21b0211115efcabfefee6d33ce22a1 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Sun, 17 Nov 2024 17:58:31 -0600 Subject: [PATCH 86/97] slightly refactors load digital device yaml --- .../src/util/load_digital_device_config_yaml.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/src/util/load_digital_device_config_yaml.cpp b/01_Libraries/ghost_v5_interfaces/src/util/load_digital_device_config_yaml.cpp index f3b7e52f..087a70cb 100644 --- a/01_Libraries/ghost_v5_interfaces/src/util/load_digital_device_config_yaml.cpp +++ b/01_Libraries/ghost_v5_interfaces/src/util/load_digital_device_config_yaml.cpp @@ -42,11 +42,11 @@ void loadDigitalDeviceConfigFromYAML( auto device_node = node["adi"][device_name]; if (!device_node) { throw std::runtime_error( - "[loadDigitalDeviceConfigFromYAML] Error: Device Sensor " + device_name + + "[loadDigitalDeviceConfigFromYAML] Error: Digital Device " + device_name + " not found!"); } - // Load IO type + // Load type std::string type; if (!loadYAMLParam(device_node, "type", type, verbose)) { throw std::runtime_error( @@ -61,7 +61,7 @@ void loadDigitalDeviceConfigFromYAML( device_config_ptr->type = devices::device_type_e::DIGITAL_OUTPUT; } else{ - throw std::runtime_error(std::string("[loadDigitalDeviceConfigFromYAML] Error: Unsupported digital device ") + type + "!"); + throw std::runtime_error(std::string("[loadDigitalDeviceConfigFromYAML] Error: Unsupported digital device type ") + type + "!"); } // Get port @@ -73,7 +73,6 @@ void loadDigitalDeviceConfigFromYAML( } char port = port_string[0]; - // Validate port const std::vector valid_ports = {'A', 'B', 'C', 'D', 'E', 'F', 'G', 'H'}; if (std::count(valid_ports.begin(), valid_ports.end(), port) == 0) { @@ -83,7 +82,7 @@ void loadDigitalDeviceConfigFromYAML( } // Convert port from A-H to 22-28 - port += 22 - 'A'; + port = port - 'A' + 22; // Set port in device config device_config_ptr->port = (int) port; From 736199da180fbfcaf4c5608285d7eaedb77f244d Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Sun, 17 Nov 2024 18:27:04 -0600 Subject: [PATCH 87/97] updates example_robot_2 yaml --- .../test/config/example_robot_2.yaml | 4 +- .../test/test_rhi_serialization.cpp | 44 +++++++++---------- .../test/test_robot_hardware_interface.cpp | 28 ++++++------ 3 files changed, 38 insertions(+), 38 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/test/config/example_robot_2.yaml b/01_Libraries/ghost_v5_interfaces/test/config/example_robot_2.yaml index d7fe7ac0..ff9dee8e 100644 --- a/01_Libraries/ghost_v5_interfaces/test/config/example_robot_2.yaml +++ b/01_Libraries/ghost_v5_interfaces/test/config/example_robot_2.yaml @@ -1,10 +1,10 @@ port_configuration: use_partner_joystick: false adi: - digital_in: + digital_in_1: port: A type: DIGITAL_INPUT - digital_out: + digital_out_1: port: B type: DIGITAL_OUTPUT devices: diff --git a/01_Libraries/ghost_v5_interfaces/test/test_rhi_serialization.cpp b/01_Libraries/ghost_v5_interfaces/test/test_rhi_serialization.cpp index 43dccc52..7126a298 100644 --- a/01_Libraries/ghost_v5_interfaces/test/test_rhi_serialization.cpp +++ b/01_Libraries/ghost_v5_interfaces/test/test_rhi_serialization.cpp @@ -78,15 +78,15 @@ TEST_F(RobotHardwareInterfaceTestFixture, testSerializationPipelineCoprocessorTo hw_interface.setDeviceData(motor_data_3); // Update Digital In - auto digital_in = getRandomDigitalInputDeviceData(); - digital_in->name = "digital_in"; - digital_in->value = false; - hw_interface.setDeviceData(digital_in); + auto digital_in_1 = getRandomDigitalInputDeviceData(); + digital_in_1->name = "digital_in_1"; + digital_in_1->value = false; + hw_interface.setDeviceData(digital_in_1); // Update Digital Out - auto digital_out = getRandomDigitalOutputDeviceData(); - digital_out->name = "digital_out"; - hw_interface.setDeviceData(digital_out); + auto digital_out_1 = getRandomDigitalOutputDeviceData(); + digital_out_1->name = "digital_out_1"; + hw_interface.setDeviceData(digital_out_1); RobotHardwareInterface hw_interface_copy(device_config_map_ptr_single_joy_, hardware_type_e::V5_BRAIN); @@ -125,16 +125,16 @@ TEST_F(RobotHardwareInterfaceTestFixture, testSerializationPipelineV5ToCoprocess hw_interface.setDeviceData(inertial_sensor_1); // Update Digital In - auto digital_in = getRandomDigitalInputDeviceData(); - digital_in->name = "digital_in"; - digital_in->value = false; - hw_interface.setDeviceData(digital_in); + auto digital_in_1 = getRandomDigitalInputDeviceData(); + digital_in_1->name = "digital_in_1"; + digital_in_1->value = false; + hw_interface.setDeviceData(digital_in_1); // Update Digital Out - auto digital_out = getRandomDigitalOutputDeviceData(); - digital_out->name = "digital_out"; - digital_out->value = false; - hw_interface.setDeviceData(digital_out); + auto digital_out_1 = getRandomDigitalOutputDeviceData(); + digital_out_1->name = "digital_out_1"; + digital_out_1->value = false; + hw_interface.setDeviceData(digital_out_1); // Update Competition State hw_interface.setDisabledStatus(getRandomBool()); @@ -182,15 +182,15 @@ TEST_F(RobotHardwareInterfaceTestFixture, testSerializationPipelineV5ToCoprocess hw_interface.setDeviceData(inertial_sensor_1); // Update Digital In - auto digital_in = getRandomDigitalInputDeviceData(); - digital_in->name = "digital_in"; - hw_interface.setDeviceData(digital_in); + auto digital_in_1 = getRandomDigitalInputDeviceData(); + digital_in_1->name = "digital_in_1"; + hw_interface.setDeviceData(digital_in_1); // Update Digital Out - auto digital_out = getRandomDigitalOutputDeviceData(); - digital_out->name = "digital_out"; - digital_out->value = false; - hw_interface.setDeviceData(digital_out); + auto digital_out_1 = getRandomDigitalOutputDeviceData(); + digital_out_1->name = "digital_out_1"; + digital_out_1->value = false; + hw_interface.setDeviceData(digital_out_1); // Update Competition State hw_interface.setDisabledStatus(getRandomBool()); diff --git a/01_Libraries/ghost_v5_interfaces/test/test_robot_hardware_interface.cpp b/01_Libraries/ghost_v5_interfaces/test/test_robot_hardware_interface.cpp index 0ca190aa..b89afa86 100644 --- a/01_Libraries/ghost_v5_interfaces/test/test_robot_hardware_interface.cpp +++ b/01_Libraries/ghost_v5_interfaces/test/test_robot_hardware_interface.cpp @@ -253,13 +253,13 @@ TEST_F(RobotHardwareInterfaceTestFixture, testDigitalOutSetterGetter) { hardware_type_e::COPROCESSOR); // Default - EXPECT_FALSE(hw_interface.getDigitalDeviceValue("digital_in")); - EXPECT_FALSE(hw_interface.getDigitalDeviceValue("digital_out")); - EXPECT_THROW(hw_interface.setDigitalInputValue("digital_in", true), std::runtime_error); - EXPECT_NO_THROW(hw_interface.setDigitalOutputValue("digital_out", true)); - EXPECT_EQ(hw_interface.getDigitalDeviceValue("digital_out"), true); - EXPECT_NO_THROW(hw_interface.setDigitalOutputValue("digital_out", false)); - EXPECT_EQ(hw_interface.getDigitalDeviceValue("digital_out"), false); + EXPECT_FALSE(hw_interface.getDigitalDeviceValue("digital_in_1")); + EXPECT_FALSE(hw_interface.getDigitalDeviceValue("digital_out_1")); + EXPECT_THROW(hw_interface.setDigitalInputValue("digital_in_1", true), std::runtime_error); + EXPECT_NO_THROW(hw_interface.setDigitalOutputValue("digital_out_1", true)); + EXPECT_EQ(hw_interface.getDigitalDeviceValue("digital_out_1"), true); + EXPECT_NO_THROW(hw_interface.setDigitalOutputValue("digital_out_1", false)); + EXPECT_EQ(hw_interface.getDigitalDeviceValue("digital_out_1"), false); } TEST_F(RobotHardwareInterfaceTestFixture, testDigitalInSetterGetter) { @@ -267,13 +267,13 @@ TEST_F(RobotHardwareInterfaceTestFixture, testDigitalInSetterGetter) { hardware_type_e::V5_BRAIN); // Default - EXPECT_FALSE(hw_interface.getDigitalDeviceValue("digital_out")); - EXPECT_FALSE(hw_interface.getDigitalDeviceValue("digital_in")); - EXPECT_THROW(hw_interface.setDigitalOutputValue("digital_out", true), std::runtime_error); - EXPECT_NO_THROW(hw_interface.setDigitalInputValue("digital_in", true)); - EXPECT_EQ(hw_interface.getDigitalDeviceValue("digital_in"), true); - EXPECT_NO_THROW(hw_interface.setDigitalInputValue("digital_in", false)); - EXPECT_EQ(hw_interface.getDigitalDeviceValue("digital_in"), false); + EXPECT_FALSE(hw_interface.getDigitalDeviceValue("digital_out_1")); + EXPECT_FALSE(hw_interface.getDigitalDeviceValue("digital_in_1")); + EXPECT_THROW(hw_interface.setDigitalOutputValue("digital_out_1", true), std::runtime_error); + EXPECT_NO_THROW(hw_interface.setDigitalInputValue("digital_in_1", true)); + EXPECT_EQ(hw_interface.getDigitalDeviceValue("digital_in_1"), true); + EXPECT_NO_THROW(hw_interface.setDigitalInputValue("digital_in_1", false)); + EXPECT_EQ(hw_interface.getDigitalDeviceValue("digital_in_1"), false); } TEST_F(RobotHardwareInterfaceTestFixture, testMotorStateGetters) { From 54eb2fb28f7ecdb760cabb79a8ea9e1986304f41 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Sun, 17 Nov 2024 18:33:04 -0600 Subject: [PATCH 88/97] update example_robot yaml --- .../test/config/example_robot.yaml | 4 ++-- .../test/util/test_device_config_factory_utils.cpp | 4 ++-- .../test/test_robot_hardware_interface_ros.cpp | 12 ++++++------ 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/test/config/example_robot.yaml b/01_Libraries/ghost_v5_interfaces/test/config/example_robot.yaml index 4eddf127..e54d1e35 100644 --- a/01_Libraries/ghost_v5_interfaces/test/config/example_robot.yaml +++ b/01_Libraries/ghost_v5_interfaces/test/config/example_robot.yaml @@ -1,10 +1,10 @@ port_configuration: use_partner_joystick: false adi: - digital_in: + digital_in_1: port: A type: DIGITAL_INPUT - digital_out: + digital_out_1: port: B type: DIGITAL_OUTPUT diff --git a/01_Libraries/ghost_v5_interfaces/test/util/test_device_config_factory_utils.cpp b/01_Libraries/ghost_v5_interfaces/test/util/test_device_config_factory_utils.cpp index 4cefb5b2..4e2960db 100644 --- a/01_Libraries/ghost_v5_interfaces/test/util/test_device_config_factory_utils.cpp +++ b/01_Libraries/ghost_v5_interfaces/test/util/test_device_config_factory_utils.cpp @@ -132,14 +132,14 @@ class DeviceConfigMapTestFixture : public ::testing::Test // Default (minimal required params) auto digital_in = std::make_shared(); digital_in->port = 22; - digital_in->name = "digital_in"; + digital_in->name = "digital_in_1"; digital_in->type = device_type_e::DIGITAL_INPUT; robot_config_ptr->addDeviceConfig(digital_in); // Default (minimal required params) auto digital_out = std::make_shared(); digital_out->port = 23; - digital_out->name = "digital_out"; + digital_out->name = "digital_out_1"; digital_out->type = device_type_e::DIGITAL_OUTPUT; robot_config_ptr->addDeviceConfig(digital_out); diff --git a/03_ROS/ghost_ros_interfaces/test/test_robot_hardware_interface_ros.cpp b/03_ROS/ghost_ros_interfaces/test/test_robot_hardware_interface_ros.cpp index 81abbb7a..91c23e8d 100644 --- a/03_ROS/ghost_ros_interfaces/test/test_robot_hardware_interface_ros.cpp +++ b/03_ROS/ghost_ros_interfaces/test/test_robot_hardware_interface_ros.cpp @@ -93,7 +93,7 @@ TEST_F(RobotHardwareInterfaceROSTestFixture, testRobotHardwareInterfaceSensorUpd rhi_input_ptr_->setDeviceData(rotation_sensor_data_ptr); auto digital_in_ptr = getRandomDigitalInputDeviceData(); - digital_in_ptr->name = "digital_in"; + digital_in_ptr->name = "digital_in_1"; rhi_input_ptr_->setDeviceData(digital_in_ptr); auto msg = std::make_shared(); @@ -112,7 +112,7 @@ TEST_F(RobotHardwareInterfaceROSTestFixture, testRobotHardwareInterfaceActuatorC auto msg = std::make_shared(); auto digital_out_ptr = getRandomDigitalOutputDeviceData(); - digital_out_ptr->name = "digital_out"; + digital_out_ptr->name = "digital_out_1"; digital_out_ptr->value = false; rhi_input_ptr_->setDeviceData(digital_out_ptr); @@ -156,11 +156,11 @@ TEST_F(RobotHardwareInterfaceROSTestFixture, testRobotHardwareInterfaceFullCycle rhi_input_ptr_->setDeviceData(motor_data_ptr); auto digital_in_ptr = getRandomDigitalInputDeviceData(); - digital_in_ptr->name = "digital_in"; + digital_in_ptr->name = "digital_in_1"; rhi_input_ptr_->setDeviceData(digital_in_ptr); auto digital_out_ptr = getRandomDigitalOutputDeviceData(); - digital_out_ptr->name = "digital_out"; + digital_out_ptr->name = "digital_out_1"; digital_out_ptr->value = false; rhi_input_ptr_->setDeviceData(digital_out_ptr); @@ -213,11 +213,11 @@ TEST_F(RobotHardwareInterfaceROSTestFixture, testRobotHardwareInterfaceFullCycle rhi_input_ptr_->setDeviceData(motor_data_ptr); auto digital_in_ptr = getRandomDigitalInputDeviceData(); - digital_in_ptr->name = "digital_in"; + digital_in_ptr->name = "digital_in_1"; rhi_input_ptr_->setDeviceData(digital_in_ptr); auto digital_out_ptr = getRandomDigitalOutputDeviceData(); - digital_out_ptr->name = "digital_out"; + digital_out_ptr->name = "digital_out_1"; digital_out_ptr->value = false; rhi_input_ptr_->setDeviceData(digital_out_ptr); From ee964296493d39b6efecb95a0baffc88f563ed4e Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Sun, 17 Nov 2024 18:33:36 -0600 Subject: [PATCH 89/97] update test_load_digital_device yaml --- .../test/config/test_load_digital_device_config.yaml | 4 ++-- .../test/util/test_load_digital_device_config_yaml.cpp | 8 ++++---- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/test/config/test_load_digital_device_config.yaml b/01_Libraries/ghost_v5_interfaces/test/config/test_load_digital_device_config.yaml index 749722b3..012c3c41 100644 --- a/01_Libraries/ghost_v5_interfaces/test/config/test_load_digital_device_config.yaml +++ b/01_Libraries/ghost_v5_interfaces/test/config/test_load_digital_device_config.yaml @@ -1,9 +1,9 @@ port_configuration: adi: - in: + digital_in_1: port: A type: DIGITAL_INPUT - out: + digital_out_1: port: B type: DIGITAL_OUTPUT devices: diff --git a/01_Libraries/ghost_v5_interfaces/test/util/test_load_digital_device_config_yaml.cpp b/01_Libraries/ghost_v5_interfaces/test/util/test_load_digital_device_config_yaml.cpp index b86ae0e3..ebef28e5 100644 --- a/01_Libraries/ghost_v5_interfaces/test/util/test_load_digital_device_config_yaml.cpp +++ b/01_Libraries/ghost_v5_interfaces/test/util/test_load_digital_device_config_yaml.cpp @@ -42,12 +42,12 @@ class TestLoadDigitalDeviceConfigYAML : public ::testing::Test // Expected motor configurations digital_input_config_ = std::make_shared(); digital_input_config_->port = 22; - digital_input_config_->name = "in"; + digital_input_config_->name = "digital_in_1"; digital_input_config_->type = device_type_e::DIGITAL_INPUT; digital_output_config_ = std::make_shared(); digital_output_config_->port = 23; - digital_output_config_->name = "out"; + digital_output_config_->name = "digital_out_1"; digital_output_config_->type = device_type_e::DIGITAL_OUTPUT; } @@ -60,12 +60,12 @@ class TestLoadDigitalDeviceConfigYAML : public ::testing::Test TEST_F(TestLoadDigitalDeviceConfigYAML, testLoadDigitalIn) { std::shared_ptr input_ptr = std::make_shared(); - loadDigitalDeviceConfigFromYAML(config_yaml_["port_configuration"], "in", input_ptr, false); + loadDigitalDeviceConfigFromYAML(config_yaml_["port_configuration"], "digital_in_1", input_ptr, false); EXPECT_EQ(*input_ptr, *digital_input_config_); } TEST_F(TestLoadDigitalDeviceConfigYAML, testLoadDigitalOut) { std::shared_ptr output_ptr = std::make_shared(); - loadDigitalDeviceConfigFromYAML(config_yaml_["port_configuration"], "out", output_ptr, false); + loadDigitalDeviceConfigFromYAML(config_yaml_["port_configuration"], "digital_out_1", output_ptr, false); EXPECT_EQ(*output_ptr, *digital_output_config_); } \ No newline at end of file From 0f5817f6bda2409d94b48edcd9bd9cb9a6137c1d Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Sun, 8 Dec 2024 16:47:36 -0600 Subject: [PATCH 90/97] adds digital device test to cmake --- 01_Libraries/ghost_v5_interfaces/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/01_Libraries/ghost_v5_interfaces/CMakeLists.txt b/01_Libraries/ghost_v5_interfaces/CMakeLists.txt index 609a8dda..53147f52 100644 --- a/01_Libraries/ghost_v5_interfaces/CMakeLists.txt +++ b/01_Libraries/ghost_v5_interfaces/CMakeLists.txt @@ -138,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) From 310cec919f6468b0c01f20369649e5ed8266a0d7 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Sun, 8 Dec 2024 16:50:51 -0600 Subject: [PATCH 91/97] reduces some duplication in cmake --- 01_Libraries/ghost_v5_interfaces/CMakeLists.txt | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/CMakeLists.txt b/01_Libraries/ghost_v5_interfaces/CMakeLists.txt index 53147f52..54e7c96b 100644 --- a/01_Libraries/ghost_v5_interfaces/CMakeLists.txt +++ b/01_Libraries/ghost_v5_interfaces/CMakeLists.txt @@ -154,10 +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 - load_digital_device_config_yaml + ${UTIL_LIBS} device_config_factory_utils yaml-cpp ) From 1ded1a17ce63bc3b23daa2e04cddfbd838d0d582 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Sun, 8 Dec 2024 17:05:33 -0600 Subject: [PATCH 92/97] adds comments and whitespace --- .../ghost_v5_interfaces/src/robot_hardware_interface.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp b/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp index 68020f75..f2de5408 100644 --- a/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp +++ b/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp @@ -72,7 +72,8 @@ RobotHardwareInterface::RobotHardwareInterface( port_to_device_name_map_.emplace(pair.config_ptr->port, val->name); // Update msg lengths based on each non-digital device - if (pair.config_ptr->type != device_type_e::DIGITAL_INPUT && pair.config_ptr->type != device_type_e::DIGITAL_OUTPUT) { + if (pair.config_ptr->type != device_type_e::DIGITAL_INPUT && + pair.config_ptr->type != device_type_e::DIGITAL_OUTPUT) { sensor_update_msg_length_ += pair.data_ptr->getSensorPacketSize(); actuator_command_msg_length_ += pair.data_ptr->getActuatorPacketSize(); } @@ -92,6 +93,7 @@ std::vector RobotHardwareInterface::serialize() const std::vector serial_data; std::unique_lock update_lock(update_mutex_); + // Competition State if (hardware_type_ == hardware_type_e::V5_BRAIN) { serial_data.push_back( packByte( @@ -102,9 +104,10 @@ std::vector RobotHardwareInterface::serialize() const serial_data.push_back((unsigned char) 0); } - // Reserve empty byte for all Digital IO Ports + // Reserve byte for Digital IO serial_data.push_back((unsigned char) 0); + // Serialize devices for (const auto & [key, val] : device_pair_port_map_) { if (val.config_ptr->type == device_type_e::DIGITAL_INPUT) { setBit(serial_data[1], (val.config_ptr->port - 22), val.data_ptr->as()->value); From a5564914688b949207117f0548035f1adc15d97f Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Sun, 8 Dec 2024 17:06:48 -0600 Subject: [PATCH 93/97] changes digital device port ranges --- .../ghost_v5_interfaces/src/robot_hardware_interface.cpp | 6 +++--- .../src/util/load_digital_device_config_yaml.cpp | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp b/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp index f2de5408..18ca4fe0 100644 --- a/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp +++ b/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp @@ -110,10 +110,10 @@ std::vector RobotHardwareInterface::serialize() const // Serialize devices for (const auto & [key, val] : device_pair_port_map_) { if (val.config_ptr->type == device_type_e::DIGITAL_INPUT) { - setBit(serial_data[1], (val.config_ptr->port - 22), val.data_ptr->as()->value); + setBit(serial_data[1], val.config_ptr->port, val.data_ptr->as()->value); } else if (val.config_ptr->type == device_type_e::DIGITAL_OUTPUT) { - setBit(serial_data[1], (val.config_ptr->port - 22), val.data_ptr->as()->value); + setBit(serial_data[1], val.config_ptr->port, val.data_ptr->as()->value); } else { auto device_serial_msg = val.data_ptr->serialize(hardware_type_); @@ -183,7 +183,7 @@ int RobotHardwareInterface::deserialize(const std::vector & msg) } if (val.config_ptr->type == device_type_e::DIGITAL_INPUT || val.config_ptr->type == device_type_e::DIGITAL_OUTPUT) { - val.data_ptr->deserialize({getBit(msg[1], (val.config_ptr->port - 22))}, hardware_type_); + val.data_ptr->deserialize({getBit(msg[1], val.config_ptr->port)}, hardware_type_); } else { auto start_itr = msg.begin() + byte_offset; diff --git a/01_Libraries/ghost_v5_interfaces/src/util/load_digital_device_config_yaml.cpp b/01_Libraries/ghost_v5_interfaces/src/util/load_digital_device_config_yaml.cpp index 087a70cb..879e0708 100644 --- a/01_Libraries/ghost_v5_interfaces/src/util/load_digital_device_config_yaml.cpp +++ b/01_Libraries/ghost_v5_interfaces/src/util/load_digital_device_config_yaml.cpp @@ -81,8 +81,8 @@ void loadDigitalDeviceConfigFromYAML( device_name + "!"); } - // Convert port from A-H to 22-28 - port = port - 'A' + 22; + // Convert port from A-H to 0-7 + port = port - 'A'; // Set port in device config device_config_ptr->port = (int) port; From 6ae2dcf8648604b695feb039e08fccaacd9200b0 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Sun, 8 Dec 2024 17:48:59 -0600 Subject: [PATCH 94/97] changes RHI deserialize call for digital devices --- .../ghost_v5_interfaces/src/robot_hardware_interface.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp b/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp index 18ca4fe0..11f21aec 100644 --- a/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp +++ b/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp @@ -182,8 +182,10 @@ int RobotHardwareInterface::deserialize(const std::vector & msg) "[RobotHardwareInterface::deserialize] Error: Attempted to deserialize with unsupported hardware type."); } - if (val.config_ptr->type == device_type_e::DIGITAL_INPUT || val.config_ptr->type == device_type_e::DIGITAL_OUTPUT) { - val.data_ptr->deserialize({getBit(msg[1], val.config_ptr->port)}, hardware_type_); + if (val.config_ptr->type == device_type_e::DIGITAL_INPUT || + val.config_ptr->type == device_type_e::DIGITAL_OUTPUT) { + std::vector data = {getBit(msg[1], val.config_ptr->port)}; + val.data_ptr->deserialize(data, hardware_type_); } else { auto start_itr = msg.begin() + byte_offset; From 3a022751df2fd175c041a3f52305a00f7f2f9597 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Sun, 8 Dec 2024 18:05:39 -0600 Subject: [PATCH 95/97] changes error messages for setDigitalXX functions --- .../ghost_v5_interfaces/src/robot_hardware_interface.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp b/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp index 11f21aec..8f021de5 100644 --- a/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp +++ b/01_Libraries/ghost_v5_interfaces/src/robot_hardware_interface.cpp @@ -489,7 +489,7 @@ bool RobotHardwareInterface::getDigitalDeviceValue(const std::string & device_na void RobotHardwareInterface::setDigitalInputValue(const std::string & device_name, bool value){ if (hardware_type_ != V5_BRAIN) { - throw std::runtime_error("[RobotHardwareInterface::setDigitalInputValue] Error: Attempted to set Digital Sensor value from Coprocessor."); + throw std::runtime_error("[RobotHardwareInterface::setDigitalInputValue] Error: Attempted to set Digital Input value from outside of V5 Brain."); } std::unique_lock update_lock(update_mutex_); @@ -501,7 +501,7 @@ void RobotHardwareInterface::setDigitalInputValue(const std::string & device_nam void RobotHardwareInterface::setDigitalOutputValue(const std::string & device_name, bool value){ if (hardware_type_ != COPROCESSOR) { - throw std::runtime_error("[RobotHardwareInterface::setDigitalOutputValue] Error: Attempted to set Digital Actuator value from V5 Brain."); + throw std::runtime_error("[RobotHardwareInterface::setDigitalOutputValue] Error: Attempted to set Digital Output value from outside of Coprocessor."); } std::unique_lock update_lock(update_mutex_); From ab1db6a35561decedf5d03f7dbfcc8fb1c9fc3b0 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Sun, 8 Dec 2024 19:03:15 -0600 Subject: [PATCH 96/97] changes whitespace --- .../src/util/load_digital_device_config_yaml.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/src/util/load_digital_device_config_yaml.cpp b/01_Libraries/ghost_v5_interfaces/src/util/load_digital_device_config_yaml.cpp index 879e0708..c3bc1e9c 100644 --- a/01_Libraries/ghost_v5_interfaces/src/util/load_digital_device_config_yaml.cpp +++ b/01_Libraries/ghost_v5_interfaces/src/util/load_digital_device_config_yaml.cpp @@ -54,13 +54,13 @@ void loadDigitalDeviceConfigFromYAML( device_name + "!"); } - if(type == "DIGITAL_INPUT"){ + if (type == "DIGITAL_INPUT") { device_config_ptr->type = devices::device_type_e::DIGITAL_INPUT; } - else if(type == "DIGITAL_OUTPUT"){ + else if (type == "DIGITAL_OUTPUT") { device_config_ptr->type = devices::device_type_e::DIGITAL_OUTPUT; } - else{ + else { throw std::runtime_error(std::string("[loadDigitalDeviceConfigFromYAML] Error: Unsupported digital device type ") + type + "!"); } From c9c06a0c44522a3efeb7c192ca1f29fec421fc95 Mon Sep 17 00:00:00 2001 From: xwilson03 Date: Sun, 8 Dec 2024 19:03:32 -0600 Subject: [PATCH 97/97] changes port range in loadDigital tests --- .../test/util/test_load_digital_device_config_yaml.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/01_Libraries/ghost_v5_interfaces/test/util/test_load_digital_device_config_yaml.cpp b/01_Libraries/ghost_v5_interfaces/test/util/test_load_digital_device_config_yaml.cpp index ebef28e5..e90c59dc 100644 --- a/01_Libraries/ghost_v5_interfaces/test/util/test_load_digital_device_config_yaml.cpp +++ b/01_Libraries/ghost_v5_interfaces/test/util/test_load_digital_device_config_yaml.cpp @@ -41,12 +41,12 @@ class TestLoadDigitalDeviceConfigYAML : public ::testing::Test // Expected motor configurations digital_input_config_ = std::make_shared(); - digital_input_config_->port = 22; + digital_input_config_->port = 0; digital_input_config_->name = "digital_in_1"; digital_input_config_->type = device_type_e::DIGITAL_INPUT; digital_output_config_ = std::make_shared(); - digital_output_config_->port = 23; + digital_output_config_->port = 1; digital_output_config_->name = "digital_out_1"; digital_output_config_->type = device_type_e::DIGITAL_OUTPUT;