diff --git a/settings.gradle b/settings.gradle index f12f105428..ac9f9c61d4 100644 --- a/settings.gradle +++ b/settings.gradle @@ -25,5 +25,6 @@ include 'simulation:gz_msgs' include 'simulation:frc_gazebo_plugins' include 'simulation:halsim_gazebo' include 'simulation:lowfi_simulation' +include 'simulation:halsim_ds_socket' include 'cameraserver' include 'myRobot' diff --git a/simulation/halsim_ds_socket/build.gradle b/simulation/halsim_ds_socket/build.gradle new file mode 100644 index 0000000000..49faf0eee9 --- /dev/null +++ b/simulation/halsim_ds_socket/build.gradle @@ -0,0 +1,8 @@ +description = "A plugin that listens on a socket so that you can use the real Driver Station software to connect to the simulation" + +ext { + includeWpiutil = true + pluginName = 'halsim_ds_socket' +} + +apply from: "${rootDir}/shared/plugins/setupBuild.gradle" diff --git a/simulation/halsim_ds_socket/src/main/native/cpp/DSCommPacket.cpp b/simulation/halsim_ds_socket/src/main/native/cpp/DSCommPacket.cpp new file mode 100644 index 0000000000..ac42363053 --- /dev/null +++ b/simulation/halsim_ds_socket/src/main/native/cpp/DSCommPacket.cpp @@ -0,0 +1,257 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "DSCommPacket.h" + +#include +#include +#include +#include +#include +#include + +#include +#include + +/*---------------------------------------------------------------------------- +** The following methods help parse and hold information about the +** driver station and it's joysticks. +**--------------------------------------------------------------------------*/ +void DSCommPacket::SetIndex(uint8_t hi, uint8_t lo) { + m_hi = hi; + m_lo = lo; +} + +void DSCommPacket::GetIndex(uint8_t& hi, uint8_t& lo) { + hi = m_hi; + lo = m_lo; +} + +void DSCommPacket::SetControl(uint8_t control, uint8_t request) { + std::memset(&m_control_word, 0, sizeof(m_control_word)); + m_control_word.enabled = (control & kEnabled) != 0; + m_control_word.autonomous = (control & kAutonomous) != 0; + m_control_word.test = (control & kTest) != 0; + m_control_word.eStop = (control & kEmergencyStop) != 0; + m_control_word.fmsAttached = (control & kFMS_Attached) != 0; + m_control_word.dsAttached = (request & kRequestNormalMask) != 0; + + m_control_sent = control; +} + +void DSCommPacket::GetControl(uint8_t& control) { control = m_control_sent; } + +void DSCommPacket::GetStatus(uint8_t& status) { status = kRobotHasCode; } + +void DSCommPacket::SetAlliance(uint8_t station_code) { + m_alliance_station = static_cast(station_code); +} + +int DSCommPacket::AddDSCommJoystickPacket(uint8_t* data, int len) { + DSCommJoystickPacket stick; + if (len > 0) { + int axis_count = *data++; + len--; + if (axis_count > len) return -1; + len -= axis_count; + for (; axis_count > 0; axis_count--) { + stick.axes.push_back(*data++); + } + } + + if (len > 2) { + stick.button_count = *data++; + stick.buttons = (*data++) << 8; + stick.buttons |= *data++; + len -= 3; + } + + if (len > 0) { + int pov_count = *data++; + len--; + if (pov_count * 2 > len) return -1; + len -= pov_count * 2; + for (; pov_count > 0; pov_count--) { + stick.povs.push_back((data[0] << 8) | data[1]); + data += 2; + } + } + + m_joystick_packets.push_back(stick); + + return len; +} + +void DSCommPacket::GetControlWord(struct ControlWord_t& control_word) { + control_word = m_control_word; +} + +void DSCommPacket::GetAllianceStation( + enum AllianceStationID_t& alliance_station) { + alliance_station = m_alliance_station; +} + +/*---------------------------------------------------------------------------- +** Communication methods +**--------------------------------------------------------------------------*/ +int DSCommPacket::DecodeTCP(uint8_t* packet, int len) { + if (len < 2) return 0; + if (packet[0] == 0 && packet[1] == 0) return 2; + int packet_len = packet[1]; + if (packet_len + 2 > len) return 0; + int packet_type = static_cast(packet[2]); + + Lock(); + if (packet_type == kGameDataType) { + std::copy( + packet + 3, + packet + 3 + + std::min(static_cast(sizeof(m_game_data)), packet_len - 1), + m_game_data); + } else if (packet_type == kJoystickNameType && len >= 7) { + int joystick = static_cast(packet[3]); + if (joystick < kMaxJoysticks) { + m_joystick_types[joystick] = static_cast(packet[5]); + int namelen = static_cast(packet[6]); + m_joystick_names[joystick] = + std::string(reinterpret_cast(packet + 7), namelen); + } + } else { + std::cerr << "TCP packet type " << packet_type << " unimplemented" + << std::endl; + for (int i = 0; i < packet_len + 2; i++) + std::fprintf(stderr, "%02x ", packet[i]); + std::fprintf(stderr, "\n"); + } + Unlock(); + return packet_len + 2; +} + +void DSCommPacket::DecodeUDP(uint8_t* packet, int len) { + if (len < 3) return; + + Lock(); + m_joystick_packets.clear(); + SetIndex(packet[0], packet[1]); + if (packet[2] != 0) { + if (len >= 6) { + SetControl(packet[3], packet[4]); + SetAlliance(packet[5]); + packet += 6; + len -= 6; + while (len > 0) { + int packet_len = *packet++; + if (packet_len > len) break; + if (*packet == kTagDsCommJoystick) { + if (AddDSCommJoystickPacket(packet + 1, packet_len - 1) < 0) break; + } + packet += packet_len; + len -= packet_len; + } + } + } + m_udp_packets++; + Unlock(); +} + +void DSCommPacket::SendJoysticks(void) { + unsigned int i; + + for (i = 0; i < kMaxJoysticks; i++) { + struct HAL_JoystickAxes axes; + struct HAL_JoystickPOVs povs; + struct HAL_JoystickButtons buttons; + struct HAL_JoystickDescriptor descriptor; + int j; + + std::memset(&axes, 0, sizeof(axes)); + std::memset(&povs, 0, sizeof(povs)); + std::memset(&buttons, 0, sizeof(buttons)); + std::memset(&descriptor, 0, sizeof(descriptor)); + + if (i < m_joystick_packets.size()) { + axes.count = std::min(static_cast(m_joystick_packets[i].axes.size()), + HAL_kMaxJoystickAxes); + for (j = 0; j < axes.count; j++) { + int8_t value = m_joystick_packets[i].axes[j]; + if (value < 0) { + axes.axes[j] = value / 128.0; + } else { + axes.axes[j] = value / 127.0; + } + } + + povs.count = std::min(static_cast(m_joystick_packets[i].povs.size()), + HAL_kMaxJoystickPOVs); + for (j = 0; j < povs.count; j++) + povs.povs[j] = m_joystick_packets[i].povs[j]; + + buttons.count = m_joystick_packets[i].button_count; + buttons.buttons = m_joystick_packets[i].buttons; + + descriptor.axisCount = axes.count; + descriptor.povCount = povs.count; + descriptor.buttonCount = buttons.count; + } + descriptor.type = m_joystick_types[i]; + m_joystick_names[i].copy(descriptor.name, sizeof(descriptor.name) - 1, 0); + + HALSIM_SetJoystickAxes(i, &axes); + HALSIM_SetJoystickPOVs(i, &povs); + HALSIM_SetJoystickButtons(i, &buttons); + HALSIM_SetJoystickDescriptor(i, &descriptor); + + /* TODO(jwhite@codeweavers.com): If we want to support rumble, etc, + implement SetJoyStickOutputs, although that would be a callback */ + } +} + +void DSCommPacket::SendTCPToHALSim(void) { + struct HAL_MatchInfo info; + Lock(); + std::strncpy(info.eventName, "Simulation", sizeof(info.eventName)); + info.matchType = HAL_MatchType::HAL_kMatchType_none; + info.matchNumber = 1; + info.replayNumber = 0; + std::copy(info.gameSpecificMessage, + info.gameSpecificMessage + + std::min(sizeof(info.gameSpecificMessage), sizeof(m_game_data)), + m_game_data); + HALSIM_SetMatchInfo(&info); + Unlock(); +} + +void DSCommPacket::SendUDPToHALSim(void) { + struct ControlWord_t control_word; + AllianceStationID_t alliance_station; + + Lock(); + GetControlWord(control_word); + GetAllianceStation(alliance_station); + auto now = std::chrono::high_resolution_clock::now(); + if (m_udp_packets == 1) { + m_match_time = 0.0; + } else if (control_word.enabled) { + std::chrono::duration delta = (now - m_packet_time); + m_match_time += delta.count(); + } + m_packet_time = now; + SendJoysticks(); + Unlock(); + + HALSIM_SetDriverStationMatchTime(m_match_time); + HALSIM_SetDriverStationEnabled(control_word.enabled); + HALSIM_SetDriverStationAutonomous(control_word.autonomous); + HALSIM_SetDriverStationTest(control_word.test); + HALSIM_SetDriverStationEStop(control_word.eStop); + HALSIM_SetDriverStationFmsAttached(control_word.fmsAttached); + HALSIM_SetDriverStationDsAttached(control_word.dsAttached); + HALSIM_SetDriverStationAllianceStationId( + static_cast(alliance_station)); + + HALSIM_NotifyDriverStationNewData(); +} diff --git a/simulation/halsim_ds_socket/src/main/native/cpp/main.cpp b/simulation/halsim_ds_socket/src/main/native/cpp/main.cpp new file mode 100644 index 0000000000..e5115383cd --- /dev/null +++ b/simulation/halsim_ds_socket/src/main/native/cpp/main.cpp @@ -0,0 +1,256 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +/*---------------------------------------------------------------------------- +** This extension reimplements enough of the FRC_Network layer to enable the +** simulator to communicate with a driver station. That includes a +** simple udp layer for communication. +** The protocol does not appear to be well documented; this implementation +** is based in part on the Toast ds_comms.cpp by Jaci and in part +** by the protocol specification given by QDriverStation. +**--------------------------------------------------------------------------*/ + +#include + +#include +#include + +#if defined(Win32) || defined(_WIN32) +#include +#pragma comment(lib, "Ws2_32.lib") +#else +#include +#include +#include +#endif + +#include + +/*---------------------------------------------------------------------------- +** Open a socket and listen for connections +** Returns socket handle on success, -1 on error +**--------------------------------------------------------------------------*/ +static int OpenListenSocket(int port, bool tcp) { +#if defined(WIN32) || defined(_WIN32) + SOCKET s; +#else + int s; +#endif + + if (tcp) + s = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP); + else + s = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); + + if (!s) { + std::perror("socket"); + return -1; + } + + int reuse = 1; +#if defined(WIN32) || defined(_WIN32) + setsockopt(s, SOL_SOCKET, SO_REUSEADDR, reinterpret_cast(&reuse), + sizeof(reuse)); +#else + setsockopt(s, SOL_SOCKET, SO_REUSEADDR, static_cast(&reuse), + sizeof(reuse)); +#endif + + struct sockaddr_in addr; + std::memset(&addr, 0, sizeof(addr)); + addr.sin_family = AF_INET; + addr.sin_port = htons(port); + addr.sin_addr.s_addr = INADDR_ANY; + + if (bind(s, (struct sockaddr*)&addr, sizeof(addr))) { + std::perror("bind"); +#if defined(WIN32) || defined(_WIN32) + closesocket(s); +#else + close(s); +#endif + return -1; + } + + if (tcp) { + if (listen(s, 10)) { +#if defined(WIN32) || defined(_WIN32) + closesocket(s); +#else + close(s); +#endif + return -1; + } + } + + return s; +} + +/*---------------------------------------------------------------------------- +** TCP thread +** This thread only dies at program shut down; it opens a socket +** and listens for incoming connections, and then processes data +** sent on the socket. +**--------------------------------------------------------------------------*/ +static void TCPThreadFunc(DSCommPacket* ds) { + static const int kTCPPort = 1740; + while (true) { + int socket = OpenListenSocket(kTCPPort, true); + if (socket < 0) { +#if defined(WIN32) || defined(_WIN32) + Sleep(1000); +#else + sleep(1); +#endif + continue; + } + + while (true) { + int client = accept(socket, NULL, 0); + if (client < 0) { +#if defined(WIN32) || defined(_WIN32) + closesocket(socket); +#else + close(socket); +#endif + break; + } + + uint8_t buf[8192]; + int len = 0; + do { + int rc = recv(client, reinterpret_cast(buf + len), + sizeof(buf) - len, 0); + if (rc <= 0) break; + len += rc; + + do { + int deduct = ds->DecodeTCP(buf, len); + if (deduct <= 0) break; + std::memmove(buf, buf + deduct, len - deduct); + len -= deduct; + if (deduct > 2) ds->SendTCPToHALSim(); + } while (true); + } while (true); + +#if defined(WIN32) || defined(_WIN32) + closesocket(client); + Sleep(1000); +#else + close(client); + sleep(1); +#endif + } + } +} + +/*---------------------------------------------------------------------------- +** Send a reply packet back to the DS +**--------------------------------------------------------------------------*/ +static void SendReplyPacket(int socket, struct sockaddr* addr, int addrlen, + int reply_port, DSCommPacket* ds) { + static const uint8_t kTagGeneral = 0x01; + struct sockaddr_in* in4 = reinterpret_cast(addr); + in4->sin_port = htons(reply_port); + + uint8_t data[8]; + std::memset(data, 0, sizeof(data)); + + ds->GetIndex(data[0], data[1]); + + data[2] = kTagGeneral; + ds->GetControl(data[3]); + ds->GetStatus(data[4]); + + data[5] = 12; // Voltage upper + data[6] = 0; // Voltage lower + data[7] = 0; // Request + +#if defined(WIN32) || defined(_WIN32) + sendto(socket, reinterpret_cast(data), sizeof(data), 0, addr, + addrlen); +#else + sendto(socket, data, sizeof(data), 0, addr, addrlen); +#endif +} + +/*---------------------------------------------------------------------------- +** UDP thread +** This thread only dies at program shut down; it opens a socket +** and listens for incoming connections, and then processes data +** sent on the socket. +**--------------------------------------------------------------------------*/ +static void UDPThreadFunc(DSCommPacket* ds) { + static const int kUDPListenPort = 1110; + static const int kUDPReplyPort = 1150; + while (true) { + int socket = OpenListenSocket(kUDPListenPort, false); + if (socket < 0) { +#if defined(WIN32) || defined(_WIN32) + Sleep(1000); +#else + sleep(1); +#endif + continue; + } + + do { + uint8_t buf[1024]; + struct sockaddr addr; +#if defined(Win32) || defined(_WIN32) + int addrlen = sizeof(addr); + int rc = recvfrom(socket, reinterpret_cast(buf), sizeof(buf), 0, + &addr, &addrlen); +#else + socklen_t addrlen = sizeof(addr); + ssize_t rc = recvfrom(socket, buf, sizeof(buf), 0, &addr, &addrlen); +#endif + + if (rc > 0) { + ds->DecodeUDP(buf, rc); + SendReplyPacket(socket, &addr, addrlen, kUDPReplyPort, ds); + ds->SendUDPToHALSim(); + } else { + break; + } + } while (true); + } +} + +/*---------------------------------------------------------------------------- +** Main entry point. We will start listen threads going, processing +** against our driver station packet +**--------------------------------------------------------------------------*/ +extern "C" { +#if defined(WIN32) || defined(_WIN32) +__declspec(dllexport) +#endif + int HALSIM_InitExtension(void) { + static DSCommPacket ds; + static std::thread udp_thread; + static std::thread tcp_thread; + static bool once = false; + + if (once) { + std::cerr << "Error: cannot invoke HALSIM_InitExtension twice." + << std::endl; + return -1; + } + once = true; + + std::cout << "DriverStationSocket Initializing." << std::endl; + + udp_thread = std::thread(UDPThreadFunc, &ds); + udp_thread.detach(); + + tcp_thread = std::thread(TCPThreadFunc, &ds); + tcp_thread.detach(); + + std::cout << "DriverStationSocket Initialized!" << std::endl; + return 0; +} +} // extern "C" diff --git a/simulation/halsim_ds_socket/src/main/native/include/DSCommJoystickPacket.h b/simulation/halsim_ds_socket/src/main/native/include/DSCommJoystickPacket.h new file mode 100644 index 0000000000..fd8aafd42f --- /dev/null +++ b/simulation/halsim_ds_socket/src/main/native/include/DSCommJoystickPacket.h @@ -0,0 +1,16 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once +#include + +typedef struct { + std::vector axes; + uint8_t button_count; + uint32_t buttons; + std::vector povs; +} DSCommJoystickPacket; diff --git a/simulation/halsim_ds_socket/src/main/native/include/DSCommPacket.h b/simulation/halsim_ds_socket/src/main/native/include/DSCommPacket.h new file mode 100644 index 0000000000..be3bf83320 --- /dev/null +++ b/simulation/halsim_ds_socket/src/main/native/include/DSCommPacket.h @@ -0,0 +1,79 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include + +class DSCommPacket { + public: + DSCommPacket(void) { std::fill_n(m_joystick_types, kMaxJoysticks, -1); } + void Lock() { m_mutex.lock(); } + void Unlock() { m_mutex.unlock(); } + void SetIndex(uint8_t hi, uint8_t lo); + void GetIndex(uint8_t& hi, uint8_t& lo); + void SetControl(uint8_t control, uint8_t request); + void GetControl(uint8_t& control); + void GetStatus(uint8_t& status); + void SetAlliance(uint8_t station_code); + int AddDSCommJoystickPacket(uint8_t* data, int len); + void GetControlWord(struct ControlWord_t& control_word); + void GetAllianceStation(enum AllianceStationID_t& allianceStation); + int DecodeTCP(uint8_t* packet, int len); + void DecodeUDP(uint8_t* packet, int len); + void SendTCPToHALSim(void); + void SendUDPToHALSim(void); + void SendJoysticks(void); + + /* TCP (FMS) types */ + static const uint8_t kGameDataType = 0x0e; + static const uint8_t kJoystickNameType = 0x02; + + /* Control word bits */ + static const uint8_t kTest = 0x01; + static const uint8_t kEnabled = 0x04; + static const uint8_t kAutonomous = 0x02; + static const uint8_t kFMS_Attached = 0x08; + static const uint8_t kEmergencyStop = 0x80; + + /* Control request bitmask */ + static const uint8_t kRequestNormalMask = 0xF0; + + /* Status bits */ + static const uint8_t kRobotHasCode = 0x20; + + /* Joystick tag bits */ + static const uint8_t kTagDsCommJoystick = 0x0c; + + /* Joystick max count */ + /* TODO(jwhite@codeweavers.com) This is a magic number in the HAL; fix it + * there */ + static const uint8_t kMaxJoysticks = 6; + + private: + uint8_t m_game_data[64]; + uint8_t m_hi; + uint8_t m_lo; + uint8_t m_control_sent; + struct ControlWord_t m_control_word; + enum AllianceStationID_t m_alliance_station; + std::vector m_joystick_packets; + std::string m_joystick_names[kMaxJoysticks]; + int m_joystick_types[kMaxJoysticks]; + std::mutex m_mutex; + int m_udp_packets; + std::chrono::high_resolution_clock::time_point m_packet_time; + double m_match_time; +}; diff --git a/simulation/halsim_ds_socket/src/main/native/include/FRCComm.h b/simulation/halsim_ds_socket/src/main/native/include/FRCComm.h new file mode 100644 index 0000000000..83737e090b --- /dev/null +++ b/simulation/halsim_ds_socket/src/main/native/include/FRCComm.h @@ -0,0 +1,74 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +/*---------------------------------------------------------------------------- + * The defines and enums in this file were copied from: + * ni-libraries/include/FRC_NetworkCommunication/FRCComm.h + * to avoid the complexities of trying to get gradle to + * reliably build against it. + *----------------------------------------------------------------------------*/ + +#ifndef WPILIB_SIMULATION_HALSIM_DS_SOCKET_SRC_MAIN_NATIVE_INCLUDE_FRCCOMM_H_ +#define WPILIB_SIMULATION_HALSIM_DS_SOCKET_SRC_MAIN_NATIVE_INCLUDE_FRCCOMM_H_ + +#ifdef _WIN32 +#include +#elif defined(__vxworks) +#include +#elif defined(__linux) +#include +#endif + +#define ERR_FRCSystem_NetCommNotResponding -44049 +#define ERR_FRCSystem_NoDSConnection -44018 + +enum AllianceStationID_t { + kAllianceStationID_red1, + kAllianceStationID_red2, + kAllianceStationID_red3, + kAllianceStationID_blue1, + kAllianceStationID_blue2, + kAllianceStationID_blue3, +}; + +enum MatchType_t { + kMatchType_none, + kMatchType_practice, + kMatchType_qualification, + kMatchType_elimination, +}; + +struct ControlWord_t { +#ifndef __vxworks + uint32_t enabled : 1; + uint32_t autonomous : 1; + uint32_t test : 1; + uint32_t eStop : 1; + uint32_t fmsAttached : 1; + uint32_t dsAttached : 1; + uint32_t control_reserved : 26; +#else + uint32_t control_reserved : 26; + uint32_t dsAttached : 1; + uint32_t fmsAttached : 1; + uint32_t eStop : 1; + uint32_t test : 1; + uint32_t autonomous : 1; + uint32_t enabled : 1; +#endif +}; + +struct JoystickAxes_t { + uint16_t count; + int16_t axes[1]; +}; + +struct JoystickPOV_t { + uint16_t count; + int16_t povs[1]; +}; +#endif // WPILIB_SIMULATION_HALSIM_DS_SOCKET_SRC_MAIN_NATIVE_INCLUDE_FRCCOMM_H_