mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
Removed modules from the simulation infrastructure and refactored FRCPlugin.
Pneumatics still have CAN modules. The refactored code is now eight plugins for sensors and actuators. There is some code reuse that should be refactored out, but that level of abstraction will wait until we figure out how these plugins are integrating with gazebo proper. Change-Id: I357e695ef05af6dda83a39ba60380686bd57d11a Closes: artf2610, artf2623
This commit is contained in:
Colby Skeggs
committed by
Alex Henning
Alex Henning
parent
3b4718fc92
commit
8ae64a12ea
13
simulation/debs/.gitignore
vendored
13
simulation/debs/.gitignore
vendored
@@ -16,6 +16,19 @@ trash/
|
||||
*/*/include/
|
||||
*/*/msgs/
|
||||
|
||||
*/*/clock/
|
||||
*/*/dc_motor/
|
||||
*/*/docs/
|
||||
*/*/encoder/
|
||||
*/*/*.doxy
|
||||
*/*/gyro/
|
||||
*/*/limit_switch/
|
||||
*/*/pneumatic_piston/
|
||||
*/*/potentiometer/
|
||||
*/*/rangefinder/
|
||||
*/*/*.gradle
|
||||
frcsim-gazebo-plugins/frcsim-gazebo-plugins/Makefile
|
||||
|
||||
# General Build Artifacts
|
||||
*.dsc
|
||||
*.deb
|
||||
|
||||
@@ -1,20 +1,19 @@
|
||||
codename=trusty
|
||||
allwpilib=../..
|
||||
|
||||
version=0.1
|
||||
package-version=1
|
||||
gazebo-plugins-version=$(version)
|
||||
gazebo-plugins-package-version=$(gazebo-plugins-version)-$(package-version)
|
||||
gazebo-models-version=$(version)
|
||||
gazebo-models-package-version=$(gazebo-models-version)-$(package-version)
|
||||
eclipse-plugins-version=$(version)
|
||||
eclipse-plugins-package-version=$(eclipse-plugins-version)-$(package-version)
|
||||
eclipse-toolchain-version=$(version)
|
||||
eclipse-toolchain-package-version=$(eclipse-toolchain-version)-$(package-version)
|
||||
libwpilibsim-version=$(version)
|
||||
libwpilibsim-package-version=$(libwpilibsim-version)-$(package-version)
|
||||
frcsim-version=$(version)
|
||||
frcsim-package-version=$(frcsim-version)-$(package-version)
|
||||
package-version = 1
|
||||
gazebo-plugins-version = 0.2
|
||||
gazebo-plugins-package-version = $(gazebo-plugins-version)-$(package-version)
|
||||
gazebo-models-version = 0.2
|
||||
gazebo-models-package-version = $(gazebo-models-version)-$(package-version)
|
||||
eclipse-plugins-version = 0.1
|
||||
eclipse-plugins-package-version = $(eclipse-plugins-version)-$(package-version)
|
||||
eclipse-toolchain-version = 0.1
|
||||
eclipse-toolchain-package-version = $(eclipse-toolchain-version)-$(package-version)
|
||||
libwpilibsim-version = 0.1
|
||||
libwpilibsim-package-version = $(libwpilibsim-version)-$(package-version)
|
||||
frcsim-version = 0.1
|
||||
frcsim-package-version = $(frcsim-version)-$(package-version)
|
||||
|
||||
all: debs update-repository
|
||||
|
||||
@@ -89,7 +88,7 @@ clean: pre-orig-clean
|
||||
pull: clean pull-gazebo-plugins pull-eclipse-plugins pull-libwpilibsim-cpp orig
|
||||
|
||||
pull-gazebo-plugins:
|
||||
cp -rf -t frcsim-gazebo-plugins/frcsim-gazebo-plugins/ $(allwpilib)/simulation/frc_gazebo_plugin/*
|
||||
cp -rf -t frcsim-gazebo-plugins/frcsim-gazebo-plugins/ $(allwpilib)/simulation/frc_gazebo_plugins/*
|
||||
echo Increment version?
|
||||
|
||||
pull-eclipse-plugins:
|
||||
|
||||
@@ -1,3 +1,9 @@
|
||||
frcsim-gazebo-models (0.2-1) trusty; urgency=medium
|
||||
|
||||
* New models using the smaller plugins
|
||||
|
||||
-- Alex Henning <alex@thoriumrobotics.com> Mon, 07 Jul 2014 12:52:50 -0700
|
||||
|
||||
frcsim-gazebo-models (0.1-1) trusty; urgency=low
|
||||
|
||||
* Initial release.
|
||||
|
||||
@@ -1 +0,0 @@
|
||||
protoc --cpp_out=src/ msgs/float64.proto
|
||||
@@ -1,3 +1,9 @@
|
||||
frcsim-gazebo-plugins (0.2-1) trusty; urgency=medium
|
||||
|
||||
* Split plugin into many parts.
|
||||
|
||||
-- Alex Henning <alex@thoriumrobotics.com> Wed, 02 Jul 2014 13:50:38 -0700
|
||||
|
||||
frcsim-gazebo-plugins (0.1-1) trusty; urgency=low
|
||||
|
||||
* Initial release
|
||||
|
||||
Binary file not shown.
@@ -1,16 +0,0 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(frc_gazebo_plugin)
|
||||
|
||||
include (FindPkgConfig)
|
||||
if (PKG_CONFIG_FOUND)
|
||||
pkg_check_modules(GAZEBO gazebo)
|
||||
endif()
|
||||
|
||||
find_package(gazebo REQUIRED)
|
||||
|
||||
file(GLOB_RECURSE SRC_FILES src/*.cpp src/*.cc)
|
||||
include_directories(src ${Boost_INCLUDE_DIR} ${GAZEBO_INCLUDE_DIRS})
|
||||
add_library(${PROJECT_NAME} SHARED ${SRC_FILES})
|
||||
|
||||
link_directories(${GAZEBO_LIBRARY_DIRS})
|
||||
target_link_libraries(${PROJECT_NAME} ${GAZEBO_LIBRARIES} tinyxml2)
|
||||
@@ -1 +0,0 @@
|
||||
protoc --cpp_out=src/ msgs/float64.proto
|
||||
@@ -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,69 +0,0 @@
|
||||
|
||||
#include "FRCPlugin.h"
|
||||
|
||||
#include <gazebo/physics/Model.hh>
|
||||
#include <gazebo/physics/Link.hh>
|
||||
#include <gazebo/physics/Joint.hh>
|
||||
#include <gazebo/physics/PhysicsEngine.hh>
|
||||
#include <gazebo/sensors/sensors.hh>
|
||||
#include <gazebo/transport/transport.hh>
|
||||
#include <gazebo/msgs/msgs.hh>
|
||||
|
||||
#include <string.h>
|
||||
#include <boost/lexical_cast.hpp>
|
||||
|
||||
#include "RobotMapParser.h"
|
||||
#include "components/Motor.h"
|
||||
#include "components/Potentiometer.h"
|
||||
#include "components/Encoder.h"
|
||||
|
||||
using namespace gazebo;
|
||||
GZ_REGISTER_MODEL_PLUGIN(FRCPlugin)
|
||||
|
||||
FRCPlugin::FRCPlugin() {}
|
||||
|
||||
FRCPlugin::~FRCPlugin() {}
|
||||
|
||||
|
||||
void FRCPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
|
||||
model = _model;
|
||||
|
||||
// Create robot components from RobotMap
|
||||
if (_sdf->HasElement("robotmap")) {
|
||||
std::string robotmap = common::SystemPaths::Instance()->FindFileURI(_sdf->Get<std::string>("robotmap"));
|
||||
std::cout << "Loading RobotMap: " << robotmap << std::endl;
|
||||
RobotMapParser map("simulator", robotmap, model);
|
||||
map.parse(components);
|
||||
} else {
|
||||
std::cerr << "No RobotMap specified this robot cannnot be controlled." << std::endl;
|
||||
}
|
||||
|
||||
// Connect to Gazebo transport for messaging
|
||||
gzNode = transport::NodePtr(new transport::Node());
|
||||
gzNode->Init("frc");
|
||||
for (int i = 0; i < components.size(); i++) {
|
||||
components[i]->connect(gzNode);
|
||||
}
|
||||
enabled = false;
|
||||
sub = gzNode->Subscribe("~/ds/state", &FRCPlugin::dsCallback, this);
|
||||
time_pub = gzNode->Advertise<msgs::Float64>("~/time");
|
||||
|
||||
// Connect to the world update event.
|
||||
// This will trigger the Update function every Gazebo iteration
|
||||
updateConn = event::Events::ConnectWorldUpdateBegin(boost::bind(&FRCPlugin::Update, this, _1));
|
||||
}
|
||||
|
||||
// Update all RobotComponents
|
||||
void FRCPlugin::Update(const common::UpdateInfo &_info) {
|
||||
seconds_msg.set_data(_info.simTime.Double());
|
||||
time_pub->Publish(seconds_msg);
|
||||
|
||||
for (int i = 0; i < components.size(); i++) {
|
||||
components[i]->update(enabled);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void FRCPlugin::dsCallback(const msgs::ConstDriverStationPtr &msg) {
|
||||
enabled = msg->enabled();
|
||||
}
|
||||
@@ -1,41 +0,0 @@
|
||||
#ifndef _driveBot_H_
|
||||
#define _driveBot_H_
|
||||
|
||||
#include <gazebo/gazebo.hh>
|
||||
#include <gazebo/physics/PhysicsIface.hh>
|
||||
|
||||
#include "components/RobotComponent.h"
|
||||
#include "msgs/msgs.h"
|
||||
|
||||
using namespace gazebo;
|
||||
|
||||
class FRCPlugin: public ModelPlugin {
|
||||
public:
|
||||
FRCPlugin();
|
||||
~FRCPlugin();
|
||||
//function to load all of the elements
|
||||
void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
|
||||
//function to update all of the elements
|
||||
void Update(const common::UpdateInfo & _info);
|
||||
|
||||
private:
|
||||
//the vector containing all of the robot components.
|
||||
std::vector<RobotComponentPtr> components;
|
||||
|
||||
physics::ModelPtr model;
|
||||
|
||||
physics::LinkPtr frame;
|
||||
|
||||
/// \brief Pointer to the world update function.
|
||||
event::ConnectionPtr updateConn;
|
||||
|
||||
transport::NodePtr gzNode;
|
||||
|
||||
transport::SubscriberPtr sub;
|
||||
volatile bool enabled;
|
||||
transport::PublisherPtr time_pub;
|
||||
msgs::Float64 seconds_msg;
|
||||
void dsCallback(const msgs::ConstDriverStationPtr &msg);
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -1,335 +0,0 @@
|
||||
|
||||
#include "RobotMapParser.h"
|
||||
|
||||
#include "components/Motor.h"
|
||||
#include "components/Piston.h"
|
||||
#include "components/Potentiometer.h"
|
||||
#include "components/Encoder.h"
|
||||
#include "components/Gyro.h"
|
||||
#include "components/InternalLimitSwitch.h"
|
||||
#include "components/ExternalLimitSwitch.h"
|
||||
#include "components/Rangefinder.h"
|
||||
#include <string.h>
|
||||
#include <boost/lexical_cast.hpp>
|
||||
#include <boost/pointer_cast.hpp>
|
||||
#include <sdf/sdf.hh>
|
||||
|
||||
RobotMapParser::RobotMapParser(string robot, string filename, physics::ModelPtr model) {
|
||||
this->robot = robot;
|
||||
this->filename = filename;
|
||||
this->model = model;
|
||||
this->world = model->GetWorld();
|
||||
}
|
||||
|
||||
/*
|
||||
* Parse the file and return a list of robot components for the robot
|
||||
*/
|
||||
void RobotMapParser::parse(vector<RobotComponentPtr> &components) {
|
||||
// Load an xml document with <robotmap> root tag
|
||||
XMLDocument doc;
|
||||
doc.LoadFile(filename.c_str());
|
||||
|
||||
// Iterate over all of the child tags and parse them accordingly.
|
||||
for (XMLNode *current = doc.FirstChildElement("robotmap")->FirstChild();
|
||||
current != NULL; current = current->NextSibling()) {
|
||||
if (current->ToElement() == NULL) {
|
||||
// We don't care about comments and other non-element types
|
||||
} else if (strcmp(current->Value(), "motor") == 0) {
|
||||
components.push_back(parseMotor(current->ToElement()));
|
||||
} else if (strcmp(current->Value(), "piston") == 0) {
|
||||
components.push_back(parsePiston(current->ToElement()));
|
||||
} else if (strcmp(current->Value(), "potentiometer") == 0) {
|
||||
components.push_back(parsePotentiometer(current->ToElement()));
|
||||
} else if (strcmp(current->Value(), "encoder") == 0) {
|
||||
components.push_back(parseEncoder(current->ToElement()));
|
||||
} else if (strcmp(current->Value(), "gyro") == 0) {
|
||||
components.push_back(parseGyro(current->ToElement()));
|
||||
} else if (strcmp(current->Value(), "internal-limit-switch") == 0) {
|
||||
components.push_back(parseInternalLimitSwitch(current->ToElement()));
|
||||
} else if (strcmp(current->Value(), "external-limit-switch") == 0) {
|
||||
components.push_back(parseExternalLimitSwitch(current->ToElement()));
|
||||
} else if (strcmp(current->Value(), "rangefinder") == 0) {
|
||||
components.push_back(parseRangefinder(current->ToElement()));
|
||||
} else {
|
||||
cerr << "Unknown element: " << current->Value() << endl;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Parse a motor tag. Motor tags have a joint the act on, a location
|
||||
* they're "plugged into" and parameters that control their
|
||||
* behavior. The template of a motor tag is as follows:
|
||||
*
|
||||
* <motor>
|
||||
* <joint>{{ joint name (string) }}</joint>
|
||||
* <location>{{ location (see below for formats }}</location>
|
||||
* <parameters>
|
||||
* <multiplier>{{ torque multiplication factor (number) }}</multiplier>
|
||||
* </parameters>
|
||||
* </motor>
|
||||
*/
|
||||
RobotComponentPtr RobotMapParser::parseMotor(XMLElement *node) {
|
||||
string location = locationToPath(node->FirstChildElement("location"));
|
||||
physics::JointPtr joint = getJoint(node);
|
||||
double multiplier = boost::lexical_cast<double>(getChildTagValue(
|
||||
node->FirstChildElement("parameters"), "multiplier").c_str());
|
||||
return new Motor(location, joint, multiplier);
|
||||
}
|
||||
|
||||
/**
|
||||
* Parse a piston tag. Piston tags have a joint the act on, a location
|
||||
* they're "plugged into", the area of the cylinder and optionally a
|
||||
* direction. The template of a piston| tag is as follows:
|
||||
*
|
||||
* <piston>
|
||||
* <joint>{{ joint name (string) }}</joint>
|
||||
* <location>{{ location (see below for formats }}</location>
|
||||
* <area units="{{ square inches (default) or square meters}}">
|
||||
* {{ area in proper units (number) }}
|
||||
* </area>
|
||||
* <forward-force>{{ area in newtons (number) }}</forward-force>
|
||||
* <reverse-force>{{ area in newtons (number) }}</reverse-force>
|
||||
* <direction>{{ forward (default) or reversed }}</direction>
|
||||
* </piston>
|
||||
*/
|
||||
RobotComponentPtr RobotMapParser::parsePiston(XMLElement *node) {
|
||||
string location = locationToPath(node->FirstChildElement("location"));
|
||||
physics::JointPtr joint = getJoint(node);
|
||||
double forward = boost::lexical_cast<double>(getChildTagValue(node, "forward-force").c_str());
|
||||
double reverse = boost::lexical_cast<double>(getChildTagValue(node, "reverse-force").c_str());
|
||||
if (getChildTagValue(node, "direction") == "reversed") {
|
||||
forward = -forward;
|
||||
reverse = -reverse;
|
||||
}
|
||||
return new Piston(location, joint, forward, reverse);
|
||||
}
|
||||
|
||||
/**
|
||||
* Parse a potentiometer tag. Potentiometer tags have a joint they measure, a
|
||||
* location they're "plugged into" and one of three types (linear,
|
||||
* degrees or radians). The template of a potentiometer tag is as
|
||||
* follows:
|
||||
*
|
||||
* <potentiometer>
|
||||
* <joint>{{ joint name (string) }}</joint>
|
||||
* <location>{{ location (see below for formats }}</location>
|
||||
* <type>{{ one of linear, degrees, radians }}</type>
|
||||
* </potentiometer>
|
||||
*/
|
||||
RobotComponentPtr RobotMapParser::parsePotentiometer(XMLElement *node) {
|
||||
string location = locationToPath(node->FirstChildElement("location"));
|
||||
physics::JointPtr joint = getJoint(node);
|
||||
|
||||
string radians_str = getChildTagValue(node, "type", "degrees");
|
||||
bool radians;
|
||||
if (radians_str == "linear" || radians_str == "radians") {
|
||||
radians = true;
|
||||
} else {
|
||||
radians = false;
|
||||
}
|
||||
|
||||
return new Potentiometer(location, joint, radians);
|
||||
}
|
||||
|
||||
/**
|
||||
* Parse an encoder tag. Encoder tags have a joint they measure, a
|
||||
* location they're "plugged into". The template of an encoder tag
|
||||
* is as follows:
|
||||
*
|
||||
* <encoder>
|
||||
* <joint>{{ joint name (string) }}</joint>
|
||||
* <location>{{ location (see below for formats }}</location>
|
||||
* </encoder>
|
||||
*/
|
||||
RobotComponentPtr RobotMapParser::parseEncoder(XMLElement *node) {
|
||||
string location = locationToPath(node->FirstChildElement("location"));
|
||||
physics::JointPtr joint = getJoint(node);
|
||||
return new Encoder(location, joint);
|
||||
}
|
||||
|
||||
/**
|
||||
* Parse a gyro tag. Gyro tags have a joint they measure, a location
|
||||
* they're "plugged into" and one of three axes (roll, pitch,
|
||||
* yaw). The template of a gyro tag is as follows:
|
||||
*
|
||||
* <gyro>
|
||||
* <link>{{ link name (string) }}</link>
|
||||
* <location>{{ location (see below for formats }}</location>
|
||||
* <axis>{{ one of yaw, pitch, roll }}</axis>
|
||||
* </gyro>
|
||||
*/
|
||||
RobotComponentPtr RobotMapParser::parseGyro(XMLElement *node) {
|
||||
string location = locationToPath(node->FirstChildElement("location"));
|
||||
physics::LinkPtr link = getLink(node);
|
||||
ROTATION axis;
|
||||
string axisString = getChildTagValue(node, "axis");
|
||||
if (axisString == "roll") axis = Roll;
|
||||
if (axisString == "pitch") axis = Pitch;
|
||||
if (axisString == "yaw") axis = Yaw;
|
||||
return new Gyro(location, link, axis);
|
||||
}
|
||||
|
||||
/**
|
||||
* Parse a InternalLimitSwitch tag. InternalLimitSwitch tags have a
|
||||
* joint they measure and a location they're "plugged into". The
|
||||
* template of an InteranlLimitSwitch tag is as follows:
|
||||
*
|
||||
* <internal-limit-switch>
|
||||
* <joint>{{ joint name (string) }}</joint>
|
||||
* <location>{{ location (see below for formats }}</location>
|
||||
* <type>{{ one of linear, degrees, radians }}</type>
|
||||
* <min>{{ min (number) }}</min>
|
||||
* <max>{{ max (number }}</max>
|
||||
* </internal-limit-switch>
|
||||
*/
|
||||
RobotComponentPtr RobotMapParser::parseInternalLimitSwitch(XMLElement *node) {
|
||||
string location = locationToPath(node->FirstChildElement("location"));
|
||||
physics::JointPtr joint = getJoint(node);
|
||||
|
||||
double min = boost::lexical_cast<double>(getChildTagValue(node, "min", "0"));
|
||||
double max = boost::lexical_cast<double>(getChildTagValue(node, "max", "1"));
|
||||
|
||||
string radians_str = getChildTagValue(node, "type", "degrees");
|
||||
bool radians;
|
||||
if (radians_str == "linear" || radians_str == "radians") {
|
||||
radians = true;
|
||||
} else {
|
||||
radians = false;
|
||||
}
|
||||
return new InternalLimitSwitch(location, joint, min, max, radians);
|
||||
}
|
||||
|
||||
/**
|
||||
* Parse a ExternalLimitSwitch tag. ExternalLimitSwitch tags have a
|
||||
* joint they measure and a location they're "plugged into". The
|
||||
* template of an ExternalLimitSwitch tag is as follows:
|
||||
*
|
||||
* <external-limit-switch>
|
||||
* <sensor>{{ sensor name (string) }}</sensor>
|
||||
* <location>{{ location (see below for formats }}</location>
|
||||
* </external-limit-switch>
|
||||
*/
|
||||
RobotComponentPtr RobotMapParser::parseExternalLimitSwitch(XMLElement *node) {
|
||||
string location = locationToPath(node->FirstChildElement("location"));
|
||||
sensors::ContactSensorPtr sensor =
|
||||
boost::dynamic_pointer_cast<sensors::ContactSensor>(getSensor(node));
|
||||
return new ExternalLimitSwitch(location, sensor);
|
||||
}
|
||||
|
||||
/**
|
||||
* Parse a rangefinder tag. Rangefinder tags have a joint they measure
|
||||
* and a location they're "plugged into". The template of a rangfinder
|
||||
* tag is as follows:
|
||||
*
|
||||
* <rangefinder>
|
||||
* <sensor>{{ sensor name (string) }}</sensor>
|
||||
* <location>{{ location (see below for formats }}</location>
|
||||
* </rangefinder>
|
||||
*/
|
||||
RobotComponentPtr RobotMapParser::parseRangefinder(XMLElement *node) {
|
||||
string location = locationToPath(node->FirstChildElement("location"));
|
||||
sensors::SonarSensorPtr sensor =
|
||||
boost::dynamic_pointer_cast<sensors::SonarSensor>(getSensor(node));
|
||||
return new Rangefinder(location, sensor);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Convert the location tag to a topic name to subscribe/publish
|
||||
* to. Currently, there are two supported location styles: double_port
|
||||
* and single port.
|
||||
*/
|
||||
string RobotMapParser::locationToPath(XMLElement *location) {
|
||||
if (location == NULL)
|
||||
return "";
|
||||
else if (location->Attribute("style", "double_port"))
|
||||
return doublePortLocationToPath(location);
|
||||
else
|
||||
return portLocationToPath(location);
|
||||
}
|
||||
|
||||
string RobotMapParser::portLocationToPath(XMLElement *location) {
|
||||
string type = getChildTagValue(location, "type");
|
||||
string module = getChildTagValue(location, "module");
|
||||
string pin = getChildTagValue(location, "pin");
|
||||
return "~/" + robot + "/" + type + "/" + module + "/" + pin;
|
||||
}
|
||||
|
||||
|
||||
string RobotMapParser::doublePortLocationToPath(XMLElement *location) {
|
||||
string type = getChildTagValue(location, "type");
|
||||
string module1 = getChildTagValue(location, "module1");
|
||||
string pin1 = getChildTagValue(location, "pin1");
|
||||
string module2 = getChildTagValue(location, "module2");
|
||||
string pin2 = getChildTagValue(location, "pin2");
|
||||
// Swap pins if needed, to maintain consistent ordering
|
||||
if ((module2 < module1) || ((module2 == module1) && (pin2 < pin1))) {
|
||||
string module = module2;
|
||||
string pin = pin2;
|
||||
module2 = module1;
|
||||
pin2 = pin1;
|
||||
module1 = module;
|
||||
pin1 = pin;
|
||||
}
|
||||
return "~/" + robot + "/" + type + "/" + module1 + "/" + pin1 + "/" + module2 + "/" + pin2;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the value of the first child tag element of the given node
|
||||
* with the matching tag name.
|
||||
*/
|
||||
string RobotMapParser::getChildTagValue(XMLElement *node, string tag, string default_value) {
|
||||
if (node->FirstChildElement(tag.c_str()) != NULL) {
|
||||
return node->FirstChildElement(tag.c_str())->FirstChild()->Value();
|
||||
} else {
|
||||
return default_value;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the matching attribute of the given node.
|
||||
*/
|
||||
string RobotMapParser::getTagAttribute(XMLElement *node, string attr, string default_value) {
|
||||
if (node->Attribute(attr.c_str())) {
|
||||
return node->Attribute(attr.c_str());
|
||||
} else {
|
||||
return default_value;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the matching attribute of the first child tag element of the given node
|
||||
* with the matching tag name.
|
||||
*/
|
||||
string RobotMapParser::getChildTagAttribute(XMLElement *node, string tag,
|
||||
string attr, string default_value) {
|
||||
if (node->FirstChildElement(tag.c_str()) != NULL) {
|
||||
string value = node->FirstChildElement(tag.c_str())->Attribute(attr.c_str());
|
||||
if (value != "") {
|
||||
return value;
|
||||
}
|
||||
}
|
||||
return default_value;
|
||||
}
|
||||
|
||||
physics::JointPtr RobotMapParser::getJoint(XMLElement *node) {
|
||||
string joint_name = node->FirstChildElement("joint")->FirstChild()->Value();
|
||||
physics::JointPtr joint = model->GetJoint(joint_name);
|
||||
if (joint == NULL) cerr << "Joint doesn't exist: " << joint_name << endl;
|
||||
return joint;
|
||||
}
|
||||
|
||||
physics::LinkPtr RobotMapParser::getLink(XMLElement *node) {
|
||||
string link_name = node->FirstChildElement("link")->FirstChild()->Value();
|
||||
physics::LinkPtr link = model->GetLink(link_name);
|
||||
if (link == NULL) cerr << "Link doesn't exist: " << link_name << endl;
|
||||
return link;
|
||||
}
|
||||
|
||||
sensors::SensorPtr RobotMapParser::getSensor(XMLElement *node) {
|
||||
string sensor_name = node->FirstChildElement("sensor")->FirstChild()->Value();
|
||||
sensors::SensorPtr sensor = sensors::get_sensor(sensor_name);
|
||||
if (sensor == NULL) cerr << "Sensor doesn't exist: " << sensor_name << endl;
|
||||
return sensor;
|
||||
}
|
||||
@@ -1,46 +0,0 @@
|
||||
|
||||
#include "components/RobotComponent.h"
|
||||
#include <gazebo/sensors/sensors.hh>
|
||||
#include <gazebo/physics/Model.hh>
|
||||
|
||||
#include <tinyxml2.h>
|
||||
|
||||
#ifndef _ROBOTMAPPARSER_H_
|
||||
#define _ROBOTMAPPARSER_H_
|
||||
|
||||
using namespace std;
|
||||
using namespace tinyxml2;
|
||||
using namespace gazebo;
|
||||
|
||||
class RobotMapParser {
|
||||
public:
|
||||
RobotMapParser(string robot, string filename, physics::ModelPtr model);
|
||||
void parse(vector<RobotComponentPtr> &components);
|
||||
|
||||
private:
|
||||
string robot, filename;
|
||||
physics::ModelPtr model;
|
||||
physics::WorldPtr world;
|
||||
|
||||
RobotComponentPtr parseMotor(XMLElement *node);
|
||||
RobotComponentPtr parsePiston(XMLElement *node);
|
||||
RobotComponentPtr parsePotentiometer(XMLElement *node);
|
||||
RobotComponentPtr parseEncoder(XMLElement *node);
|
||||
RobotComponentPtr parseGyro(XMLElement *node);
|
||||
RobotComponentPtr parseInternalLimitSwitch(XMLElement *node);
|
||||
RobotComponentPtr parseExternalLimitSwitch(XMLElement *node);
|
||||
RobotComponentPtr parseRangefinder(XMLElement *node);
|
||||
|
||||
string locationToPath(XMLElement *location);
|
||||
string portLocationToPath(XMLElement *location);
|
||||
string doublePortLocationToPath(XMLElement *location);
|
||||
|
||||
string getChildTagValue(XMLElement *node, string tag, string default_value="");
|
||||
string getTagAttribute(XMLElement *node, string attr, string default_value="");
|
||||
string getChildTagAttribute(XMLElement *node, string tag, string attr, string default_value="");
|
||||
physics::JointPtr getJoint(XMLElement *node);
|
||||
physics::LinkPtr getLink(XMLElement *node);
|
||||
sensors::SensorPtr getSensor(XMLElement *node);
|
||||
};
|
||||
|
||||
#endif /* _ROBOTMAPPARSER_H_ */
|
||||
@@ -1,52 +0,0 @@
|
||||
|
||||
#include "components/Encoder.h"
|
||||
#define _USE_MATH_DEFINES
|
||||
#include <math.h>
|
||||
|
||||
Encoder::Encoder(std::string topic, physics::JointPtr joint) {
|
||||
std::cout << "Initializing encoder: " << topic << std::endl;
|
||||
this->topic = topic;
|
||||
this->joint = joint;
|
||||
}
|
||||
|
||||
Encoder::~Encoder() {
|
||||
|
||||
}
|
||||
|
||||
void Encoder::connect(transport::NodePtr node) {
|
||||
command_sub = node->Subscribe(topic+"/control", &Encoder::callback, this);
|
||||
pos_pub = node->Advertise<msgs::Float64>(topic+"/position");
|
||||
vel_pub = node->Advertise<msgs::Float64>(topic+"/velocity");
|
||||
zero = joint->GetAngle(0).Degree();
|
||||
stopped = true;
|
||||
stop_value = 0;
|
||||
}
|
||||
|
||||
void Encoder::callback(const msgs::ConstStringPtr &msg) {
|
||||
std::string command = msg->data();
|
||||
if (command == "reset") {
|
||||
zero = joint->GetAngle(0).Degree();
|
||||
} else if (command == "start") {
|
||||
stopped = false;
|
||||
zero = (joint->GetAngle(0).Degree() - stop_value);
|
||||
} else if (command == "stop") {
|
||||
stopped = true;
|
||||
stop_value = joint->GetAngle(0).Degree();
|
||||
} else {
|
||||
std::cout << "WARNING: Encoder got unknown command '" << command << "'." << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
void Encoder::update(bool enabled) {
|
||||
if (stopped) {
|
||||
pos_msg.set_data(stop_value);
|
||||
pos_pub->Publish(pos_msg);
|
||||
vel_msg.set_data(0);
|
||||
vel_pub->Publish(vel_msg);
|
||||
} else {
|
||||
pos_msg.set_data(joint->GetAngle(0).Degree() - zero);
|
||||
pos_pub->Publish(pos_msg);
|
||||
vel_msg.set_data(joint->GetVelocity(0) * (180.0 / M_PI));
|
||||
vel_pub->Publish(vel_msg);
|
||||
}
|
||||
}
|
||||
@@ -1,28 +0,0 @@
|
||||
|
||||
#include "components/RobotComponent.h"
|
||||
|
||||
#include <gazebo/physics/Joint.hh>
|
||||
|
||||
#ifndef _COMPONENTS_ENCODER_H_
|
||||
#define _COMPONENTS_ENCODER_H_
|
||||
|
||||
class Encoder : public RobotComponent {
|
||||
public:
|
||||
Encoder(std::string topic, physics::JointPtr joint);
|
||||
virtual ~Encoder();
|
||||
|
||||
void connect(transport::NodePtr node);
|
||||
void update(bool enabled);
|
||||
private:
|
||||
std::string topic;
|
||||
bool stopped;
|
||||
double zero, stop_value;
|
||||
transport::SubscriberPtr command_sub;
|
||||
transport::PublisherPtr pos_pub, vel_pub;
|
||||
msgs::Float64 pos_msg, vel_msg;
|
||||
physics::JointPtr joint;
|
||||
|
||||
void callback(const msgs::ConstStringPtr &msg);
|
||||
};
|
||||
|
||||
#endif /* _COMPONENTS_ENCODER_H_ */
|
||||
@@ -1,21 +0,0 @@
|
||||
|
||||
#include "components/ExternalLimitSwitch.h"
|
||||
|
||||
ExternalLimitSwitch::ExternalLimitSwitch(std::string topic, sensors::ContactSensorPtr sensor) {
|
||||
std::cout << "Initializing external limit switch: " << topic << std::endl;
|
||||
this->topic = topic;
|
||||
this->sensor = sensor;
|
||||
}
|
||||
|
||||
ExternalLimitSwitch::~ExternalLimitSwitch() {
|
||||
|
||||
}
|
||||
|
||||
void ExternalLimitSwitch::connect(transport::NodePtr node) {
|
||||
pub = node->Advertise<msgs::Bool>(topic);
|
||||
}
|
||||
|
||||
void ExternalLimitSwitch::update(bool enabled) {
|
||||
msg.set_data(sensor->GetContacts().contact().size() > 0);
|
||||
pub->Publish(msg);
|
||||
}
|
||||
@@ -1,25 +0,0 @@
|
||||
|
||||
#include "components/RobotComponent.h"
|
||||
#include "msgs/bool.pb.h"
|
||||
|
||||
#include <gazebo/sensors/sensors.hh>
|
||||
|
||||
#ifndef _COMPONENTS_EXTERNAL_LIMIT_SWITCH_H_
|
||||
#define _COMPONENTS_EXTERNAL_LIMIT_SWITCH_H_
|
||||
|
||||
class ExternalLimitSwitch : public RobotComponent {
|
||||
public:
|
||||
ExternalLimitSwitch(std::string topic, sensors::ContactSensorPtr sensor);
|
||||
virtual ~ExternalLimitSwitch();
|
||||
|
||||
void connect(transport::NodePtr node);
|
||||
void update(bool enabled);
|
||||
|
||||
private:
|
||||
std::string topic;
|
||||
transport::PublisherPtr pub;
|
||||
msgs::Bool msg;
|
||||
sensors::ContactSensorPtr sensor;
|
||||
};
|
||||
|
||||
#endif /* _COMPONENTS_EXTERNAL_LIMIT_SWITCH_H_ */
|
||||
@@ -1,42 +0,0 @@
|
||||
|
||||
#include "components/Gyro.h"
|
||||
|
||||
#include <gazebo/math/gzmath.hh>
|
||||
#define _USE_MATH_DEFINES
|
||||
#include <math.h>
|
||||
|
||||
Gyro::Gyro(std::string topic, physics::LinkPtr link, ROTATION axis) {
|
||||
std::cout << "Initializing gyro: " << topic << " axis=" << axis << std::endl;
|
||||
this->topic = topic;
|
||||
this->link = link;
|
||||
this->axis = axis;
|
||||
}
|
||||
|
||||
Gyro::~Gyro() {
|
||||
|
||||
}
|
||||
|
||||
void Gyro::connect(transport::NodePtr node) {
|
||||
command_sub = node->Subscribe(topic+"/control", &Gyro::callback, this);
|
||||
pos_pub = node->Advertise<msgs::Float64>(topic+"/position");
|
||||
vel_pub = node->Advertise<msgs::Float64>(topic+"/velocity");
|
||||
}
|
||||
|
||||
void Gyro::callback(const msgs::ConstStringPtr &msg) {
|
||||
std::string command = msg->data();
|
||||
if (command == "reset") {
|
||||
zero = link->GetWorldCoGPose().rot.GetAsEuler()[axis];
|
||||
} else {
|
||||
std::cout << "WARNING: Encoder got unknown command '" << command << "'." << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
void Gyro::update(bool enabled) {
|
||||
math::Vector3 rot;
|
||||
pos_msg.set_data((link->GetWorldCoGPose().rot.GetAsEuler()[axis] - zero) * (180.0 / M_PI));
|
||||
if (pos_msg.data() < -180) pos_msg.set_data(pos_msg.data() + 360);
|
||||
else if (pos_msg.data() > 180) pos_msg.set_data(pos_msg.data() - 360);
|
||||
pos_pub->Publish(pos_msg);
|
||||
vel_msg.set_data(link->GetRelativeAngularVel()[axis] * (180.0 / M_PI));
|
||||
vel_pub->Publish(vel_msg);
|
||||
}
|
||||
@@ -1,30 +0,0 @@
|
||||
|
||||
#include "components/RobotComponent.h"
|
||||
|
||||
#include <gazebo/physics/Link.hh>
|
||||
|
||||
#ifndef _COMPONENTS_GYRO_H_
|
||||
#define _COMPONENTS_GYRO_H_
|
||||
|
||||
typedef enum {Roll, Pitch, Yaw} ROTATION;
|
||||
|
||||
class Gyro : public RobotComponent {
|
||||
public:
|
||||
Gyro(std::string topic, physics::LinkPtr link, ROTATION axis);
|
||||
virtual ~Gyro();
|
||||
|
||||
void connect(transport::NodePtr node);
|
||||
void update(bool enabled);
|
||||
private:
|
||||
ROTATION axis;
|
||||
std::string topic;
|
||||
double zero;
|
||||
transport::SubscriberPtr command_sub;
|
||||
transport::PublisherPtr pos_pub, vel_pub;
|
||||
msgs::Float64 pos_msg, vel_msg;
|
||||
physics::LinkPtr link;
|
||||
|
||||
void callback(const msgs::ConstStringPtr &msg);
|
||||
};
|
||||
|
||||
#endif /* _COMPONENTS_GYRO_H_ */
|
||||
@@ -1,32 +0,0 @@
|
||||
|
||||
#include "components/InternalLimitSwitch.h"
|
||||
|
||||
InternalLimitSwitch::InternalLimitSwitch(std::string topic, physics::JointPtr joint,
|
||||
double min, double max, bool radians) {
|
||||
std::cout << "Initializing external limit switch: " << topic << std::endl;
|
||||
this->topic = topic;
|
||||
this->joint = joint;
|
||||
this->min = min;
|
||||
this->max = max;
|
||||
this->radians = radians;
|
||||
}
|
||||
|
||||
InternalLimitSwitch::~InternalLimitSwitch() {
|
||||
|
||||
}
|
||||
|
||||
void InternalLimitSwitch::connect(transport::NodePtr node) {
|
||||
pub = node->Advertise<msgs::Bool>(topic);
|
||||
}
|
||||
|
||||
void InternalLimitSwitch::update(bool enabled) {
|
||||
double value;
|
||||
joint->GetAngle(0).Normalize();
|
||||
if (radians) {
|
||||
value = joint->GetAngle(0).Radian();
|
||||
} else {
|
||||
value = joint->GetAngle(0).Degree();
|
||||
}
|
||||
msg.set_data(value >= min && value <= max);
|
||||
pub->Publish(msg);
|
||||
}
|
||||
@@ -1,28 +0,0 @@
|
||||
|
||||
#include "components/RobotComponent.h"
|
||||
#include "msgs/bool.pb.h"
|
||||
|
||||
#include <gazebo/sensors/sensors.hh>
|
||||
|
||||
#ifndef _COMPONENTS_INTERNAL_LIMIT_SWITCH_H_
|
||||
#define _COMPONENTS_INTERNAL_LIMIT_SWITCH_H_
|
||||
|
||||
class InternalLimitSwitch : public RobotComponent {
|
||||
public:
|
||||
InternalLimitSwitch(std::string topic, physics::JointPtr joint,
|
||||
double min, double max, bool radians);
|
||||
virtual ~InternalLimitSwitch();
|
||||
|
||||
void connect(transport::NodePtr node);
|
||||
void update(bool enabled);
|
||||
|
||||
private:
|
||||
std::string topic;
|
||||
transport::PublisherPtr pub;
|
||||
msgs::Bool msg;
|
||||
physics::JointPtr joint;
|
||||
double min, max;
|
||||
bool radians;
|
||||
};
|
||||
|
||||
#endif /* _COMPONENTS_EXTERNAL_LIMIT_SWITCH_H_ */
|
||||
@@ -1,32 +0,0 @@
|
||||
|
||||
#include "components/Motor.h"
|
||||
|
||||
Motor::Motor(std::string topic, physics::JointPtr joint, double multiplier) {
|
||||
std::cout << "Initializing motor: " << topic << " -- " << multiplier << std::endl;
|
||||
this->topic = topic;
|
||||
this->multiplier = multiplier;
|
||||
this->joint = joint;
|
||||
signal = 0;
|
||||
}
|
||||
|
||||
Motor::~Motor() {
|
||||
|
||||
}
|
||||
|
||||
void Motor::connect(transport::NodePtr node) {
|
||||
sub = node->Subscribe(topic, &Motor::callback, this);
|
||||
}
|
||||
|
||||
void Motor::callback(const msgs::ConstFloat64Ptr &msg) {
|
||||
signal = msg->data();
|
||||
if (signal < -1) { signal = -1; }
|
||||
else if (signal > 1) { signal = 1; }
|
||||
}
|
||||
|
||||
void Motor::update(bool enabled) {
|
||||
if (enabled) {
|
||||
joint->SetForce(0, signal*multiplier);
|
||||
} else {
|
||||
joint->SetForce(0, 0);
|
||||
}
|
||||
}
|
||||
@@ -1,25 +0,0 @@
|
||||
|
||||
#include "components/RobotComponent.h"
|
||||
|
||||
#include <gazebo/physics/Joint.hh>
|
||||
|
||||
#ifndef _COMPONENTS_MOTOR_H_
|
||||
#define _COMPONENTS_MOTOR_H_
|
||||
|
||||
class Motor : public RobotComponent {
|
||||
public:
|
||||
Motor(std::string topic, physics::JointPtr joint, double multiplier);
|
||||
virtual ~Motor();
|
||||
|
||||
void connect(transport::NodePtr node);
|
||||
void update(bool enabled);
|
||||
private:
|
||||
std::string topic;
|
||||
double signal, multiplier;
|
||||
transport::SubscriberPtr sub;
|
||||
physics::JointPtr joint;
|
||||
|
||||
void callback(const msgs::ConstFloat64Ptr &msg);
|
||||
};
|
||||
|
||||
#endif /* _COMPONENTS_MOTOR_H_ */
|
||||
@@ -1,32 +0,0 @@
|
||||
|
||||
#include "components/Piston.h"
|
||||
|
||||
Piston::Piston(std::string topic, physics::JointPtr joint,
|
||||
double forward_force, double reverse_force) {
|
||||
std::cout << "Initializing piston: " << topic << " -- " << forward_force
|
||||
<< "/" << reverse_force << "N" << std::endl;
|
||||
this->topic = topic;
|
||||
this->forward_force = forward_force;
|
||||
this->reverse_force = reverse_force;
|
||||
this->joint = joint;
|
||||
signal = -1;
|
||||
}
|
||||
|
||||
Piston::~Piston() {
|
||||
|
||||
}
|
||||
|
||||
void Piston::connect(transport::NodePtr node) {
|
||||
sub = node->Subscribe(topic, &Piston::callback, this);
|
||||
}
|
||||
|
||||
void Piston::callback(const msgs::ConstFloat64Ptr &msg) {
|
||||
signal = msg->data();
|
||||
if (signal < -0.001) { signal = -reverse_force; }
|
||||
else if (signal > 0.001) { signal = forward_force; }
|
||||
else { signal = 0; }
|
||||
}
|
||||
|
||||
void Piston::update(bool enabled) {
|
||||
joint->SetForce(0, signal);
|
||||
}
|
||||
@@ -1,36 +0,0 @@
|
||||
|
||||
#include "components/RobotComponent.h"
|
||||
|
||||
#include <gazebo/physics/Joint.hh>
|
||||
|
||||
#ifndef _COMPONENTS_PISTON_H_
|
||||
#define _COMPONENTS_PISTON_H_
|
||||
|
||||
// PSI in Newtons/m^2
|
||||
#define PSI(x) (x*6895)
|
||||
|
||||
/**
|
||||
* Piston models a pneumatic piston. In the URDF/SDF, the joint should
|
||||
* be modeled as a prismatic joint with the limits corresponding to
|
||||
* extended and retracted positions. Pistons have three states:
|
||||
* forward, reverse and off. Currently, the forward and reverse forces
|
||||
* are specified in Newtons and are constant. Changes in pressure are
|
||||
* not modeled.
|
||||
*/
|
||||
class Piston : public RobotComponent {
|
||||
public:
|
||||
Piston(std::string topic, physics::JointPtr joint, double forward_force, double reverse_force);
|
||||
virtual ~Piston();
|
||||
|
||||
void connect(transport::NodePtr node);
|
||||
void update(bool enabled);
|
||||
private:
|
||||
std::string topic;
|
||||
double forward_force, reverse_force, signal;
|
||||
transport::SubscriberPtr sub;
|
||||
physics::JointPtr joint;
|
||||
|
||||
void callback(const msgs::ConstFloat64Ptr &msg);
|
||||
};
|
||||
|
||||
#endif /* _COMPONENTS_PISTON_H_ */
|
||||
@@ -1,27 +0,0 @@
|
||||
|
||||
#include "components/Potentiometer.h"
|
||||
|
||||
Potentiometer::Potentiometer(std::string topic, physics::JointPtr joint, bool radians) {
|
||||
std::cout << "Initializing potentiometer: " << topic << std::endl;
|
||||
this->topic = topic;
|
||||
this->joint = joint;
|
||||
this->radians = radians;
|
||||
}
|
||||
|
||||
Potentiometer::~Potentiometer() {
|
||||
|
||||
}
|
||||
|
||||
void Potentiometer::connect(transport::NodePtr node) {
|
||||
pub = node->Advertise<msgs::Float64>(topic);
|
||||
}
|
||||
|
||||
void Potentiometer::update(bool enabled) {
|
||||
joint->GetAngle(0).Normalize();
|
||||
if (radians) {
|
||||
msg.set_data(joint->GetAngle(0).Radian());
|
||||
} else {
|
||||
msg.set_data(joint->GetAngle(0).Degree());
|
||||
}
|
||||
pub->Publish(msg);
|
||||
}
|
||||
@@ -1,26 +0,0 @@
|
||||
|
||||
#include "components/RobotComponent.h"
|
||||
#include "msgs/float64.pb.h"
|
||||
|
||||
#include <gazebo/physics/Joint.hh>
|
||||
|
||||
#ifndef _COMPONENTS_POTENTIOMETER_H_
|
||||
#define _COMPONENTS_POTENTIOMETER_H_
|
||||
|
||||
class Potentiometer : public RobotComponent {
|
||||
public:
|
||||
Potentiometer(std::string topic, physics::JointPtr joint, bool radians);
|
||||
virtual ~Potentiometer();
|
||||
|
||||
void connect(transport::NodePtr node);
|
||||
void update(bool enabled);
|
||||
|
||||
private:
|
||||
std::string topic;
|
||||
transport::PublisherPtr pub;
|
||||
msgs::Float64 msg;
|
||||
physics::JointPtr joint;
|
||||
bool radians;
|
||||
};
|
||||
|
||||
#endif /* _COMPONENTS_POTENTIOMETER_H_ */
|
||||
@@ -1,21 +0,0 @@
|
||||
|
||||
#include "components/Rangefinder.h"
|
||||
|
||||
Rangefinder::Rangefinder(std::string topic, sensors::SonarSensorPtr sensor) {
|
||||
std::cout << "Initializing rangefinder: " << topic << std::endl;
|
||||
this->topic = topic;
|
||||
this->sensor = sensor;
|
||||
}
|
||||
|
||||
Rangefinder::~Rangefinder() {
|
||||
|
||||
}
|
||||
|
||||
void Rangefinder::connect(transport::NodePtr node) {
|
||||
pub = node->Advertise<msgs::Float64>(topic);
|
||||
}
|
||||
|
||||
void Rangefinder::update(bool enabled) {
|
||||
msg.set_data(sensor->GetRange());
|
||||
pub->Publish(msg);
|
||||
}
|
||||
@@ -1,26 +0,0 @@
|
||||
|
||||
#include "components/RobotComponent.h"
|
||||
#include "msgs/float64.pb.h"
|
||||
|
||||
#include <gazebo/sensors/sensors.hh>
|
||||
|
||||
#ifndef _COMPONENTS_RANGEFINDER_H_
|
||||
#define _COMPONENTS_RANGEFINDER_H_
|
||||
|
||||
class Rangefinder : public RobotComponent {
|
||||
public:
|
||||
Rangefinder(std::string topic, sensors::SonarSensorPtr sensor);
|
||||
virtual ~Rangefinder();
|
||||
|
||||
void connect(transport::NodePtr node);
|
||||
void update(bool enabled);
|
||||
|
||||
private:
|
||||
std::string topic;
|
||||
transport::PublisherPtr pub;
|
||||
msgs::Float64 msg;
|
||||
physics::LinkPtr link;
|
||||
sensors::SonarSensorPtr sensor;
|
||||
};
|
||||
|
||||
#endif /* _COMPONENTS_RANGEFINDER_H_ */
|
||||
@@ -1,25 +0,0 @@
|
||||
|
||||
#include "msgs/msgs.h"
|
||||
|
||||
#include <gazebo/transport/transport.hh>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
|
||||
#ifndef _ROBOTCOMPONENT_H_
|
||||
#define _ROBOTCOMPONENT_H_
|
||||
|
||||
#define MSG_BUFFER_SIZE 1000
|
||||
|
||||
using namespace gazebo;
|
||||
|
||||
class RobotComponent {
|
||||
public:
|
||||
RobotComponent() {}
|
||||
virtual ~RobotComponent() {}
|
||||
|
||||
virtual void connect(transport::NodePtr node) {};
|
||||
virtual void update(bool enabled) {};
|
||||
};
|
||||
|
||||
typedef RobotComponent *RobotComponentPtr;
|
||||
|
||||
#endif /* _ROBOTCOMPONENT_H_ */
|
||||
3
simulation/frc_gazebo_plugins/.gitignore
vendored
Normal file
3
simulation/frc_gazebo_plugins/.gitignore
vendored
Normal file
@@ -0,0 +1,3 @@
|
||||
plugins/
|
||||
build/
|
||||
docs/
|
||||
48
simulation/frc_gazebo_plugins/Makefile
Normal file
48
simulation/frc_gazebo_plugins/Makefile
Normal file
@@ -0,0 +1,48 @@
|
||||
prefix = /usr
|
||||
lib.dir = $(prefix)/lib
|
||||
plugin.dir = $(lib.dir)/frcsim/plugins
|
||||
build.dir = build
|
||||
|
||||
.PHONY : all build copy-plugins clean docs clean-docs
|
||||
|
||||
all: build copy-plugins
|
||||
|
||||
build:
|
||||
cd msgs && make
|
||||
cd dc_motor && make
|
||||
cd pneumatic_piston && make
|
||||
cd potentiometer && make
|
||||
cd rangefinder && make
|
||||
cd encoder && make
|
||||
cd gyro && make
|
||||
cd limit_switch && make
|
||||
cd clock && make
|
||||
|
||||
copy-plugins:
|
||||
mkdir -p plugins
|
||||
cp dc_motor/build/libgz_dc_motor.so plugins
|
||||
cp pneumatic_piston/build/libgz_pneumatic_piston.so plugins
|
||||
cp potentiometer/build/libgz_potentiometer.so plugins
|
||||
cp rangefinder/build/libgz_rangefinder.so plugins
|
||||
cp encoder/build/libgz_encoder.so plugins
|
||||
cp gyro/build/libgz_gyro.so plugins
|
||||
cp limit_switch/build/libgz_limit_switch.so plugins
|
||||
cp clock/build/libgz_clock.so plugins
|
||||
|
||||
clean: clean-docs
|
||||
cd msgs && make clean
|
||||
cd dc_motor && make clean
|
||||
cd pneumatic_piston && make clean
|
||||
cd potentiometer && make clean
|
||||
cd rangefinder && make clean
|
||||
cd encoder && make clean
|
||||
cd gyro && make clean
|
||||
cd limit_switch && make clean
|
||||
cd clock && make clean
|
||||
-rm -r plugins
|
||||
|
||||
docs:
|
||||
doxygen frc_gazebo_plugins.doxy
|
||||
|
||||
clean-docs:
|
||||
-rm -r docs
|
||||
17
simulation/frc_gazebo_plugins/clock/CMakeLists.txt
Normal file
17
simulation/frc_gazebo_plugins/clock/CMakeLists.txt
Normal file
@@ -0,0 +1,17 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(gz_clock)
|
||||
|
||||
include (FindPkgConfig)
|
||||
if (PKG_CONFIG_FOUND)
|
||||
pkg_check_modules(GAZEBO gazebo)
|
||||
endif()
|
||||
|
||||
find_package(gazebo REQUIRED)
|
||||
|
||||
file(GLOB_RECURSE SRC_FILES src/*.cpp)
|
||||
file(GLOB_RECURSE MSG_FILES ../msgs/src/*.cc)
|
||||
include_directories(src ${Boost_INCLUDE_DIR} ${GAZEBO_INCLUDE_DIRS} ../msgs/src)
|
||||
add_library(${PROJECT_NAME} SHARED ${SRC_FILES} ${MSG_FILES})
|
||||
|
||||
link_directories(${GAZEBO_LIBRARY_DIRS} ../msgs/build)
|
||||
target_link_libraries(${PROJECT_NAME} ${GAZEBO_LIBRARIES})
|
||||
@@ -12,4 +12,4 @@ clean:
|
||||
|
||||
install: all
|
||||
mkdir -p $(DESTDIR)$(plugin.dir)
|
||||
install $(build.dir)/libfrc_gazebo_plugin.so $(DESTDIR)$(plugin.dir)
|
||||
install $(build.dir)/libgz_clock.so $(DESTDIR)$(plugin.dir)
|
||||
42
simulation/frc_gazebo_plugins/clock/src/clock.cpp
Normal file
42
simulation/frc_gazebo_plugins/clock/src/clock.cpp
Normal file
@@ -0,0 +1,42 @@
|
||||
#include "clock.h"
|
||||
|
||||
#include <gazebo/physics/physics.hh>
|
||||
#include <gazebo/transport/transport.hh>
|
||||
|
||||
#include "msgs/msgs.h"
|
||||
|
||||
GZ_REGISTER_MODEL_PLUGIN(Clock)
|
||||
|
||||
Clock::Clock() {}
|
||||
|
||||
Clock::~Clock() {}
|
||||
|
||||
void Clock::Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
|
||||
this->model = model;
|
||||
|
||||
// Parse SDF properties
|
||||
if (sdf->HasElement("topic")) {
|
||||
topic = sdf->Get<std::string>("topic");
|
||||
} else {
|
||||
topic = "~/"+sdf->GetAttribute("name")->GetAsString();
|
||||
}
|
||||
|
||||
gzmsg << "Initializing clock: " << topic << std::endl;
|
||||
|
||||
// Connect to Gazebo transport for messaging
|
||||
std::string scoped_name = model->GetWorld()->GetName()+"::"+model->GetScopedName();
|
||||
boost::replace_all(scoped_name, "::", "/");
|
||||
node = transport::NodePtr(new transport::Node());
|
||||
node->Init(scoped_name);
|
||||
pub = node->Advertise<msgs::Float64>(topic);
|
||||
|
||||
// Connect to the world update event.
|
||||
// This will trigger the Update function every Gazebo iteration
|
||||
updateConn = event::Events::ConnectWorldUpdateBegin(boost::bind(&Clock::Update, this, _1));
|
||||
}
|
||||
|
||||
void Clock::Update(const common::UpdateInfo &info) {
|
||||
msgs::Float64 msg;
|
||||
msg.set_data(info.simTime.Double());
|
||||
pub->Publish(msg);
|
||||
}
|
||||
42
simulation/frc_gazebo_plugins/clock/src/clock.h
Normal file
42
simulation/frc_gazebo_plugins/clock/src/clock.h
Normal file
@@ -0,0 +1,42 @@
|
||||
#pragma once
|
||||
|
||||
#include <gazebo/gazebo.hh>
|
||||
|
||||
using namespace gazebo;
|
||||
|
||||
/**
|
||||
* \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="libgz_clock.so">
|
||||
* <topic>~/my/topic</topic>
|
||||
* </plugin>
|
||||
*
|
||||
* - `topic`: Optional. Message will be published as a gazebo.msgs.Float64.
|
||||
*
|
||||
* \todo Make WorldPlugin?
|
||||
*/
|
||||
class Clock: public ModelPlugin {
|
||||
public:
|
||||
Clock();
|
||||
~Clock();
|
||||
|
||||
/// \brief Load the clock and configures it according to the sdf.
|
||||
void Load(physics::ModelPtr model, sdf::ElementPtr sdf);
|
||||
|
||||
/// \brief Sends out time each timestep.
|
||||
void Update(const common::UpdateInfo &info);
|
||||
|
||||
private:
|
||||
std::string topic; ///< \brief Publish the time on this topic.
|
||||
physics::ModelPtr model; ///< \brief The model that this is attached to.
|
||||
event::ConnectionPtr updateConn; ///< \brief Pointer to the world update function.
|
||||
transport::NodePtr node; ///< \brief The node we're advertising on.
|
||||
transport::PublisherPtr pub; ///< \brief Publisher handle.
|
||||
};
|
||||
|
||||
17
simulation/frc_gazebo_plugins/dc_motor/CMakeLists.txt
Normal file
17
simulation/frc_gazebo_plugins/dc_motor/CMakeLists.txt
Normal file
@@ -0,0 +1,17 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(gz_dc_motor)
|
||||
|
||||
include (FindPkgConfig)
|
||||
if (PKG_CONFIG_FOUND)
|
||||
pkg_check_modules(GAZEBO gazebo)
|
||||
endif()
|
||||
|
||||
find_package(gazebo REQUIRED)
|
||||
|
||||
file(GLOB_RECURSE SRC_FILES src/*.cpp)
|
||||
file(GLOB_RECURSE MSG_FILES ../msgs/src/*.cc)
|
||||
include_directories(src ${Boost_INCLUDE_DIR} ${GAZEBO_INCLUDE_DIRS} ../msgs/src)
|
||||
add_library(${PROJECT_NAME} SHARED ${SRC_FILES} ${MSG_FILES})
|
||||
|
||||
link_directories(${GAZEBO_LIBRARY_DIRS} ../msgs/build)
|
||||
target_link_libraries(${PROJECT_NAME} ${GAZEBO_LIBRARIES})
|
||||
@@ -12,4 +12,4 @@ clean:
|
||||
|
||||
install: all
|
||||
mkdir -p $(DESTDIR)$(plugin.dir)
|
||||
install $(build.dir)/libfrc_gazebo_plugin.so $(DESTDIR)$(plugin.dir)
|
||||
install $(build.dir)/libgz_dc_motor.so $(DESTDIR)$(plugin.dir)
|
||||
53
simulation/frc_gazebo_plugins/dc_motor/src/dc_motor.cpp
Normal file
53
simulation/frc_gazebo_plugins/dc_motor/src/dc_motor.cpp
Normal file
@@ -0,0 +1,53 @@
|
||||
#include "dc_motor.h"
|
||||
|
||||
#include <gazebo/physics/physics.hh>
|
||||
#include <gazebo/transport/transport.hh>
|
||||
|
||||
GZ_REGISTER_MODEL_PLUGIN(DCMotor)
|
||||
|
||||
DCMotor::DCMotor() {}
|
||||
|
||||
DCMotor::~DCMotor() {}
|
||||
|
||||
void DCMotor::Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
|
||||
this->model = model;
|
||||
signal = 0;
|
||||
|
||||
// Parse SDF properties
|
||||
joint = model->GetJoint(sdf->Get<std::string>("joint"));
|
||||
if (sdf->HasElement("topic")) {
|
||||
topic = sdf->Get<std::string>("topic");
|
||||
} else {
|
||||
topic = "~/"+sdf->GetAttribute("name")->GetAsString();
|
||||
}
|
||||
|
||||
if (sdf->HasElement("multiplier")) {
|
||||
multiplier = sdf->Get<double>("multiplier");
|
||||
} else {
|
||||
multiplier = 1;
|
||||
}
|
||||
|
||||
gzmsg << "Initializing motor: " << topic << " joint=" << joint->GetName()
|
||||
<< " multiplier=" << multiplier << std::endl;
|
||||
|
||||
// Connect to Gazebo transport for messaging
|
||||
std::string scoped_name = model->GetWorld()->GetName()+"::"+model->GetScopedName();
|
||||
boost::replace_all(scoped_name, "::", "/");
|
||||
node = transport::NodePtr(new transport::Node());
|
||||
node->Init(scoped_name);
|
||||
sub = node->Subscribe(topic, &DCMotor::Callback, this);
|
||||
|
||||
// Connect to the world update event.
|
||||
// This will trigger the Update function every Gazebo iteration
|
||||
updateConn = event::Events::ConnectWorldUpdateBegin(boost::bind(&DCMotor::Update, this, _1));
|
||||
}
|
||||
|
||||
void DCMotor::Update(const common::UpdateInfo &info) {
|
||||
joint->SetForce(0, signal*multiplier);
|
||||
}
|
||||
|
||||
void DCMotor::Callback(const msgs::ConstFloat64Ptr &msg) {
|
||||
signal = msg->data();
|
||||
if (signal < -1) { signal = -1; }
|
||||
else if (signal > 1) { signal = 1; }
|
||||
}
|
||||
61
simulation/frc_gazebo_plugins/dc_motor/src/dc_motor.h
Normal file
61
simulation/frc_gazebo_plugins/dc_motor/src/dc_motor.h
Normal file
@@ -0,0 +1,61 @@
|
||||
#pragma once
|
||||
|
||||
#include <gazebo/gazebo.hh>
|
||||
|
||||
#include "msgs/msgs.h"
|
||||
|
||||
using namespace gazebo;
|
||||
|
||||
/**
|
||||
* \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="libgz_dc_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 ModelPlugin {
|
||||
public:
|
||||
DCMotor();
|
||||
~DCMotor();
|
||||
|
||||
/// \brief Load the dc motor and configures it according to the sdf.
|
||||
void Load(physics::ModelPtr model, sdf::ElementPtr sdf);
|
||||
|
||||
/// \brief Update the torque on the joint from the dc motor each timestep.
|
||||
void Update(const common::UpdateInfo &info);
|
||||
|
||||
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.
|
||||
physics::JointPtr joint;
|
||||
|
||||
/// \brief Callback for receiving msgs and storing the signal.
|
||||
void Callback(const msgs::ConstFloat64Ptr &msg);
|
||||
|
||||
|
||||
physics::ModelPtr model; ///< \brief The model that this is attached to.
|
||||
event::ConnectionPtr updateConn; ///< \brief Pointer to the world update function.
|
||||
transport::NodePtr node; ///< \brief The node we're advertising on.
|
||||
transport::SubscriberPtr sub; ///< \brief Subscriber handle.
|
||||
};
|
||||
17
simulation/frc_gazebo_plugins/encoder/CMakeLists.txt
Normal file
17
simulation/frc_gazebo_plugins/encoder/CMakeLists.txt
Normal file
@@ -0,0 +1,17 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(gz_encoder)
|
||||
|
||||
include (FindPkgConfig)
|
||||
if (PKG_CONFIG_FOUND)
|
||||
pkg_check_modules(GAZEBO gazebo)
|
||||
endif()
|
||||
|
||||
find_package(gazebo REQUIRED)
|
||||
|
||||
file(GLOB_RECURSE SRC_FILES src/*.cpp)
|
||||
file(GLOB_RECURSE MSG_FILES ../msgs/src/*.cc)
|
||||
include_directories(src ${Boost_INCLUDE_DIR} ${GAZEBO_INCLUDE_DIRS} ../msgs/src)
|
||||
add_library(${PROJECT_NAME} SHARED ${SRC_FILES} ${MSG_FILES})
|
||||
|
||||
link_directories(${GAZEBO_LIBRARY_DIRS} ../msgs/build)
|
||||
target_link_libraries(${PROJECT_NAME} ${GAZEBO_LIBRARIES})
|
||||
15
simulation/frc_gazebo_plugins/encoder/Makefile
Normal file
15
simulation/frc_gazebo_plugins/encoder/Makefile
Normal file
@@ -0,0 +1,15 @@
|
||||
prefix = /usr
|
||||
lib.dir = $(prefix)/lib
|
||||
plugin.dir = $(lib.dir)/frcsim/plugins
|
||||
build.dir = build
|
||||
|
||||
all:
|
||||
mkdir -p $(build.dir)
|
||||
cd ${build.dir} && cmake .. && make
|
||||
|
||||
clean:
|
||||
rm -rf $(build.dir)
|
||||
|
||||
install: all
|
||||
mkdir -p $(DESTDIR)$(plugin.dir)
|
||||
install $(build.dir)/libgz_encoder.so $(DESTDIR)$(plugin.dir)
|
||||
94
simulation/frc_gazebo_plugins/encoder/src/encoder.cpp
Normal file
94
simulation/frc_gazebo_plugins/encoder/src/encoder.cpp
Normal file
@@ -0,0 +1,94 @@
|
||||
#include "encoder.h"
|
||||
|
||||
#include <gazebo/physics/physics.hh>
|
||||
#include <gazebo/transport/transport.hh>
|
||||
|
||||
GZ_REGISTER_MODEL_PLUGIN(Encoder)
|
||||
|
||||
Encoder::Encoder() {}
|
||||
|
||||
Encoder::~Encoder() {}
|
||||
|
||||
void Encoder::Load(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;
|
||||
}
|
||||
zero = GetAngle();
|
||||
stopped = true;
|
||||
stop_value = 0;
|
||||
|
||||
gzmsg << "Initializing encoder: " << topic << " joint=" << joint->GetName()
|
||||
<< " radians=" << radians << std::endl;
|
||||
|
||||
// Connect to Gazebo transport for messaging
|
||||
std::string scoped_name = model->GetWorld()->GetName()+"::"+model->GetScopedName();
|
||||
boost::replace_all(scoped_name, "::", "/");
|
||||
node = transport::NodePtr(new transport::Node());
|
||||
node->Init(scoped_name);
|
||||
command_sub = node->Subscribe(topic+"/control", &Encoder::Callback, this);
|
||||
pos_pub = node->Advertise<msgs::Float64>(topic+"/position");
|
||||
vel_pub = node->Advertise<msgs::Float64>(topic+"/velocity");
|
||||
|
||||
// Connect to the world update event.
|
||||
// This will trigger the Update function every Gazebo iteration
|
||||
updateConn = event::Events::ConnectWorldUpdateBegin(boost::bind(&Encoder::Update, this, _1));
|
||||
}
|
||||
|
||||
void Encoder::Update(const common::UpdateInfo &info) {
|
||||
msgs::Float64 pos_msg, vel_msg;
|
||||
if (stopped) {
|
||||
pos_msg.set_data(stop_value);
|
||||
pos_pub->Publish(pos_msg);
|
||||
vel_msg.set_data(0);
|
||||
vel_pub->Publish(vel_msg);
|
||||
} else {
|
||||
pos_msg.set_data(GetAngle() - zero);
|
||||
pos_pub->Publish(pos_msg);
|
||||
vel_msg.set_data(GetVelocity());
|
||||
vel_pub->Publish(vel_msg);
|
||||
}
|
||||
}
|
||||
|
||||
void Encoder::Callback(const msgs::ConstStringPtr &msg) {
|
||||
std::string command = msg->data();
|
||||
if (command == "reset") {
|
||||
zero = GetAngle();
|
||||
} else if (command == "start") {
|
||||
stopped = false;
|
||||
zero = (GetAngle() - stop_value);
|
||||
} else if (command == "stop") {
|
||||
stopped = true;
|
||||
stop_value = GetAngle();
|
||||
} else {
|
||||
gzerr << "WARNING: Encoder got unknown command '" << command << "'." << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
double Encoder::GetAngle() {
|
||||
if (radians) {
|
||||
return joint->GetAngle(0).Radian();
|
||||
} else {
|
||||
return joint->GetAngle(0).Degree();
|
||||
}
|
||||
}
|
||||
|
||||
double Encoder::GetVelocity() {
|
||||
if (radians) {
|
||||
return joint->GetVelocity(0);
|
||||
} else {
|
||||
return joint->GetVelocity(0) * (180.0 / M_PI);
|
||||
}
|
||||
}
|
||||
|
||||
81
simulation/frc_gazebo_plugins/encoder/src/encoder.h
Normal file
81
simulation/frc_gazebo_plugins/encoder/src/encoder.h
Normal file
@@ -0,0 +1,81 @@
|
||||
#pragma once
|
||||
|
||||
#include <gazebo/gazebo.hh>
|
||||
|
||||
#include "msgs/msgs.h"
|
||||
|
||||
using namespace gazebo;
|
||||
|
||||
/**
|
||||
* \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="libgz_encoder.so">
|
||||
* <joint>Joint Name</joint>
|
||||
* <topic>~/my/topic</topic>
|
||||
* <units>{degrees, radians}</units>
|
||||
* </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)
|
||||
* - `units`: Optional. Defaults to radians.
|
||||
*/
|
||||
class Encoder: public ModelPlugin {
|
||||
public:
|
||||
Encoder();
|
||||
~Encoder();
|
||||
|
||||
/// \brief Load the encoder and configures it according to the sdf.
|
||||
void Load(physics::ModelPtr model, sdf::ElementPtr sdf);
|
||||
|
||||
/// \brief Sends out the encoder reading each timestep.
|
||||
void Update(const 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 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
|
||||
physics::JointPtr joint;
|
||||
|
||||
/// \brief Callback for handling control data
|
||||
void Callback(const 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();
|
||||
|
||||
physics::ModelPtr model; ///< \brief The model that this is attached to.
|
||||
event::ConnectionPtr updateConn; ///< \brief Pointer to the world update function.
|
||||
transport::NodePtr node; ///< \brief The node we're advertising on.
|
||||
transport::SubscriberPtr command_sub; ///< \brief Subscriber handle.
|
||||
transport::PublisherPtr pos_pub, vel_pub; ///< \brief Publisher handles.
|
||||
};
|
||||
2303
simulation/frc_gazebo_plugins/frc_gazebo_plugins.doxy
Normal file
2303
simulation/frc_gazebo_plugins/frc_gazebo_plugins.doxy
Normal file
File diff suppressed because it is too large
Load Diff
17
simulation/frc_gazebo_plugins/gyro/CMakeLists.txt
Normal file
17
simulation/frc_gazebo_plugins/gyro/CMakeLists.txt
Normal file
@@ -0,0 +1,17 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(gz_gyro)
|
||||
|
||||
include (FindPkgConfig)
|
||||
if (PKG_CONFIG_FOUND)
|
||||
pkg_check_modules(GAZEBO gazebo)
|
||||
endif()
|
||||
|
||||
find_package(gazebo REQUIRED)
|
||||
|
||||
file(GLOB_RECURSE SRC_FILES src/*.cpp)
|
||||
file(GLOB_RECURSE MSG_FILES ../msgs/src/*.cc)
|
||||
include_directories(src ${Boost_INCLUDE_DIR} ${GAZEBO_INCLUDE_DIRS} ../msgs/src)
|
||||
add_library(${PROJECT_NAME} SHARED ${SRC_FILES} ${MSG_FILES})
|
||||
|
||||
link_directories(${GAZEBO_LIBRARY_DIRS} ../msgs/build)
|
||||
target_link_libraries(${PROJECT_NAME} ${GAZEBO_LIBRARIES})
|
||||
15
simulation/frc_gazebo_plugins/gyro/Makefile
Normal file
15
simulation/frc_gazebo_plugins/gyro/Makefile
Normal file
@@ -0,0 +1,15 @@
|
||||
prefix = /usr
|
||||
lib.dir = $(prefix)/lib
|
||||
plugin.dir = $(lib.dir)/frcsim/plugins
|
||||
build.dir = build
|
||||
|
||||
all:
|
||||
mkdir -p $(build.dir)
|
||||
cd ${build.dir} && cmake .. && make
|
||||
|
||||
clean:
|
||||
rm -rf $(build.dir)
|
||||
|
||||
install: all
|
||||
mkdir -p $(DESTDIR)$(plugin.dir)
|
||||
install $(build.dir)/libgz_gyro.so $(DESTDIR)$(plugin.dir)
|
||||
100
simulation/frc_gazebo_plugins/gyro/src/gyro.cpp
Normal file
100
simulation/frc_gazebo_plugins/gyro/src/gyro.cpp
Normal file
@@ -0,0 +1,100 @@
|
||||
#include "gyro.h"
|
||||
|
||||
#include <gazebo/physics/physics.hh>
|
||||
#include <gazebo/transport/transport.hh>
|
||||
|
||||
GZ_REGISTER_MODEL_PLUGIN(Gyro)
|
||||
|
||||
Gyro::Gyro() {}
|
||||
|
||||
Gyro::~Gyro() {}
|
||||
|
||||
void Gyro::Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
|
||||
this->model = model;
|
||||
|
||||
// Parse SDF properties
|
||||
link = model->GetLink(sdf->Get<std::string>("link"));
|
||||
if (sdf->HasElement("topic")) {
|
||||
topic = sdf->Get<std::string>("topic");
|
||||
} else {
|
||||
topic = "~/"+sdf->GetAttribute("name")->GetAsString();
|
||||
}
|
||||
|
||||
std::string axisString = sdf->Get<std::string>("axis");
|
||||
if (axisString == "roll") axis = Roll;
|
||||
if (axisString == "pitch") axis = Pitch;
|
||||
if (axisString == "yaw") axis = Yaw;
|
||||
|
||||
if (sdf->HasElement("units")) {
|
||||
radians = sdf->Get<std::string>("units") != "degrees";
|
||||
} else {
|
||||
radians = true;
|
||||
}
|
||||
zero = GetAngle();
|
||||
|
||||
gzmsg << "Initializing gyro: " << topic << " link=" << link->GetName()
|
||||
<< " axis=" << axis << " radians=" << radians << std::endl;
|
||||
|
||||
// Connect to Gazebo transport for messaging
|
||||
std::string scoped_name = model->GetWorld()->GetName()+"::"+model->GetScopedName();
|
||||
boost::replace_all(scoped_name, "::", "/");
|
||||
node = transport::NodePtr(new transport::Node());
|
||||
node->Init(scoped_name);
|
||||
command_sub = node->Subscribe(topic+"/control", &Gyro::Callback, this);
|
||||
pos_pub = node->Advertise<msgs::Float64>(topic+"/position");
|
||||
vel_pub = node->Advertise<msgs::Float64>(topic+"/velocity");
|
||||
|
||||
// Connect to the world update event.
|
||||
// This will trigger the Update function every Gazebo iteration
|
||||
updateConn = event::Events::ConnectWorldUpdateBegin(boost::bind(&Gyro::Update, this, _1));
|
||||
}
|
||||
|
||||
void Gyro::Update(const common::UpdateInfo &info) {
|
||||
msgs::Float64 pos_msg, vel_msg;
|
||||
pos_msg.set_data(Limit(GetAngle() - zero));
|
||||
pos_pub->Publish(pos_msg);
|
||||
vel_msg.set_data(GetVelocity());
|
||||
vel_pub->Publish(vel_msg);
|
||||
}
|
||||
|
||||
void Gyro::Callback(const msgs::ConstStringPtr &msg) {
|
||||
std::string command = msg->data();
|
||||
if (command == "reset") {
|
||||
zero = GetAngle();
|
||||
} else {
|
||||
gzerr << "WARNING: Gyro got unknown command '" << command << "'." << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
double Gyro::GetAngle() {
|
||||
if (radians) {
|
||||
return link->GetWorldCoGPose().rot.GetAsEuler()[axis];
|
||||
} else {
|
||||
return link->GetWorldCoGPose().rot.GetAsEuler()[axis] * (180.0 / M_PI);
|
||||
}
|
||||
}
|
||||
|
||||
double Gyro::GetVelocity() {
|
||||
if (radians) {
|
||||
return link->GetRelativeAngularVel()[axis];
|
||||
} else {
|
||||
return link->GetRelativeAngularVel()[axis] * (180.0 / M_PI);
|
||||
}
|
||||
}
|
||||
|
||||
double Gyro::Limit(double value) {
|
||||
if (radians) {
|
||||
while (true) {
|
||||
if (value < -M_PI) value += 2*M_PI;
|
||||
else if (value > M_PI) value -= 2*M_PI;
|
||||
else break;
|
||||
}
|
||||
} else {
|
||||
while (true) {
|
||||
if (value < -180) value += 360;
|
||||
else if (value > 180) value -= 360;
|
||||
else break;
|
||||
}
|
||||
}
|
||||
return value;
|
||||
}
|
||||
81
simulation/frc_gazebo_plugins/gyro/src/gyro.h
Normal file
81
simulation/frc_gazebo_plugins/gyro/src/gyro.h
Normal file
@@ -0,0 +1,81 @@
|
||||
#pragma once
|
||||
|
||||
#include <gazebo/gazebo.hh>
|
||||
|
||||
#include "msgs/msgs.h"
|
||||
|
||||
using namespace gazebo;
|
||||
|
||||
/// \brief The axis about which to measure rotation.
|
||||
typedef enum {Roll /*X*/, Pitch /*Y*/, Yaw /*Z*/} ROTATION;
|
||||
|
||||
/**
|
||||
* \brief Plugin for reading the speed and relative angle of a link.
|
||||
*
|
||||
* This plugin publishes the angle since last reset and the speed
|
||||
* which a link is rotating about some axis to subtopics of the given
|
||||
* topic every physics update. There is also a control topic that
|
||||
* takes one command: "reset", which sets the current angle as zero.
|
||||
*
|
||||
* To add a gyro to your robot, add the following XML to your robot
|
||||
* model:
|
||||
*
|
||||
* <plugin name="my_gyro" filename="libgz_gyro.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 ModelPlugin {
|
||||
public:
|
||||
Gyro();
|
||||
~Gyro();
|
||||
|
||||
/// \brief Load the gyro and configures it according to the sdf.
|
||||
void Load(physics::ModelPtr model, sdf::ElementPtr sdf);
|
||||
|
||||
/// \brief Sends out the gyro reading each timestep.
|
||||
void Update(const 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
|
||||
physics::LinkPtr link;
|
||||
|
||||
/// \brief Callback for handling control data
|
||||
void Callback(const 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);
|
||||
|
||||
physics::ModelPtr model; ///< \brief The model that this is attached to.
|
||||
event::ConnectionPtr updateConn; ///< \brief Pointer to the world update function.
|
||||
transport::NodePtr node; ///< \brief The node we're advertising on.
|
||||
transport::SubscriberPtr command_sub; ///< \brief Subscriber handle.
|
||||
transport::PublisherPtr pos_pub, vel_pub; ///< \brief Publisher handles.
|
||||
};
|
||||
17
simulation/frc_gazebo_plugins/limit_switch/CMakeLists.txt
Normal file
17
simulation/frc_gazebo_plugins/limit_switch/CMakeLists.txt
Normal file
@@ -0,0 +1,17 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(gz_limit_switch)
|
||||
|
||||
include (FindPkgConfig)
|
||||
if (PKG_CONFIG_FOUND)
|
||||
pkg_check_modules(GAZEBO gazebo)
|
||||
endif()
|
||||
|
||||
find_package(gazebo REQUIRED)
|
||||
|
||||
file(GLOB_RECURSE SRC_FILES src/*.cpp)
|
||||
file(GLOB_RECURSE MSG_FILES ../msgs/src/*.cc)
|
||||
include_directories(src ${Boost_INCLUDE_DIR} ${GAZEBO_INCLUDE_DIRS} ../msgs/src)
|
||||
add_library(${PROJECT_NAME} SHARED ${SRC_FILES} ${MSG_FILES})
|
||||
|
||||
link_directories(${GAZEBO_LIBRARY_DIRS} ../msgs/build)
|
||||
target_link_libraries(${PROJECT_NAME} ${GAZEBO_LIBRARIES})
|
||||
15
simulation/frc_gazebo_plugins/limit_switch/Makefile
Normal file
15
simulation/frc_gazebo_plugins/limit_switch/Makefile
Normal file
@@ -0,0 +1,15 @@
|
||||
prefix = /usr
|
||||
lib.dir = $(prefix)/lib
|
||||
plugin.dir = $(lib.dir)/frcsim/plugins
|
||||
build.dir = build
|
||||
|
||||
all:
|
||||
mkdir -p $(build.dir)
|
||||
cd ${build.dir} && cmake .. && make
|
||||
|
||||
clean:
|
||||
rm -rf $(build.dir)
|
||||
|
||||
install: all
|
||||
mkdir -p $(DESTDIR)$(plugin.dir)
|
||||
install $(build.dir)/libgz_limit_switch.so $(DESTDIR)$(plugin.dir)
|
||||
@@ -0,0 +1,17 @@
|
||||
#include "external_limit_switch.h"
|
||||
|
||||
#include <gazebo/sensors/sensors.hh>
|
||||
#include <boost/pointer_cast.hpp>
|
||||
|
||||
ExternalLimitSwitch::ExternalLimitSwitch(sdf::ElementPtr sdf) {
|
||||
sensor = boost::dynamic_pointer_cast<sensors::ContactSensor>(
|
||||
sensors::get_sensor(sdf->Get<std::string>("sensor")));
|
||||
|
||||
gzmsg << "\texternal limit switch: " << " sensor=" << sensor->GetName() << std::endl;
|
||||
}
|
||||
|
||||
ExternalLimitSwitch::~ExternalLimitSwitch() {}
|
||||
|
||||
bool ExternalLimitSwitch::Get() {
|
||||
return sensor->GetContacts().contact().size() > 0;
|
||||
}
|
||||
@@ -0,0 +1,19 @@
|
||||
#pragma once
|
||||
|
||||
#include <gazebo/gazebo.hh>
|
||||
|
||||
#include "switch.h"
|
||||
|
||||
using namespace gazebo;
|
||||
|
||||
class ExternalLimitSwitch : public Switch {
|
||||
public:
|
||||
ExternalLimitSwitch(sdf::ElementPtr sdf);
|
||||
~ExternalLimitSwitch();
|
||||
|
||||
/// \brief Returns true when the switch is triggered.
|
||||
virtual bool Get();
|
||||
|
||||
private:
|
||||
sensors::ContactSensorPtr sensor;
|
||||
};
|
||||
@@ -0,0 +1,31 @@
|
||||
#include "internal_limit_switch.h"
|
||||
|
||||
#include <gazebo/physics/physics.hh>
|
||||
|
||||
InternalLimitSwitch::InternalLimitSwitch(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;
|
||||
}
|
||||
|
||||
InternalLimitSwitch::~InternalLimitSwitch() {}
|
||||
|
||||
bool InternalLimitSwitch::Get() {
|
||||
double value;
|
||||
joint->GetAngle(0).Normalize();
|
||||
if (radians) {
|
||||
value = joint->GetAngle(0).Radian();
|
||||
} else {
|
||||
value = joint->GetAngle(0).Degree();
|
||||
}
|
||||
return value >= min && value <= max;
|
||||
}
|
||||
@@ -0,0 +1,21 @@
|
||||
#pragma once
|
||||
|
||||
#include <gazebo/gazebo.hh>
|
||||
|
||||
#include "switch.h"
|
||||
|
||||
using namespace gazebo;
|
||||
|
||||
class InternalLimitSwitch : public Switch {
|
||||
public:
|
||||
InternalLimitSwitch(physics::ModelPtr model, sdf::ElementPtr sdf);
|
||||
~InternalLimitSwitch();
|
||||
|
||||
/// \brief Returns true when the switch is triggered.
|
||||
virtual bool Get();
|
||||
|
||||
private:
|
||||
physics::JointPtr joint;
|
||||
double min, max;
|
||||
bool radians;
|
||||
};
|
||||
@@ -0,0 +1,52 @@
|
||||
#include "limit_switch.h"
|
||||
#include "internal_limit_switch.h"
|
||||
#include "external_limit_switch.h"
|
||||
|
||||
#include <gazebo/physics/physics.hh>
|
||||
#include <gazebo/transport/transport.hh>
|
||||
|
||||
#include "msgs/msgs.h"
|
||||
|
||||
GZ_REGISTER_MODEL_PLUGIN(LimitSwitch)
|
||||
|
||||
LimitSwitch::LimitSwitch() {}
|
||||
|
||||
LimitSwitch::~LimitSwitch() {}
|
||||
|
||||
void LimitSwitch::Load(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();
|
||||
}
|
||||
std::string type = sdf->Get<std::string>("type");
|
||||
|
||||
gzmsg << "Initializing limit switch: " << topic << " type=" << type << std::endl;
|
||||
if (type == "internal") {
|
||||
ls = new InternalLimitSwitch(model, sdf);
|
||||
} else if (type == "external") {
|
||||
ls = new ExternalLimitSwitch(sdf);
|
||||
} else {
|
||||
gzerr << "WARNING: unsupported limit switch type " << type;
|
||||
}
|
||||
|
||||
// Connect to Gazebo transport for messaging
|
||||
std::string scoped_name = model->GetWorld()->GetName()+"::"+model->GetScopedName();
|
||||
boost::replace_all(scoped_name, "::", "/");
|
||||
node = transport::NodePtr(new transport::Node());
|
||||
node->Init(scoped_name);
|
||||
pub = node->Advertise<msgs::Bool>(topic);
|
||||
|
||||
// Connect to the world update event.
|
||||
// This will trigger the Update function every Gazebo iteration
|
||||
updateConn = event::Events::ConnectWorldUpdateBegin(boost::bind(&LimitSwitch::Update, this, _1));
|
||||
}
|
||||
|
||||
void LimitSwitch::Update(const common::UpdateInfo &info) {
|
||||
msgs::Bool msg;
|
||||
msg.set_data(ls->Get());
|
||||
pub->Publish(msg);
|
||||
}
|
||||
@@ -0,0 +1,78 @@
|
||||
#pragma once
|
||||
|
||||
#include <gazebo/gazebo.hh>
|
||||
|
||||
#include "switch.h"
|
||||
|
||||
using namespace gazebo;
|
||||
|
||||
/**
|
||||
* \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="libgz_limit_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="libgz_limit_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.Float64.
|
||||
* - `type`: Required. The type of limit switch that this is
|
||||
*
|
||||
* 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 ModelPlugin {
|
||||
public:
|
||||
LimitSwitch();
|
||||
~LimitSwitch();
|
||||
|
||||
/// \brief Load the limit switch and configures it according to the sdf.
|
||||
void Load(physics::ModelPtr model, sdf::ElementPtr sdf);
|
||||
|
||||
/// \brief Sends out the limit switch reading each timestep.
|
||||
void Update(const 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;
|
||||
|
||||
|
||||
physics::ModelPtr model; ///< \brief The model that this is attached to.
|
||||
event::ConnectionPtr updateConn; ///< \brief Pointer to the world update function.
|
||||
transport::NodePtr node; ///< \brief The node we're advertising on.
|
||||
transport::PublisherPtr pub; ///< \brief Publisher handle.
|
||||
};
|
||||
9
simulation/frc_gazebo_plugins/limit_switch/src/switch.h
Normal file
9
simulation/frc_gazebo_plugins/limit_switch/src/switch.h
Normal file
@@ -0,0 +1,9 @@
|
||||
#pragma once
|
||||
|
||||
class Switch {
|
||||
public:
|
||||
virtual ~Switch() {}
|
||||
|
||||
/// \brief Returns true when the switch is triggered.
|
||||
virtual bool Get() = 0;
|
||||
};
|
||||
@@ -1,5 +1,5 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(frc_gazebo_plugin)
|
||||
project(gz_msgs)
|
||||
|
||||
include (FindPkgConfig)
|
||||
if (PKG_CONFIG_FOUND)
|
||||
@@ -13,4 +13,4 @@ include_directories(src ${Boost_INCLUDE_DIR} ${GAZEBO_INCLUDE_DIRS})
|
||||
add_library(${PROJECT_NAME} SHARED ${SRC_FILES})
|
||||
|
||||
link_directories(${GAZEBO_LIBRARY_DIRS})
|
||||
target_link_libraries(${PROJECT_NAME} ${GAZEBO_LIBRARIES} tinyxml2)
|
||||
target_link_libraries(${PROJECT_NAME} ${GAZEBO_LIBRARIES})
|
||||
15
simulation/frc_gazebo_plugins/msgs/Makefile
Normal file
15
simulation/frc_gazebo_plugins/msgs/Makefile
Normal file
@@ -0,0 +1,15 @@
|
||||
prefix = /usr
|
||||
lib.dir = $(prefix)/lib
|
||||
plugin.dir = $(lib.dir)/frcsim/plugins
|
||||
build.dir = build
|
||||
|
||||
all:
|
||||
mkdir -p $(build.dir)
|
||||
cd ${build.dir} && cmake .. && make
|
||||
|
||||
clean:
|
||||
rm -rf $(build.dir)
|
||||
|
||||
install: all
|
||||
mkdir -p $(DESTDIR)$(plugin.dir)
|
||||
install $(build.dir)/libgz_msgs.so $(DESTDIR)$(plugin.dir)
|
||||
@@ -0,0 +1,17 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(gz_pneumatic_piston)
|
||||
|
||||
include (FindPkgConfig)
|
||||
if (PKG_CONFIG_FOUND)
|
||||
pkg_check_modules(GAZEBO gazebo)
|
||||
endif()
|
||||
|
||||
find_package(gazebo REQUIRED)
|
||||
|
||||
file(GLOB_RECURSE SRC_FILES src/*.cpp)
|
||||
file(GLOB_RECURSE MSG_FILES ../msgs/src/*.cc)
|
||||
include_directories(src ${Boost_INCLUDE_DIR} ${GAZEBO_INCLUDE_DIRS} ../msgs/src)
|
||||
add_library(${PROJECT_NAME} SHARED ${SRC_FILES} ${MSG_FILES})
|
||||
|
||||
link_directories(${GAZEBO_LIBRARY_DIRS} ../msgs/build)
|
||||
target_link_libraries(${PROJECT_NAME} ${GAZEBO_LIBRARIES})
|
||||
15
simulation/frc_gazebo_plugins/pneumatic_piston/Makefile
Normal file
15
simulation/frc_gazebo_plugins/pneumatic_piston/Makefile
Normal file
@@ -0,0 +1,15 @@
|
||||
prefix = /usr
|
||||
lib.dir = $(prefix)/lib
|
||||
plugin.dir = $(lib.dir)/frcsim/plugins
|
||||
build.dir = build
|
||||
|
||||
all:
|
||||
mkdir -p $(build.dir)
|
||||
cd ${build.dir} && cmake .. && make
|
||||
|
||||
clean:
|
||||
rm -rf $(build.dir)
|
||||
|
||||
install: all
|
||||
mkdir -p $(DESTDIR)$(plugin.dir)
|
||||
install $(build.dir)/libgz_pneumatic_piston.so $(DESTDIR)$(plugin.dir)
|
||||
@@ -0,0 +1,55 @@
|
||||
#include "pneumatic_piston.h"
|
||||
|
||||
#include <gazebo/physics/physics.hh>
|
||||
#include <gazebo/transport/transport.hh>
|
||||
|
||||
GZ_REGISTER_MODEL_PLUGIN(PneumaticPiston)
|
||||
|
||||
PneumaticPiston::PneumaticPiston() {}
|
||||
|
||||
PneumaticPiston::~PneumaticPiston() {}
|
||||
|
||||
void PneumaticPiston::Load(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();
|
||||
}
|
||||
|
||||
forward_force = sdf->Get<double>("forward-force");
|
||||
reverse_force = -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;
|
||||
|
||||
// Connect to Gazebo transport for messaging
|
||||
std::string scoped_name = model->GetWorld()->GetName()+"::"+model->GetScopedName();
|
||||
boost::replace_all(scoped_name, "::", "/");
|
||||
node = transport::NodePtr(new transport::Node());
|
||||
node->Init(scoped_name);
|
||||
sub = node->Subscribe(topic, &PneumaticPiston::Callback, this);
|
||||
|
||||
// Connect to the world update event.
|
||||
// This will trigger the Update function every Gazebo iteration
|
||||
updateConn = event::Events::ConnectWorldUpdateBegin(boost::bind(&PneumaticPiston::Update, this, _1));
|
||||
}
|
||||
|
||||
void PneumaticPiston::Update(const common::UpdateInfo &info) {
|
||||
joint->SetForce(0, signal);
|
||||
}
|
||||
|
||||
void PneumaticPiston::Callback(const msgs::ConstFloat64Ptr &msg) {
|
||||
if (msg->data() < -0.001) { signal = -reverse_force; }
|
||||
else if (msg->data() > 0.001) { signal = forward_force; }
|
||||
}
|
||||
@@ -0,0 +1,75 @@
|
||||
#pragma once
|
||||
|
||||
#include <gazebo/gazebo.hh>
|
||||
|
||||
#include "msgs/msgs.h"
|
||||
|
||||
using namespace gazebo;
|
||||
|
||||
/**
|
||||
* \brief Plugin for controlling a joint with a pneumatic piston.
|
||||
*
|
||||
* This plugin subscribes to a topic to get a signal. It accepts three
|
||||
* values:
|
||||
*
|
||||
* - 1: Apply the forward force to the joint.
|
||||
* - 0: Maintain last applied force
|
||||
* - -1: Apply the reverse force to the joint.
|
||||
*
|
||||
* Every physics update the joint's torque is set to reflect the
|
||||
* signal.
|
||||
*
|
||||
* To add a pneumatic piston to your robot, add the following XML to
|
||||
* your robot model:
|
||||
*
|
||||
* <plugin name="my_piston" filename="libgz_pneumatic_piston.so">
|
||||
* <joint>Joint Name</joint>
|
||||
* <topic>~/my/topic</topic>
|
||||
* <direction>{forward, reversed}</direction>
|
||||
* <forward-force>Number</forward-force>
|
||||
* <reverse-force>Number</reverse-force>
|
||||
* </plugin>
|
||||
*
|
||||
* - `joint`: Name of the joint this Dc motor is attached to.
|
||||
* - `topic`: Optional. Message type should be gazebo.msgs.Float64.
|
||||
* - `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.
|
||||
*
|
||||
* \todo Signal should probably be made a tri-state message.
|
||||
*/
|
||||
class PneumaticPiston: public ModelPlugin {
|
||||
public:
|
||||
PneumaticPiston();
|
||||
~PneumaticPiston();
|
||||
|
||||
/// \brief Load the pneumatic piston and configures it according to the sdf.
|
||||
void Load(physics::ModelPtr model, sdf::ElementPtr sdf);
|
||||
|
||||
/// \brief Updat the force the piston applies on the joint.
|
||||
void Update(const common::UpdateInfo &info);
|
||||
|
||||
private:
|
||||
/// \brief Topic to read control signal from.
|
||||
std::string topic;
|
||||
|
||||
/// \brief The signal is one of: {-1,0,1}.
|
||||
double signal;
|
||||
|
||||
/// \brief The magic force multipliers for each direction.
|
||||
double forward_force, reverse_force;
|
||||
|
||||
/// \brief The joint that this pneumatic piston actuates.
|
||||
physics::JointPtr joint;
|
||||
|
||||
/// \brief Callback for receiving msgs and updating the torque.
|
||||
void Callback(const msgs::ConstFloat64Ptr &msg);
|
||||
|
||||
|
||||
physics::ModelPtr model; ///< \brief The model that this is attached to.
|
||||
event::ConnectionPtr updateConn; ///< \brief Pointer to the world update function.
|
||||
transport::NodePtr node; ///< \brief The node we're advertising on.
|
||||
transport::SubscriberPtr sub; ///< \brief Subscriber handle.
|
||||
};
|
||||
17
simulation/frc_gazebo_plugins/potentiometer/CMakeLists.txt
Normal file
17
simulation/frc_gazebo_plugins/potentiometer/CMakeLists.txt
Normal file
@@ -0,0 +1,17 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(gz_potentiometer)
|
||||
|
||||
include (FindPkgConfig)
|
||||
if (PKG_CONFIG_FOUND)
|
||||
pkg_check_modules(GAZEBO gazebo)
|
||||
endif()
|
||||
|
||||
find_package(gazebo REQUIRED)
|
||||
|
||||
file(GLOB_RECURSE SRC_FILES src/*.cpp)
|
||||
file(GLOB_RECURSE MSG_FILES ../msgs/src/*.cc)
|
||||
include_directories(src ${Boost_INCLUDE_DIR} ${GAZEBO_INCLUDE_DIRS} ../msgs/src)
|
||||
add_library(${PROJECT_NAME} SHARED ${SRC_FILES} ${MSG_FILES})
|
||||
|
||||
link_directories(${GAZEBO_LIBRARY_DIRS} ../msgs/build)
|
||||
target_link_libraries(${PROJECT_NAME} ${GAZEBO_LIBRARIES})
|
||||
15
simulation/frc_gazebo_plugins/potentiometer/Makefile
Normal file
15
simulation/frc_gazebo_plugins/potentiometer/Makefile
Normal file
@@ -0,0 +1,15 @@
|
||||
prefix = /usr
|
||||
lib.dir = $(prefix)/lib
|
||||
plugin.dir = $(lib.dir)/frcsim/plugins
|
||||
build.dir = build
|
||||
|
||||
all:
|
||||
mkdir -p $(build.dir)
|
||||
cd ${build.dir} && cmake .. && make
|
||||
|
||||
clean:
|
||||
rm -rf $(build.dir)
|
||||
|
||||
install: all
|
||||
mkdir -p $(DESTDIR)$(plugin.dir)
|
||||
install $(build.dir)/libgz_potentiometer.so $(DESTDIR)$(plugin.dir)
|
||||
@@ -0,0 +1,56 @@
|
||||
#include "potentiometer.h"
|
||||
|
||||
#include <gazebo/physics/physics.hh>
|
||||
#include <gazebo/transport/transport.hh>
|
||||
#include <boost/algorithm/string.hpp>
|
||||
|
||||
#include "msgs/msgs.h"
|
||||
|
||||
GZ_REGISTER_MODEL_PLUGIN(Potentiometer)
|
||||
|
||||
Potentiometer::Potentiometer() {}
|
||||
|
||||
Potentiometer::~Potentiometer() {}
|
||||
|
||||
void Potentiometer::Load(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;
|
||||
}
|
||||
|
||||
gzmsg << "Initializing potentiometer: " << topic << " joint=" << joint->GetName()
|
||||
<< " radians=" << radians << std::endl;
|
||||
|
||||
// Connect to Gazebo transport for messaging
|
||||
std::string scoped_name = model->GetWorld()->GetName()+"::"+model->GetScopedName();
|
||||
boost::replace_all(scoped_name, "::", "/");
|
||||
node = transport::NodePtr(new transport::Node());
|
||||
node->Init(scoped_name);
|
||||
pub = node->Advertise<msgs::Float64>(topic);
|
||||
|
||||
// Connect to the world update event.
|
||||
// This will trigger the Update function every Gazebo iteration
|
||||
updateConn = event::Events::ConnectWorldUpdateBegin(boost::bind(&Potentiometer::Update, this, _1));
|
||||
}
|
||||
|
||||
void Potentiometer::Update(const common::UpdateInfo &info) {
|
||||
joint->GetAngle(0).Normalize();
|
||||
msgs::Float64 msg;
|
||||
if (radians) {
|
||||
msg.set_data(joint->GetAngle(0).Radian());
|
||||
} else {
|
||||
msg.set_data(joint->GetAngle(0).Degree());
|
||||
}
|
||||
pub->Publish(msg);
|
||||
}
|
||||
@@ -0,0 +1,52 @@
|
||||
#pragma once
|
||||
|
||||
#include <gazebo/gazebo.hh>
|
||||
|
||||
using namespace gazebo;
|
||||
|
||||
/**
|
||||
* \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="libgz_potentiometer.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.
|
||||
* - `units`: Optional. Defaults to radians.
|
||||
*/
|
||||
class Potentiometer: public ModelPlugin {
|
||||
public:
|
||||
Potentiometer();
|
||||
~Potentiometer();
|
||||
|
||||
/// \brief Load the potentiometer and configures it according to the sdf.
|
||||
void Load(physics::ModelPtr model, sdf::ElementPtr sdf);
|
||||
|
||||
/// \brief Sends out the potentiometer reading each timestep.
|
||||
void Update(const 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 The joint that this potentiometer measures
|
||||
physics::JointPtr joint;
|
||||
|
||||
|
||||
physics::ModelPtr model; ///< \brief The model that this is attached to.
|
||||
event::ConnectionPtr updateConn; ///< \brief Pointer to the world update function.
|
||||
transport::NodePtr node; ///< \brief The node we're advertising on.
|
||||
transport::PublisherPtr pub; ///< \brief Publisher handle.
|
||||
};
|
||||
17
simulation/frc_gazebo_plugins/rangefinder/CMakeLists.txt
Normal file
17
simulation/frc_gazebo_plugins/rangefinder/CMakeLists.txt
Normal file
@@ -0,0 +1,17 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(gz_rangefinder)
|
||||
|
||||
include (FindPkgConfig)
|
||||
if (PKG_CONFIG_FOUND)
|
||||
pkg_check_modules(GAZEBO gazebo)
|
||||
endif()
|
||||
|
||||
find_package(gazebo REQUIRED)
|
||||
|
||||
file(GLOB_RECURSE SRC_FILES src/*.cpp)
|
||||
file(GLOB_RECURSE MSG_FILES ../msgs/src/*.cc)
|
||||
include_directories(src ${Boost_INCLUDE_DIR} ${GAZEBO_INCLUDE_DIRS} ../msgs/src)
|
||||
add_library(${PROJECT_NAME} SHARED ${SRC_FILES} ${MSG_FILES})
|
||||
|
||||
link_directories(${GAZEBO_LIBRARY_DIRS} ../msgs/build)
|
||||
target_link_libraries(${PROJECT_NAME} ${GAZEBO_LIBRARIES})
|
||||
15
simulation/frc_gazebo_plugins/rangefinder/Makefile
Normal file
15
simulation/frc_gazebo_plugins/rangefinder/Makefile
Normal file
@@ -0,0 +1,15 @@
|
||||
prefix = /usr
|
||||
lib.dir = $(prefix)/lib
|
||||
plugin.dir = $(lib.dir)/frcsim/plugins
|
||||
build.dir = build
|
||||
|
||||
all:
|
||||
mkdir -p $(build.dir)
|
||||
cd ${build.dir} && cmake .. && make
|
||||
|
||||
clean:
|
||||
rm -rf $(build.dir)
|
||||
|
||||
install: all
|
||||
mkdir -p $(DESTDIR)$(plugin.dir)
|
||||
install $(build.dir)/libgz_rangefinder.so $(DESTDIR)$(plugin.dir)
|
||||
@@ -0,0 +1,47 @@
|
||||
#include "rangefinder.h"
|
||||
|
||||
#include <gazebo/physics/physics.hh>
|
||||
#include <gazebo/transport/transport.hh>
|
||||
#include <gazebo/sensors/sensors.hh>
|
||||
|
||||
#include <boost/pointer_cast.hpp>
|
||||
|
||||
#include "msgs/msgs.h"
|
||||
|
||||
GZ_REGISTER_MODEL_PLUGIN(Rangefinder)
|
||||
|
||||
Rangefinder::Rangefinder() {}
|
||||
|
||||
Rangefinder::~Rangefinder() {}
|
||||
|
||||
void Rangefinder::Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
|
||||
this->model = model;
|
||||
|
||||
// Parse SDF properties
|
||||
sensor = boost::dynamic_pointer_cast<sensors::SonarSensor>(
|
||||
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->GetName() << std::endl;
|
||||
|
||||
// Connect to Gazebo transport for messaging
|
||||
std::string scoped_name = model->GetWorld()->GetName()+"::"+model->GetScopedName();
|
||||
boost::replace_all(scoped_name, "::", "/");
|
||||
node = transport::NodePtr(new transport::Node());
|
||||
node->Init(scoped_name);
|
||||
pub = node->Advertise<msgs::Float64>(topic);
|
||||
|
||||
// Connect to the world update event.
|
||||
// This will trigger the Update function every Gazebo iteration
|
||||
updateConn = event::Events::ConnectWorldUpdateBegin(boost::bind(&Rangefinder::Update, this, _1));
|
||||
}
|
||||
|
||||
void Rangefinder::Update(const common::UpdateInfo &info) {
|
||||
msgs::Float64 msg;
|
||||
msg.set_data(sensor->GetRange());
|
||||
pub->Publish(msg);
|
||||
}
|
||||
46
simulation/frc_gazebo_plugins/rangefinder/src/rangefinder.h
Normal file
46
simulation/frc_gazebo_plugins/rangefinder/src/rangefinder.h
Normal file
@@ -0,0 +1,46 @@
|
||||
#pragma once
|
||||
|
||||
#include <gazebo/gazebo.hh>
|
||||
|
||||
using namespace gazebo;
|
||||
|
||||
/**
|
||||
* \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="libgz_rangefinder.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 ModelPlugin {
|
||||
public:
|
||||
Rangefinder();
|
||||
~Rangefinder();
|
||||
|
||||
/// \brief Load the rangefinder and configures it according to the sdf.
|
||||
void Load(physics::ModelPtr model, sdf::ElementPtr sdf);
|
||||
|
||||
/// \brief Sends out the rangefinder reading each timestep.
|
||||
void Update(const common::UpdateInfo &info);
|
||||
|
||||
private:
|
||||
/// \brief Publish the range on this topic.
|
||||
std::string topic;
|
||||
|
||||
/// \brief The sonar sensor that this rangefinder uses
|
||||
sensors::SonarSensorPtr sensor;
|
||||
|
||||
physics::ModelPtr model; ///< \brief The model that this is attached to.
|
||||
event::ConnectionPtr updateConn; ///< \brief Pointer to the world update function.
|
||||
transport::NodePtr node; ///< \brief The node we're advertising on.
|
||||
transport::PublisherPtr pub; ///< \brief Publisher handle.
|
||||
};
|
||||
@@ -17,7 +17,7 @@ void AnalogInput::InitAnalogInput(uint32_t channel)
|
||||
|
||||
m_channel = channel;
|
||||
char buffer[50];
|
||||
int n = sprintf(buffer, "analog/1/%d", channel);
|
||||
int n = sprintf(buffer, "analog/%d", channel);
|
||||
m_impl = new SimFloatInput(buffer);
|
||||
|
||||
LiveWindow::GetInstance()->AddSensor("AnalogInput", channel, this);
|
||||
|
||||
@@ -17,7 +17,7 @@ void DigitalInput::InitDigitalInput(uint32_t channel)
|
||||
m_table = NULL;
|
||||
char buf[64];
|
||||
m_channel = channel;
|
||||
int n = sprintf(buf, "dio/1/%d", channel);
|
||||
int n = sprintf(buf, "dio/%d", channel);
|
||||
m_impl = new SimDigitalInput(buf);
|
||||
}
|
||||
|
||||
|
||||
@@ -41,7 +41,7 @@ void Encoder::InitEncoder(int channelA, int channelB, bool reverseDirection, Enc
|
||||
m_reverseDirection = reverseDirection;
|
||||
}
|
||||
char buffer[50];
|
||||
int n = sprintf(buffer, "dio/1/%d/1/%d", channelA, channelB);
|
||||
int n = sprintf(buffer, "dio/%d/%d", channelA, channelB);
|
||||
impl = new SimEncoder(buffer);
|
||||
}
|
||||
|
||||
|
||||
@@ -29,7 +29,7 @@ void Gyro::InitGyro(int channel)
|
||||
SetPIDSourceParameter(kAngle);
|
||||
|
||||
char buffer[50];
|
||||
int n = sprintf(buffer, "analog/1/%d", channel);
|
||||
int n = sprintf(buffer, "analog/%d", channel);
|
||||
impl = new SimGyro(buffer);
|
||||
|
||||
LiveWindow::GetInstance()->AddSensor("Gyro", channel, this);
|
||||
|
||||
@@ -140,7 +140,7 @@ void Timer::Stop()
|
||||
Synchronized sync(m_semaphore);
|
||||
if (m_running)
|
||||
{
|
||||
m_accumulatedTime = temp;
|
||||
m_accumulatedTime = temp;
|
||||
m_running = false;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -40,7 +40,7 @@ public class AnalogInput extends SensorBase implements PIDSource,
|
||||
*/
|
||||
public AnalogInput(final int channel) {
|
||||
m_channel = channel;
|
||||
m_impl = new SimFloatInput("simulator/analog/1/"+channel); // TODO: If module numbers cannot be specified, remove them from the communication system.
|
||||
m_impl = new SimFloatInput("simulator/analog/"+channel);
|
||||
|
||||
LiveWindow.addSensor("AnalogInput", channel, this);
|
||||
}
|
||||
|
||||
@@ -31,7 +31,7 @@ public class DigitalInput implements IInputOutput,
|
||||
* the port for the digital input
|
||||
*/
|
||||
public DigitalInput(int channel) {
|
||||
impl = new SimDigitalInput("simulator/dio/1/"+channel);
|
||||
impl = new SimDigitalInput("simulator/dio/"+channel);
|
||||
m_channel = channel;
|
||||
}
|
||||
|
||||
|
||||
@@ -64,7 +64,7 @@ public class Encoder extends SensorBase implements CounterBase, PIDSource,
|
||||
} else {
|
||||
m_reverseDirection = reverseDirection;
|
||||
}
|
||||
impl = new SimEncoder("simulator/dio/1/"+aChannel+"/1/"+bChannel);
|
||||
impl = new SimEncoder("simulator/dio/"+aChannel+"/"+bChannel);
|
||||
setDistancePerPulse(1);
|
||||
|
||||
}
|
||||
|
||||
@@ -37,7 +37,7 @@ public class Gyro extends SensorBase implements PIDSource, ISensor,
|
||||
* first turned on while it's sitting at rest before the competition starts.
|
||||
*/
|
||||
private void initGyro(int channel) {
|
||||
impl = new SimGyro("simulator/analog/1/"+channel);
|
||||
impl = new SimGyro("simulator/analog/"+channel);
|
||||
|
||||
reset();
|
||||
setPIDSourceParameter(PIDSourceParameter.kAngle);
|
||||
|
||||
@@ -25,7 +25,7 @@ public class Jaguar implements SpeedController, PIDOutput, MotorSafety, LiveWind
|
||||
*/
|
||||
private void initJaguar(final int channel) {
|
||||
this.channel = channel;
|
||||
impl = new SimSpeedController("simulator/pwm/1/"+channel);
|
||||
impl = new SimSpeedController("simulator/pwm/"+channel);
|
||||
|
||||
m_safetyHelper = new MotorSafetyHelper(this);
|
||||
m_safetyHelper.setExpiration(0.0);
|
||||
|
||||
@@ -36,7 +36,7 @@ public class Talon implements SpeedController, PIDOutput, MotorSafety, LiveWindo
|
||||
*/
|
||||
private void initTalon(final int channel) {
|
||||
this.channel = channel;
|
||||
impl = new SimSpeedController("simulator/pwm/1/"+channel);
|
||||
impl = new SimSpeedController("simulator/pwm/"+channel);
|
||||
|
||||
m_safetyHelper = new MotorSafetyHelper(this);
|
||||
m_safetyHelper.setExpiration(0.0);
|
||||
|
||||
@@ -38,7 +38,7 @@ public class Victor implements SpeedController, PIDOutput, MotorSafety, LiveWind
|
||||
*/
|
||||
private void initVictor(final int channel) {
|
||||
this.channel = channel;
|
||||
impl = new SimSpeedController("simulator/pwm/1/"+channel);
|
||||
impl = new SimSpeedController("simulator/pwm/"+channel);
|
||||
|
||||
m_safetyHelper = new MotorSafetyHelper(this);
|
||||
m_safetyHelper.setExpiration(0.0);
|
||||
|
||||
Reference in New Issue
Block a user