int main( int argc, char** argv )
{
// initialize ROS
ros::init(argc, argv, "lwr_hw_interface", ros::init_options::NoSigintHandler);
// ros spinner
ros::AsyncSpinner spinner(1);
spinner.start();
// custom signal handlers
signal(SIGTERM, quitRequested);
signal(SIGINT, quitRequested);
signal(SIGHUP, quitRequested);
// create a node
ros::NodeHandle lwr_nh;
std::string robot_namespace = lwr_nh.getNamespace();
ROS_INFO("kuka_lwr_hw_rt_node -> **** node handle namespace = '%s'",robot_namespace.c_str());
// Remove the first two characters.
std::string robot_namespace_param;
int nb_char_to_remove = 2;
robot_namespace_param = robot_namespace.substr(nb_char_to_remove,robot_namespace.size()-nb_char_to_remove);
ROS_INFO("kuka_lwr_hw_rt_node -> **** node handle robot_namespace_param = '%s'",robot_namespace_param.c_str());
// If robot_namespace is "kuka_lwr_left" the Robot Hardware need to open the fri driver file contains in ros param : fri_driver_file_left
// If robot_namespace is "kuka_lwr_right" the Robot Hardware need to open the fri driver file contains in ros param : fri_driver_file_right
std::string file;
if (robot_namespace.compare("//kuka_lwr_left") == 0)
lwr_nh.param("/fri_driver_file_left", file, std::string(""));
else
lwr_nh.param("/fri_driver_file_right", file, std::string(""));
ROS_INFO("fril node -> driver file = %s, namespace = %s",file.c_str(),robot_namespace_param.c_str());
// Get the the general robot description URDF XML from the parameter server
// Be careful ! the 'robot_description' parameter is set inside a 'group' (a namespace) in the launch file.
// so don't forget to concatenate the namespace !
std::string urdf_string = getURDF(lwr_nh, robot_namespace + "/robot_description");
// construct and start the real lwr
lwr_hw::LWRHWFRIL lwr_robot;
lwr_robot.create(robot_namespace_param, urdf_string);
lwr_robot.setInitFile(file);
if(!lwr_robot.init())
{
ROS_FATAL_NAMED("lwr_hw","Could not initialize robot real interface");
return -1;
}
// timer variables
struct timespec ts = {0, 0};
ros::Time last(ts.tv_sec, ts.tv_nsec), now(ts.tv_sec, ts.tv_nsec);
ros::Duration period(1.0);
ROS_INFO("Before controller_manager::ControllerManager !");
//the controller manager
controller_manager::ControllerManager manager(&lwr_robot, lwr_nh);
// run as fast as possible
while( !g_quit )
{
// get the time / period
if (!clock_gettime(CLOCK_REALTIME, &ts))
{
now.sec = ts.tv_sec;
now.nsec = ts.tv_nsec;
period = now - last;
last = now;
}
else
{
ROS_FATAL("Failed to poll realtime clock!");
break;
}
// read the state from the lwr
lwr_robot.read(now, period);
// update the controllers
manager.update(now, period);
// write the command to the lwr
lwr_robot.write(now, period);
}
std::cerr<<"Stopping spinner..."<<std::endl;
spinner.stop();
std::cerr<<"Stopping LWR..."<<std::endl;
lwr_robot.stop();
std::cerr<<"This node was killed!"<<std::endl;
return 0;
}
// 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.");
}
bool loadControllerManagerFromURDF()
{
std::string urdf_string = getURDF(robot_description_);
// initialize TiXmlDocument doc with a string
TiXmlDocument doc;
if (!doc.Parse(urdf_string.c_str()) && doc.Error())
{
ROS_ERROR("Could not load the gazebo_ros_control plugin's"
" configuration file: %s\n", urdf_string.c_str());
return false;
}
// debug
//doc.Print();
//std::cout << *(doc.RootElement()) << std::endl;
/* THIS IS USEFUL BUT NOT FOR NOW
// Pulls out the list of actuators used in the robot configuration.
struct GetActuators : public TiXmlVisitor
{
std::set<std::string> actuators;
virtual bool VisitEnter(const TiXmlElement &elt, const TiXmlAttribute *)
{
if (elt.ValueStr() == std::string("actuator") && elt.Attribute("name"))
actuators.insert(elt.Attribute("name"));
else if (elt.ValueStr() ==
std::string("rightActuator") && elt.Attribute("name"))
actuators.insert(elt.Attribute("name"));
else if (elt.ValueStr() ==
std::string("leftActuator") && elt.Attribute("name"))
actuators.insert(elt.Attribute("name"));
return true;
}
} get_actuators;
doc.RootElement()->Accept(&get_actuators);
// Places the found actuators into the hardware interface.
std::set<std::string>::iterator it;
for (it = get_actuators.actuators.begin();
it != get_actuators.actuators.end(); ++it)
{
std::cout << " adding actuator " << (*it) << std::endl;
//pr2_hardware_interface::Actuator* pr2_actuator =
// new pr2_hardware_interface::Actuator(*it);
//pr2_actuator->state_.is_enabled_ = true;
//hardware_interface_.addActuator(pr2_actuator);
}
// Find joints in transmission tags
TiXmlElement *root = doc.RootElement();
// Constructs the transmissions by parsing custom xml.
TiXmlElement *xit = NULL;
for (xit = root->FirstChildElement("transmission"); xit;
xit = xit->NextSiblingElement("transmission"))
{
// Get <transmission type=""> property
std::string type(xit->Attribute("type")); // \todo error check this?
Transmission *t = NULL;
try {
// Backwards compatibility for using non-namespaced controller types
if (!transmission_loader_->isClassAvailable(type))
{
std::vector<std::string> classes = transmission_loader_->getDeclaredClasses();
for(unsigned int i = 0; i < classes.size(); ++i)
{
if(type == transmission_loader_->getName(classes[i]))
{
ROS_WARN("The deprecated transmission type %s was not found. Using the namespaced version %s instead. "
"Please update your urdf file to use the namespaced version.",
type.c_str(), classes[i].c_str());
type = classes[i];
break;
}
}
}
t = transmission_loader_->createClassInstance(type);
}
catch (const std::runtime_error &ex)
{
ROS_ERROR("Could not load class %s: %s", type.c_str(), ex.what());
}
if (!t)
ROS_ERROR("Unknown transmission type: %s", type.c_str());
else if (!t->initXml(xit, this)){
ROS_ERROR("Failed to initialize transmission");
delete t;
}
else // Success!
transmissions_.push_back(t);
}
*/
return true;
}
