From ba8761e39e00fcfb16824c8b965d3e8475f925d3 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Tue, 1 Nov 2016 23:09:51 -0700 Subject: [PATCH] "using" directives are no longer used in global namespaces (#219) --- hal/lib/athena/ChipObject.h | 4 +- .../frc_gazebo_plugins/clock/src/clock.cpp | 12 ++--- .../frc_gazebo_plugins/clock/src/clock.h | 28 ++++++---- .../dc_motor/src/dc_motor.cpp | 10 ++-- .../dc_motor/src/dc_motor.h | 28 +++++----- .../encoder/src/encoder.cpp | 16 +++--- .../frc_gazebo_plugins/encoder/src/encoder.h | 32 +++++++----- .../frc_gazebo_plugins/gyro/src/gyro.cpp | 16 +++--- simulation/frc_gazebo_plugins/gyro/src/gyro.h | 32 +++++++----- .../src/external_limit_switch.cpp | 4 +- .../limit_switch/src/external_limit_switch.h | 4 +- .../src/internal_limit_switch.cpp | 2 +- .../limit_switch/src/internal_limit_switch.h | 6 +-- .../limit_switch/src/limit_switch.cpp | 12 ++--- .../limit_switch/src/limit_switch.h | 24 +++++---- .../pneumatic_piston/src/pneumatic_piston.cpp | 11 ++-- .../pneumatic_piston/src/pneumatic_piston.h | 28 +++++----- .../potentiometer/src/potentiometer.cpp | 12 ++--- .../potentiometer/src/potentiometer.h | 26 ++++++---- .../rangefinder/src/rangefinder.cpp | 16 +++--- .../rangefinder/src/rangefinder.h | 26 ++++++---- .../frc_gazebo_plugins/servo/src/servo.cpp | 10 ++-- .../frc_gazebo_plugins/servo/src/servo.h | 28 +++++----- wpilibc/sim/include/DriverStation.h | 24 ++++----- wpilibc/sim/include/RobotBase.h | 2 +- wpilibc/sim/include/simulation/MainNode.h | 16 +++--- .../include/simulation/SimContinuousOutput.h | 4 +- .../sim/include/simulation/SimDigitalInput.h | 6 +-- wpilibc/sim/include/simulation/SimEncoder.h | 10 ++-- .../sim/include/simulation/SimFloatInput.h | 6 +-- wpilibc/sim/include/simulation/SimGyro.h | 10 ++-- wpilibc/sim/include/simulation/simTime.h | 2 +- wpilibc/sim/src/DriverStation.cpp | 51 +++++++++++-------- wpilibc/sim/src/Timer.cpp | 2 +- .../src/simulation/SimContinuousOutput.cpp | 4 +- .../sim/src/simulation/SimDigitalInput.cpp | 2 +- wpilibc/sim/src/simulation/SimEncoder.cpp | 10 ++-- wpilibc/sim/src/simulation/SimFloatInput.cpp | 2 +- wpilibc/sim/src/simulation/SimGyro.cpp | 10 ++-- 39 files changed, 292 insertions(+), 256 deletions(-) diff --git a/hal/lib/athena/ChipObject.h b/hal/lib/athena/ChipObject.h index 64225c880e..189c483676 100644 --- a/hal/lib/athena/ChipObject.h +++ b/hal/lib/athena/ChipObject.h @@ -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 diff --git a/simulation/frc_gazebo_plugins/clock/src/clock.cpp b/simulation/frc_gazebo_plugins/clock/src/clock.cpp index 094c206f5b..f8ded9fddd 100644 --- a/simulation/frc_gazebo_plugins/clock/src/clock.cpp +++ b/simulation/frc_gazebo_plugins/clock/src/clock.cpp @@ -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(topic); + pub = node->Advertise(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); } diff --git a/simulation/frc_gazebo_plugins/clock/src/clock.h b/simulation/frc_gazebo_plugins/clock/src/clock.h index 101a68e986..000bb0fb3e 100644 --- a/simulation/frc_gazebo_plugins/clock/src/clock.h +++ b/simulation/frc_gazebo_plugins/clock/src/clock.h @@ -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; }; diff --git a/simulation/frc_gazebo_plugins/dc_motor/src/dc_motor.cpp b/simulation/frc_gazebo_plugins/dc_motor/src/dc_motor.cpp index 9c3bd77ab4..c4bb3292d0 100644 --- a/simulation/frc_gazebo_plugins/dc_motor/src/dc_motor.cpp +++ b/simulation/frc_gazebo_plugins/dc_motor/src/dc_motor.cpp @@ -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; diff --git a/simulation/frc_gazebo_plugins/dc_motor/src/dc_motor.h b/simulation/frc_gazebo_plugins/dc_motor/src/dc_motor.h index 6c0e4d8658..6c7e100218 100644 --- a/simulation/frc_gazebo_plugins/dc_motor/src/dc_motor.h +++ b/simulation/frc_gazebo_plugins/dc_motor/src/dc_motor.h @@ -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; }; diff --git a/simulation/frc_gazebo_plugins/encoder/src/encoder.cpp b/simulation/frc_gazebo_plugins/encoder/src/encoder.cpp index a48369ce23..1c2dd0a7e0 100644 --- a/simulation/frc_gazebo_plugins/encoder/src/encoder.cpp +++ b/simulation/frc_gazebo_plugins/encoder/src/encoder.cpp @@ -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(topic + "/position"); - vel_pub = node->Advertise(topic + "/velocity"); + pos_pub = node->Advertise(topic + "/position"); + vel_pub = node->Advertise(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(); diff --git a/simulation/frc_gazebo_plugins/encoder/src/encoder.h b/simulation/frc_gazebo_plugins/encoder/src/encoder.h index c11121044a..101c270350 100644 --- a/simulation/frc_gazebo_plugins/encoder/src/encoder.h +++ b/simulation/frc_gazebo_plugins/encoder/src/encoder.h @@ -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; }; diff --git a/simulation/frc_gazebo_plugins/gyro/src/gyro.cpp b/simulation/frc_gazebo_plugins/gyro/src/gyro.cpp index 09b18cb71d..087ce8561d 100644 --- a/simulation/frc_gazebo_plugins/gyro/src/gyro.cpp +++ b/simulation/frc_gazebo_plugins/gyro/src/gyro.cpp @@ -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(topic + "/position"); - vel_pub = node->Advertise(topic + "/velocity"); + pos_pub = node->Advertise(topic + "/position"); + vel_pub = node->Advertise(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(); diff --git a/simulation/frc_gazebo_plugins/gyro/src/gyro.h b/simulation/frc_gazebo_plugins/gyro/src/gyro.h index 3064ba0d77..b6dab22c5e 100644 --- a/simulation/frc_gazebo_plugins/gyro/src/gyro.h +++ b/simulation/frc_gazebo_plugins/gyro/src/gyro.h @@ -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; }; diff --git a/simulation/frc_gazebo_plugins/limit_switch/src/external_limit_switch.cpp b/simulation/frc_gazebo_plugins/limit_switch/src/external_limit_switch.cpp index 65e3366934..b6664b3882 100644 --- a/simulation/frc_gazebo_plugins/limit_switch/src/external_limit_switch.cpp +++ b/simulation/frc_gazebo_plugins/limit_switch/src/external_limit_switch.cpp @@ -10,8 +10,8 @@ #include ExternalLimitSwitch::ExternalLimitSwitch(sdf::ElementPtr sdf) { - sensor = std::dynamic_pointer_cast( - sensors::get_sensor(sdf->Get("sensor"))); + sensor = std::dynamic_pointer_cast( + gazebo::sensors::get_sensor(sdf->Get("sensor"))); gzmsg << "\texternal limit switch: " << " sensor=" << sensor->Name() << std::endl; diff --git a/simulation/frc_gazebo_plugins/limit_switch/src/external_limit_switch.h b/simulation/frc_gazebo_plugins/limit_switch/src/external_limit_switch.h index 25c0ccbd29..072b90ec9f 100644 --- a/simulation/frc_gazebo_plugins/limit_switch/src/external_limit_switch.h +++ b/simulation/frc_gazebo_plugins/limit_switch/src/external_limit_switch.h @@ -17,8 +17,6 @@ #include #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; }; diff --git a/simulation/frc_gazebo_plugins/limit_switch/src/internal_limit_switch.cpp b/simulation/frc_gazebo_plugins/limit_switch/src/internal_limit_switch.cpp index 198003de1e..b7f9b05dae 100644 --- a/simulation/frc_gazebo_plugins/limit_switch/src/internal_limit_switch.cpp +++ b/simulation/frc_gazebo_plugins/limit_switch/src/internal_limit_switch.cpp @@ -9,7 +9,7 @@ #include -InternalLimitSwitch::InternalLimitSwitch(physics::ModelPtr model, +InternalLimitSwitch::InternalLimitSwitch(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) { joint = model->GetJoint(sdf->Get("joint")); min = sdf->Get("min"); diff --git a/simulation/frc_gazebo_plugins/limit_switch/src/internal_limit_switch.h b/simulation/frc_gazebo_plugins/limit_switch/src/internal_limit_switch.h index a19e930c60..0e97cca173 100644 --- a/simulation/frc_gazebo_plugins/limit_switch/src/internal_limit_switch.h +++ b/simulation/frc_gazebo_plugins/limit_switch/src/internal_limit_switch.h @@ -16,17 +16,15 @@ #include #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; }; diff --git a/simulation/frc_gazebo_plugins/limit_switch/src/limit_switch.cpp b/simulation/frc_gazebo_plugins/limit_switch/src/limit_switch.cpp index 4af287e28c..c6587082b4 100644 --- a/simulation/frc_gazebo_plugins/limit_switch/src/limit_switch.cpp +++ b/simulation/frc_gazebo_plugins/limit_switch/src/limit_switch.cpp @@ -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(topic); + pub = node->Advertise(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); } diff --git a/simulation/frc_gazebo_plugins/limit_switch/src/limit_switch.h b/simulation/frc_gazebo_plugins/limit_switch/src/limit_switch.h index a2ecc6a0b2..c69b5fa1b3 100644 --- a/simulation/frc_gazebo_plugins/limit_switch/src/limit_switch.h +++ b/simulation/frc_gazebo_plugins/limit_switch/src/limit_switch.h @@ -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; }; diff --git a/simulation/frc_gazebo_plugins/pneumatic_piston/src/pneumatic_piston.cpp b/simulation/frc_gazebo_plugins/pneumatic_piston/src/pneumatic_piston.cpp index c520d95fcc..6ee467f2da 100644 --- a/simulation/frc_gazebo_plugins/pneumatic_piston/src/pneumatic_piston.cpp +++ b/simulation/frc_gazebo_plugins/pneumatic_piston/src/pneumatic_piston.cpp @@ -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) { diff --git a/simulation/frc_gazebo_plugins/pneumatic_piston/src/pneumatic_piston.h b/simulation/frc_gazebo_plugins/pneumatic_piston/src/pneumatic_piston.h index 7de33e439c..36c7419bea 100644 --- a/simulation/frc_gazebo_plugins/pneumatic_piston/src/pneumatic_piston.h +++ b/simulation/frc_gazebo_plugins/pneumatic_piston/src/pneumatic_piston.h @@ -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; }; diff --git a/simulation/frc_gazebo_plugins/potentiometer/src/potentiometer.cpp b/simulation/frc_gazebo_plugins/potentiometer/src/potentiometer.cpp index c59a3e2a9b..449618e47e 100644 --- a/simulation/frc_gazebo_plugins/potentiometer/src/potentiometer.cpp +++ b/simulation/frc_gazebo_plugins/potentiometer/src/potentiometer.cpp @@ -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(topic); + pub = node->Advertise(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 { diff --git a/simulation/frc_gazebo_plugins/potentiometer/src/potentiometer.h b/simulation/frc_gazebo_plugins/potentiometer/src/potentiometer.h index db43a2b054..546276f483 100644 --- a/simulation/frc_gazebo_plugins/potentiometer/src/potentiometer.h +++ b/simulation/frc_gazebo_plugins/potentiometer/src/potentiometer.h @@ -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; }; diff --git a/simulation/frc_gazebo_plugins/rangefinder/src/rangefinder.cpp b/simulation/frc_gazebo_plugins/rangefinder/src/rangefinder.cpp index 7be903566d..4a58770bf7 100644 --- a/simulation/frc_gazebo_plugins/rangefinder/src/rangefinder.cpp +++ b/simulation/frc_gazebo_plugins/rangefinder/src/rangefinder.cpp @@ -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::get_sensor(sdf->Get("sensor"))); + sensor = std::dynamic_pointer_cast( + gazebo::sensors::get_sensor(sdf->Get("sensor"))); if (sdf->HasElement("topic")) { topic = sdf->Get("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(topic); + pub = node->Advertise(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); } diff --git a/simulation/frc_gazebo_plugins/rangefinder/src/rangefinder.h b/simulation/frc_gazebo_plugins/rangefinder/src/rangefinder.h index 673febceec..82be41f543 100644 --- a/simulation/frc_gazebo_plugins/rangefinder/src/rangefinder.h +++ b/simulation/frc_gazebo_plugins/rangefinder/src/rangefinder.h @@ -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; }; diff --git a/simulation/frc_gazebo_plugins/servo/src/servo.cpp b/simulation/frc_gazebo_plugins/servo/src/servo.cpp index 1e0984edb1..5a29378259 100644 --- a/simulation/frc_gazebo_plugins/servo/src/servo.cpp +++ b/simulation/frc_gazebo_plugins/servo/src/servo.cpp @@ -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; diff --git a/simulation/frc_gazebo_plugins/servo/src/servo.h b/simulation/frc_gazebo_plugins/servo/src/servo.h index 0f47bd84e9..7cf0ce0883 100644 --- a/simulation/frc_gazebo_plugins/servo/src/servo.h +++ b/simulation/frc_gazebo_plugins/servo/src/servo.h @@ -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; }; diff --git a/wpilibc/sim/include/DriverStation.h b/wpilibc/sim/include/DriverStation.h index a8799413fa..32e891b719 100644 --- a/wpilibc/sim/include/DriverStation.h +++ b/wpilibc/sim/include/DriverStation.h @@ -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 diff --git a/wpilibc/sim/include/RobotBase.h b/wpilibc/sim/include/RobotBase.h index 3b22fc9e06..73d564f62d 100644 --- a/wpilibc/sim/include/RobotBase.h +++ b/wpilibc/sim/include/RobotBase.h @@ -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 diff --git a/wpilibc/sim/include/simulation/MainNode.h b/wpilibc/sim/include/simulation/MainNode.h index 574bd5da58..9cef08b018 100644 --- a/wpilibc/sim/include/simulation/MainNode.h +++ b/wpilibc/sim/include/simulation/MainNode.h @@ -14,8 +14,6 @@ #include "simulation/gz_msgs/msgs.h" -using namespace gazebo; - namespace frc { class MainNode { @@ -26,14 +24,14 @@ class MainNode { } template - 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(topic, queueLimit, latch); } template - static transport::SubscriberPtr Subscribe( + static gazebo::transport::SubscriberPtr Subscribe( const std::string& topic, void (T::*fp)(const boost::shared_ptr&), T* obj, bool latching = false) { @@ -41,20 +39,20 @@ class MainNode { } template - static transport::SubscriberPtr Subscribe( + static gazebo::transport::SubscriberPtr Subscribe( const std::string& topic, void (*fp)(const boost::shared_ptr&), 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 { diff --git a/wpilibc/sim/include/simulation/SimContinuousOutput.h b/wpilibc/sim/include/simulation/SimContinuousOutput.h index 67e6f22ccb..0b287a749c 100644 --- a/wpilibc/sim/include/simulation/SimContinuousOutput.h +++ b/wpilibc/sim/include/simulation/SimContinuousOutput.h @@ -19,13 +19,11 @@ #include #endif -using namespace gazebo; - namespace frc { class SimContinuousOutput { private: - transport::PublisherPtr pub; + gazebo::transport::PublisherPtr pub; float speed; public: diff --git a/wpilibc/sim/include/simulation/SimDigitalInput.h b/wpilibc/sim/include/simulation/SimDigitalInput.h index ad8c4b1c2b..9e29711413 100644 --- a/wpilibc/sim/include/simulation/SimDigitalInput.h +++ b/wpilibc/sim/include/simulation/SimDigitalInput.h @@ -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 diff --git a/wpilibc/sim/include/simulation/SimEncoder.h b/wpilibc/sim/include/simulation/SimEncoder.h index f5608b99a8..afbd330661 100644 --- a/wpilibc/sim/include/simulation/SimEncoder.h +++ b/wpilibc/sim/include/simulation/SimEncoder.h @@ -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 diff --git a/wpilibc/sim/include/simulation/SimFloatInput.h b/wpilibc/sim/include/simulation/SimFloatInput.h index 7b06c2dc5d..a58062e289 100644 --- a/wpilibc/sim/include/simulation/SimFloatInput.h +++ b/wpilibc/sim/include/simulation/SimFloatInput.h @@ -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 diff --git a/wpilibc/sim/include/simulation/SimGyro.h b/wpilibc/sim/include/simulation/SimGyro.h index f9b2bc997b..dac84e1779 100644 --- a/wpilibc/sim/include/simulation/SimGyro.h +++ b/wpilibc/sim/include/simulation/SimGyro.h @@ -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 diff --git a/wpilibc/sim/include/simulation/simTime.h b/wpilibc/sim/include/simulation/simTime.h index 8e31d55e03..4cb95e05ee 100644 --- a/wpilibc/sim/include/simulation/simTime.h +++ b/wpilibc/sim/include/simulation/simTime.h @@ -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); } } diff --git a/wpilibc/sim/src/DriverStation.cpp b/wpilibc/sim/src/DriverStation.cpp index 957ac4fe5a..ff7811b0be 100644 --- a/wpilibc/sim/src/DriverStation.cpp +++ b/wpilibc/sim/src/DriverStation.cpp @@ -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 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 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 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 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 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); } diff --git a/wpilibc/sim/src/Timer.cpp b/wpilibc/sim/src/Timer.cpp index bba241c0ab..f396a6b54f 100644 --- a/wpilibc/sim/src/Timer.cpp +++ b/wpilibc/sim/src/Timer.cpp @@ -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(); } diff --git a/wpilibc/sim/src/simulation/SimContinuousOutput.cpp b/wpilibc/sim/src/simulation/SimContinuousOutput.cpp index 8a19cc0715..225dea6955 100644 --- a/wpilibc/sim/src/simulation/SimContinuousOutput.cpp +++ b/wpilibc/sim/src/simulation/SimContinuousOutput.cpp @@ -12,12 +12,12 @@ using namespace frc; SimContinuousOutput::SimContinuousOutput(std::string topic) { - pub = MainNode::Advertise("~/simulator/" + topic); + pub = MainNode::Advertise("~/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); } diff --git a/wpilibc/sim/src/simulation/SimDigitalInput.cpp b/wpilibc/sim/src/simulation/SimDigitalInput.cpp index 07fb2def3f..78ea34b151 100644 --- a/wpilibc/sim/src/simulation/SimDigitalInput.cpp +++ b/wpilibc/sim/src/simulation/SimDigitalInput.cpp @@ -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(); } diff --git a/wpilibc/sim/src/simulation/SimEncoder.cpp b/wpilibc/sim/src/simulation/SimEncoder.cpp index fa78179f38..2fbf88e134 100644 --- a/wpilibc/sim/src/simulation/SimEncoder.cpp +++ b/wpilibc/sim/src/simulation/SimEncoder.cpp @@ -12,8 +12,8 @@ using namespace frc; SimEncoder::SimEncoder(std::string topic) { - commandPub = - MainNode::Advertise("~/simulator/" + topic + "/control"); + commandPub = MainNode::Advertise("~/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(); } diff --git a/wpilibc/sim/src/simulation/SimFloatInput.cpp b/wpilibc/sim/src/simulation/SimFloatInput.cpp index 78e1785806..abc38e3e39 100644 --- a/wpilibc/sim/src/simulation/SimFloatInput.cpp +++ b/wpilibc/sim/src/simulation/SimFloatInput.cpp @@ -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(); } diff --git a/wpilibc/sim/src/simulation/SimGyro.cpp b/wpilibc/sim/src/simulation/SimGyro.cpp index 0a1275b182..21c16f3193 100644 --- a/wpilibc/sim/src/simulation/SimGyro.cpp +++ b/wpilibc/sim/src/simulation/SimGyro.cpp @@ -12,8 +12,8 @@ using namespace frc; SimGyro::SimGyro(std::string topic) { - commandPub = - MainNode::Advertise("~/simulator/" + topic + "/control"); + commandPub = MainNode::Advertise("~/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(); }