mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-23 01:21:42 +00:00
merged from frcsim branch
verified to work on real robots adds sim eclipse plugins, fixed JavaGazebo, made wpilibC++Sim build on windows - Java and C++ simulation robot programs run on windows - simulation eclipse plugin delivers models and gazebo plugins - Java Gazebo now respects GAZEBO_IP variables and can work across networks - hal and network tables win32 hacked to work on windows - smart dashboard broken on windows due to network tables hacks - wpilibC++Sim, gz_msgs, and frcsim_gazebo_plugins build with CMake - removed constexpr for cross platform compatibility - msgs generated using .protos as a part of build process - some spare and unused cmake/pom files deleted - simulation ubuntu debians removed entirely - refactored CMake project flags and macros - updated to match non-sim C++ API - fixed and updated documentation - servo added to simulation Change-Id: Ia702ff0f1fee10d77f543810ad88f56696443b05
This commit is contained in:
70
simulation/frc_gazebo_plugins/servo/src/servo.cpp
Normal file
70
simulation/frc_gazebo_plugins/servo/src/servo.cpp
Normal file
@@ -0,0 +1,70 @@
|
||||
#ifdef _WIN32
|
||||
// Ensure that Winsock2.h is included before Windows.h, which can get
|
||||
// pulled in by anybody (e.g., Boost).
|
||||
#include <Winsock2.h>
|
||||
#endif
|
||||
|
||||
#include "servo.h"
|
||||
|
||||
#include <gazebo/physics/physics.hh>
|
||||
#include <gazebo/transport/transport.hh>
|
||||
|
||||
GZ_REGISTER_MODEL_PLUGIN(Servo)
|
||||
|
||||
Servo::Servo() {}
|
||||
|
||||
Servo::~Servo() {}
|
||||
|
||||
void Servo::Load(physics::ModelPtr model, sdf::ElementPtr sdf){
|
||||
this->model = model;
|
||||
signal = 0;
|
||||
|
||||
//parse SDF Properries
|
||||
joint = model->GetJoint(sdf->Get<std::string>("joint"));
|
||||
if (sdf->HasElement("topic")) {
|
||||
topic = sdf->Get<std::string>("topic");
|
||||
}
|
||||
else {
|
||||
topic = "~/"+sdf->GetAttribute("name")->GetAsString();
|
||||
}
|
||||
|
||||
if (sdf->HasElement("torque")) {
|
||||
torque = sdf->Get<double>("torque");
|
||||
}
|
||||
else {
|
||||
torque = 5;
|
||||
}
|
||||
|
||||
gzmsg << "initializing awesome servo: " << topic
|
||||
<< " joint=" << joint->GetName()
|
||||
<< " torque=" << torque << std::endl;
|
||||
|
||||
//Connect to Gazebo transport for messaging
|
||||
std::string scoped_name = model->GetWorld()->GetName()+"::"+model->GetScopedName();
|
||||
boost::replace_all(scoped_name, "::","/");
|
||||
node = transport::NodePtr(new transport::Node());
|
||||
node->Init(scoped_name);
|
||||
sub = node->Subscribe(topic, &Servo::Callback, this);
|
||||
|
||||
//connect to the world update event
|
||||
//this will call update every iteration
|
||||
updateConn = event::Events::ConnectWorldUpdateBegin(boost::bind(&Servo::Update, this, _1));
|
||||
}
|
||||
|
||||
void Servo::Update(const common::UpdateInfo &info){
|
||||
//torque is in kg*cm
|
||||
//joint->SetAngle(0,signal*180);
|
||||
if (joint->GetAngle(0) < signal){
|
||||
joint->SetForce(0,torque);
|
||||
}
|
||||
else if (joint->GetAngle(0) > signal){
|
||||
joint->SetForce(0,torque);
|
||||
}
|
||||
joint->SetForce(0,0);
|
||||
}
|
||||
|
||||
void Servo::Callback(const msgs::ConstFloat64Ptr &msg){
|
||||
signal = msg->data();
|
||||
if (signal < -1) { signal = -1; }
|
||||
else if (signal > 1) { signal = 1; }
|
||||
}
|
||||
Reference in New Issue
Block a user