/** on loading of the plugin
* @param _parent Parent Model
*/
void Puck::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
printf("Loading Puck Plugin of model %s\n", _parent->GetName().c_str());
// Store the pointer to the model
this->model_ = _parent;
// Listen to the update event. This event is broadcast every
// simulation iteration.
this->update_connection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&Puck::OnUpdate, this, _1));
// Create the communication Node for communication with fawkes
this->node_ = transport::NodePtr(new transport::Node());
// the namespace is set to the world name!
this->node_->Init(model_->GetWorld()->GetName());
// register visual publisher
this->visual_pub_ = this->node_->Advertise<msgs::Visual>("~/visual");
// initialize without rings or cap
this->ring_count_ = 0;
this->have_cap = false;
this->announced_ = false;
this->new_puck_publisher = this->node_->Advertise<gazsim_msgs::NewPuck>("~/new_puck");
// subscribe for puck commands
this->command_subscriber = this->node_->Subscribe(std::string("~/pucks/cmd"), &Puck::on_command_msg, this);
// publisher for workpiece command results
this->workpiece_result_pub_ = node_->Advertise<gazsim_msgs::WorkpieceResult>("~/pucks/cmd/result");
if (!_sdf->HasElement("baseColor")) {
printf("SDF for base has no baseColor configured, defaulting to RED!\n");
base_color_ = gazsim_msgs::Color::RED;
} else {
std::string config_color = _sdf->GetElement("baseColor")->Get<std::string>();
if (config_color == "RED") {
base_color_ = gazsim_msgs::Color::RED;
} else if (config_color == "BLACK") {
base_color_ = gazsim_msgs::Color::BLACK;
} else if (config_color == "SILVER") {
base_color_ = gazsim_msgs::Color::SILVER;
} else {
printf("SDF for base has no baseColor configured, defaulting to RED!\n");
base_color_ = gazsim_msgs::Color::RED;
}
printf("Base spawns in color %s\n", config_color.c_str());
}
delivery_pub_ = node_->Advertise<llsf_msgs::SetOrderDeliveredByColor>(TOPIC_SET_ORDER_DELIVERY_BY_COLOR);
}
void GazeboPressurePlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
if (kPrintOnPluginLoad) {
gzdbg << __FUNCTION__ << "() called." << std::endl;
}
gzdbg << "_model = " << _model->GetName() << std::endl;
// Store the pointer to the model and the world.
model_ = _model;
world_ = model_->GetWorld();
//==============================================//
//========== READ IN PARAMS FROM SDF ===========//
//==============================================//
// Use the robot namespace to create the node handle.
if (_sdf->HasElement("robotNamespace"))
namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
else
gzerr << "[gazebo_pressure_plugin] Please specify a robotNamespace.\n";
// Get node handle.
node_handle_ = transport::NodePtr(new transport::Node());
// Initialise with default namespace (typically /gazebo/default/).
node_handle_->Init();
std::string link_name;
if (_sdf->HasElement("linkName"))
link_name = _sdf->GetElement("linkName")->Get<std::string>();
else
gzerr << "[gazebo_pressure_plugin] Please specify a linkName.\n";
// Get the pointer to the link.
link_ = model_->GetLink(link_name);
if (link_ == NULL)
gzthrow("[gazebo_pressure_plugin] Couldn't find specified link \"" << link_name << "\".");
frame_id_ = link_name;
// Retrieve the rest of the SDF parameters.
getSdfParam<std::string>(_sdf, "pressureTopic", pressure_topic_, kDefaultPressurePubTopic);
getSdfParam<double>(_sdf, "referenceAltitude", ref_alt_, kDefaultRefAlt);
getSdfParam<double>(_sdf, "pressureVariance", pressure_var_, kDefaultPressureVar);
CHECK(pressure_var_ >= 0.0);
// Initialize the normal distribution for pressure.
double mean = 0.0;
pressure_n_[0] = NormalDistribution(mean, sqrt(pressure_var_));
// Listen to the update event. This event is broadcast every simulation
// iteration.
this->updateConnection_ = event::Events::ConnectWorldUpdateBegin(
boost::bind(&GazeboPressurePlugin::OnUpdate, this, _1));
//==============================================//
//=== POPULATE STATIC PARTS OF PRESSURE MSG ====//
//==============================================//
pressure_message_.mutable_header()->set_frame_id(frame_id_);
pressure_message_.set_variance(pressure_var_);
}
void GazeboFwDynamicsPlugin::Load(physics::ModelPtr _model,
sdf::ElementPtr _sdf) {
if (kPrintOnPluginLoad) {
gzdbg << __FUNCTION__ << "() called." << std::endl;
}
gzdbg << "_model = " << _model->GetName() << std::endl;
// Store the pointer to the model.
model_ = _model;
world_ = model_->GetWorld();
namespace_.clear();
// Get the robot namespace.
if (_sdf->HasElement("robotNamespace"))
namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
else
gzerr << "[gazebo_fw_dynamics_plugin] Please specify a robotNamespace.\n";
// Create the node handle.
node_handle_ = transport::NodePtr(new transport::Node());
// Initialise with default namespace (typically /gazebo/default/).
node_handle_->Init();
// Get the link name.
std::string link_name;
if (_sdf->HasElement("linkName"))
link_name = _sdf->GetElement("linkName")->Get<std::string>();
else
gzerr << "[gazebo_fw_dynamics_plugin] Please specify a linkName.\n";
// Get the pointer to the link.
link_ = model_->GetLink(link_name);
if (link_ == NULL) {
gzthrow("[gazebo_fw_dynamics_plugin] Couldn't find specified link \""
<< link_name << "\".");
}
// Get the path to fixed-wing aerodynamics parameters YAML file. If not
// provided, default Techpod parameters are used.
if (_sdf->HasElement("aeroParamsYAML")) {
std::string aero_params_yaml =
_sdf->GetElement("aeroParamsYAML")->Get<std::string>();
aero_params_.LoadAeroParamsYAML(aero_params_yaml);
} else {
gzwarn << "[gazebo_fw_dynamics_plugin] No aerodynamic paramaters YAML file"
<< " specified, using default Techpod parameters.\n";
}
// Get the path to fixed-wing vehicle parameters YAML file. If not provided,
// default Techpod parameters are used.
if (_sdf->HasElement("vehicleParamsYAML")) {
std::string vehicle_params_yaml =
_sdf->GetElement("vehicleParamsYAML")->Get<std::string>();
vehicle_params_.LoadVehicleParamsYAML(vehicle_params_yaml);
} else {
gzwarn << "[gazebo_fw_dynamics_plugin] No vehicle paramaters YAML file"
<< " specified, using default Techpod parameters.\n";
}
// Get the rest of the sdf parameters.
getSdfParam<bool>(_sdf, "isInputJoystick", is_input_joystick_,
kDefaultIsInputJoystick);
getSdfParam<std::string>(_sdf, "actuatorsSubTopic",
actuators_sub_topic_,
mav_msgs::default_topics::COMMAND_ACTUATORS);
getSdfParam<std::string>(_sdf, "rollPitchYawrateThrustSubTopic",
roll_pitch_yawrate_thrust_sub_topic_,
mav_msgs::default_topics::
COMMAND_ROLL_PITCH_YAWRATE_THRUST);
getSdfParam<std::string>(_sdf, "windSpeedSubTopic",
wind_speed_sub_topic_,
mav_msgs::default_topics::WIND_SPEED);
// Listen to the update event. This event is broadcast every
// simulation iteration.
this->updateConnection_ = event::Events::ConnectWorldUpdateBegin(
boost::bind(&GazeboFwDynamicsPlugin::OnUpdate, this, _1));
}
// Load necessary data from the .sdf file
void GazeboUUVPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
namespace_.clear();
getSdfParam<std::string>(
_sdf, "robotNamespace", namespace_, namespace_, true);
node_handle_ = transport::NodePtr(new transport::Node());
node_handle_->Init(namespace_);
getSdfParam<std::string>(_sdf, "linkName", link_name_, link_name_, true);
// get the base link, thus the base hippocampus model
link_ = _model->GetLink(link_name_);
// get the child links, these are the links which represents the rotors of the hippocampus
rotor_links_ = link_->GetChildJointsLinks();
for(int i = 0; i < rotor_links_.size(); i++) {
std::cout << "Rotor Link:" << rotor_links_[i]->GetScopedName() << "\n";
command_[i] = 0.0;
}
getSdfParam<std::string>(
_sdf, "commandSubTopic", command_sub_topic_, command_sub_topic_);
// subscribe to the commands (actuator outputs from the mixer from PX4)
command_sub_ = node_handle_->Subscribe<mav_msgs::msgs::CommandMotorSpeed>(
"~/" + _model->GetName() + command_sub_topic_, &GazeboUUVPlugin::CommandCallback, this);
update_connection_ = event::Events::ConnectWorldUpdateBegin(
boost::bind(&GazeboUUVPlugin::OnUpdate, this, _1));
// get force and torque parameters for force and torque calculations of the rotors from motor_speed
getSdfParam<double>(
_sdf, "motorForceConstant", motor_force_constant_, motor_force_constant_);
getSdfParam<double>(
_sdf, "motorTorqueConstant", motor_torque_constant_, motor_torque_constant_);
// parameters for added mass and damping
ignition::math::Vector3d added_mass_linear(0,0,0);
getSdfParam<ignition::math::Vector3d>(
_sdf, "addedMassLinear", added_mass_linear, added_mass_linear);
X_udot_ = added_mass_linear[0];
Y_vdot_ = added_mass_linear[1];
Z_wdot_ = added_mass_linear[2];
ignition::math::Vector3d added_mass_angular(0,0,0);
getSdfParam<ignition::math::Vector3d>(
_sdf, "addedMassAngular", added_mass_angular, added_mass_angular);
K_pdot_ = added_mass_angular[0];
M_qdot_ = added_mass_angular[1];
N_rdot_ = added_mass_angular[2];
ignition::math::Vector3d damping_linear(0,0,0);
getSdfParam<ignition::math::Vector3d>(
_sdf, "dampingLinear", damping_linear, damping_linear);
X_u_ = damping_linear[0];
Y_v_ = damping_linear[1];
Z_w_ = damping_linear[2];
ignition::math::Vector3d damping_angular(0,0,0);
getSdfParam<ignition::math::Vector3d>(
_sdf, "dampingAngular", damping_angular, damping_angular);
K_p_ = damping_angular[0];
M_q_ = damping_angular[1];
N_r_ = damping_angular[2];
// variables for debugging
time_ = 0.0;
counter_ = 0.0;
}
void GazeboJointStateClient::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
// Make sure the ROS node for Gazebo has already been initalized
if (!ros::isInitialized())
{
ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
<< "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' "
<< "in the gazebo_ros package)");
return;
}
// get joint names from parameters
std::string armNamespace = _parent->GetName();
if (_sdf->HasElement("robot_components_namespace"))
{
sdf::ElementPtr armParamElem = _sdf->GetElement("robot_components_namespace");
armNamespace = armParamElem->Get<std::string>();
}
else
{
ROS_WARN("SDF Element 'robot_components_namespace' not defined, so using robot name as namespace for components.");
}
// ROS_INFO_STREAM("GazeboJointStateClient loading joints from components namespace '"<<armNamespace<<"'");
ROS_INFO_STREAM("GazeboJointStateClient: Loading arm component parameters from "<< armNamespace);
joints = ArmComponentsNameManagerPtr(new ArmComponentsNameManager(armNamespace,false));
if (!joints->waitToLoadParameters(1, 3, 0.5))
{
ROS_FATAL_STREAM("Cannot load arm components for robot "<<_parent->GetName()<<" from namespace "<<armNamespace);
return;
}
physics::BasePtr jcChild = _parent->GetChild(physics::JointControllerThreadsafe::UniqueName());
if (!jcChild.get())
{
ROS_ERROR("Cannot load GazeboJointStateClient if no default JointControllerThreadsafe is set for the model");
throw std::string("Cannot load GazeboJointStateClient if no default JointController is set for the model");
}
physics::JointControllerThreadsafePtr ptr =
boost::dynamic_pointer_cast<physics::JointControllerThreadsafe>(jcChild);
if (!ptr.get())
{
ROS_ERROR_STREAM("Cannot load GazeboJointStateClient if child '"
<< physics::JointControllerThreadsafe::UniqueName()
<< "' is not of class JointControllerThreadsafe");
throw std::string("Cannot load GazeboJointStateClient if child '"
+ physics::JointControllerThreadsafe::UniqueName()
+ "' is not of class JointControllerThreadsafe");
}
jointController = ptr;
// get joint names from parameters
std::vector<std::string> joint_names;
joints->getJointNames(joint_names, true);
const std::vector<float>& arm_init = joints->getArmJointsInitPose();
const std::vector<float>& gripper_init = joints->getGripperJointsInitPose();
// Print the joint names to help debugging
std::map<std::string, physics::JointPtr > jntMap = jointController->GetJoints();
for (std::map<std::string, physics::JointPtr>::iterator it = jntMap.begin(); it != jntMap.end(); ++it)
{
physics::JointPtr j = it->second;
ROS_INFO_STREAM("Gazebo joint: '"<<j->GetName()<<"' is registered as '"<<it->first<<"'");
}
// check if the joint names maintained in 'joints' match the names in gazebo,
// that the joints can be used by this class, and if yes, load PID controllers.
int i = 0;
for (std::vector<std::string>::iterator it = joint_names.begin();
it != joint_names.end(); ++it)
{
// ROS_INFO_STREAM("Local joint name: '"<<*it<<"'");
physics::JointPtr joint = _parent->GetJoint(*it);
if (!joint.get())
{
ROS_FATAL_STREAM("Joint name " << *it << " not found as robot joint");
throw std::string("Joint not found");
}
std::string scopedName = joint->GetScopedName();
std::map<std::string, physics::JointPtr >::iterator jit = jntMap.find(scopedName);
if (jit == jntMap.end())
{
ROS_ERROR_STREAM("Joint name " << *it << " not found in joint controller joints");
throw std::string("Joint not found");
}
++i;
}
model = _parent;
jsSub = nh.subscribe(jointStateTopic, 1, &GazeboJointStateClient::JointStateCallback, this);
}