mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-28 02:11:43 +00:00
Repaired simulation build on linux
Reverted to old driverstation and joystick code because we're not ready for windows drive station yet updated paths to reflect new wpilib organization fixed name of gazebo topic (if you want /gazebo/frc/time use ~/time) included network tables in wpilibJavaSim Added ds script, and improved frcsim script always start gazebo with verbose Change-Id: I3c54b7000019a5985079a88200896a8069e69b86
This commit is contained in:
Peter_Mitrano
@@ -1,112 +1,142 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) FIRST 2008. All Rights Reserved. */
|
||||
/* Copyright (c) FIRST 2008. 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 $(WIND_BASE)/WPILib. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
#pragma once
|
||||
|
||||
#include "simulation/gz_msgs/msgs.h"
|
||||
|
||||
#ifdef _WIN32
|
||||
// Ensure that Winsock2.h is included before Windows.h, which can get
|
||||
// pulled in by anybody (e.g., Boost).
|
||||
#include <Winsock2.h>
|
||||
#endif
|
||||
|
||||
#include <gazebo/transport/transport.hh>
|
||||
#include "SensorBase.h"
|
||||
#include "RobotState.h"
|
||||
#include "HAL/HAL.hpp"
|
||||
#include "HAL/cpp/Semaphore.hpp"
|
||||
#include "HAL/cpp/priority_mutex.h"
|
||||
#include "HAL/cpp/priority_condition_variable.h"
|
||||
#include <mutex>
|
||||
#include <condition_variable>
|
||||
|
||||
struct HALControlWord;
|
||||
struct HALCommonControlData;
|
||||
class AnalogInput;
|
||||
|
||||
using namespace gazebo;
|
||||
|
||||
/**
|
||||
* Provide access to the network communication data to / from the Driver
|
||||
* Station.
|
||||
* Provide access to the network communication data to / from the Driver Station.
|
||||
*/
|
||||
class DriverStation : public SensorBase, public RobotStateInterface {
|
||||
public:
|
||||
enum Alliance { kRed, kBlue, kInvalid };
|
||||
class DriverStation : public SensorBase, public RobotStateInterface
|
||||
{
|
||||
public:
|
||||
enum Alliance
|
||||
{
|
||||
kRed,
|
||||
kBlue,
|
||||
kInvalid
|
||||
};
|
||||
|
||||
virtual ~DriverStation();
|
||||
static DriverStation &GetInstance();
|
||||
static void ReportError(std::string error);
|
||||
virtual ~DriverStation() = default;
|
||||
static DriverStation &GetInstance();
|
||||
static void ReportError(std::string error);
|
||||
|
||||
static const uint32_t kJoystickPorts = 6;
|
||||
static const uint32_t kBatteryChannel = 7;
|
||||
static const uint32_t kJoystickPorts = 4;
|
||||
static const uint32_t kJoystickAxes = 6;
|
||||
|
||||
float GetStickAxis(uint32_t stick, uint32_t axis);
|
||||
int GetStickPOV(uint32_t stick, uint32_t pov);
|
||||
uint32_t GetStickButtons(uint32_t stick) const;
|
||||
bool GetStickButton(uint32_t stick, uint8_t button);
|
||||
float GetStickAxis(uint32_t stick, uint32_t axis);
|
||||
bool GetStickButton(uint32_t stick, uint32_t button);
|
||||
short GetStickButtons(uint32_t stick);
|
||||
|
||||
int GetStickAxisCount(uint32_t stick) const;
|
||||
int GetStickPOVCount(uint32_t stick) const;
|
||||
int GetStickButtonCount(uint32_t stick) const;
|
||||
float GetAnalogIn(uint32_t channel);
|
||||
bool GetDigitalIn(uint32_t channel);
|
||||
void SetDigitalOut(uint32_t channel, bool value);
|
||||
bool GetDigitalOut(uint32_t channel);
|
||||
|
||||
bool GetJoystickIsXbox(uint32_t stick) const;
|
||||
int GetJoystickType(uint32_t stick) const;
|
||||
std::string GetJoystickName(uint32_t stick) const;
|
||||
int GetJoystickAxisType(uint32_t stick, uint8_t axis) const;
|
||||
bool IsEnabled() const;
|
||||
bool IsDisabled() const;
|
||||
bool IsAutonomous() const;
|
||||
bool IsOperatorControl() const;
|
||||
bool IsTest() const;
|
||||
bool IsFMSAttached() const;
|
||||
|
||||
bool IsEnabled() const override;
|
||||
bool IsDisabled() const override;
|
||||
bool IsAutonomous() const override;
|
||||
bool IsOperatorControl() const override;
|
||||
bool IsTest() const override;
|
||||
bool IsDSAttached() const;
|
||||
bool IsNewControlData() const;
|
||||
bool IsFMSAttached() const;
|
||||
bool IsSysActive() const;
|
||||
bool IsSysBrownedOut() const;
|
||||
uint32_t GetPacketNumber() const;
|
||||
Alliance GetAlliance() const;
|
||||
uint32_t GetLocation() const;
|
||||
void WaitForData();
|
||||
double GetMatchTime() const;
|
||||
float GetBatteryVoltage() const;
|
||||
uint16_t GetTeamNumber() const;
|
||||
|
||||
Alliance GetAlliance() const;
|
||||
uint32_t GetLocation() const;
|
||||
void WaitForData();
|
||||
double GetMatchTime() const;
|
||||
float GetBatteryVoltage() const;
|
||||
|
||||
/** Only to be used to tell the Driver Station what code you claim to be
|
||||
* executing
|
||||
* for diagnostic purposes only
|
||||
* @param entering If true, starting disabled code; if false, leaving disabled
|
||||
* code */
|
||||
void InDisabled(bool entering) { m_userInDisabled = entering; }
|
||||
/** Only to be used to tell the Driver Station what code you claim to be
|
||||
* executing
|
||||
* for diagnostic purposes only
|
||||
* @param entering If true, starting autonomous code; if false, leaving
|
||||
* autonomous code */
|
||||
void InAutonomous(bool entering) { m_userInAutonomous = entering; }
|
||||
/** Only to be used to tell the Driver Station what code you claim to be
|
||||
* executing
|
||||
* for diagnostic purposes only
|
||||
* @param entering If true, starting teleop code; if false, leaving teleop
|
||||
* code */
|
||||
void InOperatorControl(bool entering) { m_userInTeleop = entering; }
|
||||
/** Only to be used to tell the Driver Station what code you claim to be
|
||||
* executing
|
||||
* for diagnostic purposes only
|
||||
* @param entering If true, starting test code; if false, leaving test code */
|
||||
void InTest(bool entering) { m_userInTest = entering; }
|
||||
|
||||
protected:
|
||||
DriverStation();
|
||||
void IncrementUpdateNumber()
|
||||
{
|
||||
m_updateNumber++;
|
||||
}
|
||||
|
||||
void GetData();
|
||||
/** Only to be used to tell the Driver Station what code you claim to be executing
|
||||
* for diagnostic purposes only
|
||||
* @param entering If true, starting disabled code; if false, leaving disabled code */
|
||||
void InDisabled(bool entering)
|
||||
{
|
||||
m_userInDisabled = entering;
|
||||
}
|
||||
/** Only to be used to tell the Driver Station what code you claim to be executing
|
||||
* for diagnostic purposes only
|
||||
* @param entering If true, starting autonomous code; if false, leaving autonomous code */
|
||||
void InAutonomous(bool entering)
|
||||
{
|
||||
m_userInAutonomous = entering;
|
||||
}
|
||||
/** Only to be used to tell the Driver Station what code you claim to be executing
|
||||
* for diagnostic purposes only
|
||||
* @param entering If true, starting teleop code; if false, leaving teleop code */
|
||||
void InOperatorControl(bool entering)
|
||||
{
|
||||
m_userInTeleop = entering;
|
||||
}
|
||||
/** Only to be used to tell the Driver Station what code you claim to be executing
|
||||
* for diagnostic purposes only
|
||||
* @param entering If true, starting test code; if false, leaving test code */
|
||||
void InTest(bool entering)
|
||||
{
|
||||
m_userInTest = entering;
|
||||
}
|
||||
|
||||
private:
|
||||
static DriverStation *m_instance;
|
||||
void ReportJoystickUnpluggedError(std::string message);
|
||||
void Run();
|
||||
protected:
|
||||
DriverStation();
|
||||
|
||||
HALJoystickAxes m_joystickAxes[kJoystickPorts];
|
||||
HALJoystickPOVs m_joystickPOVs[kJoystickPorts];
|
||||
HALJoystickButtons m_joystickButtons[kJoystickPorts];
|
||||
HALJoystickDescriptor m_joystickDescriptor[kJoystickPorts];
|
||||
mutable Semaphore m_newControlData{Semaphore::kEmpty};
|
||||
mutable priority_condition_variable m_packetDataAvailableCond;
|
||||
priority_mutex m_packetDataAvailableMutex;
|
||||
std::condition_variable_any m_waitForDataCond;
|
||||
priority_mutex m_waitForDataMutex;
|
||||
bool m_userInDisabled = false;
|
||||
bool m_userInAutonomous = false;
|
||||
bool m_userInTeleop = false;
|
||||
bool m_userInTest = false;
|
||||
double m_nextMessageTime = 0;
|
||||
private:
|
||||
static void InitTask(DriverStation *ds);
|
||||
static DriverStation *m_instance;
|
||||
static uint8_t m_updateNumber;
|
||||
///< TODO: Get rid of this and use the semaphore signaling
|
||||
static const float kUpdatePeriod;
|
||||
|
||||
void stateCallback(const msgs::ConstDriverStationPtr &msg);
|
||||
void joystickCallback(const msgs::ConstFRCJoystickPtr &msg, int i);
|
||||
void joystickCallback0(const msgs::ConstFRCJoystickPtr &msg);
|
||||
void joystickCallback1(const msgs::ConstFRCJoystickPtr &msg);
|
||||
void joystickCallback2(const msgs::ConstFRCJoystickPtr &msg);
|
||||
void joystickCallback3(const msgs::ConstFRCJoystickPtr &msg);
|
||||
void joystickCallback4(const msgs::ConstFRCJoystickPtr &msg);
|
||||
void joystickCallback5(const msgs::ConstFRCJoystickPtr &msg);
|
||||
|
||||
uint8_t m_digitalOut = 0;
|
||||
std::condition_variable m_waitForDataCond;
|
||||
std::mutex m_waitForDataMutex;
|
||||
mutable std::recursive_mutex m_stateMutex;
|
||||
std::recursive_mutex m_joystickMutex;
|
||||
double m_approxMatchTimeOffset = 0;
|
||||
bool m_userInDisabled = false;
|
||||
bool m_userInAutonomous = false;
|
||||
bool m_userInTeleop = false;
|
||||
bool m_userInTest = false;
|
||||
|
||||
transport::SubscriberPtr stateSub;
|
||||
transport::SubscriberPtr joysticksSub[6];
|
||||
msgs::DriverStationPtr state;
|
||||
msgs::FRCJoystickPtr joysticks[6];
|
||||
};
|
||||
|
||||
Reference in New Issue
Block a user