// Overloaded Gazebo entry point
void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::ElementPtr sdf)
{
ROS_INFO_STREAM_NAMED("gazebo_ros_control","Loading gazebo_ros_control plugin");
// Save pointers to the model
parent_model_ = parent;
sdf_ = sdf;
// Error message if the model couldn't be found
if (!parent_model_)
{
ROS_ERROR_STREAM_NAMED("loadThread","parent model is NULL");
return;
}
// Check that ROS has been initialized
if(!ros::isInitialized())
{
ROS_FATAL_STREAM_NAMED("gazebo_ros_control","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 namespace for nodehandle
if(sdf_->HasElement("robotNamespace"))
{
robot_namespace_ = sdf_->GetElement("robotNamespace")->Get<std::string>();
}
else
{
robot_namespace_ = parent_model_->GetName(); // default
}
// Get robot_description ROS param name
if (sdf_->HasElement("robotParam"))
{
robot_description_ = sdf_->GetElement("robotParam")->Get<std::string>();
}
else
{
robot_description_ = "robot_description"; // default
}
// Get the robot simulation interface type
if(sdf_->HasElement("robotSimType"))
{
robot_hw_sim_type_str_ = sdf_->Get<std::string>("robotSimType");
}
else
{
robot_hw_sim_type_str_ = "gazebo_ros_control/DefaultRobotHWSim";
ROS_DEBUG_STREAM_NAMED("loadThread","Using default plugin for RobotHWSim (none specified in URDF/SDF)\""<<robot_hw_sim_type_str_<<"\"");
}
// Get the Gazebo simulation period
ros::Duration gazebo_period(parent_model_->GetWorld()->GetPhysicsEngine()->GetMaxStepSize());
// Decide the plugin control period
if(sdf_->HasElement("controlPeriod"))
{
control_period_ = ros::Duration(sdf_->Get<double>("controlPeriod"));
// Check the period against the simulation period
if( control_period_ < gazebo_period )
{
ROS_ERROR_STREAM_NAMED("gazebo_ros_control","Desired controller update period ("<<control_period_
<<" s) is faster than the gazebo simulation period ("<<gazebo_period<<" s).");
}
else if( control_period_ > gazebo_period )
{
ROS_WARN_STREAM_NAMED("gazebo_ros_control","Desired controller update period ("<<control_period_
<<" s) is slower than the gazebo simulation period ("<<gazebo_period<<" s).");
}
}
else
{
control_period_ = gazebo_period;
ROS_DEBUG_STREAM_NAMED("gazebo_ros_control","Control period not found in URDF/SDF, defaulting to Gazebo period of "
<< control_period_);
}
// Get parameters/settings for controllers from ROS param server
model_nh_ = ros::NodeHandle(robot_namespace_);
// Initialize the emergency stop code.
e_stop_active_ = false;
last_e_stop_active_ = false;
if (sdf_->HasElement("eStopTopic"))
{
const std::string e_stop_topic = sdf_->GetElement("eStopTopic")->Get<std::string>();
e_stop_sub_ = model_nh_.subscribe(e_stop_topic, 1, &GazeboRosControlPlugin::eStopCB, this);
}
ROS_INFO_NAMED("gazebo_ros_control", "Starting gazebo_ros_control plugin in namespace: %s", robot_namespace_.c_str());
// Read urdf from ros parameter server then
// setup actuators and mechanism control node.
// This call will block if ROS is not properly initialized.
const std::string urdf_string = getURDF(robot_description_);
if (!parseTransmissionsFromURDF(urdf_string))
{
ROS_ERROR_NAMED("gazebo_ros_control", "Error parsing URDF in gazebo_ros_control plugin, plugin not active.\n");
return;
}
// Load the RobotHWSim abstraction to interface the controllers with the gazebo model
try
{
robot_hw_sim_loader_.reset
(new pluginlib::ClassLoader<gazebo_ros_control::RobotHWSim>
("gazebo_ros_control",
"gazebo_ros_control::RobotHWSim"));
robot_hw_sim_ = robot_hw_sim_loader_->createInstance(robot_hw_sim_type_str_);
urdf::Model urdf_model;
const urdf::Model *const urdf_model_ptr = urdf_model.initString(urdf_string) ? &urdf_model : NULL;
if(!robot_hw_sim_->initSim(robot_namespace_, model_nh_, parent_model_, urdf_model_ptr, transmissions_))
{
ROS_FATAL_NAMED("gazebo_ros_control","Could not initialize robot simulation interface");
return;
}
// Create the controller manager
ROS_DEBUG_STREAM_NAMED("ros_control_plugin","Loading controller_manager");
controller_manager_.reset
(new controller_manager::ControllerManager(robot_hw_sim_.get(), model_nh_));
// Listen to the update event. This event is broadcast every simulation iteration.
update_connection_ =
gazebo::event::Events::ConnectWorldUpdateBegin
(boost::bind(&GazeboRosControlPlugin::Update, this));
}
catch(pluginlib::LibraryLoadException &ex)
{
ROS_FATAL_STREAM_NAMED("gazebo_ros_control","Failed to create robot simulation interface loader: "<<ex.what());
}
ROS_INFO_NAMED("gazebo_ros_control", "Loaded gazebo_ros_control.");
}
void LWRHW::create(std::string name, std::string urdf_string)
{
std::cout << "Creating a KUKA LWR 4+ called: " << name << std::endl;
// SET NAME AND MODEL
robot_namespace_ = name;
urdf_string_ = urdf_string;
// ALLOCATE MEMORY
// JOINT NAMES ARE TAKEN FROM URDF NAME CONVENTION
joint_names_.push_back( robot_namespace_ + std::string("_0_joint") );
joint_names_.push_back( robot_namespace_ + std::string("_1_joint") );
joint_names_.push_back( robot_namespace_ + std::string("_2_joint") );
joint_names_.push_back( robot_namespace_ + std::string("_3_joint") );
joint_names_.push_back( robot_namespace_ + std::string("_4_joint") );
joint_names_.push_back( robot_namespace_ + std::string("_5_joint") );
joint_names_.push_back( robot_namespace_ + std::string("_6_joint") );
// VARIABLES
joint_position_.resize(n_joints_);
joint_position_prev_.resize(n_joints_);
joint_velocity_.resize(n_joints_);
joint_effort_.resize(n_joints_);
joint_stiffness_.resize(n_joints_);
joint_damping_.resize(n_joints_);
joint_position_command_.resize(n_joints_);
joint_velocity_command_.resize(n_joints_);
joint_effort_command_.resize(n_joints_);
joint_stiffness_command_.resize(n_joints_);
joint_damping_command_.resize(n_joints_);
joint_lower_limits_.resize(n_joints_);
joint_upper_limits_.resize(n_joints_);
joint_lower_limits_stiffness_.resize(n_joints_);
joint_upper_limits_stiffness_.resize(n_joints_);
joint_effort_limits_.resize(n_joints_);
// RESET VARIABLES
reset();
std::cout << "Parsing transmissions from the URDF..." << std::endl;
// GET TRANSMISSIONS THAT BELONG TO THIS LWR 4+ ARM
if (!parseTransmissionsFromURDF(urdf_string_))
{
std::cout << "lwr_hw: " << "Error parsing URDF in lwr_hw.\n" << std::endl;
return;
}
std::cout << "Registering interfaces..." << std::endl;
const urdf::Model *const urdf_model_ptr = urdf_model_.initString(urdf_string_) ? &urdf_model_ : NULL;
registerInterfaces(urdf_model_ptr, transmissions_);
std::cout << "Initializing KDL variables..." << std::endl;
// INIT KDL STUFF
initKDLdescription(urdf_model_ptr);
std::cout << "Succesfully created an abstract LWR 4+ ARM with interfaces to ROS control" << std::endl;
}
