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

Bring back the gazebo plugins (#1063)

Browse Source 
The models and meshes are not included. We will need
to find an alternate way to reintegrate these and use them.

* Add simulation/gz_msgs back, and build with Gradle.

* Add back in the frc simulation plugins for gazebo.

* Add a new shared library, halsim_gazebo.

This library will become the interface between the
HAL sim layer and gazebo.

* Preserve the first channel number used in created Encoders in the Sim MockData.

This allows us to use the DIO channel number to connect with simulated encoders.

* Have the HAL Simulator set the reverse direction on creation.

This enables a simulator to be aware of the direction.

* Add a drive_motor plugin.

This is a bit of a 'magic' motor, which allows us to build robot
models that drive in a more realistic fashion.  It does this
by apply forces directly to the chassis, rather than relying on
the complex motion dynamics of a driven wheel.

This in turn allows the model to reduce wheel friction,
reducing scrub, and allowing for a more natural driving experience.
This commit is contained in:
Jeremy White
2018-06-30 02:45:21 -05:00
committed by Peter Johnson
parent 70960b0251
commit ebd41fe0bb
60 changed files with 5278 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@@ -202,7 +202,6 @@ ipch/
*.ncb
*.opendb
*.opensdf
*.sdf
*.cachefile
*.VC.db
*.VC.VC.opendb

2
.styleguide
View File

@@ -13,6 +13,8 @@ generatedFileExclude {
ni-libraries/include/
ni-libraries/lib/
FRCNetComm\.java$
simulation/frc_gazebo_plugins/frcgazebo/
simulation/gz_msgs/src/include/simulation/gz_msgs/msgs\.h$
}
modifiableFileExclude {

1
hal/src/main/native/include/MockData/EncoderData.h
View File

@@ -17,6 +17,7 @@ extern "C" {
#endif
void HALSIM_ResetEncoderData(int32_t index);
int16_t HALSIM_GetDigitalChannelA(int32_t index);
int32_t HALSIM_RegisterEncoderInitializedCallback(int32_t index,
HAL_NotifyCallback callback,
void* param,

3
hal/src/main/native/sim/Encoder.cpp
View File

@@ -81,7 +81,10 @@ HAL_EncoderHandle HAL_InitializeEncoder(
return HAL_kInvalidHandle;
}
int16_t index = getHandleIndex(handle);
SimEncoderData[index].SetDigitalChannelA(
getHandleIndex(digitalSourceHandleA));
SimEncoderData[index].SetInitialized(true);
SimEncoderData[index].SetReverseDirection(reverseDirection);
// TODO: Add encoding type to Sim data
encoder->index = index;
encoder->nativeHandle = nativeHandle;

11
hal/src/main/native/sim/MockData/EncoderData.cpp
View File

@@ -22,6 +22,7 @@ void InitializeEncoderData() {
EncoderData* hal::SimEncoderData;
void EncoderData::ResetData() {
m_digitalChannelA = 0;
m_initialized = false;
m_initializedCallbacks = nullptr;
m_count = 0;
@@ -42,6 +43,12 @@ void EncoderData::ResetData() {
m_distancePerPulseCallbacks = nullptr;
}
void EncoderData::SetDigitalChannelA(int16_t channel) {
m_digitalChannelA = channel;
}
int16_t EncoderData::GetDigitalChannelA() const { return m_digitalChannelA; }
int32_t EncoderData::RegisterInitializedCallback(HAL_NotifyCallback callback,
void* param,
HAL_Bool initialNotify) {
@@ -373,6 +380,10 @@ void HALSIM_ResetEncoderData(int32_t index) {
SimEncoderData[index].ResetData();
}
int16_t HALSIM_GetDigitalChannelA(int32_t index) {
return SimEncoderData[index].GetDigitalChannelA();
}
int32_t HALSIM_RegisterEncoderInitializedCallback(int32_t index,
HAL_NotifyCallback callback,
void* param,

4
hal/src/main/native/sim/MockData/EncoderDataInternal.h
View File

@@ -19,6 +19,9 @@
namespace hal {
class EncoderData {
public:
void SetDigitalChannelA(int16_t channel);
int16_t GetDigitalChannelA() const;
int32_t RegisterInitializedCallback(HAL_NotifyCallback callback, void* param,
HAL_Bool initialNotify);
void CancelInitializedCallback(int32_t uid);
@@ -86,6 +89,7 @@ class EncoderData {
private:
wpi::mutex m_registerMutex;
std::atomic<int16_t> m_digitalChannelA{0};
std::atomic<HAL_Bool> m_initialized{false};
std::shared_ptr<NotifyListenerVector> m_initializedCallbacks = nullptr;
std::atomic<int32_t> m_count{0};

3
settings.gradle
View File

@@ -21,5 +21,8 @@ include 'simulation:halsim_print'
include 'simulation:halsim_lowfi'
include 'simulation:halsim_adx_gyro_accelerometer'
include 'simulation:halsim_ds_nt'
include 'simulation:gz_msgs'
include 'simulation:frc_gazebo_plugins'
include 'simulation:halsim_gazebo'
include 'cameraserver'
include 'myRobot'

45
simulation/frc_gazebo_plugins/README.md Normal file
View File

@@ -0,0 +1,45 @@
Gazebo plugins for FRC
======================
These plugins enable the use of the Gazebo physics simulator to model robots,
and then use WPILIB programming libraries to control those simulated robots.
At the present time, this only builds on Linux, and has a fairly complex
installation process.
You must have the gazebo development packages available on Linux.
The gradle build system will automatically skip building this directory
if those requirements are not met.
This command:
`./gradlew build -PmakeSim`
will force it to attempt to build.
TODO: The meshes for the models are large, and are not hosted in
the main allwpilib repository. An alternate method of hosting
and using is being built at the time of this commit. You must
manually locate and copy the the models and world from that
either into /usr/share/frcgazebo or into build/share/frcgazebo
in order to use this facility.
Once you have built it, then a command like this:
`simulation/frc_gazebo_plugins/build/bin/frcgazebo PacGoat2014.world`
should run the Gazebo simulation using our plugins against the
2014 model, with the PacGoat robot on the field.
The halsim_gazebo simulation library will provide a method for
running robot code to control a simulated robot.
TODO: Package the plugins more nicely, and then put instructions here.
TODO: Build and package the plugins for Windows. The key there is to get gazebo
working on Windows. This link was a useful trove of information on Windows/Gazebo
issues as of September, 2017:
https://bitbucket.org/osrf/gazebo/issues/2129/visual-studio-2015-compatibility
Note that the sense remains that this will be difficult at best.

113
simulation/frc_gazebo_plugins/build.gradle Normal file
View File

@@ -0,0 +1,113 @@
description = "A set of C++ plugins to interface the FRC Simulator with Gazebo."
apply plugin: 'edu.wpi.first.NativeUtils'
apply plugin: 'cpp'
apply plugin: "google-test"
ext.skipAthena = true
/* If gz_msgs or gazebo is not available, do not attempt a build */
def gazebo_version = ""
def gazebo_cppflags = ""
def gazebo_linker_args = ""
try {
gazebo_version = "pkg-config --modversion gazebo".execute().text.trim()
println "Gazebo version is [${gazebo_version}]"
gazebo_cppflags = "pkg-config --cflags gazebo".execute().text.split()
gazebo_linker_args = "pkg-config --libs gazebo protobuf".execute().text.split()
} catch(Exception ex) { }
if (!gazebo_version?.trim()) {
println "Gazebo development files are not available. (pkg-config --modversion gazebo failed)"
if (project.hasProperty("makeSim")) {
/* Force the build even though we did not find protobuf. */
println "makeSim set. Forcing build - failure likely."
}
else {
ext.skip_frc_plugins = true
println "Skipping FRC Plugins."
}
}
evaluationDependsOn(":simulation:gz_msgs")
def gz_msgs_project = project(":simulation:gz_msgs")
tasks.whenTaskAdded { task ->
task.onlyIf { !gz_msgs_project.hasProperty('skip_gz_msgs') && !project.hasProperty('skip_frc_plugins') }
}
model {
components {
clock(NativeLibrarySpec)
dc_motor(NativeLibrarySpec)
encoder(NativeLibrarySpec)
gyro(NativeLibrarySpec)
limit_switch(NativeLibrarySpec)
potentiometer(NativeLibrarySpec)
pneumatic_piston(NativeLibrarySpec)
rangefinder(NativeLibrarySpec)
servo(NativeLibrarySpec)
drive_motor(NativeLibrarySpec)
all { component ->
sources {
cpp.lib library: "${component.name}", linkage: "static"
}
}
}
/* TODO: Finish writing the test case */
/* We pass the name of the .so and a .world file to each test */
testSuites {
all { test->
def library_file
testedComponent.binaries.withType(SharedLibraryBinarySpec).each { b->
library_file = b.sharedLibraryFile
}
tasks.withType(RunTestExecutable) {
args library_file, file("src/${baseName}/world/${baseName}.world")
}
}
}
binaries {
all {
linker.args gazebo_linker_args
cppCompiler.args gazebo_cppflags
lib project: ":simulation:gz_msgs", library: "gz_msgs", linkage: "static"
}
/* TODO: build only shared object? Figure out why this doesn't work? */
withType(StaticLibraryBinarySpec) {
buildable = false
}
withType(GoogleTestTestSuiteBinarySpec) {
/* We currently only have a test for the clock plugin */
/* TODO: learn how to add this back to gmock */
//if (it.projectScopedName.contains("clockTest")) {
// buildable = true
// project(':gmock').addGmockToLinker(it)
//}
//else {
buildable = false
//}
}
}
}
task copyScript(type: Copy, group: "FRC Gazebo", description: "Copy the frcgazebo script to the output directory.") {
from "scripts"
into "$project.buildDir/bin"
fileMode 0755
}
build.dependsOn copyScript
/* TODO: Publish this library */

2303
simulation/frc_gazebo_plugins/frc_gazebo_plugins.doxy Normal file
View File

File diff suppressed because it is too large Load Diff

63
simulation/frc_gazebo_plugins/scripts/frcgazebo Normal file
View File

@@ -0,0 +1,63 @@
#!/bin/bash
#---------------------------------------------------------------------------
# Copyright (c) 2018 FIRST. All Rights Reserved.
# Open Source Software - may be modified and shared by FRC teams. The code
# must be accompanied by the FIRST BSD license file in the root directory of
# the project.
#----------------------------------------------------------------------------
#----------------------------------------------------------------------------
# Invoke gazebo, giving it the environment variables
# it needs to find the FRC plugins and models.
#----------------------------------------------------------------------------
usage() {
echo $1: Invoke Gazebo for FRC
echo Usage:
echo " $1 name-of-world-file"
}
d=`dirname "$0"`
fulldir=`(cd "$d"; pwd)`
sharedir=/usr/share/frcgazebo
if [ -d "$fulldir/../share" ] ; then
sharedir=`(cd "$fulldir/../share/frcgazebo"; pwd)`
fi
# While we wait for the frc gazebo models to have a proper
# home, we require the user to make them accessible
if [ ! -d "$sharedir" ] ; then
cravedir=`(cd "$fulldir/../"; pwd)`
echo Error: you must manually place the models and world into $cravedir/share/frcgazebo
exit 2
fi
libsdir=/invalid/directory
if [ -d "$fulldir/../libs" ] ; then
libsdir=`(cd "$fulldir/../libs"; pwd)`
fi
# Use exactly the file they gave us, or find it in ../share/frcgazebo/worlds,
# or find it there by adding a .world extension
world="$1"
if [ ! -f "$world" ] ; then
world="$sharedir/worlds/$1"
fi
if [ ! -f "$world" ] ; then
world="$sharedir/worlds/$1.world"
fi
if [ $# -ne 1 -o "x${1:0:1}" = "x-" ] ; then
usage `basename $0`
exit 1
fi
if [ ! -f "$world" ] ; then
echo Could not find $1 directly or in $sharedir/worlds
exit 2
fi
export GAZEBO_MODEL_PATH="$sharedir/models":$GAZEBO_MODEL_PATH
for x in `find "$libsdir" -type d -name shared` ; do
export GAZEBO_PLUGIN_PATH="$x:$GAZEBO_PLUGIN_PATH"
done
gazebo "$world"

44
simulation/frc_gazebo_plugins/src/clock/cpp/clock.cpp Normal file
View File

@@ -0,0 +1,44 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "clock.h"
#include <boost/algorithm/string/replace.hpp>
GZ_REGISTER_MODEL_PLUGIN(Clock)
void Clock::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) {
this->model = model;
// Parse SDF properties
if (sdf->HasElement("topic")) {
topic = sdf->Get<std::string>("topic");
} else {
topic = "~/" + sdf->GetAttribute("name")->GetAsString();
}
gzmsg << "Initializing clock: " << topic << std::endl;
// Connect to Gazebo transport for messaging
std::string scoped_name =
model->GetWorld()->GetName() + "::" + model->GetScopedName();
boost::replace_all(scoped_name, "::", "/");
node = gazebo::transport::NodePtr(new gazebo::transport::Node());
node->Init(scoped_name);
pub = node->Advertise<gazebo::msgs::Float64>(topic);
// Connect to the world update event.
// This will trigger the Update function every Gazebo iteration
updateConn = gazebo::event::Events::ConnectWorldUpdateBegin(
boost::bind(&Clock::Update, this, _1));
}
void Clock::Update(const gazebo::common::UpdateInfo& info) {
gazebo::msgs::Float64 msg;
msg.set_data(info.simTime.Double());
pub->Publish(msg);
}

58
simulation/frc_gazebo_plugins/src/clock/headers/clock.h Normal file
View File

@@ -0,0 +1,58 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <string>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/transport/transport.hh>
#include "simulation/gz_msgs/msgs.h"
/**
* \brief Plugin for publishing the simulation time.
*
* This plugin publishes the simualtaion time in seconds every physics
* update.
*
* To add a clock to your robot, add the following XML to your robot
* model:
*
* <plugin name="my_clock" filename="libclock.so">
* <topic>~/my/topic</topic>
* </plugin>
*
* - `topic`: Optional. Message will be published as a gazebo.msgs.Float64.
*
* \todo Make WorldPlugin?
*/
class Clock : public gazebo::ModelPlugin {
public:
/// \brief Load the clock and configures it according to the sdf.
void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf);
/// \brief Sends out time each timestep.
void Update(const gazebo::common::UpdateInfo& info);
private:
/// \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;
};

75
simulation/frc_gazebo_plugins/src/clockTest/cpp/clockTest.cpp Normal file
View File

@@ -0,0 +1,75 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <simulation/gz_msgs/msgs.h>
#include <gazebo/gazebo.hh>
#include <gazebo/gazebo_client.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/sensors/sensors.hh>
#include <gazebo/transport/transport.hh>
#include "gtest/gtest.h"
using namespace testing;
static char* library;
static char* world_sdf;
static double latest_time;
void cb(gazebo::msgs::ConstFloat64Ptr& msg) { latest_time = msg->data(); }
TEST(ClockTests, test_clock) {
gazebo::physics::WorldPtr world;
ASSERT_TRUE(library);
ASSERT_TRUE(!access(library, X_OK | R_OK));
ASSERT_TRUE(world_sdf);
ASSERT_TRUE(!access(world_sdf, R_OK));
gazebo::addPlugin(library);
ASSERT_TRUE(gazebo::setupServer());
world = gazebo::loadWorld(world_sdf);
ASSERT_TRUE(world != NULL);
ASSERT_TRUE(gazebo::client::setup());
gazebo::transport::NodePtr node(new gazebo::transport::Node());
node->Init();
gazebo::transport::SubscriberPtr sub =
node->Subscribe("/gazebo/frc/time", cb);
gazebo::sensors::run_once(true);
gazebo::sensors::run_threads();
gazebo::common::Time::MSleep(1);
double start = latest_time;
for (unsigned int i = 0; i < 1000; ++i) {
gazebo::runWorld(world, 1);
gazebo::sensors::run_once();
gazebo::common::Time::MSleep(1);
}
double end = latest_time;
ASSERT_TRUE(end > start);
ASSERT_TRUE(gazebo::client::shutdown());
ASSERT_TRUE(gazebo::shutdown());
}
int main(int argc, char** argv) {
testing::InitGoogleTest(&argc, argv);
if (argc >= 1) library = argv[1];
if (argc >= 2) world_sdf = argv[2];
return RUN_ALL_TESTS();
}

17
simulation/frc_gazebo_plugins/src/clockTest/world/clockTest.world Normal file
View File

@@ -0,0 +1,17 @@
<?xml version="1.0" ?>
<sdf version="1.5">
<world name="default">
<scene>
<ambient>0.2 0.2 0.2 1</ambient>
<background>1 1 1 1</background>
<shadows>false</shadows>
<grid>false</grid>
<origin_visual>false</origin_visual>
</scene>
<model name='Clock'>
<plugin name='clock' filename='libclock.so'>
<topic>/gazebo/frc/time</topic>
</plugin>
</model>
</world>
</sdf>

60
simulation/frc_gazebo_plugins/src/dc_motor/cpp/dc_motor.cpp Normal file
View File

@@ -0,0 +1,60 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "dc_motor.h"
#include <boost/algorithm/string/replace.hpp>
GZ_REGISTER_MODEL_PLUGIN(DCMotor)
void DCMotor::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) {
this->model = model;
signal = 0;
// Parse SDF properties
joint = model->GetJoint(sdf->Get<std::string>("joint"));
if (sdf->HasElement("topic")) {
topic = sdf->Get<std::string>("topic");
} else {
topic = "~/" + sdf->GetAttribute("name")->GetAsString();
}
if (sdf->HasElement("multiplier")) {
multiplier = sdf->Get<double>("multiplier");
} else {
multiplier = 1;
}
gzmsg << "Initializing motor: " << topic << " joint=" << joint->GetName()
<< " multiplier=" << multiplier << std::endl;
// Connect to Gazebo transport for messaging
std::string scoped_name =
model->GetWorld()->GetName() + "::" + model->GetScopedName();
boost::replace_all(scoped_name, "::", "/");
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 = gazebo::event::Events::ConnectWorldUpdateBegin(
boost::bind(&DCMotor::Update, this, _1));
}
void DCMotor::Update(const gazebo::common::UpdateInfo& info) {
joint->SetForce(0, signal * multiplier);
}
void DCMotor::Callback(const gazebo::msgs::ConstFloat64Ptr& msg) {
signal = msg->data();
if (signal < -1) {
signal = -1;
} else if (signal > 1) {
signal = 1;
}
}

73
simulation/frc_gazebo_plugins/src/dc_motor/headers/dc_motor.h Normal file
View File

@@ -0,0 +1,73 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <string>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/transport/transport.hh>
#include "simulation/gz_msgs/msgs.h"
/**
* \brief Plugin for controlling a joint with a DC motor.
*
* This plugin subscribes to a topic to get a signal in the range
* [-1,1]. Every physics update the joint's torque is set as
* multiplier*signal.
*
* To add a DC motor to your robot, add the following XML to your
* robot model:
*
* <plugin name="my_motor" filename="libdc_motor.so">
* <joint>Joint Name</joint>
* <topic>~/my/topic</topic>
* <multiplier>Number</multiplier>
* </plugin>
*
* - `joint`: Name of the joint this Dc motor is attached to.
* - `topic`: Optional. Message type should be gazebo.msgs.Float64.
* - `multiplier`: Optional. Defaults to 1.
*/
class DCMotor : public gazebo::ModelPlugin {
public:
/// \brief Load the dc motor and configures it according to the 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 gazebo::common::UpdateInfo& info);
private:
/// \brief Topic to read control signal from.
std::string topic;
/// \brief The pwm signal limited to the range [-1,1].
double signal;
/// \brief The magic torque multiplier. torque=multiplier*signal
double multiplier;
/// \brief The joint that this dc motor drives.
gazebo::physics::JointPtr joint;
/// \brief Callback for receiving msgs and storing the signal.
void Callback(const gazebo::msgs::ConstFloat64Ptr& msg);
/// \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;
};

134
simulation/frc_gazebo_plugins/src/drive_motor/cpp/drive_motor.cpp Normal file
View File

@@ -0,0 +1,134 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "drive_motor.h"
#include <boost/algorithm/string/replace.hpp>
GZ_REGISTER_MODEL_PLUGIN(DriveMotor)
void DriveMotor::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) {
this->model = model;
signal = 0;
// Parse SDF properties
joint = model->GetJoint(sdf->Get<std::string>("joint"));
if (!joint) {
gzerr << "Error initializing drive motor: could not get joint";
return;
}
parent = joint->GetParent();
if (!parent) {
gzerr << "Error initializing drive motor: could not get parent";
return;
}
child = joint->GetChild();
if (!child) {
gzerr << "Error initializing drive motor: could not get child";
return;
}
if (sdf->HasElement("topic")) {
topic = sdf->Get<std::string>("topic");
} else {
topic = "~/" + sdf->GetAttribute("name")->GetAsString();
}
if (sdf->HasElement("max_speed")) {
maxSpeed = sdf->Get<double>("max_speed");
} else {
maxSpeed = 0;
}
if (sdf->HasElement("multiplier")) {
multiplier = sdf->Get<double>("multiplier");
} else {
multiplier = 1;
}
if (sdf->HasElement("dx")) {
dx = sdf->Get<double>("dx");
} else {
dx = 0;
}
if (sdf->HasElement("dy")) {
dy = sdf->Get<double>("dy");
} else {
dy = 0;
}
if (sdf->HasElement("dz")) {
dz = sdf->Get<double>("dz");
} else {
dz = 0;
}
gzmsg << "Initializing drive motor: " << topic
<< " parent=" << parent->GetName() << " directions=" << dx << " " << dy
<< " " << dz << " multiplier=" << multiplier << std::endl;
// Connect to Gazebo transport for messaging
std::string scoped_name =
model->GetWorld()->GetName() + "::" + model->GetScopedName();
boost::replace_all(scoped_name, "::", "/");
node = gazebo::transport::NodePtr(new gazebo::transport::Node());
node->Init(scoped_name);
sub = node->Subscribe(topic, &DriveMotor::Callback, this);
// Connect to the world update event.
// This will trigger the Update function every Gazebo iteration
updateConn = gazebo::event::Events::ConnectWorldUpdateBegin(
boost::bind(&DriveMotor::Update, this, _1));
}
static double computeForce(double input, double velocity, double max) {
double output = input;
if (max == 0.0) return output;
if (std::fabs(velocity) >= max) {
output = 0;
} else {
double reduce = (max - std::fabs(velocity)) / max;
output *= reduce;
}
return output;
}
void DriveMotor::Update(const gazebo::common::UpdateInfo& info) {
#if GAZEBO_MAJOR_VERSION >= 8
ignition::math::Vector3d velocity = parent->RelativeLinearVel();
#else
ignition::math::Vector3d velocity = parent->GetRelativeLinearVel().Ign();
#endif
if (signal == 0) return;
double x = computeForce(signal * dx * multiplier, velocity.X(),
std::fabs(maxSpeed * dx));
double y = computeForce(signal * dy * multiplier, velocity.Y(),
std::fabs(maxSpeed * dy));
double z = computeForce(signal * dz * multiplier, velocity.Z(),
std::fabs(maxSpeed * dz));
ignition::math::Vector3d force(x, y, z);
#if GAZEBO_MAJOR_VERSION >= 8
parent->AddLinkForce(force, child->RelativePose().Pos());
#else
parent->AddLinkForce(force, child->GetRelativePose().Ign().Pos());
#endif
}
void DriveMotor::Callback(const gazebo::msgs::ConstFloat64Ptr& msg) {
signal = msg->data();
if (signal < -1) {
signal = -1;
} else if (signal > 1) {
signal = 1;
}
}

126
simulation/frc_gazebo_plugins/src/drive_motor/headers/drive_motor.h Normal file
View File

@@ -0,0 +1,126 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <string>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/transport/transport.hh>
#include "simulation/gz_msgs/msgs.h"
/**
* \brief Plugin for simulating a drive motor
*
* This plugin attempts to overcome a limitation in gazebo.
* That is, most normal FRC robots rely on wheels that have good
* traction in one direction, and less traction in the opposite
* direction.
*
* Gazebo does not model that well (in fact, drive wheels are
* quite hard to simulate).
*
* So this plugin subscribes to a PWM output signal and applies
* a force to the chassis at the proscribed point in hopefully
* the correct direction. The SDF model can then have lower friction,
* and it should turn more naturally.
*
* This plugin also attempts to simulate the limitations of a drive
* motor, most notably the maximum speed any given motor can spin at.
* The initial implemention is quite naive; just a linear reduction
* in force as a product of velocity/max velocity.
*
* Nicely, this plugin let's you generate a force in any of
* three axes. That is helpful for simulating a mecanum drive.
*
* This plugin subscribes to a topic to get a signal in the range
* [-1,1]. Every physics update the joint's torque is set as
* multiplier*signal*direction.
*
* To add a drive motor to your robot, add the following XML to your
* robot model:
*
* <plugin name="my_motor" filename="libdrive_motor.so">
* <joint>Joint Name</joint>
* <topic>~/my/topic</topic>
* <multiplier>Number</multiplier>
* <max_speed>Number</max_speed>
* <dx>-1, 0, or 1</dx>
* <dy>-1, 0, or 1</dy>
* <dz>-1, 0, or 1</dz>
* </plugin>
*
* - `joint`: Name of the joint this Dc motor is attached to.
* - `topic`: Optional. Message type should be gazebo.msgs.Float64.
* A typical topic looks like this:
* /gazebo/frc/simulator/pwm/<n>
* - `multiplier`: Optional. Defaults to 1. Force applied by this motor.
* This is force in Newtons.
* - `max_speed`: Optional. Defaults to no maximum.
* This is, in theory, meters/second. Note that friction
* and other forces will also slow down a robot.
* In practice, this term can be tuned until the robot feels right.
* - `dx`: These three constants must be set to either -1, 0, or 1
* - `dy`: This controls whether or not the motor produces force
* - `dz`: along a given axis, and what direction. Each defaults to 0.
* These are relative to the frame of the parent link of the joint.
* So they are usually relative to a chassis.
* The force is applied at the point that the joint connects to
* the parent link.
*/
class DriveMotor : public gazebo::ModelPlugin {
public:
/// \brief Load the dc motor and configures it according to the sdf.
void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf);
/// \brief Update the force on the parent of the joint from each timestep.
void Update(const gazebo::common::UpdateInfo& info);
private:
/// \brief Topic to read control signal from.
std::string topic;
/// \brief The pwm signal limited to the range [-1,1].
double signal;
/// \brief The robot's maximum speed
double maxSpeed;
/// \brief The magic drive force multipliers. force=multiplier*signal
double multiplier;
/// \brief The directional constants limited to -1, 0, or 1.
double dx;
double dy;
double dz;
/// \brief The joint that this motor drives.
gazebo::physics::JointPtr joint;
/// \brief The parent of this joint; usually a chassis
gazebo::physics::LinkPtr parent;
/// \brief The child of this joint; usually a wheel
gazebo::physics::LinkPtr child;
/// \brief Callback for receiving msgs and storing the signal.
void Callback(const gazebo::msgs::ConstFloat64Ptr& msg);
/// \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;
};

102
simulation/frc_gazebo_plugins/src/encoder/cpp/encoder.cpp Normal file
View File

@@ -0,0 +1,102 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "encoder.h"
#include <boost/algorithm/string/replace.hpp>
GZ_REGISTER_MODEL_PLUGIN(Encoder)
void Encoder::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) {
this->model = model;
// Parse SDF properties
joint = model->GetJoint(sdf->Get<std::string>("joint"));
if (sdf->HasElement("topic")) {
topic = sdf->Get<std::string>("topic");
} else {
topic = "~/" + sdf->GetAttribute("name")->GetAsString();
}
if (sdf->HasElement("units")) {
radians = sdf->Get<std::string>("units") != "degrees";
} else {
radians = true;
}
multiplier = 1.0;
zero = GetAngle();
stopped = true;
stop_value = 0;
if (sdf->HasElement("multiplier"))
multiplier = sdf->Get<double>("multiplier");
gzmsg << "Initializing encoder: " << topic << " joint=" << joint->GetName()
<< " radians=" << radians << " multiplier=" << multiplier << std::endl;
// Connect to Gazebo transport for messaging
std::string scoped_name =
model->GetWorld()->GetName() + "::" + model->GetScopedName();
boost::replace_all(scoped_name, "::", "/");
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<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 = gazebo::event::Events::ConnectWorldUpdateBegin(
boost::bind(&Encoder::Update, this, _1));
}
void Encoder::Update(const gazebo::common::UpdateInfo& info) {
gazebo::msgs::Float64 pos_msg, vel_msg;
if (stopped) {
pos_msg.set_data(stop_value * multiplier);
pos_pub->Publish(pos_msg);
vel_msg.set_data(0);
vel_pub->Publish(vel_msg);
} else {
pos_msg.set_data((GetAngle() - zero) * multiplier);
pos_pub->Publish(pos_msg);
vel_msg.set_data(GetVelocity() * multiplier);
vel_pub->Publish(vel_msg);
}
}
void Encoder::Callback(const gazebo::msgs::ConstStringPtr& msg) {
std::string command = msg->data();
if (command == "reset") {
zero = GetAngle();
} else if (command == "start") {
stopped = false;
zero = (GetAngle() - stop_value);
} else if (command == "stop") {
stopped = true;
stop_value = GetAngle();
} else {
gzerr << "WARNING: Encoder got unknown command '" << command << "'."
<< std::endl;
}
}
double Encoder::GetAngle() {
if (radians) {
return joint->GetAngle(0).Radian();
} else {
return joint->GetAngle(0).Degree();
}
}
double Encoder::GetVelocity() {
if (radians) {
return joint->GetVelocity(0);
} else {
return joint->GetVelocity(0) * (180.0 / M_PI);
}
}

112
simulation/frc_gazebo_plugins/src/encoder/headers/encoder.h Normal file
View File

@@ -0,0 +1,112 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <string>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/transport/transport.hh>
#include "simulation/gz_msgs/msgs.h"
/**
* \brief Plugin for reading the speed and relative angle of a joint.
*
* This plugin publishes the angle since last reset and the speed of a
* given joint to subtopics of the given topic every physics
* update. There is also a control subtopic that takes three commands:
* "start", "stop" and "reset":
*
* - "start": Start counting ticks from the current count.
* - "stop": Stop counting ticks, pauses updates.
* - "reset": Set the current angle to zero.
*
* To add a encoder to your robot, add the following XML to your
* robot model:
*
* <plugin name="my_encoder" filename="libencoder.so">
* <joint>Joint Name</joint>
* <topic>~/my/topic</topic>
* <units>{degrees, radians}</units>
* <multiplier>Number</multiplier>
* </plugin>
*
* - `joint`: Name of the joint this encoder is attached to.
* - `topic`: Optional. Used as the root for subtopics. `topic`/position
* (gazebo.msgs.Float64),
* `topic`/velocity (gazebo.msgs.Float64), `topic`/control
* (gazebo.msgs.String)
* The suggested value for topic is of the form
* ~/simulator/encoder/dio/<n>
* where <n> is the number of the first digital input channel
* used to formulate the encoder
*
* - `units`: Optional. Defaults to radians.
* - `multiplier`: Optional. Defaults to 1.
* This can be used to make the simulated encoder
* return a comparable number of ticks to a 'real' encoder
* Useful facts: A 'degrees' encoder will report 360 ticks/revolution.
* The k4X encoder type can add another multiple of 4 into the mix.
*/
class Encoder : public gazebo::ModelPlugin {
public:
/// \brief Load the encoder and configures it according to the sdf.
void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf);
/// \brief Sends out the encoder reading each timestep.
void Update(const gazebo::common::UpdateInfo& info);
private:
/// \brief Root topic for subtopics on this topic.
std::string topic;
/// \brief Whether or not this encoder measures radians or degrees.
bool radians;
/// \brief A factor to mulitply this output by.
double multiplier;
/// \brief Whether or not the encoder has been stopped.
bool stopped;
/// \brief The zero value of the encoder.
double zero;
/// \brief The value the encoder stopped counting at
double stop_value;
/// \brief The joint that this encoder measures
gazebo::physics::JointPtr joint;
/// \brief Callback for handling control data
void Callback(const gazebo::msgs::ConstStringPtr& msg);
/// \brief Gets the current angle, taking into account whether to
/// return radians or degrees.
double GetAngle();
/// \brief Gets the current velocity, taking into account whether to
/// return radians/second or degrees/second.
double GetVelocity();
/// \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;
};

111
simulation/frc_gazebo_plugins/src/gyro/cpp/gyro.cpp Normal file
View File

@@ -0,0 +1,111 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "gyro.h"
#include <boost/algorithm/string/replace.hpp>
GZ_REGISTER_MODEL_PLUGIN(Gyro)
void Gyro::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) {
this->model = model;
// Parse SDF properties
link = model->GetLink(sdf->Get<std::string>("link"));
if (sdf->HasElement("topic")) {
topic = sdf->Get<std::string>("topic");
} else {
topic = "~/" + sdf->GetAttribute("name")->GetAsString();
}
std::string axisString = sdf->Get<std::string>("axis");
if (axisString == "roll") axis = Roll;
if (axisString == "pitch") axis = Pitch;
if (axisString == "yaw") axis = Yaw;
if (sdf->HasElement("units")) {
radians = sdf->Get<std::string>("units") != "degrees";
} else {
radians = true;
}
zero = GetAngle();
gzmsg << "Initializing gyro: " << topic << " link=" << link->GetName()
<< " axis=" << axis << " radians=" << radians << std::endl;
// Connect to Gazebo transport for messaging
std::string scoped_name =
model->GetWorld()->GetName() + "::" + model->GetScopedName();
boost::replace_all(scoped_name, "::", "/");
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<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 = gazebo::event::Events::ConnectWorldUpdateBegin(
boost::bind(&Gyro::Update, this, _1));
}
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 gazebo::msgs::ConstStringPtr& msg) {
std::string command = msg->data();
if (command == "reset") {
zero = GetAngle();
} else {
gzerr << "WARNING: Gyro got unknown command '" << command << "'."
<< std::endl;
}
}
double Gyro::GetAngle() {
if (radians) {
return link->GetWorldCoGPose().rot.GetAsEuler()[axis];
} else {
return link->GetWorldCoGPose().rot.GetAsEuler()[axis] * (180.0 / M_PI);
}
}
double Gyro::GetVelocity() {
if (radians) {
return link->GetRelativeAngularVel()[axis];
} else {
return link->GetRelativeAngularVel()[axis] * (180.0 / M_PI);
}
}
double Gyro::Limit(double value) {
if (radians) {
while (true) {
if (value < -M_PI)
value += 2 * M_PI;
else if (value > M_PI)
value -= 2 * M_PI;
else
break;
}
} else {
while (true) {
if (value < -180)
value += 360;
else if (value > 180)
value -= 360;
else
break;
}
}
return value;
}

98
simulation/frc_gazebo_plugins/src/gyro/headers/gyro.h Normal file
View File

@@ -0,0 +1,98 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <string>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/transport/transport.hh>
#include "simulation/gz_msgs/msgs.h"
/// \brief The axis about which to measure rotation.
typedef enum { Roll /*X*/, Pitch /*Y*/, Yaw /*Z*/ } ROTATION;
/**
* \brief Plugin for reading the speed and relative angle of a link.
*
* This plugin publishes the angle since last reset and the speed
* which a link is rotating about some axis to subtopics of the given
* topic every physics update. There is also a control topic that
* takes one command: "reset", which sets the current angle as zero.
*
* To add a gyro to your robot, add the following XML to your robot
* model:
*
* <plugin name="my_gyro" filename="libgyro.so">
* <link>Joint Name</link>
* <topic>~/my/topic</topic>
* <units>{degrees, radians}</units>
* </plugin>
*
* - `link`: Name of the link this potentiometer is attached to.
* - `topic`: Optional. Used as the root for subtopics. `topic`/position
* (gazebo.msgs.Float64),
* `topic`/velocity (gazebo.msgs.Float64), `topic`/control
* (gazebo.msgs.String)
* - `units`; Optional, defaults to radians.
*/
class Gyro : public gazebo::ModelPlugin {
public:
/// \brief Load the gyro and configures it according to the sdf.
void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf);
/// \brief Sends out the gyro reading each timestep.
void Update(const gazebo::common::UpdateInfo& info);
private:
/// \brief Publish the angle on this topic.
std::string topic;
/// \brief Whether or not this gyro measures radians or degrees.
bool radians;
/// \brief The axis to measure rotation about.
ROTATION axis;
/// \brief The zero value of the gyro.
double zero;
/// \brief The link that this gyro measures
gazebo::physics::LinkPtr link;
/// \brief Callback for handling control data
void Callback(const gazebo::msgs::ConstStringPtr& msg);
/// \brief Gets the current angle, taking into account whether to
/// return radians or degrees.
double GetAngle();
/// \brief Gets the current velocity, taking into account whether to
/// return radians/second or degrees/second.
double GetVelocity();
/// \brief Limit the value to either [-180,180] or [-PI,PI]
/// depending on whether or radians or degrees are being used.
double Limit(double value);
/// \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;
};

22
simulation/frc_gazebo_plugins/src/limit_switch/cpp/external_limit_switch.cpp Normal file
View File

@@ -0,0 +1,22 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "external_limit_switch.h"
#include <string>
ExternalLimitSwitch::ExternalLimitSwitch(sdf::ElementPtr sdf) {
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;
}
bool ExternalLimitSwitch::Get() {
return sensor->Contacts().contact().size() > 0;
}

38
simulation/frc_gazebo_plugins/src/limit_switch/cpp/internal_limit_switch.cpp Normal file
View File

@@ -0,0 +1,38 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "internal_limit_switch.h"
#include <string>
InternalLimitSwitch::InternalLimitSwitch(gazebo::physics::ModelPtr model,
sdf::ElementPtr sdf) {
joint = model->GetJoint(sdf->Get<std::string>("joint"));
min = sdf->Get<double>("min");
max = sdf->Get<double>("max");
if (sdf->HasElement("units")) {
radians = sdf->Get<std::string>("units") != "degrees";
} else {
radians = true;
}
gzmsg << "\tinternal limit switch: "
<< " type=" << joint->GetName() << " range=[" << min << "," << max
<< "] radians=" << radians << std::endl;
}
bool InternalLimitSwitch::Get() {
double value;
joint->GetAngle(0).Normalize();
if (radians) {
value = joint->GetAngle(0).Radian();
} else {
value = joint->GetAngle(0).Degree();
}
return value >= min && value <= max;
}

57
simulation/frc_gazebo_plugins/src/limit_switch/cpp/limit_switch.cpp Normal file
View File

@@ -0,0 +1,57 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "limit_switch.h"
#include <boost/algorithm/string/replace.hpp>
GZ_REGISTER_MODEL_PLUGIN(LimitSwitch)
void LimitSwitch::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) {
this->model = model;
// Parse SDF properties
if (sdf->HasElement("topic")) {
topic = sdf->Get<std::string>("topic");
} else {
topic = "~/" + sdf->GetAttribute("name")->GetAsString();
}
invert = sdf->HasElement("invert");
std::string type = sdf->Get<std::string>("type");
gzmsg << "Initializing limit switch: " << topic << " type=" << type
<< std::endl;
if (type == "internal") {
ls = new InternalLimitSwitch(model, sdf);
} else if (type == "external") {
ls = new ExternalLimitSwitch(sdf);
} else {
gzerr << "WARNING: unsupported limit switch type " << type;
}
// Connect to Gazebo transport for messaging
std::string scoped_name =
model->GetWorld()->GetName() + "::" + model->GetScopedName();
boost::replace_all(scoped_name, "::", "/");
node = gazebo::transport::NodePtr(new gazebo::transport::Node());
node->Init(scoped_name);
pub = node->Advertise<gazebo::msgs::Bool>(topic);
// Connect to the world update event.
// This will trigger the Update function every Gazebo iteration
updateConn = gazebo::event::Events::ConnectWorldUpdateBegin(
boost::bind(&LimitSwitch::Update, this, _1));
}
void LimitSwitch::Update(const gazebo::common::UpdateInfo& info) {
gazebo::msgs::Bool msg;
if (invert)
msg.set_data(!ls->Get());
else
msg.set_data(ls->Get());
pub->Publish(msg);
}

29
simulation/frc_gazebo_plugins/src/limit_switch/headers/external_limit_switch.h Normal file
View File

@@ -0,0 +1,29 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <boost/pointer_cast.hpp>
#include <gazebo/gazebo.hh>
#include <gazebo/sensors/sensors.hh>
#ifdef _WIN32
#include <Winsock2.h>
#endif
#include "switch.h"
class ExternalLimitSwitch : public Switch {
public:
explicit ExternalLimitSwitch(sdf::ElementPtr sdf);
/// \brief Returns true when the switch is triggered.
virtual bool Get();
private:
gazebo::sensors::ContactSensorPtr sensor;
};

30
simulation/frc_gazebo_plugins/src/limit_switch/headers/internal_limit_switch.h Normal file
View File

@@ -0,0 +1,30 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#ifdef _WIN32
#include <Winsock2.h>
#endif
#include "switch.h"
class InternalLimitSwitch : public Switch {
public:
InternalLimitSwitch(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf);
/// \brief Returns true when the switch is triggered.
virtual bool Get();
private:
gazebo::physics::JointPtr joint;
double min, max;
bool radians;
};

99
simulation/frc_gazebo_plugins/src/limit_switch/headers/limit_switch.h Normal file
View File

@@ -0,0 +1,99 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <string>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/transport/transport.hh>
#include "external_limit_switch.h"
#include "internal_limit_switch.h"
#include "simulation/gz_msgs/msgs.h"
#include "switch.h"
/**
* \brief Plugin for reading limit switches.
*
* This plugin publishes whether or not the limit switch has been
* triggered every physics update. There are two types of limit switches:
*
* - Internal: Measure joint limits. Triggerd if the joint is within
* some range.
* - External: Measure interactions with the outside world. Triggerd
* if some link is in collision.
*
* To add a limit swithch to your robot, add the following XML to your
* robot model.
*
* Internal:
*
* <plugin name="my_limit_switch" filename="liblimit_switch.so">
* <topic>~/my/topic</topic>
* <type>internal</type>
* <joint>Joint Name</joint>
* <units>{degrees, radians}</units>
* <min>Number</min>
* <max>Number</max>
* </plugin>
*
* External:
*
* <plugin name="my_limit_switch" filename="liblimit_switch.so">
* <topic>~/my/topic</topic>
* <type>external</type>
* <sensor>Sensor Name</sensor>
* </plugin>
*
* Common:
* - `topic`: Optional. Message will be published as a gazebo.msgs.Bool.
* Recommended values are of the form:
* /gazebo/frc/simulator/dio/n
* - `type`: Required. The type of limit switch that this is
* - `invert`: Optional. If given, the output meaning will be inverted
*
* Internal
* - `joint`: Name of the joint this potentiometer is attached to.
* - `units`: Optional. Defaults to radians.
* - `min`: Minimum angle considered triggered.
* - `max`: Maximum angle considered triggered.
*
* External
* - `sensor`: Name of the contact sensor that this limit switch uses.
*/
class LimitSwitch : public gazebo::ModelPlugin {
public:
/// \brief Load the limit switch and configures it according to the sdf.
void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf);
/// \brief Sends out the limit switch reading each timestep.
void Update(const gazebo::common::UpdateInfo& info);
private:
/// \brief Publish the limit switch value on this topic.
std::string topic;
/// \brief LimitSwitch object, currently internal or external.
Switch* ls;
/// \brief Indicate if we should invert the output
bool invert;
/// \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;
};

16
simulation/frc_gazebo_plugins/src/limit_switch/headers/switch.h Normal file
View File

@@ -0,0 +1,16 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
class Switch {
public:
virtual ~Switch() = default;
/// \brief Returns true when the switch is triggered.
virtual bool Get() = 0;
};

95
simulation/frc_gazebo_plugins/src/pneumatic_piston/cpp/pneumatic_piston.cpp Normal file
View File

@@ -0,0 +1,95 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "pneumatic_piston.h"
#include <boost/algorithm/string/replace.hpp>
#include <gazebo/physics/physics.hh>
#include <gazebo/transport/transport.hh>
#ifdef _WIN32
// Ensure that Winsock2.h is included before Windows.h, which can get
// pulled in by anybody (e.g., Boost).
#include <Winsock2.h>
#endif
GZ_REGISTER_MODEL_PLUGIN(PneumaticPiston)
void PneumaticPiston::Load(gazebo::physics::ModelPtr model,
sdf::ElementPtr sdf) {
this->model = model;
forward_signal = reverse_signal = false;
// Parse SDF properties
joint = model->GetJoint(sdf->Get<std::string>("joint"));
if (sdf->HasElement("topic")) {
topic = sdf->Get<std::string>("topic");
} else {
topic = "~/" + sdf->GetAttribute("name")->GetAsString();
}
if (sdf->HasElement("reverse-topic")) {
reverse_topic = sdf->Get<std::string>("reverse-topic");
} else {
reverse_topic.clear();
}
forward_force = sdf->Get<double>("forward-force");
if (sdf->HasElement("reverse-force"))
reverse_force = -1.0 * sdf->Get<double>("reverse-force");
if (sdf->HasElement("direction") &&
sdf->Get<std::string>("direction") == "reversed") {
forward_force = -forward_force;
reverse_force = -reverse_force;
}
gzmsg << "Initializing piston: " << topic << " joint=" << joint->GetName()
<< " forward_force=" << forward_force
<< " reverse_force=" << reverse_force << std::endl;
if (!reverse_topic.empty())
gzmsg << "Reversing on topic " << reverse_topic << std::endl;
// Connect to Gazebo transport for messaging
std::string scoped_name =
model->GetWorld()->GetName() + "::" + model->GetScopedName();
boost::replace_all(scoped_name, "::", "/");
node = gazebo::transport::NodePtr(new gazebo::transport::Node());
node->Init(scoped_name);
sub = node->Subscribe(topic, &PneumaticPiston::ForwardCallback, this);
if (!reverse_topic.empty())
sub_reverse =
node->Subscribe(reverse_topic, &PneumaticPiston::ReverseCallback, this);
// Connect to the world update event.
// This will trigger the Update function every Gazebo iteration
updateConn = gazebo::event::Events::ConnectWorldUpdateBegin(
boost::bind(&PneumaticPiston::Update, this, _1));
}
void PneumaticPiston::Update(const gazebo::common::UpdateInfo& info) {
double force = 0.0;
if (forward_signal) {
force = forward_force;
} else {
/* For DoubleSolenoids, the second signal must be present
for us to apply the reverse force. For SingleSolenoids,
the lack of the forward signal suffices.
Note that a true simulation would not allow a SingleSolenoid to
have reverse force, but we put that in the hands of the model builder.*/
if (reverse_topic.empty() || reverse_signal) force = reverse_force;
}
joint->SetForce(0, force);
}
void PneumaticPiston::ForwardCallback(const gazebo::msgs::ConstBoolPtr& msg) {
forward_signal = msg->data();
}
void PneumaticPiston::ReverseCallback(const gazebo::msgs::ConstBoolPtr& msg) {
reverse_signal = msg->data();
}

102
simulation/frc_gazebo_plugins/src/pneumatic_piston/headers/pneumatic_piston.h Normal file
View File

@@ -0,0 +1,102 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <string>
#include <gazebo/gazebo.hh>
#include "simulation/gz_msgs/msgs.h"
/**
* \brief Plugin for controlling a joint with a pneumatic piston.
*
* This plugin subscribes to topics to get the solenoid state for a piston
* It needs one signal for the forward signal. For a double solenoid,
* a second signal can be sent.
* Each signal is a boolean.
*
* Every physics update the joint's torque is set to reflect the
* signal, using the configured force
*
* To add a pneumatic piston to your robot, add the following XML to
* your robot model:
*
* <plugin name="my_piston" filename="libpneumatic_piston.so">
* <joint>Joint Name</joint>
* <topic>/gazebo/frc/simulator/pneumatics/1/1</topic>
* <reverse-topic>/gazebo/frc/simulator/pneumatics/1/2</reverse-topic>
* <direction>{forward, reversed}</direction>
* <forward-force>Number</forward-force>
* <reverse-force>Number</reverse-force>
* </plugin>
*
* - `joint`: Name of the joint this piston is attached to.
* - `topic`: Optional. Forward Solenoid signal name. type gazebo.msgs.Bool.
* If not given, the name given for the plugin will be used.
* A pattern of /gazebo/frc/simulator/pneumatics/1/n is good.
* The first number represents the PCM module. Only 1 is supported.
* The second number represents the channel on the PCM.
* - `topic-reverse`: Optional. If given, represents the reverse channel.
* Message type should be gazebo.msgs.Bool.
* - `direction`: Optional. Defaults to forward. Reversed if the piston
* pushes in the opposite direction of the joint axis.
* - `forward-force`: Force to apply in the forward direction.
* - `reverse-force`: Force to apply in the reverse direction.
* For a single solenoid, you would expect '0',
* but we allow model builders to provide a value.
*
*/
class PneumaticPiston : public gazebo::ModelPlugin {
public:
/// \brief Load the pneumatic piston and configures it according to the sdf.
void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf);
/// \brief Updat the force the piston applies on the joint.
void Update(const gazebo::common::UpdateInfo& info);
private:
/// \brief Topic to read forward control signal from.
std::string topic;
/// \brief Topic to read reverse control signal from.
std::string reverse_topic;
/// \brief Whether the solenoid to open forward is on
bool forward_signal;
/// \brief Whether the solenoid to open in reverse is on
bool reverse_signal;
/// \brief The magic force multipliers for each direction.
double forward_force, reverse_force;
/// \brief The joint that this pneumatic piston actuates.
gazebo::physics::JointPtr joint;
/// \brief Callback for receiving msgs and updating the solenoid state
void ForwardCallback(const gazebo::msgs::ConstBoolPtr& msg);
/// \brief Callback for receiving msgs and updating the reverse solenoid state
void ReverseCallback(const gazebo::msgs::ConstBoolPtr& msg);
/// \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;
/// \brief Subscriber handle for reverse topic
gazebo::transport::SubscriberPtr sub_reverse;
};

70
simulation/frc_gazebo_plugins/src/potentiometer/cpp/potentiometer.cpp Normal file
View File

@@ -0,0 +1,70 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "potentiometer.h"
#include <boost/algorithm/string/replace.hpp>
#include <gazebo/physics/physics.hh>
#include <gazebo/transport/transport.hh>
#ifdef _WIN32
// Ensure that Winsock2.h is included before Windows.h, which can get
// pulled in by anybody (e.g., Boost).
#include <Winsock2.h>
#endif
GZ_REGISTER_MODEL_PLUGIN(Potentiometer)
void Potentiometer::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) {
this->model = model;
// Parse SDF properties
joint = model->GetJoint(sdf->Get<std::string>("joint"));
if (sdf->HasElement("topic")) {
topic = sdf->Get<std::string>("topic");
} else {
topic = "~/" + sdf->GetAttribute("name")->GetAsString();
}
if (sdf->HasElement("units")) {
radians = sdf->Get<std::string>("units") != "degrees";
} else {
radians = true;
}
multiplier = 1.0;
if (sdf->HasElement("multiplier"))
multiplier = sdf->Get<double>("multiplier");
gzmsg << "Initializing potentiometer: " << topic
<< " joint=" << joint->GetName() << " radians=" << radians
<< " multiplier=" << multiplier << std::endl;
// Connect to Gazebo transport for messaging
std::string scoped_name =
model->GetWorld()->GetName() + "::" + model->GetScopedName();
boost::replace_all(scoped_name, "::", "/");
node = gazebo::transport::NodePtr(new gazebo::transport::Node());
node->Init(scoped_name);
pub = node->Advertise<gazebo::msgs::Float64>(topic);
// Connect to the world update event.
// This will trigger the Update function every Gazebo iteration
updateConn = gazebo::event::Events::ConnectWorldUpdateBegin(
boost::bind(&Potentiometer::Update, this, _1));
}
void Potentiometer::Update(const gazebo::common::UpdateInfo& info) {
joint->GetAngle(0).Normalize();
gazebo::msgs::Float64 msg;
if (radians) {
msg.set_data(joint->GetAngle(0).Radian() * multiplier);
} else {
msg.set_data(joint->GetAngle(0).Degree() * multiplier);
}
pub->Publish(msg);
}

73
simulation/frc_gazebo_plugins/src/potentiometer/headers/potentiometer.h Normal file
View File

@@ -0,0 +1,73 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <string>
#include <gazebo/gazebo.hh>
#include "simulation/gz_msgs/msgs.h"
/**
* \brief Plugin for reading the angle of a joint.
*
* This plugin publishes the angle of a joint to the topic every
* physics update. Supports reading in either radians or degrees.
*
* To add a potentiometer to your robot, add the following XML to your
* robot model:
*
* <plugin name="my_pot" filename="libpotentiometer.so">
* <joint>Joint Name</joint>
* <topic>~/my/topic</topic>
* <units>{degrees, radians}</units>
* </plugin>
*
* - `joint`: Name of the joint this potentiometer is attached to.
* - `topic`: Optional. Message will be published as a gazebo.msgs.Float64.
* The default topic name will be the plugin name.
* Recommended topic names are of the form:
* /gazebo/frc/simulator/analog/n
* where n is the analog channel for the pot.
* - `multiplier`: Optional. Amount to multiply output with.
* Useful when a rotary pot returns something other than an angle
* - `units`: Optional. Defaults to radians.
*/
class Potentiometer : public gazebo::ModelPlugin {
public:
/// \brief Load the potentiometer and configures it according to the sdf.
void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf);
/// \brief Sends out the potentiometer reading each timestep.
void Update(const gazebo::common::UpdateInfo& info);
private:
/// \brief Publish the angle on this topic.
std::string topic;
/// \brief Whether or not this potentiometer measures radians or degrees.
bool radians;
/// \brief A scaling factor to apply to this potentiometer.
double multiplier;
/// \brief The joint that this potentiometer measures
gazebo::physics::JointPtr joint;
/// \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;
};

56
simulation/frc_gazebo_plugins/src/rangefinder/cpp/rangefinder.cpp Normal file
View File

@@ -0,0 +1,56 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "rangefinder.h"
#include <boost/algorithm/string/replace.hpp>
#include <gazebo/physics/physics.hh>
#include <gazebo/sensors/sensors.hh>
#include <gazebo/transport/transport.hh>
#ifdef _WIN32
// Ensure that Winsock2.h is included before Windows.h, which can get
// pulled in by anybody (e.g., Boost).
#include <Winsock2.h>
#endif
GZ_REGISTER_MODEL_PLUGIN(Rangefinder)
void Rangefinder::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) {
this->model = model;
// Parse SDF properties
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 {
topic = "~/" + sdf->GetAttribute("name")->GetAsString();
}
gzmsg << "Initializing rangefinder: " << topic << " sensor=" << sensor->Name()
<< std::endl;
// Connect to Gazebo transport for messaging
std::string scoped_name =
model->GetWorld()->GetName() + "::" + model->GetScopedName();
boost::replace_all(scoped_name, "::", "/");
node = gazebo::transport::NodePtr(new gazebo::transport::Node());
node->Init(scoped_name);
pub = node->Advertise<gazebo::msgs::Float64>(topic);
// Connect to the world update event.
// This will trigger the Update function every Gazebo iteration
updateConn = gazebo::event::Events::ConnectWorldUpdateBegin(
boost::bind(&Rangefinder::Update, this, _1));
}
void Rangefinder::Update(const gazebo::common::UpdateInfo& info) {
gazebo::msgs::Float64 msg;
msg.set_data(sensor->Range());
pub->Publish(msg);
}

59
simulation/frc_gazebo_plugins/src/rangefinder/headers/rangefinder.h Normal file
View File

@@ -0,0 +1,59 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <string>
#include <gazebo/gazebo.hh>
#include "simulation/gz_msgs/msgs.h"
/**
* \brief Plugin for reading the range of obstacles.
*
* This plugin publishes the range of obstacles detected by a sonar
* rangefinder every physics update.
*
* To add a rangefinder to your robot, add the following XML to your
* robot model:
*
* <plugin name="my_rangefinder" filename="librangefinder.so">
* <sensor>Sensor Name</sensor>
* <topic>~/my/topic</topic>
* </plugin>
*
* - `sensor`: Name of the sonar sensor that this rangefinder uses.
* - `topic`: Optional. Message will be published as a gazebo.msgs.Float64.
*/
class Rangefinder : public gazebo::ModelPlugin {
public:
/// \brief Load the rangefinder and configures it according to the sdf.
void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf);
/// \brief Sends out the rangefinder reading each timestep.
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
gazebo::sensors::SonarSensorPtr sensor;
/// \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;
};

75
simulation/frc_gazebo_plugins/src/servo/cpp/servo.cpp Normal file
View File

@@ -0,0 +1,75 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "servo.h"
#include <boost/algorithm/string/replace.hpp>
#include <gazebo/physics/physics.hh>
#include <gazebo/transport/transport.hh>
#ifdef _WIN32
// Ensure that Winsock2.h is included before Windows.h, which can get
// pulled in by anybody (e.g., Boost).
#include <Winsock2.h>
#endif
GZ_REGISTER_MODEL_PLUGIN(Servo)
void Servo::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) {
this->model = model;
signal = 0;
// parse SDF Properries
joint = model->GetJoint(sdf->Get<std::string>("joint"));
if (sdf->HasElement("topic")) {
topic = sdf->Get<std::string>("topic");
} else {
topic = "~/" + sdf->GetAttribute("name")->GetAsString();
}
if (sdf->HasElement("torque")) {
torque = sdf->Get<double>("torque");
} else {
torque = 5;
}
gzmsg << "initializing servo: " << topic << " joint=" << joint->GetName()
<< " torque=" << torque << std::endl;
// Connect to Gazebo transport for messaging
std::string scoped_name =
model->GetWorld()->GetName() + "::" + model->GetScopedName();
boost::replace_all(scoped_name, "::", "/");
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 = gazebo::event::Events::ConnectWorldUpdateBegin(
boost::bind(&Servo::Update, this, _1));
}
void Servo::Update(const gazebo::common::UpdateInfo& info) {
// torque is in kg*cm
// joint->SetAngle(0,signal*180);
if (joint->GetAngle(0) < signal) {
joint->SetForce(0, torque);
} else if (joint->GetAngle(0) > signal) {
joint->SetForce(0, torque);
}
joint->SetForce(0, 0);
}
void Servo::Callback(const gazebo::msgs::ConstFloat64Ptr& msg) {
signal = msg->data();
if (signal < -1) {
signal = -1;
} else if (signal > 1) {
signal = 1;
}
}

70
simulation/frc_gazebo_plugins/src/servo/headers/servo.h Normal file
View File

@@ -0,0 +1,70 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <string>
#include <gazebo/gazebo.hh>
#include "simulation/gz_msgs/msgs.h"
/**
* \brief Plugin for controlling a servo.
*
* This plugin subscribes to a topic to get a signal in the range
* [-1,1]. Every physics update the joint's torque is set as
* multiplier*signal.
*
* To add a servo to your robot, add the following XML to your robot
* model:
*
* <plugin name="my_servo" filename="libservo.so">
* <joint>Joint Name</joint>
* <topic>/gzebo/frc/simulator/pwm/1</topic>
* <zero_position>0</zero_position>
* </plugin>
*
* - `link`: Name of the link the servo is attached to.
* - `topic`: Optional. Message type should be gazebo.msgs.Float64.
*/
class Servo : public gazebo::ModelPlugin {
public:
/// \brief load the servo and configure it according to the 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 gazebo::common::UpdateInfo& info);
private:
/// \brief Topic to read control signal from.
std::string topic;
/// \brief the pwm signal limited to the range [-1,1]
double signal;
/// \brief the torque of the motor in kg/cm
double torque;
/// \brief the joint that this servo moves
gazebo::physics::JointPtr joint;
/// \brief Callback for receiving msgs and storing the signal
void Callback(const gazebo::msgs::ConstFloat64Ptr& msg);
/// \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;
};

12
simulation/gz_msgs/README.md Normal file
View File

@@ -0,0 +1,12 @@
gz_msgs
=======
gz_msgs is a library that allows the transmission of messages
to and from Java and C++ programs.
gz_msgs currently requires libprotobuf-dev on Debian like systems.
If it's not found via pkg-config, then it's build is diabled.
You can force it by specifying -PmakeSim on the gradle command line.
If you are installing FRCSim with the script, then this *should* have be done for you.

105
simulation/gz_msgs/build.gradle Normal file
View File

@@ -0,0 +1,105 @@
description = "A C++ and Java library to pass FRC Simulation Messages in and out of Gazebo."
apply plugin: 'cpp'
apply plugin: 'java'
apply plugin: 'com.google.protobuf'
apply plugin: 'edu.wpi.first.NativeUtils'
/* The simulation does not run on real hardware; so we always skip Athena */
ext.skipAthena = true
repositories {
mavenCentral()
}
buildscript {
repositories {
mavenCentral()
}
dependencies {
classpath 'com.google.protobuf:protobuf-gradle-plugin:+'
}
}
/* Use a sort of poor man's autoconf to find the protobuf development
files; on Debian, those are supplied by libprotobuf-dev.
This should get skipped on Windows.
TODO: Add Windows support for the simulation code */
def protobuf_version = ""
try {
protobuf_version = "pkg-config --modversion protobuf".execute().text.trim()
println "Protobuf version is [${protobuf_version}]"
} catch(Exception ex) {
}
if (!protobuf_version?.trim()) {
println "Protobuf is not available. (pkg-config --modversion protobuf failed)"
protobuf_version = "+"
if (project.hasProperty("makeSim")) {
/* Force the build even though we did not find protobuf. */
println "makeSim set. Forcing build - failure likely."
}
else {
ext.skip_gz_msgs = true
println "Skipping gz_msgs."
}
}
tasks.whenTaskAdded { task ->
task.onlyIf { !project.hasProperty('skip_gz_msgs') }
}
dependencies {
compile "com.google.protobuf:protobuf-java:${protobuf_version}"
compile "com.google.protobuf:protoc:${protobuf_version}"
}
/* There is a nice gradle plugin for protobuf, and the protoc tool
is included; using it simplifies our build process.
The trick is that we have to use the same version as the system
copy of libprotobuf-dev */
protobuf {
protoc {
artifact = "com.google.protobuf:protoc:${protobuf_version}"
}
generatedFilesBaseDir = "$buildDir/generated"
generateProtoTasks {
all().each { task ->
task.builtins {
cpp {
outputSubDir = 'simulation/gz_msgs'
}
}
}
}
}
model {
components {
gz_msgs(NativeLibrarySpec) {
sources {
cpp {
source {
srcDir "$buildDir/generated/main/simulation/gz_msgs"
builtBy tasks.generateProto
}
exportedHeaders {
srcDir "src/include"
srcDir "$buildDir/generated/main"
}
}
}
/* We must compile with -fPIC to link the static library into an so */
binaries {
all {
cppCompiler.args "-fPIC"
}
}
}
}
}

47
simulation/gz_msgs/src/include/simulation/gz_msgs/msgs.h Normal file
View File

@@ -0,0 +1,47 @@
/* This was originally auto generated by cmake.
However, it changes infrequently enough that it makes
sense to simply revise it by hand any time we change
the protobuf messages expressed in the .proto files */
#ifndef _FRC_MSGS_H_
#define _FRC_MSGS_H_
#ifdef _WIN32
//include this before anything else includes windows.h
//putting this here saves putting it in more files
#include <Winsock2.h>
#endif
#include "simulation/gz_msgs/bool.pb.h"
#include "simulation/gz_msgs/driver-station.pb.h"
#include "simulation/gz_msgs/float64.pb.h"
#include "simulation/gz_msgs/frc_joystick.pb.h"
#include <gazebo/msgs/msgs.hh>
#include <boost/shared_ptr.hpp>
namespace gazebo {
namespace msgs {
typedef GzString String;
typedef boost::shared_ptr< gazebo::msgs::String > StringPtr;
typedef const boost::shared_ptr< const gazebo::msgs::String > ConstStringPtr;
typedef boost::shared_ptr< msgs::Float64 > Float64Ptr;
typedef const boost::shared_ptr< const msgs::Float64 > ConstFloat64Ptr;
typedef boost::shared_ptr< msgs::Bool > BoolPtr;
typedef const boost::shared_ptr< const msgs::Bool > ConstBoolPtr;
typedef boost::shared_ptr< msgs::FRCJoystick > FRCJoystickPtr;
typedef const boost::shared_ptr< const msgs::FRCJoystick > ConstFRCJoystickPtr;
typedef boost::shared_ptr< msgs::DriverStation > DriverStationPtr;
typedef const boost::shared_ptr< const msgs::DriverStation > ConstDriverStationPtr;
}
}
#endif /* _FRC_MSGS_H_ */

15
simulation/gz_msgs/src/main/proto/bool.proto Normal file
View File

@@ -0,0 +1,15 @@
package gazebo.msgs;
/// \ingroup gazebo_msgs
/// \interface Bool
/// \brief A message for boolean data
/// \verbatim
option java_outer_classname = "GzBool";
message Bool
{
required bool data = 1;
}
/// \endverbatim

21
simulation/gz_msgs/src/main/proto/driver-station.proto Normal file
View File

@@ -0,0 +1,21 @@
package gazebo.msgs;
/// \ingroup gazebo_msgs
/// \interface DriverStation
/// \brief A message for DriverStation data
/// \verbatim
option java_outer_classname = "GzDriverStation";
message DriverStation
{
required bool enabled = 1;
enum State {
AUTO = 0;
TELEOP = 1;
TEST = 2;
}
required State state = 2;
}
/// \endverbatim

15
simulation/gz_msgs/src/main/proto/float64.proto Normal file
View File

@@ -0,0 +1,15 @@
package gazebo.msgs;
/// \ingroup gazebo_msgs
/// \interface Float64
/// \brief A message for floating point data
/// \verbatim
option java_outer_classname = "GzFloat64";
message Float64
{
required double data = 1;
}
/// \endverbatim

16
simulation/gz_msgs/src/main/proto/frc_joystick.proto Normal file
View File

@@ -0,0 +1,16 @@
package gazebo.msgs;
/// \ingroup gazebo_msgs
/// \interface Joystick
/// \brief A message for joystick data
/// \verbatim
option java_outer_classname = "GzFRCJoystick";
message FRCJoystick
{
repeated double axes = 1;
repeated bool buttons = 2;
}
/// \endverbatim

50
simulation/halsim_gazebo/build.gradle Normal file
View File

@@ -0,0 +1,50 @@
description = "A shared object library that will interface between a robot and the Gazebo plugins."
apply plugin: 'edu.wpi.first.NativeUtils'
apply plugin: 'cpp'
ext.skipAthena = true
/* If gz_msgs or gazebo is not available, do not attempt a build */
def gazebo_version = ""
def gazebo_cppflags = ""
def gazebo_linker_args = ""
try {
gazebo_version = "pkg-config --modversion gazebo".execute().text.trim()
println "Gazebo version is [${gazebo_version}]"
gazebo_cppflags = "pkg-config --cflags gazebo".execute().text.split()
gazebo_linker_args = "pkg-config --libs gazebo protobuf".execute().text.split()
} catch(Exception ex) { }
if (!gazebo_version?.trim()) {
println "Gazebo development files are not available. (pkg-config --modversion gazebo failed)"
if (project.hasProperty("makeSim")) {
/* Force the build even though we did not find protobuf. */
println "makeSim set. Forcing build - failure likely."
}
else {
ext.skip_frc_plugins = true
println "Skipping FRC Plugins."
}
}
evaluationDependsOn(":simulation:gz_msgs")
def gz_msgs_project = project(":simulation:gz_msgs")
tasks.whenTaskAdded { task ->
task.onlyIf { !gz_msgs_project.hasProperty('skip_gz_msgs') && !project.hasProperty('skip_frc_plugins') }
}
model {
binaries {
all {
linker.args gazebo_linker_args
cppCompiler.args gazebo_cppflags
lib project: ":simulation:gz_msgs", library: "gz_msgs", linkage: "static"
}
}
}
/* TODO: Publish this library */

47
simulation/halsim_gazebo/src/halsim_gazebo/cpp/GazeboAnalogIn.cpp Normal file
View File

@@ -0,0 +1,47 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2014-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "GazeboAnalogIn.h"
#include <string>
#include <HAL/Power.h>
#include "MockData/AnalogInData.h"
#include "MockData/HAL_Value.h"
#include "MockData/NotifyListener.h"
static void init_callback(const char* name, void* param,
const struct HAL_Value* value) {
GazeboAnalogIn* ain = static_cast<GazeboAnalogIn*>(param);
ain->SetInitialized(value->data.v_boolean);
if (ain->IsInitialized()) {
ain->Listen();
}
}
GazeboAnalogIn::GazeboAnalogIn(int index, HALSimGazebo* halsim) {
m_index = index;
m_halsim = halsim;
m_sub = NULL;
HALSIM_RegisterAnalogInInitializedCallback(index, init_callback, this, true);
}
void GazeboAnalogIn::Listen() {
if (!m_sub)
m_sub = m_halsim->node.Subscribe<gazebo::msgs::Float64>(
"~/simulator/analog/" + std::to_string(m_index),
&GazeboAnalogIn::Callback, this);
}
void GazeboAnalogIn::Callback(const gazebo::msgs::ConstFloat64Ptr& msg) {
/* This value is going to be divided by the 5V rail in the HAL, so
we multiply by that value to make the change neutral */
int32_t status;
HALSIM_SetAnalogInVoltage(m_index,
msg->data() * HAL_GetUserVoltage5V(&status));
}

41
simulation/halsim_gazebo/src/halsim_gazebo/cpp/GazeboDIO.cpp Normal file
View File

@@ -0,0 +1,41 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2014-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "GazeboDIO.h"
#include <string>
#include "MockData/DIOData.h"
#include "MockData/HAL_Value.h"
#include "MockData/NotifyListener.h"
static void init_callback(const char* name, void* param,
const struct HAL_Value* value) {
GazeboDIO* dio = static_cast<GazeboDIO*>(param);
dio->SetInitialized(value->data.v_boolean);
if (dio->IsInitialized()) {
dio->Listen();
}
}
GazeboDIO::GazeboDIO(int index, HALSimGazebo* halsim) {
m_index = index;
m_halsim = halsim;
m_sub = NULL;
HALSIM_RegisterDIOInitializedCallback(index, init_callback, this, true);
}
void GazeboDIO::Listen() {
if (!m_sub)
m_sub = m_halsim->node.Subscribe<gazebo::msgs::Bool>(
"~/simulator/dio/" + std::to_string(m_index), &GazeboDIO::Callback,
this);
}
void GazeboDIO::Callback(const gazebo::msgs::ConstBoolPtr& msg) {
HALSIM_SetDIOValue(m_index, msg->data());
}

75
simulation/halsim_gazebo/src/halsim_gazebo/cpp/GazeboEncoder.cpp Normal file
View File

@@ -0,0 +1,75 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2014-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "GazeboEncoder.h"
#include <string>
#include "MockData/EncoderData.h"
#include "MockData/HAL_Value.h"
#include "MockData/NotifyListener.h"
static void encoder_init_callback(const char* name, void* param,
const struct HAL_Value* value) {
GazeboEncoder* encoder = static_cast<GazeboEncoder*>(param);
encoder->SetInitialized(value->data.v_boolean);
if (encoder->IsInitialized()) {
encoder->Control("start");
encoder->Listen();
}
}
static void encoder_reset_callback(const char* name, void* param,
const struct HAL_Value* value) {
GazeboEncoder* encoder = static_cast<GazeboEncoder*>(param);
if (encoder->IsInitialized() && value->data.v_boolean)
encoder->Control("reset");
}
static void encoder_reverse_callback(const char* name, void* param,
const struct HAL_Value* value) {
GazeboEncoder* encoder = static_cast<GazeboEncoder*>(param);
if (encoder->IsInitialized()) encoder->SetReverse(value->data.v_boolean);
}
GazeboEncoder::GazeboEncoder(int index, HALSimGazebo* halsim) {
m_index = index;
m_halsim = halsim;
m_reverse = false;
m_pub = NULL;
m_sub = NULL;
HALSIM_RegisterEncoderInitializedCallback(index, encoder_init_callback, this,
true);
HALSIM_RegisterEncoderResetCallback(index, encoder_reset_callback, this,
true);
HALSIM_RegisterEncoderReverseDirectionCallback(
index, encoder_reverse_callback, this, true);
}
void GazeboEncoder::Control(const char* command) {
if (!m_pub) {
m_pub = m_halsim->node.Advertise<gazebo::msgs::String>(
"~/simulator/encoder/dio/" +
std::to_string(HALSIM_GetDigitalChannelA(m_index)) + "/control");
m_pub->WaitForConnection(gazebo::common::Time(1, 0));
}
gazebo::msgs::String msg;
msg.set_data(command);
if (m_pub) m_pub->Publish(msg);
}
void GazeboEncoder::Listen() {
if (!m_sub)
m_sub = m_halsim->node.Subscribe<gazebo::msgs::Float64>(
"~/simulator/encoder/dio/" +
std::to_string(HALSIM_GetDigitalChannelA(m_index)) + "/position",
&GazeboEncoder::Callback, this);
}
void GazeboEncoder::Callback(const gazebo::msgs::ConstFloat64Ptr& msg) {
HALSIM_SetEncoderCount(m_index, msg->data() * (m_reverse ? -1 : 1));
}

20
simulation/halsim_gazebo/src/halsim_gazebo/cpp/GazeboNode.cpp Normal file
View File

@@ -0,0 +1,20 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2014-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "GazeboNode.h"
bool GazeboNode::Connect() {
bool success = gazebo::client::setup();
if (success) {
main = gazebo::transport::NodePtr(new gazebo::transport::Node());
main->Init("frc");
gazebo::transport::run();
}
return success;
}

54
simulation/halsim_gazebo/src/halsim_gazebo/cpp/GazeboPCM.cpp Normal file
View File

@@ -0,0 +1,54 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2014-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "GazeboPCM.h"
#include <string>
#include "MockData/HAL_Value.h"
#include "MockData/NotifyListener.h"
#include "MockData/PCMData.h"
#include "simulation/gz_msgs/msgs.h"
static void init_callback(const char* name, void* param,
const struct HAL_Value* value) {
GazeboPCM* pcm = static_cast<GazeboPCM*>(param);
pcm->SetInitialized(value->data.v_boolean);
}
static void output_callback(const char* name, void* param,
const struct HAL_Value* value) {
GazeboPCM* pcm = static_cast<GazeboPCM*>(param);
if (pcm->IsInitialized()) pcm->Publish(value->data.v_boolean);
}
GazeboPCM::GazeboPCM(int index, int channel, HALSimGazebo* halsim) {
m_index = index;
m_channel = channel;
m_halsim = halsim;
m_pub = NULL;
HALSIM_RegisterPCMSolenoidInitializedCallback(index, channel, init_callback,
this, true);
HALSIM_RegisterPCMSolenoidOutputCallback(index, channel, output_callback,
this, true);
}
void GazeboPCM::Publish(bool value) {
if (!m_pub) {
m_pub = m_halsim->node.Advertise<gazebo::msgs::Bool>(
"~/simulator/pneumatic/" + std::to_string(m_index + 1) + "/" +
std::to_string(m_channel));
m_pub->WaitForConnection(gazebo::common::Time(1, 0));
}
gazebo::msgs::Bool msg;
msg.set_data(value);
if (m_pub) m_pub->Publish(msg);
}
void GazeboPCM_SetPressureSwitch(int index, bool value) {
HALSIM_SetPCMPressureSwitch(index, value);
}

45
simulation/halsim_gazebo/src/halsim_gazebo/cpp/GazeboPWM.cpp Normal file
View File

@@ -0,0 +1,45 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2014-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "GazeboPWM.h"
#include <string>
#include "MockData/HAL_Value.h"
#include "MockData/NotifyListener.h"
#include "MockData/PWMData.h"
#include "simulation/gz_msgs/msgs.h"
static void init_callback(const char* name, void* param,
const struct HAL_Value* value) {
GazeboPWM* pwm = static_cast<GazeboPWM*>(param);
pwm->SetInitialized(value->data.v_boolean);
}
static void speed_callback(const char* name, void* param,
const struct HAL_Value* value) {
GazeboPWM* pwm = static_cast<GazeboPWM*>(param);
if (pwm->IsInitialized()) pwm->Publish(value->data.v_double);
}
GazeboPWM::GazeboPWM(int port, HALSimGazebo* halsim) {
m_port = port;
m_halsim = halsim;
HALSIM_RegisterPWMInitializedCallback(port, init_callback, this, true);
HALSIM_RegisterPWMSpeedCallback(port, speed_callback, this, true);
}
void GazeboPWM::Publish(double value) {
if (!m_pub) {
m_pub = m_halsim->node.Advertise<gazebo::msgs::Float64>(
"~/simulator/pwm/" + std::to_string(m_port));
m_pub->WaitForConnection(gazebo::common::Time(1, 0));
}
gazebo::msgs::Float64 msg;
msg.set_data(value);
if (m_pub) m_pub->Publish(msg);
}

54
simulation/halsim_gazebo/src/halsim_gazebo/cpp/main.cpp Normal file
View File

@@ -0,0 +1,54 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <iostream>
#include <HAL/Ports.h>
#include "GazeboAnalogIn.h"
#include "GazeboDIO.h"
#include "GazeboEncoder.h"
#include "GazeboPCM.h"
#include "GazeboPWM.h"
#include "HALSimGazebo.h"
/* Currently, robots never terminate, so we keep a single static object
to access Gazebo with and it is never properly released or cleaned up. */
static HALSimGazebo halsim;
extern "C" {
int HALSIM_InitExtension(void) {
std::cout << "Gazebo Simulator Initializing." << std::endl;
if (!halsim.node.Connect()) {
std::cerr << "Error: unable to connect to Gazebo. Is it running?."
<< std::endl;
return -1;
}
std::cout << "Gazebo Simulator Connected." << std::endl;
for (int i = 0; i < HALSimGazebo::kPWMCount; i++)
halsim.pwms[i] = new GazeboPWM(i, &halsim);
for (int i = 0; i < HALSimGazebo::kPCMCount; i++)
halsim.pcms[i] = new GazeboPCM(0, i, &halsim);
GazeboPCM_SetPressureSwitch(0, true);
for (int i = 0; i < HALSimGazebo::kEncoderCount; i++)
halsim.encoders[i] = new GazeboEncoder(i, &halsim);
int analog_in_count = HAL_GetNumAnalogInputs();
for (int i = 0; i < analog_in_count; i++)
halsim.analog_inputs.push_back(new GazeboAnalogIn(i, &halsim));
int dio_count = HAL_GetNumDigitalChannels();
for (int i = 0; i < dio_count; i++)
halsim.dios.push_back(new GazeboDIO(i, &halsim));
return 0;
}
} // extern "C"

26
simulation/halsim_gazebo/src/halsim_gazebo/headers/GazeboAnalogIn.h Normal file
View File

@@ -0,0 +1,26 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2014-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include "HALSimGazebo.h"
#include "simulation/gz_msgs/msgs.h"
class GazeboAnalogIn {
public:
GazeboAnalogIn(int index, HALSimGazebo* halsim);
void SetInitialized(bool value) { m_initialized = value; }
bool IsInitialized(void) { return m_initialized; }
void Listen(void);
private:
HALSimGazebo* m_halsim;
int m_index;
bool m_initialized;
void Callback(const gazebo::msgs::ConstFloat64Ptr& msg);
gazebo::transport::SubscriberPtr m_sub;
};

26
simulation/halsim_gazebo/src/halsim_gazebo/headers/GazeboDIO.h Normal file
View File

@@ -0,0 +1,26 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2014-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include "HALSimGazebo.h"
#include "simulation/gz_msgs/msgs.h"
class GazeboDIO {
public:
GazeboDIO(int index, HALSimGazebo* halsim);
void SetInitialized(bool value) { m_initialized = value; }
bool IsInitialized(void) { return m_initialized; }
void Listen(void);
private:
HALSimGazebo* m_halsim;
int m_index;
bool m_initialized;
void Callback(const gazebo::msgs::ConstBoolPtr& msg);
gazebo::transport::SubscriberPtr m_sub;
};

32
simulation/halsim_gazebo/src/halsim_gazebo/headers/GazeboEncoder.h Normal file
View File

@@ -0,0 +1,32 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2014-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include "HALSimGazebo.h"
#include "simulation/gz_msgs/msgs.h"
class GazeboEncoder {
public:
GazeboEncoder(int index, HALSimGazebo* halsim);
void SetInitialized(bool value) { m_initialized = value; }
bool IsInitialized(void) { return m_initialized; }
void SetReverse(bool value) { m_reverse = value; }
void Control(const char* command);
void Listen(void);
private:
HALSimGazebo* m_halsim;
int m_index;
bool m_initialized;
bool m_reverse;
void Callback(const gazebo::msgs::ConstFloat64Ptr& msg);
gazebo::transport::PublisherPtr m_pub;
gazebo::transport::SubscriberPtr m_sub;
};

43
simulation/halsim_gazebo/src/halsim_gazebo/headers/GazeboNode.h Normal file
View File

@@ -0,0 +1,43 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2014-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <string>
#include <gazebo/gazebo_client.hh>
#include <gazebo/transport/transport.hh>
class GazeboNode {
public:
bool Connect();
template <typename M>
gazebo::transport::PublisherPtr Advertise(const std::string& topic,
int queueLimit = 10,
bool latch = false) {
return main->Advertise<M>(topic, queueLimit, latch);
}
template <typename M, typename T>
gazebo::transport::SubscriberPtr Subscribe(
const std::string& topic,
void (T::*fp)(const boost::shared_ptr<M const>&), T* obj,
bool latching = false) {
return main->Subscribe(topic, fp, obj, latching);
}
template <typename M>
gazebo::transport::SubscriberPtr Subscribe(
const std::string& topic, void (*fp)(const boost::shared_ptr<M const>&),
bool latching = false) {
return main->Subscribe(topic, fp, latching);
}
private:
gazebo::transport::NodePtr main;
};

29
simulation/halsim_gazebo/src/halsim_gazebo/headers/GazeboPCM.h Normal file
View File

@@ -0,0 +1,29 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2014-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include "HALSimGazebo.h"
class GazeboPCM {
public:
GazeboPCM(int index, int channel, HALSimGazebo* halsim);
void Publish(bool value);
void SetInitialized(bool value) { m_initialized = value; }
bool IsInitialized(void) { return m_initialized; }
private:
HALSimGazebo* m_halsim;
int m_index;
int m_channel;
bool m_initialized;
gazebo::transport::PublisherPtr m_pub;
};
void GazeboPCM_SetPressureSwitch(int index, bool value);

24
simulation/halsim_gazebo/src/halsim_gazebo/headers/GazeboPWM.h Normal file
View File

@@ -0,0 +1,24 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2014-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include "HALSimGazebo.h"
class GazeboPWM {
public:
GazeboPWM(int port, HALSimGazebo* halsim);
void SetInitialized(bool value) { m_initialized = value; }
bool IsInitialized(void) { return m_initialized; }
void Publish(double value);
private:
HALSimGazebo* m_halsim;
gazebo::transport::PublisherPtr m_pub;
bool m_initialized;
int m_port;
};

32
simulation/halsim_gazebo/src/halsim_gazebo/headers/HALSimGazebo.h Normal file
View File

@@ -0,0 +1,32 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2014-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <vector>
#include "GazeboNode.h"
class GazeboPWM;
class GazeboPCM;
class GazeboEncoder;
class GazeboAnalogIn;
class GazeboDIO;
class HALSimGazebo {
public:
static const int kPWMCount = 20;
static const int kPCMCount = 8;
static const int kEncoderCount = 8;
GazeboNode node;
GazeboPWM* pwms[kPWMCount];
GazeboPCM* pcms[kPCMCount];
GazeboEncoder* encoders[kEncoderCount];
std::vector<GazeboAnalogIn*> analog_inputs;
std::vector<GazeboDIO*> dios;
};