Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

Add halsim_ds_socket to allow a simulated robot to talk to the real DS (#1180)

Browse Source 
This implements enough of the UDP and TCP protocol used by the FRC
driver station to allow us to talk to either QDriverStation or to the
real Driver Station.

This was inspired by a similar function in Toast by Jaci, and also
uses a lot of the research found in the QDriverStation project.
This commit is contained in:
Jeremy White
2018-07-22 17:00:06 -05:00
committed by Peter Johnson
parent 5bf5821138
commit 74a306d47a
7 changed files with 691 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

257
simulation/halsim_ds_socket/src/main/native/cpp/DSCommPacket.cpp Normal file
View File

@@ -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 <algorithm>
#include <chrono>
#include <cstring>
#include <iostream>
#include <thread>
#include <vector>
#include <FRCComm.h>
#include <MockData/DriverStationData.h>
/*----------------------------------------------------------------------------
** 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<enum AllianceStationID_t>(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<int>(packet[2]);
Lock();
if (packet_type == kGameDataType) {
std::copy(
packet + 3,
packet + 3 +
std::min(static_cast<int>(sizeof(m_game_data)), packet_len - 1),
m_game_data);
} else if (packet_type == kJoystickNameType && len >= 7) {
int joystick = static_cast<int>(packet[3]);
if (joystick < kMaxJoysticks) {
m_joystick_types[joystick] = static_cast<int>(packet[5]);
int namelen = static_cast<int>(packet[6]);
m_joystick_names[joystick] =
std::string(reinterpret_cast<char*>(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<int>(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<int>(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<double> 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<HAL_AllianceStationID>(alliance_station));
HALSIM_NotifyDriverStationNewData();
}

256
simulation/halsim_ds_socket/src/main/native/cpp/main.cpp Normal file
View File

@@ -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 <sys/types.h>
#include <cstring>
#include <iostream>
#if defined(Win32) || defined(_WIN32)
#include <windows.h>
#pragma comment(lib, "Ws2_32.lib")
#else
#include <arpa/inet.h>
#include <sys/socket.h>
#include <unistd.h>
#endif
#include <DSCommPacket.h>
/*----------------------------------------------------------------------------
** 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<const char*>(&reuse),
sizeof(reuse));
#else
setsockopt(s, SOL_SOCKET, SO_REUSEADDR, static_cast<void*>(&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<char*>(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<struct sockaddr_in*>(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<const char*>(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<char*>(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"