mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
[sim] Remove unmaintained Gazebo support (#4736)
This commit is contained in:
Tyler Veness
19
.github/workflows/gazebo.yml
vendored
19
.github/workflows/gazebo.yml
vendored
@@ -1,19 +0,0 @@
|
||||
name: Gazebo
|
||||
|
||||
on: [pull_request, push]
|
||||
|
||||
concurrency:
|
||||
group: ${{ github.workflow }}-${{ github.head_ref || github.ref }}
|
||||
cancel-in-progress: true
|
||||
|
||||
jobs:
|
||||
build:
|
||||
name: "Build"
|
||||
runs-on: ubuntu-22.04
|
||||
container: wpilib/gazebo-ubuntu:22.04
|
||||
steps:
|
||||
- uses: actions/checkout@v3
|
||||
with:
|
||||
fetch-depth: 0
|
||||
- name: Build with Gradle
|
||||
run: ./gradlew simulation:frc_gazebo_plugins:build simulation:halsim_gazebo:build -PbuildServer -PforceGazebo
|
||||
24
README.md
24
README.md
@@ -15,7 +15,6 @@ Welcome to the WPILib project. This repository contains the HAL, WPILibJ, and WP
|
||||
- [Faster builds](#faster-builds)
|
||||
- [Using Development Builds](#using-development-builds)
|
||||
- [Custom toolchain location](#custom-toolchain-location)
|
||||
- [Gazebo simulation](#gazebo-simulation)
|
||||
- [Formatting/Linting](#formattinglinting)
|
||||
- [CMake](#cmake)
|
||||
- [Publishing](#publishing)
|
||||
@@ -114,25 +113,6 @@ If you have installed the FRC Toolchain to a directory other than the default, o
|
||||
./gradlew build -PtoolChainPath=some/path/to/frc/toolchain/bin
|
||||
```
|
||||
|
||||
### Gazebo simulation
|
||||
|
||||
If you also want to force building Gazebo simulation support, add -PforceGazebo. This requires gazebo_transport. We have tested on 14.04 and 15.05, but any correct install of Gazebo should work, even on Windows if you build Gazebo from source. Correct means CMake needs to be able to find gazebo-config.cmake. See [The Gazebo website](https://gazebosim.org/) for installation instructions.
|
||||
|
||||
```bash
|
||||
./gradlew build -PforceGazebo
|
||||
```
|
||||
|
||||
If you prefer to use CMake directly, the you can still do so.
|
||||
The common CMake tasks are wpilibcSim, frc_gazebo_plugins, and gz_msgs
|
||||
|
||||
```bash
|
||||
mkdir build #run this in the root of allwpilib
|
||||
cd build
|
||||
cmake ..
|
||||
make
|
||||
```
|
||||
|
||||
|
||||
### Formatting/linting
|
||||
|
||||
#### wpiformat
|
||||
@@ -164,9 +144,7 @@ The maven artifacts are described in [MavenArtifacts.md](MavenArtifacts.md)
|
||||
|
||||
## Structure and Organization
|
||||
|
||||
The main WPILib code you're probably looking for is in WPILibJ and WPILibC. Those directories are split into shared, sim, and athena. Athena contains the WPILib code meant to run on your roboRIO. Sim is WPILib code meant to run on your computer with Gazebo, and shared is code shared between the two. Shared code must be platform-independent, since it will be compiled with both the ARM cross-compiler and whatever desktop compiler you are using (g++, msvc, etc...).
|
||||
|
||||
The Simulation directory contains extra simulation tools and libraries, such as gz_msgs and JavaGazebo. See sub-directories for more information.
|
||||
The main WPILib code you're probably looking for is in WPILibJ and WPILibC. Those directories are split into shared, sim, and athena. Athena contains the WPILib code meant to run on your roboRIO. Sim is WPILib code meant to run on your computer, and shared is code shared between the two. Shared code must be platform-independent, since it will be compiled with both the ARM cross-compiler and whatever desktop compiler you are using (g++, msvc, etc...).
|
||||
|
||||
The integration test directories for C++ and Java contain test code that runs on our test-system. When you submit code for review, it is tested by those programs. If you add new functionality you should make sure to write tests for it so we don't break it in the future.
|
||||
|
||||
|
||||
@@ -10,8 +10,7 @@
|
||||
* @defgroup hal_extensions Simulator Extensions
|
||||
* @ingroup hal_capi
|
||||
* HAL Simulator Extensions. These are libraries that provide additional
|
||||
* simulator functionality, such as a Gazebo interface, or a more light weight
|
||||
* simulation.
|
||||
* simulator functionality.
|
||||
*
|
||||
* An extension must expose the HALSIM_InitExtension entry point which is
|
||||
* invoked after the library is loaded.
|
||||
|
||||
@@ -39,9 +39,6 @@ include 'glass'
|
||||
include 'outlineviewer'
|
||||
include 'roborioteamnumbersetter'
|
||||
include 'datalogtool'
|
||||
include 'simulation:gz_msgs'
|
||||
include 'simulation:frc_gazebo_plugins'
|
||||
include 'simulation:halsim_gazebo'
|
||||
include 'simulation:halsim_ds_socket'
|
||||
include 'simulation:halsim_gui'
|
||||
include 'simulation:halsim_ws_core'
|
||||
|
||||
@@ -1,9 +1,6 @@
|
||||
if (WITH_GUI)
|
||||
add_subdirectory(halsim_gui)
|
||||
endif()
|
||||
#add_subdirectory(gz_msgs)
|
||||
#add_subdirectory(frc_gazebo_plugins)
|
||||
#add_subdirectory(halsim_gazebo)
|
||||
add_subdirectory(halsim_ds_socket)
|
||||
add_subdirectory(halsim_ws_core)
|
||||
add_subdirectory(halsim_ws_client)
|
||||
|
||||
@@ -1,45 +0,0 @@
|
||||
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 -PforceGazebo`
|
||||
|
||||
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.
|
||||
@@ -1,113 +0,0 @@
|
||||
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.skiplinuxathena = true
|
||||
ext.skiplinuxarm32 = true
|
||||
|
||||
apply from: "${rootDir}/shared/config.gradle"
|
||||
|
||||
/* 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 (project.hasProperty("forceGazebo")) {
|
||||
if (!gazebo_version?.trim()) {
|
||||
println "Gazebo development files are not available. (pkg-config --modversion gazebo failed)"
|
||||
println "forceGazebo 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 ->
|
||||
component.targetBuildTypes 'debug'
|
||||
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 */
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,63 +0,0 @@
|
||||
#!/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"
|
||||
@@ -1,41 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this 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()->Name() + "::" + 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);
|
||||
}
|
||||
@@ -1,55 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this 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;
|
||||
};
|
||||
@@ -1,78 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this 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(ClockTest, 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();
|
||||
}
|
||||
@@ -1,17 +0,0 @@
|
||||
<?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>
|
||||
@@ -1,57 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this 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()->Name() + "::" + 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;
|
||||
}
|
||||
}
|
||||
@@ -1,70 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this 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;
|
||||
};
|
||||
@@ -1,134 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this 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()->Name() + "::" + 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;
|
||||
}
|
||||
}
|
||||
@@ -1,123 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this 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;
|
||||
};
|
||||
@@ -1,99 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this 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()->Name() + "::" + 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->Position(0);
|
||||
} else {
|
||||
return joint->Position(0) * (180.0 / M_PI);
|
||||
}
|
||||
}
|
||||
|
||||
double Encoder::GetVelocity() {
|
||||
if (radians) {
|
||||
return joint->GetVelocity(0);
|
||||
} else {
|
||||
return joint->GetVelocity(0) * (180.0 / M_PI);
|
||||
}
|
||||
}
|
||||
@@ -1,109 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this 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 multiply 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;
|
||||
};
|
||||
@@ -1,111 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this 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()->Name() + "::" + 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->WorldCoGPose().Rot().Euler()[axis];
|
||||
} else {
|
||||
return link->WorldCoGPose().Rot().Euler()[axis] * (180.0 / M_PI);
|
||||
}
|
||||
}
|
||||
|
||||
double Gyro::GetVelocity() {
|
||||
if (radians) {
|
||||
return link->RelativeAngularVel()[axis];
|
||||
} else {
|
||||
return link->RelativeAngularVel()[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;
|
||||
}
|
||||
@@ -1,95 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this 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.
|
||||
enum ROTATION { Roll /*X*/, Pitch /*Y*/, Yaw /*Z*/ };
|
||||
|
||||
/**
|
||||
* \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;
|
||||
};
|
||||
@@ -1,19 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this 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;
|
||||
}
|
||||
@@ -1,34 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this 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;
|
||||
if (radians) {
|
||||
value = joint->Position(0);
|
||||
} else {
|
||||
value = joint->Position(0) * (180.0 / M_PI);
|
||||
}
|
||||
return value >= min && value <= max;
|
||||
}
|
||||
@@ -1,54 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this 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()->Name() + "::" + 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);
|
||||
}
|
||||
@@ -1,26 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this 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.
|
||||
bool Get() override;
|
||||
|
||||
private:
|
||||
gazebo::sensors::ContactSensorPtr sensor;
|
||||
};
|
||||
@@ -1,27 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this 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.
|
||||
bool Get() override;
|
||||
|
||||
private:
|
||||
gazebo::physics::JointPtr joint;
|
||||
double min, max;
|
||||
bool radians;
|
||||
};
|
||||
@@ -1,96 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this 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;
|
||||
};
|
||||
@@ -1,13 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#pragma once
|
||||
|
||||
class Switch {
|
||||
public:
|
||||
virtual ~Switch() = default;
|
||||
|
||||
/// \brief Returns true when the switch is triggered.
|
||||
virtual bool Get() = 0;
|
||||
};
|
||||
@@ -1,93 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this 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()->Name() + "::" + 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();
|
||||
}
|
||||
@@ -1,99 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this 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;
|
||||
};
|
||||
@@ -1,66 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this 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()->Name() + "::" + 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) {
|
||||
gazebo::msgs::Float64 msg;
|
||||
if (radians) {
|
||||
msg.set_data(joint->Position(0) * multiplier);
|
||||
} else {
|
||||
msg.set_data(joint->Position(0) * (180.0 / M_PI) * multiplier);
|
||||
}
|
||||
pub->Publish(msg);
|
||||
}
|
||||
@@ -1,70 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this 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;
|
||||
};
|
||||
@@ -1,53 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this 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()->Name() + "::" + 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);
|
||||
}
|
||||
@@ -1,56 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this 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;
|
||||
};
|
||||
@@ -1,72 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this 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()->Name() + "::" + 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->Position(0) < signal) {
|
||||
joint->SetForce(0, torque);
|
||||
} else if (joint->Position(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;
|
||||
}
|
||||
}
|
||||
@@ -1,67 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this 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;
|
||||
};
|
||||
@@ -1,12 +0,0 @@
|
||||
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 -PforceGazebo on the gradle command line.
|
||||
|
||||
If you are installing FRCSim with the script, then this *should* have be done for you.
|
||||
@@ -1,99 +0,0 @@
|
||||
plugins {
|
||||
id 'cpp'
|
||||
id 'java'
|
||||
id 'com.google.protobuf' version '0.8.17'
|
||||
id 'edu.wpi.first.NativeUtils'
|
||||
}
|
||||
|
||||
description = "A C++ and Java library to pass FRC Simulation Messages in and out of Gazebo."
|
||||
|
||||
/* The simulation does not run on real hardware; so we always skip Athena */
|
||||
ext.skiplinuxathena = true
|
||||
ext.skiplinuxarm32 = true
|
||||
apply from: "${rootDir}/shared/config.gradle"
|
||||
|
||||
/* 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) {
|
||||
}
|
||||
|
||||
ext.skip_gz_msgs = false
|
||||
|
||||
if (project.hasProperty("forceGazebo")) {
|
||||
if (!protobuf_version?.trim()) {
|
||||
println "Protobuf is not available. (pkg-config --modversion protobuf failed)"
|
||||
println "forceGazebo 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') }
|
||||
}
|
||||
|
||||
if (!ext.skip_gz_msgs) {
|
||||
dependencies {
|
||||
implementation "com.google.protobuf:protobuf-java:${protobuf_version}"
|
||||
implementation "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"
|
||||
|
||||
// Disable -Wzero-length-array on Clang
|
||||
if (it.targetPlatform.operatingSystem.isMacOsX()) {
|
||||
it.cppCompiler.args.add('-Wno-error=zero-length-array')
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,47 +0,0 @@
|
||||
/* 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_ */
|
||||
@@ -1,15 +0,0 @@
|
||||
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
|
||||
@@ -1,21 +0,0 @@
|
||||
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
|
||||
@@ -1,15 +0,0 @@
|
||||
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
|
||||
@@ -1,16 +0,0 @@
|
||||
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
|
||||
@@ -1,55 +0,0 @@
|
||||
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.skiplinuxathena = true
|
||||
ext.skiplinuxarm32 = true
|
||||
ext.pluginName = 'halsim_gazebo'
|
||||
|
||||
/* 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 (project.hasProperty("forceGazebo")) {
|
||||
if (!gazebo_version?.trim()) {
|
||||
println "Gazebo development files are not available. (pkg-config --modversion gazebo failed)"
|
||||
println "forceGazebo 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")
|
||||
|
||||
if (!gz_msgs_project.hasProperty('skip_gz_msgs') && !project.hasProperty('skip_frc_plugins')) {
|
||||
|
||||
apply from: "${rootDir}/shared/plugins/setupBuild.gradle"
|
||||
}
|
||||
|
||||
model {
|
||||
binaries {
|
||||
all {
|
||||
if (it instanceof StaticLibraryBinarySpec) {
|
||||
it.buildable = false
|
||||
return
|
||||
}
|
||||
linker.args gazebo_linker_args
|
||||
cppCompiler.args gazebo_cppflags
|
||||
lib project: ":simulation:gz_msgs", library: "gz_msgs", linkage: "static"
|
||||
lib project: ":wpiutil", library: "wpiutil", linkage: "static"
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* TODO: Publish this library */
|
||||
@@ -1,42 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#include "GazeboAnalogIn.h"
|
||||
|
||||
#include <fmt/format.h>
|
||||
#include <hal/Power.h>
|
||||
#include <hal/Value.h>
|
||||
#include <hal/simulation/AnalogInData.h>
|
||||
#include <hal/simulation/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>(
|
||||
fmt::format("~/simulator/analog/{}", 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 = 0;
|
||||
HALSIM_SetAnalogInVoltage(m_index,
|
||||
msg->data() * HAL_GetUserVoltage5V(&status));
|
||||
}
|
||||
@@ -1,36 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#include "GazeboDIO.h"
|
||||
|
||||
#include <fmt/format.h>
|
||||
#include <hal/Value.h>
|
||||
#include <hal/simulation/DIOData.h>
|
||||
#include <hal/simulation/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>(
|
||||
fmt::format("~/simulator/dio/{}", m_index), &GazeboDIO::Callback, this);
|
||||
}
|
||||
|
||||
void GazeboDIO::Callback(const gazebo::msgs::ConstBoolPtr& msg) {
|
||||
HALSIM_SetDIOValue(m_index, msg->data());
|
||||
}
|
||||
@@ -1,73 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#include "GazeboEncoder.h"
|
||||
|
||||
#include <fmt/format.h>
|
||||
#include <hal/Value.h>
|
||||
#include <hal/simulation/EncoderData.h>
|
||||
#include <hal/simulation/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>(
|
||||
fmt::format("~/simulator/encoder/dio/{}/control",
|
||||
HALSIM_GetEncoderDigitalChannelA(m_index)));
|
||||
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>(
|
||||
fmt::format("~/simulator/encoder/dio/{}/position",
|
||||
HALSIM_GetEncoderDigitalChannelA(m_index)),
|
||||
&GazeboEncoder::Callback, this);
|
||||
}
|
||||
|
||||
void GazeboEncoder::Callback(const gazebo::msgs::ConstFloat64Ptr& msg) {
|
||||
HALSIM_SetEncoderCount(m_index, msg->data() * (m_reverse ? -1 : 1));
|
||||
}
|
||||
@@ -1,17 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this 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;
|
||||
}
|
||||
@@ -1,51 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#include "GazeboPCM.h"
|
||||
|
||||
#include <fmt/format.h>
|
||||
#include <hal/Value.h>
|
||||
#include <hal/simulation/CTREPCMData.h>
|
||||
#include <hal/simulation/NotifyListener.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_RegisterCTREPCMInitializedCallback(index, init_callback, this, true);
|
||||
HALSIM_RegisterCTREPCMSolenoidOutputCallback(index, channel, output_callback,
|
||||
this, true);
|
||||
}
|
||||
|
||||
void GazeboPCM::Publish(bool value) {
|
||||
if (!m_pub) {
|
||||
m_pub = m_halsim->node.Advertise<gazebo::msgs::Bool>(
|
||||
fmt::format("~/simulator/pneumatic/{}/{}", m_index + 1, 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_SetCTREPCMPressureSwitch(index, value);
|
||||
}
|
||||
@@ -1,44 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#include "GazeboPWM.h"
|
||||
|
||||
#include <fmt/format.h>
|
||||
#include <hal/Value.h>
|
||||
#include <hal/simulation/NotifyListener.h>
|
||||
#include <hal/simulation/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>(
|
||||
fmt::format("~/simulator/pwm/{}", 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);
|
||||
}
|
||||
@@ -1,50 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#include <fmt/core.h>
|
||||
#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) {
|
||||
fmt::print("Gazebo Simulator Initializing.\n");
|
||||
|
||||
if (!halsim.node.Connect()) {
|
||||
fmt::print(stderr,
|
||||
"Error: unable to connect to Gazebo. Is it running?.\n");
|
||||
return -1;
|
||||
}
|
||||
fmt::print("Gazebo Simulator Connected.\n");
|
||||
|
||||
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"
|
||||
@@ -1,23 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this 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;
|
||||
};
|
||||
@@ -1,23 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this 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;
|
||||
};
|
||||
@@ -1,29 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this 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;
|
||||
};
|
||||
@@ -1,40 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this 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;
|
||||
};
|
||||
@@ -1,26 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this 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);
|
||||
@@ -1,21 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this 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;
|
||||
};
|
||||
@@ -1,29 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this 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;
|
||||
};
|
||||
Reference in New Issue
Block a user