mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-26 01:51:41 +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];
|
||||
};
|
||||
|
||||
@@ -1,6 +1,5 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
@@ -8,9 +7,7 @@
|
||||
#ifndef JOYSTICK_H_
|
||||
#define JOYSTICK_H_
|
||||
|
||||
#include <cstdint>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
#include "GenericHID.h"
|
||||
#include "ErrorBase.h"
|
||||
|
||||
@@ -18,101 +15,64 @@ class DriverStation;
|
||||
|
||||
/**
|
||||
* Handle input from standard Joysticks connected to the Driver Station.
|
||||
* This class handles standard input that comes from the Driver Station. Each
|
||||
* time a value is requested
|
||||
* the most recent value is returned. There is a single class instance for each
|
||||
* joystick and the mapping
|
||||
* This class handles standard input that comes from the Driver Station. Each time a value is requested
|
||||
* the most recent value is returned. There is a single class instance for each joystick and the mapping
|
||||
* of ports to hardware buttons depends on the code in the driver station.
|
||||
*/
|
||||
class Joystick : public GenericHID, public ErrorBase {
|
||||
public:
|
||||
static const uint32_t kDefaultXAxis = 0;
|
||||
static const uint32_t kDefaultYAxis = 1;
|
||||
static const uint32_t kDefaultZAxis = 2;
|
||||
static const uint32_t kDefaultTwistAxis = 2;
|
||||
static const uint32_t kDefaultThrottleAxis = 3;
|
||||
typedef enum {
|
||||
kXAxis,
|
||||
kYAxis,
|
||||
kZAxis,
|
||||
kTwistAxis,
|
||||
kThrottleAxis,
|
||||
kNumAxisTypes
|
||||
} AxisType;
|
||||
static const uint32_t kDefaultTriggerButton = 1;
|
||||
static const uint32_t kDefaultTopButton = 2;
|
||||
typedef enum { kTriggerButton, kTopButton, kNumButtonTypes } ButtonType;
|
||||
typedef enum { kLeftRumble, kRightRumble } RumbleType;
|
||||
typedef enum {
|
||||
kUnknown = -1,
|
||||
kXInputUnknown = 0,
|
||||
kXInputGamepad = 1,
|
||||
kXInputWheel = 2,
|
||||
kXInputArcadeStick = 3,
|
||||
kXInputFlightStick = 4,
|
||||
kXInputDancePad = 5,
|
||||
kXInputGuitar = 6,
|
||||
kXInputGuitar2 = 7,
|
||||
kXInputDrumKit = 8,
|
||||
kXInputGuitar3 = 11,
|
||||
kXInputArcadePad = 19,
|
||||
kHIDJoystick = 20,
|
||||
kHIDGamepad = 21,
|
||||
kHIDDriving = 22,
|
||||
kHIDFlight = 23,
|
||||
kHID1stPerson = 24
|
||||
} HIDType;
|
||||
explicit Joystick(uint32_t port);
|
||||
Joystick(uint32_t port, uint32_t numAxisTypes, uint32_t numButtonTypes);
|
||||
virtual ~Joystick() = default;
|
||||
class Joystick : public GenericHID, public ErrorBase
|
||||
{
|
||||
public:
|
||||
static const uint32_t kDefaultXAxis = 1;
|
||||
static const uint32_t kDefaultYAxis = 2;
|
||||
static const uint32_t kDefaultZAxis = 3;
|
||||
static const uint32_t kDefaultTwistAxis = 4;
|
||||
static const uint32_t kDefaultThrottleAxis = 3;
|
||||
typedef enum
|
||||
{
|
||||
kXAxis, kYAxis, kZAxis, kTwistAxis, kThrottleAxis, kNumAxisTypes
|
||||
} AxisType;
|
||||
static const uint32_t kDefaultTriggerButton = 1;
|
||||
static const uint32_t kDefaultTopButton = 2;
|
||||
typedef enum
|
||||
{
|
||||
kTriggerButton, kTopButton, kNumButtonTypes
|
||||
} ButtonType;
|
||||
|
||||
Joystick(const Joystick&) = delete;
|
||||
Joystick& operator=(const Joystick&) = delete;
|
||||
explicit Joystick(uint32_t port);
|
||||
Joystick(uint32_t port, uint32_t numAxisTypes, uint32_t numButtonTypes);
|
||||
virtual ~Joystick() = default;
|
||||
|
||||
uint32_t GetAxisChannel(AxisType axis) const;
|
||||
void SetAxisChannel(AxisType axis, uint32_t channel);
|
||||
Joystick(const Joystick&) = delete;
|
||||
Joystick& operator=(const Joystick&) = delete;
|
||||
|
||||
virtual float GetX(JoystickHand hand = kRightHand) const override;
|
||||
virtual float GetY(JoystickHand hand = kRightHand) const override;
|
||||
virtual float GetZ() const override;
|
||||
virtual float GetTwist() const override;
|
||||
virtual float GetThrottle() const override;
|
||||
virtual float GetAxis(AxisType axis) const;
|
||||
float GetRawAxis(uint32_t axis) const override;
|
||||
uint32_t GetAxisChannel(AxisType axis);
|
||||
void SetAxisChannel(AxisType axis, uint32_t channel);
|
||||
|
||||
virtual bool GetTrigger(JoystickHand hand = kRightHand) const override;
|
||||
virtual bool GetTop(JoystickHand hand = kRightHand) const override;
|
||||
virtual bool GetBumper(JoystickHand hand = kRightHand) const override;
|
||||
virtual bool GetRawButton(uint32_t button) const override;
|
||||
virtual int GetPOV(uint32_t pov = 0) const override;
|
||||
bool GetButton(ButtonType button) const;
|
||||
static Joystick *GetStickForPort(uint32_t port);
|
||||
virtual float GetX(JoystickHand hand = kRightHand) const override;
|
||||
virtual float GetY(JoystickHand hand = kRightHand) const override;
|
||||
virtual float GetZ() const override;
|
||||
virtual float GetTwist() const override;
|
||||
virtual float GetThrottle() const override;
|
||||
virtual float GetAxis(AxisType axis) const;
|
||||
float GetRawAxis(uint32_t axis) const override;
|
||||
|
||||
virtual float GetMagnitude() const;
|
||||
virtual float GetDirectionRadians() const;
|
||||
virtual float GetDirectionDegrees() const;
|
||||
virtual bool GetTrigger(JoystickHand hand = kRightHand) const override;
|
||||
virtual bool GetTop(JoystickHand hand = kRightHand) const override;
|
||||
virtual bool GetBumper(JoystickHand hand = kRightHand) const override;
|
||||
virtual bool GetRawButton(uint32_t button) const override;
|
||||
virtual int GetPOV(uint32_t pov = 1) const override;
|
||||
bool GetButton(ButtonType button) const;
|
||||
static Joystick* GetStickForPort(uint32_t port);
|
||||
|
||||
bool GetIsXbox() const;
|
||||
Joystick::HIDType GetType() const;
|
||||
std::string GetName() const;
|
||||
int GetAxisType(uint8_t axis) const;
|
||||
virtual float GetMagnitude() const;
|
||||
virtual float GetDirectionRadians() const;
|
||||
virtual float GetDirectionDegrees() const;
|
||||
|
||||
int GetAxisCount() const;
|
||||
int GetButtonCount() const;
|
||||
int GetPOVCount() const;
|
||||
|
||||
void SetRumble(RumbleType type, float value);
|
||||
void SetOutput(uint8_t outputNumber, bool value);
|
||||
void SetOutputs(uint32_t value);
|
||||
|
||||
private:
|
||||
DriverStation &m_ds;
|
||||
uint32_t m_port;
|
||||
::std::vector<uint32_t> m_axes;
|
||||
::std::vector<uint32_t> m_buttons;
|
||||
uint32_t m_outputs = 0;
|
||||
uint16_t m_leftRumble = 0;
|
||||
uint16_t m_rightRumble = 0;
|
||||
private:
|
||||
DriverStation &m_ds;
|
||||
uint32_t m_port;
|
||||
std::unique_ptr<uint32_t[]> m_axes;
|
||||
std::unique_ptr<uint32_t[]> m_buttons;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
@@ -47,6 +47,7 @@ protected:
|
||||
RobotBase& operator=(const RobotBase&) = delete;
|
||||
|
||||
DriverStation &m_ds;
|
||||
transport::SubscriberPtr time_sub;
|
||||
|
||||
private:
|
||||
static RobotBase *m_instance;
|
||||
|
||||
@@ -39,10 +39,16 @@ public:
|
||||
transport::NodePtr main;
|
||||
private:
|
||||
MainNode() {
|
||||
gazebo::client::setup();
|
||||
main = transport::NodePtr(new transport::Node());
|
||||
main->Init("frc");
|
||||
gazebo::transport::run();
|
||||
bool success = gazebo::client::setup();
|
||||
|
||||
if (success){
|
||||
main = transport::NodePtr(new transport::Node());
|
||||
main->Init("frc");
|
||||
gazebo::transport::run();
|
||||
}
|
||||
else {
|
||||
std::cout << "An error has occured setting up gazebo_client!" << std::endl;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
Reference in New Issue
Block a user