mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-26 01:51:41 +00:00
"using" directives are no longer used in global namespaces (#219)
This commit is contained in:
committed by
Peter Johnson
parent
78f0b1562c
commit
ba8761e39e
@@ -40,6 +40,6 @@
|
||||
#include "FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tSysWatchdog.h"
|
||||
|
||||
// FIXME: these should not be here!
|
||||
using namespace nFPGA;
|
||||
using namespace nRoboRIO_FPGANamespace;
|
||||
using namespace nFPGA; // NOLINT
|
||||
using namespace nRoboRIO_FPGANamespace; // NOLINT
|
||||
#pragma GCC diagnostic pop
|
||||
|
||||
@@ -11,7 +11,7 @@
|
||||
|
||||
GZ_REGISTER_MODEL_PLUGIN(Clock)
|
||||
|
||||
void Clock::Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
|
||||
void Clock::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) {
|
||||
this->model = model;
|
||||
|
||||
// Parse SDF properties
|
||||
@@ -27,18 +27,18 @@ void Clock::Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
|
||||
std::string scoped_name =
|
||||
model->GetWorld()->GetName() + "::" + model->GetScopedName();
|
||||
boost::replace_all(scoped_name, "::", "/");
|
||||
node = transport::NodePtr(new transport::Node());
|
||||
node = gazebo::transport::NodePtr(new gazebo::transport::Node());
|
||||
node->Init(scoped_name);
|
||||
pub = node->Advertise<msgs::Float64>(topic);
|
||||
pub = node->Advertise<gazebo::msgs::Float64>(topic);
|
||||
|
||||
// Connect to the world update event.
|
||||
// This will trigger the Update function every Gazebo iteration
|
||||
updateConn = event::Events::ConnectWorldUpdateBegin(
|
||||
updateConn = gazebo::event::Events::ConnectWorldUpdateBegin(
|
||||
boost::bind(&Clock::Update, this, _1));
|
||||
}
|
||||
|
||||
void Clock::Update(const common::UpdateInfo& info) {
|
||||
msgs::Float64 msg;
|
||||
void Clock::Update(const gazebo::common::UpdateInfo& info) {
|
||||
gazebo::msgs::Float64 msg;
|
||||
msg.set_data(info.simTime.Double());
|
||||
pub->Publish(msg);
|
||||
}
|
||||
|
||||
@@ -15,8 +15,6 @@
|
||||
|
||||
#include "simulation/gz_msgs/msgs.h"
|
||||
|
||||
using namespace gazebo;
|
||||
|
||||
/**
|
||||
* \brief Plugin for publishing the simulation time.
|
||||
*
|
||||
@@ -34,19 +32,27 @@ using namespace gazebo;
|
||||
*
|
||||
* \todo Make WorldPlugin?
|
||||
*/
|
||||
class Clock : public ModelPlugin {
|
||||
class Clock : public gazebo::ModelPlugin {
|
||||
public:
|
||||
/// \brief Load the clock and configures it according to the sdf.
|
||||
void Load(physics::ModelPtr model, sdf::ElementPtr sdf);
|
||||
void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf);
|
||||
|
||||
/// \brief Sends out time each timestep.
|
||||
void Update(const common::UpdateInfo& info);
|
||||
void Update(const gazebo::common::UpdateInfo& info);
|
||||
|
||||
private:
|
||||
std::string topic; ///< \brief Publish the time on this topic.
|
||||
physics::ModelPtr model; ///< \brief The model that this is attached to.
|
||||
event::ConnectionPtr
|
||||
updateConn; ///< \brief Pointer to the world update function.
|
||||
transport::NodePtr node; ///< \brief The node we're advertising on.
|
||||
transport::PublisherPtr pub; ///< \brief Publisher handle.
|
||||
/// \brief Publish the time on this topic.
|
||||
std::string topic;
|
||||
|
||||
/// \brief The model to which this is attached.
|
||||
gazebo::physics::ModelPtr model;
|
||||
|
||||
/// \brief Pointer to the world update function.
|
||||
gazebo::event::ConnectionPtr updateConn;
|
||||
|
||||
/// \brief The node on which we're advertising.
|
||||
gazebo::transport::NodePtr node;
|
||||
|
||||
/// \brief Publisher handle.
|
||||
gazebo::transport::PublisherPtr pub;
|
||||
};
|
||||
|
||||
@@ -11,7 +11,7 @@
|
||||
|
||||
GZ_REGISTER_MODEL_PLUGIN(DCMotor)
|
||||
|
||||
void DCMotor::Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
|
||||
void DCMotor::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) {
|
||||
this->model = model;
|
||||
signal = 0;
|
||||
|
||||
@@ -36,21 +36,21 @@ void DCMotor::Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
|
||||
std::string scoped_name =
|
||||
model->GetWorld()->GetName() + "::" + model->GetScopedName();
|
||||
boost::replace_all(scoped_name, "::", "/");
|
||||
node = transport::NodePtr(new transport::Node());
|
||||
node = gazebo::transport::NodePtr(new gazebo::transport::Node());
|
||||
node->Init(scoped_name);
|
||||
sub = node->Subscribe(topic, &DCMotor::Callback, this);
|
||||
|
||||
// Connect to the world update event.
|
||||
// This will trigger the Update function every Gazebo iteration
|
||||
updateConn = event::Events::ConnectWorldUpdateBegin(
|
||||
updateConn = gazebo::event::Events::ConnectWorldUpdateBegin(
|
||||
boost::bind(&DCMotor::Update, this, _1));
|
||||
}
|
||||
|
||||
void DCMotor::Update(const common::UpdateInfo& info) {
|
||||
void DCMotor::Update(const gazebo::common::UpdateInfo& info) {
|
||||
joint->SetForce(0, signal * multiplier);
|
||||
}
|
||||
|
||||
void DCMotor::Callback(const msgs::ConstFloat64Ptr& msg) {
|
||||
void DCMotor::Callback(const gazebo::msgs::ConstFloat64Ptr& msg) {
|
||||
signal = msg->data();
|
||||
if (signal < -1) {
|
||||
signal = -1;
|
||||
|
||||
@@ -15,8 +15,6 @@
|
||||
|
||||
#include "simulation/gz_msgs/msgs.h"
|
||||
|
||||
using namespace gazebo;
|
||||
|
||||
/**
|
||||
* \brief Plugin for controlling a joint with a DC motor.
|
||||
*
|
||||
@@ -37,13 +35,13 @@ using namespace gazebo;
|
||||
* - `topic`: Optional. Message type should be gazebo.msgs.Float64.
|
||||
* - `multiplier`: Optional. Defaults to 1.
|
||||
*/
|
||||
class DCMotor : public ModelPlugin {
|
||||
class DCMotor : public gazebo::ModelPlugin {
|
||||
public:
|
||||
/// \brief Load the dc motor and configures it according to the sdf.
|
||||
void Load(physics::ModelPtr model, sdf::ElementPtr sdf);
|
||||
void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf);
|
||||
|
||||
/// \brief Update the torque on the joint from the dc motor each timestep.
|
||||
void Update(const common::UpdateInfo& info);
|
||||
void Update(const gazebo::common::UpdateInfo& info);
|
||||
|
||||
private:
|
||||
/// \brief Topic to read control signal from.
|
||||
@@ -56,14 +54,20 @@ class DCMotor : public ModelPlugin {
|
||||
double multiplier;
|
||||
|
||||
/// \brief The joint that this dc motor drives.
|
||||
physics::JointPtr joint;
|
||||
gazebo::physics::JointPtr joint;
|
||||
|
||||
/// \brief Callback for receiving msgs and storing the signal.
|
||||
void Callback(const msgs::ConstFloat64Ptr& msg);
|
||||
void Callback(const gazebo::msgs::ConstFloat64Ptr& msg);
|
||||
|
||||
physics::ModelPtr model; ///< \brief The model that this is attached to.
|
||||
event::ConnectionPtr
|
||||
updateConn; ///< \brief Pointer to the world update function.
|
||||
transport::NodePtr node; ///< \brief The node we're advertising on.
|
||||
transport::SubscriberPtr sub; ///< \brief Subscriber handle.
|
||||
/// \brief The model to which this is attached.
|
||||
gazebo::physics::ModelPtr model;
|
||||
|
||||
/// \brief Pointer toe the world update function.
|
||||
gazebo::event::ConnectionPtr updateConn;
|
||||
|
||||
/// \brief The node on which we're advertising.
|
||||
gazebo::transport::NodePtr node;
|
||||
|
||||
/// \brief Subscriber handle.
|
||||
gazebo::transport::SubscriberPtr sub;
|
||||
};
|
||||
|
||||
@@ -11,7 +11,7 @@
|
||||
|
||||
GZ_REGISTER_MODEL_PLUGIN(Encoder)
|
||||
|
||||
void Encoder::Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
|
||||
void Encoder::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) {
|
||||
this->model = model;
|
||||
|
||||
// Parse SDF properties
|
||||
@@ -38,20 +38,20 @@ void Encoder::Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
|
||||
std::string scoped_name =
|
||||
model->GetWorld()->GetName() + "::" + model->GetScopedName();
|
||||
boost::replace_all(scoped_name, "::", "/");
|
||||
node = transport::NodePtr(new transport::Node());
|
||||
node = gazebo::transport::NodePtr(new gazebo::transport::Node());
|
||||
node->Init(scoped_name);
|
||||
command_sub = node->Subscribe(topic + "/control", &Encoder::Callback, this);
|
||||
pos_pub = node->Advertise<msgs::Float64>(topic + "/position");
|
||||
vel_pub = node->Advertise<msgs::Float64>(topic + "/velocity");
|
||||
pos_pub = node->Advertise<gazebo::msgs::Float64>(topic + "/position");
|
||||
vel_pub = node->Advertise<gazebo::msgs::Float64>(topic + "/velocity");
|
||||
|
||||
// Connect to the world update event.
|
||||
// This will trigger the Update function every Gazebo iteration
|
||||
updateConn = event::Events::ConnectWorldUpdateBegin(
|
||||
updateConn = gazebo::event::Events::ConnectWorldUpdateBegin(
|
||||
boost::bind(&Encoder::Update, this, _1));
|
||||
}
|
||||
|
||||
void Encoder::Update(const common::UpdateInfo& info) {
|
||||
msgs::Float64 pos_msg, vel_msg;
|
||||
void Encoder::Update(const gazebo::common::UpdateInfo& info) {
|
||||
gazebo::msgs::Float64 pos_msg, vel_msg;
|
||||
if (stopped) {
|
||||
pos_msg.set_data(stop_value);
|
||||
pos_pub->Publish(pos_msg);
|
||||
@@ -65,7 +65,7 @@ void Encoder::Update(const common::UpdateInfo& info) {
|
||||
}
|
||||
}
|
||||
|
||||
void Encoder::Callback(const msgs::ConstStringPtr& msg) {
|
||||
void Encoder::Callback(const gazebo::msgs::ConstStringPtr& msg) {
|
||||
std::string command = msg->data();
|
||||
if (command == "reset") {
|
||||
zero = GetAngle();
|
||||
|
||||
@@ -15,8 +15,6 @@
|
||||
|
||||
#include "simulation/gz_msgs/msgs.h"
|
||||
|
||||
using namespace gazebo;
|
||||
|
||||
/**
|
||||
* \brief Plugin for reading the speed and relative angle of a joint.
|
||||
*
|
||||
@@ -45,13 +43,13 @@ using namespace gazebo;
|
||||
* (gazebo.msgs.String)
|
||||
* - `units`: Optional. Defaults to radians.
|
||||
*/
|
||||
class Encoder : public ModelPlugin {
|
||||
class Encoder : public gazebo::ModelPlugin {
|
||||
public:
|
||||
/// \brief Load the encoder and configures it according to the sdf.
|
||||
void Load(physics::ModelPtr model, sdf::ElementPtr sdf);
|
||||
void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf);
|
||||
|
||||
/// \brief Sends out the encoder reading each timestep.
|
||||
void Update(const common::UpdateInfo& info);
|
||||
void Update(const gazebo::common::UpdateInfo& info);
|
||||
|
||||
private:
|
||||
/// \brief Root topic for subtopics on this topic.
|
||||
@@ -70,10 +68,10 @@ class Encoder : public ModelPlugin {
|
||||
double stop_value;
|
||||
|
||||
/// \brief The joint that this encoder measures
|
||||
physics::JointPtr joint;
|
||||
gazebo::physics::JointPtr joint;
|
||||
|
||||
/// \brief Callback for handling control data
|
||||
void Callback(const msgs::ConstStringPtr& msg);
|
||||
void Callback(const gazebo::msgs::ConstStringPtr& msg);
|
||||
|
||||
/// \brief Gets the current angle, taking into account whether to
|
||||
/// return radians or degrees.
|
||||
@@ -83,10 +81,18 @@ class Encoder : public ModelPlugin {
|
||||
/// return radians/second or degrees/second.
|
||||
double GetVelocity();
|
||||
|
||||
physics::ModelPtr model; ///< \brief The model that this is attached to.
|
||||
event::ConnectionPtr
|
||||
updateConn; ///< \brief Pointer to the world update function.
|
||||
transport::NodePtr node; ///< \brief The node we're advertising on.
|
||||
transport::SubscriberPtr command_sub; ///< \brief Subscriber handle.
|
||||
transport::PublisherPtr pos_pub, vel_pub; ///< \brief Publisher handles.
|
||||
/// \brief The model to which this is attached.
|
||||
gazebo::physics::ModelPtr model;
|
||||
|
||||
/// \brief Pointer to the world update function.
|
||||
gazebo::event::ConnectionPtr updateConn;
|
||||
|
||||
/// \brief The node on which we're advertising.
|
||||
gazebo::transport::NodePtr node;
|
||||
|
||||
/// \brief Subscriber handle.
|
||||
gazebo::transport::SubscriberPtr command_sub;
|
||||
|
||||
/// \brief Publisher handles.
|
||||
gazebo::transport::PublisherPtr pos_pub, vel_pub;
|
||||
};
|
||||
|
||||
@@ -11,7 +11,7 @@
|
||||
|
||||
GZ_REGISTER_MODEL_PLUGIN(Gyro)
|
||||
|
||||
void Gyro::Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
|
||||
void Gyro::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) {
|
||||
this->model = model;
|
||||
|
||||
// Parse SDF properties
|
||||
@@ -41,27 +41,27 @@ void Gyro::Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
|
||||
std::string scoped_name =
|
||||
model->GetWorld()->GetName() + "::" + model->GetScopedName();
|
||||
boost::replace_all(scoped_name, "::", "/");
|
||||
node = transport::NodePtr(new transport::Node());
|
||||
node = gazebo::transport::NodePtr(new gazebo::transport::Node());
|
||||
node->Init(scoped_name);
|
||||
command_sub = node->Subscribe(topic + "/control", &Gyro::Callback, this);
|
||||
pos_pub = node->Advertise<msgs::Float64>(topic + "/position");
|
||||
vel_pub = node->Advertise<msgs::Float64>(topic + "/velocity");
|
||||
pos_pub = node->Advertise<gazebo::msgs::Float64>(topic + "/position");
|
||||
vel_pub = node->Advertise<gazebo::msgs::Float64>(topic + "/velocity");
|
||||
|
||||
// Connect to the world update event.
|
||||
// This will trigger the Update function every Gazebo iteration
|
||||
updateConn = event::Events::ConnectWorldUpdateBegin(
|
||||
updateConn = gazebo::event::Events::ConnectWorldUpdateBegin(
|
||||
boost::bind(&Gyro::Update, this, _1));
|
||||
}
|
||||
|
||||
void Gyro::Update(const common::UpdateInfo& info) {
|
||||
msgs::Float64 pos_msg, vel_msg;
|
||||
void Gyro::Update(const gazebo::common::UpdateInfo& info) {
|
||||
gazebo::msgs::Float64 pos_msg, vel_msg;
|
||||
pos_msg.set_data(Limit(GetAngle() - zero));
|
||||
pos_pub->Publish(pos_msg);
|
||||
vel_msg.set_data(GetVelocity());
|
||||
vel_pub->Publish(vel_msg);
|
||||
}
|
||||
|
||||
void Gyro::Callback(const msgs::ConstStringPtr& msg) {
|
||||
void Gyro::Callback(const gazebo::msgs::ConstStringPtr& msg) {
|
||||
std::string command = msg->data();
|
||||
if (command == "reset") {
|
||||
zero = GetAngle();
|
||||
|
||||
@@ -15,8 +15,6 @@
|
||||
|
||||
#include "simulation/gz_msgs/msgs.h"
|
||||
|
||||
using namespace gazebo;
|
||||
|
||||
/// \brief The axis about which to measure rotation.
|
||||
typedef enum { Roll /*X*/, Pitch /*Y*/, Yaw /*Z*/ } ROTATION;
|
||||
|
||||
@@ -44,13 +42,13 @@ typedef enum { Roll /*X*/, Pitch /*Y*/, Yaw /*Z*/ } ROTATION;
|
||||
* (gazebo.msgs.String)
|
||||
* - `units`; Optional, defaults to radians.
|
||||
*/
|
||||
class Gyro : public ModelPlugin {
|
||||
class Gyro : public gazebo::ModelPlugin {
|
||||
public:
|
||||
/// \brief Load the gyro and configures it according to the sdf.
|
||||
void Load(physics::ModelPtr model, sdf::ElementPtr sdf);
|
||||
void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf);
|
||||
|
||||
/// \brief Sends out the gyro reading each timestep.
|
||||
void Update(const common::UpdateInfo& info);
|
||||
void Update(const gazebo::common::UpdateInfo& info);
|
||||
|
||||
private:
|
||||
/// \brief Publish the angle on this topic.
|
||||
@@ -66,10 +64,10 @@ class Gyro : public ModelPlugin {
|
||||
double zero;
|
||||
|
||||
/// \brief The link that this gyro measures
|
||||
physics::LinkPtr link;
|
||||
gazebo::physics::LinkPtr link;
|
||||
|
||||
/// \brief Callback for handling control data
|
||||
void Callback(const msgs::ConstStringPtr& msg);
|
||||
void Callback(const gazebo::msgs::ConstStringPtr& msg);
|
||||
|
||||
/// \brief Gets the current angle, taking into account whether to
|
||||
/// return radians or degrees.
|
||||
@@ -83,10 +81,18 @@ class Gyro : public ModelPlugin {
|
||||
/// depending on whether or radians or degrees are being used.
|
||||
double Limit(double value);
|
||||
|
||||
physics::ModelPtr model; ///< \brief The model that this is attached to.
|
||||
event::ConnectionPtr
|
||||
updateConn; ///< \brief Pointer to the world update function.
|
||||
transport::NodePtr node; ///< \brief The node we're advertising on.
|
||||
transport::SubscriberPtr command_sub; ///< \brief Subscriber handle.
|
||||
transport::PublisherPtr pos_pub, vel_pub; ///< \brief Publisher handles.
|
||||
/// \brief The model to which this is attached.
|
||||
gazebo::physics::ModelPtr model;
|
||||
|
||||
/// \brief Pointer to the world update function.
|
||||
gazebo::event::ConnectionPtr updateConn;
|
||||
|
||||
/// \brief The node on which we're advertising.
|
||||
gazebo::transport::NodePtr node;
|
||||
|
||||
/// \brief Subscriber handle.
|
||||
gazebo::transport::SubscriberPtr command_sub;
|
||||
|
||||
/// \brief Publisher handles.
|
||||
gazebo::transport::PublisherPtr pos_pub, vel_pub;
|
||||
};
|
||||
|
||||
@@ -10,8 +10,8 @@
|
||||
#include <string>
|
||||
|
||||
ExternalLimitSwitch::ExternalLimitSwitch(sdf::ElementPtr sdf) {
|
||||
sensor = std::dynamic_pointer_cast<sensors::ContactSensor>(
|
||||
sensors::get_sensor(sdf->Get<std::string>("sensor")));
|
||||
sensor = std::dynamic_pointer_cast<gazebo::sensors::ContactSensor>(
|
||||
gazebo::sensors::get_sensor(sdf->Get<std::string>("sensor")));
|
||||
|
||||
gzmsg << "\texternal limit switch: "
|
||||
<< " sensor=" << sensor->Name() << std::endl;
|
||||
|
||||
@@ -17,8 +17,6 @@
|
||||
#include <Winsock2.h>
|
||||
#endif
|
||||
|
||||
using namespace gazebo;
|
||||
|
||||
class ExternalLimitSwitch : public Switch {
|
||||
public:
|
||||
explicit ExternalLimitSwitch(sdf::ElementPtr sdf);
|
||||
@@ -27,5 +25,5 @@ class ExternalLimitSwitch : public Switch {
|
||||
virtual bool Get();
|
||||
|
||||
private:
|
||||
sensors::ContactSensorPtr sensor;
|
||||
gazebo::sensors::ContactSensorPtr sensor;
|
||||
};
|
||||
|
||||
@@ -9,7 +9,7 @@
|
||||
|
||||
#include <string>
|
||||
|
||||
InternalLimitSwitch::InternalLimitSwitch(physics::ModelPtr model,
|
||||
InternalLimitSwitch::InternalLimitSwitch(gazebo::physics::ModelPtr model,
|
||||
sdf::ElementPtr sdf) {
|
||||
joint = model->GetJoint(sdf->Get<std::string>("joint"));
|
||||
min = sdf->Get<double>("min");
|
||||
|
||||
@@ -16,17 +16,15 @@
|
||||
#include <Winsock2.h>
|
||||
#endif
|
||||
|
||||
using namespace gazebo;
|
||||
|
||||
class InternalLimitSwitch : public Switch {
|
||||
public:
|
||||
InternalLimitSwitch(physics::ModelPtr model, sdf::ElementPtr sdf);
|
||||
InternalLimitSwitch(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf);
|
||||
|
||||
/// \brief Returns true when the switch is triggered.
|
||||
virtual bool Get();
|
||||
|
||||
private:
|
||||
physics::JointPtr joint;
|
||||
gazebo::physics::JointPtr joint;
|
||||
double min, max;
|
||||
bool radians;
|
||||
};
|
||||
|
||||
@@ -11,7 +11,7 @@
|
||||
|
||||
GZ_REGISTER_MODEL_PLUGIN(LimitSwitch)
|
||||
|
||||
void LimitSwitch::Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
|
||||
void LimitSwitch::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) {
|
||||
this->model = model;
|
||||
|
||||
// Parse SDF properties
|
||||
@@ -36,18 +36,18 @@ void LimitSwitch::Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
|
||||
std::string scoped_name =
|
||||
model->GetWorld()->GetName() + "::" + model->GetScopedName();
|
||||
boost::replace_all(scoped_name, "::", "/");
|
||||
node = transport::NodePtr(new transport::Node());
|
||||
node = gazebo::transport::NodePtr(new gazebo::transport::Node());
|
||||
node->Init(scoped_name);
|
||||
pub = node->Advertise<msgs::Bool>(topic);
|
||||
pub = node->Advertise<gazebo::msgs::Bool>(topic);
|
||||
|
||||
// Connect to the world update event.
|
||||
// This will trigger the Update function every Gazebo iteration
|
||||
updateConn = event::Events::ConnectWorldUpdateBegin(
|
||||
updateConn = gazebo::event::Events::ConnectWorldUpdateBegin(
|
||||
boost::bind(&LimitSwitch::Update, this, _1));
|
||||
}
|
||||
|
||||
void LimitSwitch::Update(const common::UpdateInfo& info) {
|
||||
msgs::Bool msg;
|
||||
void LimitSwitch::Update(const gazebo::common::UpdateInfo& info) {
|
||||
gazebo::msgs::Bool msg;
|
||||
msg.set_data(ls->Get());
|
||||
pub->Publish(msg);
|
||||
}
|
||||
|
||||
@@ -18,8 +18,6 @@
|
||||
#include "simulation/gz_msgs/msgs.h"
|
||||
#include "switch.h"
|
||||
|
||||
using namespace gazebo;
|
||||
|
||||
/**
|
||||
* \brief Plugin for reading limit switches.
|
||||
*
|
||||
@@ -66,13 +64,13 @@ using namespace gazebo;
|
||||
* External
|
||||
* - `sensor`: Name of the contact sensor that this limit switch uses.
|
||||
*/
|
||||
class LimitSwitch : public ModelPlugin {
|
||||
class LimitSwitch : public gazebo::ModelPlugin {
|
||||
public:
|
||||
/// \brief Load the limit switch and configures it according to the sdf.
|
||||
void Load(physics::ModelPtr model, sdf::ElementPtr sdf);
|
||||
void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf);
|
||||
|
||||
/// \brief Sends out the limit switch reading each timestep.
|
||||
void Update(const common::UpdateInfo& info);
|
||||
void Update(const gazebo::common::UpdateInfo& info);
|
||||
|
||||
private:
|
||||
/// \brief Publish the limit switch value on this topic.
|
||||
@@ -81,9 +79,15 @@ class LimitSwitch : public ModelPlugin {
|
||||
/// \brief LimitSwitch object, currently internal or external.
|
||||
Switch* ls;
|
||||
|
||||
physics::ModelPtr model; ///< \brief The model that this is attached to.
|
||||
event::ConnectionPtr
|
||||
updateConn; ///< \brief Pointer to the world update function.
|
||||
transport::NodePtr node; ///< \brief The node we're advertising on.
|
||||
transport::PublisherPtr pub; ///< \brief Publisher handle.
|
||||
/// \brief The model to which this is attached.
|
||||
gazebo::physics::ModelPtr model;
|
||||
|
||||
/// \brief Pointer to the world update function.
|
||||
gazebo::event::ConnectionPtr updateConn;
|
||||
|
||||
/// \brief The node on which we're advertising.
|
||||
gazebo::transport::NodePtr node;
|
||||
|
||||
/// \brief Publisher handle.
|
||||
gazebo::transport::PublisherPtr pub;
|
||||
};
|
||||
|
||||
@@ -19,7 +19,8 @@
|
||||
|
||||
GZ_REGISTER_MODEL_PLUGIN(PneumaticPiston)
|
||||
|
||||
void PneumaticPiston::Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
|
||||
void PneumaticPiston::Load(gazebo::physics::ModelPtr model,
|
||||
sdf::ElementPtr sdf) {
|
||||
this->model = model;
|
||||
signal = 0;
|
||||
|
||||
@@ -48,21 +49,21 @@ void PneumaticPiston::Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
|
||||
std::string scoped_name =
|
||||
model->GetWorld()->GetName() + "::" + model->GetScopedName();
|
||||
boost::replace_all(scoped_name, "::", "/");
|
||||
node = transport::NodePtr(new transport::Node());
|
||||
node = gazebo::transport::NodePtr(new gazebo::transport::Node());
|
||||
node->Init(scoped_name);
|
||||
sub = node->Subscribe(topic, &PneumaticPiston::Callback, this);
|
||||
|
||||
// Connect to the world update event.
|
||||
// This will trigger the Update function every Gazebo iteration
|
||||
updateConn = event::Events::ConnectWorldUpdateBegin(
|
||||
updateConn = gazebo::event::Events::ConnectWorldUpdateBegin(
|
||||
boost::bind(&PneumaticPiston::Update, this, _1));
|
||||
}
|
||||
|
||||
void PneumaticPiston::Update(const common::UpdateInfo& info) {
|
||||
void PneumaticPiston::Update(const gazebo::common::UpdateInfo& info) {
|
||||
joint->SetForce(0, signal);
|
||||
}
|
||||
|
||||
void PneumaticPiston::Callback(const msgs::ConstFloat64Ptr& msg) {
|
||||
void PneumaticPiston::Callback(const gazebo::msgs::ConstFloat64Ptr& msg) {
|
||||
if (msg->data() < -0.001) {
|
||||
signal = -reverse_force;
|
||||
} else if (msg->data() > 0.001) {
|
||||
|
||||
@@ -13,8 +13,6 @@
|
||||
|
||||
#include "simulation/gz_msgs/msgs.h"
|
||||
|
||||
using namespace gazebo;
|
||||
|
||||
/**
|
||||
* \brief Plugin for controlling a joint with a pneumatic piston.
|
||||
*
|
||||
@@ -49,13 +47,13 @@ using namespace gazebo;
|
||||
*
|
||||
* \todo Signal should probably be made a tri-state message.
|
||||
*/
|
||||
class PneumaticPiston : public ModelPlugin {
|
||||
class PneumaticPiston : public gazebo::ModelPlugin {
|
||||
public:
|
||||
/// \brief Load the pneumatic piston and configures it according to the sdf.
|
||||
void Load(physics::ModelPtr model, sdf::ElementPtr sdf);
|
||||
void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf);
|
||||
|
||||
/// \brief Updat the force the piston applies on the joint.
|
||||
void Update(const common::UpdateInfo& info);
|
||||
void Update(const gazebo::common::UpdateInfo& info);
|
||||
|
||||
private:
|
||||
/// \brief Topic to read control signal from.
|
||||
@@ -68,14 +66,20 @@ class PneumaticPiston : public ModelPlugin {
|
||||
double forward_force, reverse_force;
|
||||
|
||||
/// \brief The joint that this pneumatic piston actuates.
|
||||
physics::JointPtr joint;
|
||||
gazebo::physics::JointPtr joint;
|
||||
|
||||
/// \brief Callback for receiving msgs and updating the torque.
|
||||
void Callback(const msgs::ConstFloat64Ptr& msg);
|
||||
void Callback(const gazebo::msgs::ConstFloat64Ptr& msg);
|
||||
|
||||
physics::ModelPtr model; ///< \brief The model that this is attached to.
|
||||
event::ConnectionPtr
|
||||
updateConn; ///< \brief Pointer to the world update function.
|
||||
transport::NodePtr node; ///< \brief The node we're advertising on.
|
||||
transport::SubscriberPtr sub; ///< \brief Subscriber handle.
|
||||
/// \brief The model to which this is attached.
|
||||
gazebo::physics::ModelPtr model;
|
||||
|
||||
/// \brief Pointer to the world update function.
|
||||
gazebo::event::ConnectionPtr updateConn;
|
||||
|
||||
/// \brief The node on which we're advertising.
|
||||
gazebo::transport::NodePtr node;
|
||||
|
||||
/// \brief Subscriber handle.
|
||||
gazebo::transport::SubscriberPtr sub;
|
||||
};
|
||||
|
||||
@@ -19,7 +19,7 @@
|
||||
|
||||
GZ_REGISTER_MODEL_PLUGIN(Potentiometer)
|
||||
|
||||
void Potentiometer::Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
|
||||
void Potentiometer::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) {
|
||||
this->model = model;
|
||||
|
||||
// Parse SDF properties
|
||||
@@ -43,19 +43,19 @@ void Potentiometer::Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
|
||||
std::string scoped_name =
|
||||
model->GetWorld()->GetName() + "::" + model->GetScopedName();
|
||||
boost::replace_all(scoped_name, "::", "/");
|
||||
node = transport::NodePtr(new transport::Node());
|
||||
node = gazebo::transport::NodePtr(new gazebo::transport::Node());
|
||||
node->Init(scoped_name);
|
||||
pub = node->Advertise<msgs::Float64>(topic);
|
||||
pub = node->Advertise<gazebo::msgs::Float64>(topic);
|
||||
|
||||
// Connect to the world update event.
|
||||
// This will trigger the Update function every Gazebo iteration
|
||||
updateConn = event::Events::ConnectWorldUpdateBegin(
|
||||
updateConn = gazebo::event::Events::ConnectWorldUpdateBegin(
|
||||
boost::bind(&Potentiometer::Update, this, _1));
|
||||
}
|
||||
|
||||
void Potentiometer::Update(const common::UpdateInfo& info) {
|
||||
void Potentiometer::Update(const gazebo::common::UpdateInfo& info) {
|
||||
joint->GetAngle(0).Normalize();
|
||||
msgs::Float64 msg;
|
||||
gazebo::msgs::Float64 msg;
|
||||
if (radians) {
|
||||
msg.set_data(joint->GetAngle(0).Radian());
|
||||
} else {
|
||||
|
||||
@@ -13,8 +13,6 @@
|
||||
|
||||
#include "simulation/gz_msgs/msgs.h"
|
||||
|
||||
using namespace gazebo;
|
||||
|
||||
/**
|
||||
* \brief Plugin for reading the angle of a joint.
|
||||
*
|
||||
@@ -34,13 +32,13 @@ using namespace gazebo;
|
||||
* - `topic`: Optional. Message will be published as a gazebo.msgs.Float64.
|
||||
* - `units`: Optional. Defaults to radians.
|
||||
*/
|
||||
class Potentiometer : public ModelPlugin {
|
||||
class Potentiometer : public gazebo::ModelPlugin {
|
||||
public:
|
||||
/// \brief Load the potentiometer and configures it according to the sdf.
|
||||
void Load(physics::ModelPtr model, sdf::ElementPtr sdf);
|
||||
void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf);
|
||||
|
||||
/// \brief Sends out the potentiometer reading each timestep.
|
||||
void Update(const common::UpdateInfo& info);
|
||||
void Update(const gazebo::common::UpdateInfo& info);
|
||||
|
||||
private:
|
||||
/// \brief Publish the angle on this topic.
|
||||
@@ -50,11 +48,17 @@ class Potentiometer : public ModelPlugin {
|
||||
bool radians;
|
||||
|
||||
/// \brief The joint that this potentiometer measures
|
||||
physics::JointPtr joint;
|
||||
gazebo::physics::JointPtr joint;
|
||||
|
||||
physics::ModelPtr model; ///< \brief The model that this is attached to.
|
||||
event::ConnectionPtr
|
||||
updateConn; ///< \brief Pointer to the world update function.
|
||||
transport::NodePtr node; ///< \brief The node we're advertising on.
|
||||
transport::PublisherPtr pub; ///< \brief Publisher handle.
|
||||
/// \brief The model to which this is attached.
|
||||
gazebo::physics::ModelPtr model;
|
||||
|
||||
/// \brief Pointer to the world update function.
|
||||
gazebo::event::ConnectionPtr updateConn;
|
||||
|
||||
/// \brief The node on which we're advertising.
|
||||
gazebo::transport::NodePtr node;
|
||||
|
||||
/// \brief Publisher handle.
|
||||
gazebo::transport::PublisherPtr pub;
|
||||
};
|
||||
|
||||
@@ -20,12 +20,12 @@
|
||||
|
||||
GZ_REGISTER_MODEL_PLUGIN(Rangefinder)
|
||||
|
||||
void Rangefinder::Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
|
||||
void Rangefinder::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) {
|
||||
this->model = model;
|
||||
|
||||
// Parse SDF properties
|
||||
sensor = std::dynamic_pointer_cast<sensors::SonarSensor>(
|
||||
sensors::get_sensor(sdf->Get<std::string>("sensor")));
|
||||
sensor = std::dynamic_pointer_cast<gazebo::sensors::SonarSensor>(
|
||||
gazebo::sensors::get_sensor(sdf->Get<std::string>("sensor")));
|
||||
if (sdf->HasElement("topic")) {
|
||||
topic = sdf->Get<std::string>("topic");
|
||||
} else {
|
||||
@@ -39,18 +39,18 @@ void Rangefinder::Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
|
||||
std::string scoped_name =
|
||||
model->GetWorld()->GetName() + "::" + model->GetScopedName();
|
||||
boost::replace_all(scoped_name, "::", "/");
|
||||
node = transport::NodePtr(new transport::Node());
|
||||
node = gazebo::transport::NodePtr(new gazebo::transport::Node());
|
||||
node->Init(scoped_name);
|
||||
pub = node->Advertise<msgs::Float64>(topic);
|
||||
pub = node->Advertise<gazebo::msgs::Float64>(topic);
|
||||
|
||||
// Connect to the world update event.
|
||||
// This will trigger the Update function every Gazebo iteration
|
||||
updateConn = event::Events::ConnectWorldUpdateBegin(
|
||||
updateConn = gazebo::event::Events::ConnectWorldUpdateBegin(
|
||||
boost::bind(&Rangefinder::Update, this, _1));
|
||||
}
|
||||
|
||||
void Rangefinder::Update(const common::UpdateInfo& info) {
|
||||
msgs::Float64 msg;
|
||||
void Rangefinder::Update(const gazebo::common::UpdateInfo& info) {
|
||||
gazebo::msgs::Float64 msg;
|
||||
msg.set_data(sensor->Range());
|
||||
pub->Publish(msg);
|
||||
}
|
||||
|
||||
@@ -13,8 +13,6 @@
|
||||
|
||||
#include "simulation/gz_msgs/msgs.h"
|
||||
|
||||
using namespace gazebo;
|
||||
|
||||
/**
|
||||
* \brief Plugin for reading the range of obstacles.
|
||||
*
|
||||
@@ -32,24 +30,30 @@ using namespace gazebo;
|
||||
* - `sensor`: Name of the sonar sensor that this rangefinder uses.
|
||||
* - `topic`: Optional. Message will be published as a gazebo.msgs.Float64.
|
||||
*/
|
||||
class Rangefinder : public ModelPlugin {
|
||||
class Rangefinder : public gazebo::ModelPlugin {
|
||||
public:
|
||||
/// \brief Load the rangefinder and configures it according to the sdf.
|
||||
void Load(physics::ModelPtr model, sdf::ElementPtr sdf);
|
||||
void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf);
|
||||
|
||||
/// \brief Sends out the rangefinder reading each timestep.
|
||||
void Update(const common::UpdateInfo& info);
|
||||
void Update(const gazebo::common::UpdateInfo& info);
|
||||
|
||||
private:
|
||||
/// \brief Publish the range on this topic.
|
||||
std::string topic;
|
||||
|
||||
/// \brief The sonar sensor that this rangefinder uses
|
||||
sensors::SonarSensorPtr sensor;
|
||||
gazebo::sensors::SonarSensorPtr sensor;
|
||||
|
||||
physics::ModelPtr model; ///< \brief The model that this is attached to.
|
||||
event::ConnectionPtr
|
||||
updateConn; ///< \brief Pointer to the world update function.
|
||||
transport::NodePtr node; ///< \brief The node we're advertising on.
|
||||
transport::PublisherPtr pub; ///< \brief Publisher handle.
|
||||
/// \brief The model to which this is attached.
|
||||
gazebo::physics::ModelPtr model;
|
||||
|
||||
/// \brief Pointer to the world update function.
|
||||
gazebo::event::ConnectionPtr updateConn;
|
||||
|
||||
/// \brief The node on which we're advertising.
|
||||
gazebo::transport::NodePtr node;
|
||||
|
||||
/// \brief Publisher handle.
|
||||
gazebo::transport::PublisherPtr pub;
|
||||
};
|
||||
|
||||
@@ -19,7 +19,7 @@
|
||||
|
||||
GZ_REGISTER_MODEL_PLUGIN(Servo)
|
||||
|
||||
void Servo::Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
|
||||
void Servo::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) {
|
||||
this->model = model;
|
||||
signal = 0;
|
||||
|
||||
@@ -44,17 +44,17 @@ void Servo::Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
|
||||
std::string scoped_name =
|
||||
model->GetWorld()->GetName() + "::" + model->GetScopedName();
|
||||
boost::replace_all(scoped_name, "::", "/");
|
||||
node = transport::NodePtr(new transport::Node());
|
||||
node = gazebo::transport::NodePtr(new gazebo::transport::Node());
|
||||
node->Init(scoped_name);
|
||||
sub = node->Subscribe(topic, &Servo::Callback, this);
|
||||
|
||||
// connect to the world update event
|
||||
// this will call update every iteration
|
||||
updateConn = event::Events::ConnectWorldUpdateBegin(
|
||||
updateConn = gazebo::event::Events::ConnectWorldUpdateBegin(
|
||||
boost::bind(&Servo::Update, this, _1));
|
||||
}
|
||||
|
||||
void Servo::Update(const common::UpdateInfo& info) {
|
||||
void Servo::Update(const gazebo::common::UpdateInfo& info) {
|
||||
// torque is in kg*cm
|
||||
// joint->SetAngle(0,signal*180);
|
||||
if (joint->GetAngle(0) < signal) {
|
||||
@@ -65,7 +65,7 @@ void Servo::Update(const common::UpdateInfo& info) {
|
||||
joint->SetForce(0, 0);
|
||||
}
|
||||
|
||||
void Servo::Callback(const msgs::ConstFloat64Ptr& msg) {
|
||||
void Servo::Callback(const gazebo::msgs::ConstFloat64Ptr& msg) {
|
||||
signal = msg->data();
|
||||
if (signal < -1) {
|
||||
signal = -1;
|
||||
|
||||
@@ -13,8 +13,6 @@
|
||||
|
||||
#include "simulation/gz_msgs/msgs.h"
|
||||
|
||||
using namespace gazebo;
|
||||
|
||||
/**
|
||||
* \brief Plugin for controlling a servo.
|
||||
*
|
||||
@@ -34,13 +32,13 @@ using namespace gazebo;
|
||||
* - `link`: Name of the link the servo is attached to.
|
||||
* - `topic`: Optional. Message type should be gazebo.msgs.Float64.
|
||||
*/
|
||||
class Servo : public ModelPlugin {
|
||||
class Servo : public gazebo::ModelPlugin {
|
||||
public:
|
||||
/// \brief load the servo and configure it according to the sdf
|
||||
void Load(physics::ModelPtr model, sdf::ElementPtr sdf);
|
||||
void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf);
|
||||
|
||||
/// \brief Update the torque on the joint from the dc motor each timestep.
|
||||
void Update(const common::UpdateInfo& info);
|
||||
void Update(const gazebo::common::UpdateInfo& info);
|
||||
|
||||
private:
|
||||
/// \brief Topic to read control signal from.
|
||||
@@ -53,14 +51,20 @@ class Servo : public ModelPlugin {
|
||||
double torque;
|
||||
|
||||
/// \brief the joint that this servo moves
|
||||
physics::JointPtr joint;
|
||||
gazebo::physics::JointPtr joint;
|
||||
|
||||
/// \brief Callback for receiving msgs and storing the signal
|
||||
void Callback(const msgs::ConstFloat64Ptr& msg);
|
||||
void Callback(const gazebo::msgs::ConstFloat64Ptr& msg);
|
||||
|
||||
physics::ModelPtr model; ///< \brief The model that this is attached to
|
||||
event::ConnectionPtr
|
||||
updateConn; ///< \brief The Pointer to the world update function
|
||||
transport::NodePtr node; ///< \brief The node we're advertising torque on
|
||||
transport::SubscriberPtr sub; ///< \brief the Subscriber for the pwm signal
|
||||
/// \brief The model to which this is attached
|
||||
gazebo::physics::ModelPtr model;
|
||||
|
||||
/// \brief The pointer to the world update function
|
||||
gazebo::event::ConnectionPtr updateConn;
|
||||
|
||||
/// \brief The node on which we're advertising torque
|
||||
gazebo::transport::NodePtr node;
|
||||
|
||||
/// \brief The subscriber for the PWM signal
|
||||
gazebo::transport::SubscriberPtr sub;
|
||||
};
|
||||
|
||||
@@ -119,14 +119,14 @@ class DriverStation : public SensorBase, public RobotStateInterface {
|
||||
///< 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);
|
||||
void stateCallback(const gazebo::msgs::ConstDriverStationPtr& msg);
|
||||
void joystickCallback(const gazebo::msgs::ConstFRCJoystickPtr& msg, int i);
|
||||
void joystickCallback0(const gazebo::msgs::ConstFRCJoystickPtr& msg);
|
||||
void joystickCallback1(const gazebo::msgs::ConstFRCJoystickPtr& msg);
|
||||
void joystickCallback2(const gazebo::msgs::ConstFRCJoystickPtr& msg);
|
||||
void joystickCallback3(const gazebo::msgs::ConstFRCJoystickPtr& msg);
|
||||
void joystickCallback4(const gazebo::msgs::ConstFRCJoystickPtr& msg);
|
||||
void joystickCallback5(const gazebo::msgs::ConstFRCJoystickPtr& msg);
|
||||
|
||||
int m_digitalOut = 0;
|
||||
std::condition_variable m_waitForDataCond;
|
||||
@@ -140,10 +140,10 @@ class DriverStation : public SensorBase, public RobotStateInterface {
|
||||
bool m_userInTeleop = false;
|
||||
bool m_userInTest = false;
|
||||
|
||||
transport::SubscriberPtr stateSub;
|
||||
transport::SubscriberPtr joysticksSub[6];
|
||||
msgs::DriverStationPtr state;
|
||||
msgs::FRCJoystickPtr joysticks[6];
|
||||
gazebo::transport::SubscriberPtr stateSub;
|
||||
gazebo::transport::SubscriberPtr joysticksSub[6];
|
||||
gazebo::msgs::DriverStationPtr state;
|
||||
gazebo::msgs::FRCJoystickPtr joysticks[6];
|
||||
};
|
||||
|
||||
} // namespace frc
|
||||
|
||||
@@ -50,7 +50,7 @@ class RobotBase {
|
||||
RobotBase& operator=(const RobotBase&) = delete;
|
||||
|
||||
DriverStation& m_ds;
|
||||
transport::SubscriberPtr time_sub;
|
||||
gazebo::transport::SubscriberPtr time_sub;
|
||||
};
|
||||
|
||||
} // namespace frc
|
||||
|
||||
@@ -14,8 +14,6 @@
|
||||
|
||||
#include "simulation/gz_msgs/msgs.h"
|
||||
|
||||
using namespace gazebo;
|
||||
|
||||
namespace frc {
|
||||
|
||||
class MainNode {
|
||||
@@ -26,14 +24,14 @@ class MainNode {
|
||||
}
|
||||
|
||||
template <typename M>
|
||||
static transport::PublisherPtr Advertise(const std::string& topic,
|
||||
int queueLimit = 10,
|
||||
bool latch = false) {
|
||||
static gazebo::transport::PublisherPtr Advertise(const std::string& topic,
|
||||
int queueLimit = 10,
|
||||
bool latch = false) {
|
||||
return GetInstance()->main->Advertise<M>(topic, queueLimit, latch);
|
||||
}
|
||||
|
||||
template <typename M, typename T>
|
||||
static transport::SubscriberPtr Subscribe(
|
||||
static gazebo::transport::SubscriberPtr Subscribe(
|
||||
const std::string& topic,
|
||||
void (T::*fp)(const boost::shared_ptr<M const>&), T* obj,
|
||||
bool latching = false) {
|
||||
@@ -41,20 +39,20 @@ class MainNode {
|
||||
}
|
||||
|
||||
template <typename M>
|
||||
static transport::SubscriberPtr Subscribe(
|
||||
static gazebo::transport::SubscriberPtr Subscribe(
|
||||
const std::string& topic, void (*fp)(const boost::shared_ptr<M const>&),
|
||||
bool latching = false) {
|
||||
return GetInstance()->main->Subscribe(topic, fp, latching);
|
||||
}
|
||||
|
||||
transport::NodePtr main;
|
||||
gazebo::transport::NodePtr main;
|
||||
|
||||
private:
|
||||
MainNode() {
|
||||
bool success = gazebo::client::setup();
|
||||
|
||||
if (success) {
|
||||
main = transport::NodePtr(new transport::Node());
|
||||
main = gazebo::transport::NodePtr(new gazebo::transport::Node());
|
||||
main->Init("frc");
|
||||
gazebo::transport::run();
|
||||
} else {
|
||||
|
||||
@@ -19,13 +19,11 @@
|
||||
#include <Winsock2.h>
|
||||
#endif
|
||||
|
||||
using namespace gazebo;
|
||||
|
||||
namespace frc {
|
||||
|
||||
class SimContinuousOutput {
|
||||
private:
|
||||
transport::PublisherPtr pub;
|
||||
gazebo::transport::PublisherPtr pub;
|
||||
float speed;
|
||||
|
||||
public:
|
||||
|
||||
@@ -13,8 +13,6 @@
|
||||
|
||||
#include "simulation/gz_msgs/msgs.h"
|
||||
|
||||
using namespace gazebo;
|
||||
|
||||
namespace frc {
|
||||
|
||||
class SimDigitalInput {
|
||||
@@ -28,8 +26,8 @@ class SimDigitalInput {
|
||||
|
||||
private:
|
||||
bool value;
|
||||
transport::SubscriberPtr sub;
|
||||
void callback(const msgs::ConstBoolPtr& msg);
|
||||
gazebo::transport::SubscriberPtr sub;
|
||||
void callback(const gazebo::msgs::ConstBoolPtr& msg);
|
||||
};
|
||||
|
||||
} // namespace frc
|
||||
|
||||
@@ -14,8 +14,6 @@
|
||||
|
||||
#include "simulation/gz_msgs/msgs.h"
|
||||
|
||||
using namespace gazebo;
|
||||
|
||||
namespace frc {
|
||||
|
||||
class SimEncoder {
|
||||
@@ -32,10 +30,10 @@ class SimEncoder {
|
||||
void sendCommand(std::string cmd);
|
||||
|
||||
double position, velocity;
|
||||
transport::SubscriberPtr posSub, velSub;
|
||||
transport::PublisherPtr commandPub;
|
||||
void positionCallback(const msgs::ConstFloat64Ptr& msg);
|
||||
void velocityCallback(const msgs::ConstFloat64Ptr& msg);
|
||||
gazebo::transport::SubscriberPtr posSub, velSub;
|
||||
gazebo::transport::PublisherPtr commandPub;
|
||||
void positionCallback(const gazebo::msgs::ConstFloat64Ptr& msg);
|
||||
void velocityCallback(const gazebo::msgs::ConstFloat64Ptr& msg);
|
||||
};
|
||||
|
||||
} // namespace frc
|
||||
|
||||
@@ -13,8 +13,6 @@
|
||||
|
||||
#include "simulation/gz_msgs/msgs.h"
|
||||
|
||||
using namespace gazebo;
|
||||
|
||||
namespace frc {
|
||||
|
||||
class SimFloatInput {
|
||||
@@ -28,8 +26,8 @@ class SimFloatInput {
|
||||
|
||||
private:
|
||||
double value;
|
||||
transport::SubscriberPtr sub;
|
||||
void callback(const msgs::ConstFloat64Ptr& msg);
|
||||
gazebo::transport::SubscriberPtr sub;
|
||||
void callback(const gazebo::msgs::ConstFloat64Ptr& msg);
|
||||
};
|
||||
|
||||
} // namespace frc
|
||||
|
||||
@@ -13,8 +13,6 @@
|
||||
|
||||
#include "simulation/gz_msgs/msgs.h"
|
||||
|
||||
using namespace gazebo;
|
||||
|
||||
namespace frc {
|
||||
|
||||
class SimGyro {
|
||||
@@ -29,10 +27,10 @@ class SimGyro {
|
||||
void sendCommand(std::string cmd);
|
||||
|
||||
double position, velocity;
|
||||
transport::SubscriberPtr posSub, velSub;
|
||||
transport::PublisherPtr commandPub;
|
||||
void positionCallback(const msgs::ConstFloat64Ptr& msg);
|
||||
void velocityCallback(const msgs::ConstFloat64Ptr& msg);
|
||||
gazebo::transport::SubscriberPtr posSub, velSub;
|
||||
gazebo::transport::PublisherPtr commandPub;
|
||||
void positionCallback(const gazebo::msgs::ConstFloat64Ptr& msg);
|
||||
void velocityCallback(const gazebo::msgs::ConstFloat64Ptr& msg);
|
||||
};
|
||||
|
||||
} // namespace frc
|
||||
|
||||
@@ -21,6 +21,6 @@
|
||||
namespace wpilib {
|
||||
namespace internal {
|
||||
extern double simTime;
|
||||
extern void time_callback(const msgs::ConstFloat64Ptr& msg);
|
||||
extern void time_callback(const gazebo::msgs::ConstFloat64Ptr& msg);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -29,26 +29,26 @@ int DriverStation::m_updateNumber = 0;
|
||||
* This is only called once the first time GetInstance() is called
|
||||
*/
|
||||
DriverStation::DriverStation() {
|
||||
state = msgs::DriverStationPtr(new msgs::DriverStation());
|
||||
state = gazebo::msgs::DriverStationPtr(new gazebo::msgs::DriverStation());
|
||||
stateSub =
|
||||
MainNode::Subscribe("~/ds/state", &DriverStation::stateCallback, this);
|
||||
// TODO: for loop + boost bind
|
||||
joysticks[0] = msgs::FRCJoystickPtr(new msgs::FRCJoystick());
|
||||
joysticks[0] = gazebo::msgs::FRCJoystickPtr(new gazebo::msgs::FRCJoystick());
|
||||
joysticksSub[0] = MainNode::Subscribe(
|
||||
"~/ds/joysticks/0", &DriverStation::joystickCallback0, this);
|
||||
joysticks[1] = msgs::FRCJoystickPtr(new msgs::FRCJoystick());
|
||||
joysticks[1] = gazebo::msgs::FRCJoystickPtr(new gazebo::msgs::FRCJoystick());
|
||||
joysticksSub[1] = MainNode::Subscribe(
|
||||
"~/ds/joysticks/1", &DriverStation::joystickCallback1, this);
|
||||
joysticks[2] = msgs::FRCJoystickPtr(new msgs::FRCJoystick());
|
||||
joysticks[2] = gazebo::msgs::FRCJoystickPtr(new gazebo::msgs::FRCJoystick());
|
||||
joysticksSub[2] = MainNode::Subscribe(
|
||||
"~/ds/joysticks/2", &DriverStation::joystickCallback2, this);
|
||||
joysticks[3] = msgs::FRCJoystickPtr(new msgs::FRCJoystick());
|
||||
joysticks[3] = gazebo::msgs::FRCJoystickPtr(new gazebo::msgs::FRCJoystick());
|
||||
joysticksSub[3] = MainNode::Subscribe(
|
||||
"~/ds/joysticks/5", &DriverStation::joystickCallback3, this);
|
||||
joysticks[4] = msgs::FRCJoystickPtr(new msgs::FRCJoystick());
|
||||
joysticks[4] = gazebo::msgs::FRCJoystickPtr(new gazebo::msgs::FRCJoystick());
|
||||
joysticksSub[4] = MainNode::Subscribe(
|
||||
"~/ds/joysticks/4", &DriverStation::joystickCallback4, this);
|
||||
joysticks[5] = msgs::FRCJoystickPtr(new msgs::FRCJoystick());
|
||||
joysticks[5] = gazebo::msgs::FRCJoystickPtr(new gazebo::msgs::FRCJoystick());
|
||||
joysticksSub[5] = MainNode::Subscribe(
|
||||
"~/ds/joysticks/5", &DriverStation::joystickCallback5, this);
|
||||
}
|
||||
@@ -137,7 +137,7 @@ int16_t DriverStation::GetStickButtons(int stick) {
|
||||
int16_t btns = 0, btnid;
|
||||
|
||||
std::unique_lock<std::recursive_mutex> lock(m_joystickMutex);
|
||||
msgs::FRCJoystickPtr joy = joysticks[stick];
|
||||
gazebo::msgs::FRCJoystickPtr joy = joysticks[stick];
|
||||
for (btnid = 0; btnid < joy->buttons().size() && btnid < 12; btnid++) {
|
||||
if (joysticks[stick]->buttons(btnid)) {
|
||||
btns |= (1 << btnid);
|
||||
@@ -212,8 +212,9 @@ bool DriverStation::IsDisabled() const { return !IsEnabled(); }
|
||||
|
||||
bool DriverStation::IsAutonomous() const {
|
||||
std::unique_lock<std::recursive_mutex> lock(m_stateMutex);
|
||||
return state != nullptr ? state->state() == msgs::DriverStation_State_AUTO
|
||||
: false;
|
||||
return state != nullptr
|
||||
? state->state() == gazebo::msgs::DriverStation_State_AUTO
|
||||
: false;
|
||||
}
|
||||
|
||||
bool DriverStation::IsOperatorControl() const {
|
||||
@@ -222,8 +223,9 @@ bool DriverStation::IsOperatorControl() const {
|
||||
|
||||
bool DriverStation::IsTest() const {
|
||||
std::unique_lock<std::recursive_mutex> lock(m_stateMutex);
|
||||
return state != nullptr ? state->state() == msgs::DriverStation_State_TEST
|
||||
: false;
|
||||
return state != nullptr
|
||||
? state->state() == gazebo::msgs::DriverStation_State_TEST
|
||||
: false;
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -360,7 +362,8 @@ void DriverStation::ReportError(bool is_error, int code,
|
||||
*/
|
||||
uint16_t DriverStation::GetTeamNumber() const { return 348; }
|
||||
|
||||
void DriverStation::stateCallback(const msgs::ConstDriverStationPtr& msg) {
|
||||
void DriverStation::stateCallback(
|
||||
const gazebo::msgs::ConstDriverStationPtr& msg) {
|
||||
{
|
||||
std::unique_lock<std::recursive_mutex> lock(m_stateMutex);
|
||||
*state = *msg;
|
||||
@@ -372,32 +375,38 @@ void DriverStation::stateCallback(const msgs::ConstDriverStationPtr& msg) {
|
||||
m_waitForDataCond.notify_all();
|
||||
}
|
||||
|
||||
void DriverStation::joystickCallback(const msgs::ConstFRCJoystickPtr& msg,
|
||||
int i) {
|
||||
void DriverStation::joystickCallback(
|
||||
const gazebo::msgs::ConstFRCJoystickPtr& msg, int i) {
|
||||
std::unique_lock<std::recursive_mutex> lock(m_joystickMutex);
|
||||
*(joysticks[i]) = *msg;
|
||||
}
|
||||
|
||||
void DriverStation::joystickCallback0(const msgs::ConstFRCJoystickPtr& msg) {
|
||||
void DriverStation::joystickCallback0(
|
||||
const gazebo::msgs::ConstFRCJoystickPtr& msg) {
|
||||
joystickCallback(msg, 0);
|
||||
}
|
||||
|
||||
void DriverStation::joystickCallback1(const msgs::ConstFRCJoystickPtr& msg) {
|
||||
void DriverStation::joystickCallback1(
|
||||
const gazebo::msgs::ConstFRCJoystickPtr& msg) {
|
||||
joystickCallback(msg, 1);
|
||||
}
|
||||
|
||||
void DriverStation::joystickCallback2(const msgs::ConstFRCJoystickPtr& msg) {
|
||||
void DriverStation::joystickCallback2(
|
||||
const gazebo::msgs::ConstFRCJoystickPtr& msg) {
|
||||
joystickCallback(msg, 2);
|
||||
}
|
||||
|
||||
void DriverStation::joystickCallback3(const msgs::ConstFRCJoystickPtr& msg) {
|
||||
void DriverStation::joystickCallback3(
|
||||
const gazebo::msgs::ConstFRCJoystickPtr& msg) {
|
||||
joystickCallback(msg, 3);
|
||||
}
|
||||
|
||||
void DriverStation::joystickCallback4(const msgs::ConstFRCJoystickPtr& msg) {
|
||||
void DriverStation::joystickCallback4(
|
||||
const gazebo::msgs::ConstFRCJoystickPtr& msg) {
|
||||
joystickCallback(msg, 4);
|
||||
}
|
||||
|
||||
void DriverStation::joystickCallback5(const msgs::ConstFRCJoystickPtr& msg) {
|
||||
void DriverStation::joystickCallback5(
|
||||
const gazebo::msgs::ConstFRCJoystickPtr& msg) {
|
||||
joystickCallback(msg, 5);
|
||||
}
|
||||
|
||||
@@ -19,7 +19,7 @@ double simTime = 0;
|
||||
std::condition_variable time_wait;
|
||||
std::mutex time_wait_mutex;
|
||||
|
||||
void time_callback(const msgs::ConstFloat64Ptr& msg) {
|
||||
void time_callback(const gazebo::msgs::ConstFloat64Ptr& msg) {
|
||||
simTime = msg->data();
|
||||
time_wait.notify_all();
|
||||
}
|
||||
|
||||
@@ -12,12 +12,12 @@
|
||||
using namespace frc;
|
||||
|
||||
SimContinuousOutput::SimContinuousOutput(std::string topic) {
|
||||
pub = MainNode::Advertise<msgs::Float64>("~/simulator/" + topic);
|
||||
pub = MainNode::Advertise<gazebo::msgs::Float64>("~/simulator/" + topic);
|
||||
std::cout << "Initialized ~/simulator/" + topic << std::endl;
|
||||
}
|
||||
|
||||
void SimContinuousOutput::Set(float speed) {
|
||||
msgs::Float64 msg;
|
||||
gazebo::msgs::Float64 msg;
|
||||
msg.set_data(speed);
|
||||
pub->Publish(msg);
|
||||
}
|
||||
|
||||
@@ -19,6 +19,6 @@ SimDigitalInput::SimDigitalInput(std::string topic) {
|
||||
|
||||
bool SimDigitalInput::Get() { return value; }
|
||||
|
||||
void SimDigitalInput::callback(const msgs::ConstBoolPtr& msg) {
|
||||
void SimDigitalInput::callback(const gazebo::msgs::ConstBoolPtr& msg) {
|
||||
value = msg->data();
|
||||
}
|
||||
|
||||
@@ -12,8 +12,8 @@
|
||||
using namespace frc;
|
||||
|
||||
SimEncoder::SimEncoder(std::string topic) {
|
||||
commandPub =
|
||||
MainNode::Advertise<msgs::GzString>("~/simulator/" + topic + "/control");
|
||||
commandPub = MainNode::Advertise<gazebo::msgs::GzString>("~/simulator/" +
|
||||
topic + "/control");
|
||||
|
||||
posSub = MainNode::Subscribe("~/simulator/" + topic + "/position",
|
||||
&SimEncoder::positionCallback, this);
|
||||
@@ -41,15 +41,15 @@ double SimEncoder::GetPosition() { return position; }
|
||||
double SimEncoder::GetVelocity() { return velocity; }
|
||||
|
||||
void SimEncoder::sendCommand(std::string cmd) {
|
||||
msgs::GzString msg;
|
||||
gazebo::msgs::GzString msg;
|
||||
msg.set_data(cmd);
|
||||
commandPub->Publish(msg);
|
||||
}
|
||||
|
||||
void SimEncoder::positionCallback(const msgs::ConstFloat64Ptr& msg) {
|
||||
void SimEncoder::positionCallback(const gazebo::msgs::ConstFloat64Ptr& msg) {
|
||||
position = msg->data();
|
||||
}
|
||||
|
||||
void SimEncoder::velocityCallback(const msgs::ConstFloat64Ptr& msg) {
|
||||
void SimEncoder::velocityCallback(const gazebo::msgs::ConstFloat64Ptr& msg) {
|
||||
velocity = msg->data();
|
||||
}
|
||||
|
||||
@@ -19,6 +19,6 @@ SimFloatInput::SimFloatInput(std::string topic) {
|
||||
|
||||
double SimFloatInput::Get() { return value; }
|
||||
|
||||
void SimFloatInput::callback(const msgs::ConstFloat64Ptr& msg) {
|
||||
void SimFloatInput::callback(const gazebo::msgs::ConstFloat64Ptr& msg) {
|
||||
value = msg->data();
|
||||
}
|
||||
|
||||
@@ -12,8 +12,8 @@
|
||||
using namespace frc;
|
||||
|
||||
SimGyro::SimGyro(std::string topic) {
|
||||
commandPub =
|
||||
MainNode::Advertise<msgs::GzString>("~/simulator/" + topic + "/control");
|
||||
commandPub = MainNode::Advertise<gazebo::msgs::GzString>("~/simulator/" +
|
||||
topic + "/control");
|
||||
|
||||
posSub = MainNode::Subscribe("~/simulator/" + topic + "/position",
|
||||
&SimGyro::positionCallback, this);
|
||||
@@ -37,15 +37,15 @@ double SimGyro::GetAngle() { return position; }
|
||||
double SimGyro::GetVelocity() { return velocity; }
|
||||
|
||||
void SimGyro::sendCommand(std::string cmd) {
|
||||
msgs::GzString msg;
|
||||
gazebo::msgs::GzString msg;
|
||||
msg.set_data(cmd);
|
||||
commandPub->Publish(msg);
|
||||
}
|
||||
|
||||
void SimGyro::positionCallback(const msgs::ConstFloat64Ptr& msg) {
|
||||
void SimGyro::positionCallback(const gazebo::msgs::ConstFloat64Ptr& msg) {
|
||||
position = msg->data();
|
||||
}
|
||||
|
||||
void SimGyro::velocityCallback(const msgs::ConstFloat64Ptr& msg) {
|
||||
void SimGyro::velocityCallback(const gazebo::msgs::ConstFloat64Ptr& msg) {
|
||||
velocity = msg->data();
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user