gazebo::physics::LinkPtr Mps::getLinkEndingWith(physics::ModelPtr model, std::string ending) {
std::vector<gazebo::physics::LinkPtr> links = model->GetLinks();
for (unsigned int i=0; i<links.size(); i++) {
if (ends_with(links[i]->GetName(), ending))
return links[i];
}
return gazebo::physics::LinkPtr();
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosVacuumGripper::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
ROS_INFO("Loading gazebo_ros_vacuum_gripper");
// Set attached model;
parent_ = _model;
// Get the world name.
world_ = _model->GetWorld();
// load parameters
robot_namespace_ = "";
if (_sdf->HasElement("robotNamespace"))
robot_namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
if (!_sdf->HasElement("bodyName"))
{
ROS_FATAL("vacuum_gripper plugin missing <bodyName>, cannot proceed");
return;
}
else
link_name_ = _sdf->GetElement("bodyName")->Get<std::string>();
link_ = _model->GetLink(link_name_);
if (!link_)
{
std::string found;
physics::Link_V links = _model->GetLinks();
for (size_t i = 0; i < links.size(); i++) {
found += std::string(" ") + links[i]->GetName();
}
ROS_FATAL("gazebo_ros_vacuum_gripper plugin error: link named: %s does not exist", link_name_.c_str());
ROS_FATAL("gazebo_ros_vacuum_gripper plugin error: You should check it exists and is not connected with fixed joint");
ROS_FATAL("gazebo_ros_vacuum_gripper plugin error: Found links are: %s", found.c_str());
return;
}
if (!_sdf->HasElement("topicName"))
{
ROS_FATAL("vacuum_gripper plugin missing <serviceName>, cannot proceed");
return;
}
else
topic_name_ = _sdf->GetElement("topicName")->Get<std::string>();
// Make sure the ROS node for Gazebo has already been initialized
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;
}
rosnode_ = new ros::NodeHandle(robot_namespace_);
// Custom Callback Queue
ros::AdvertiseOptions ao = ros::AdvertiseOptions::create<std_msgs::Bool>(
topic_name_, 1,
boost::bind(&GazeboRosVacuumGripper::Connect, this),
boost::bind(&GazeboRosVacuumGripper::Disconnect, this),
ros::VoidPtr(), &queue_);
pub_ = rosnode_->advertise(ao);
// Custom Callback Queue
ros::AdvertiseServiceOptions aso1 =
ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
"on", boost::bind(&GazeboRosVacuumGripper::OnServiceCallback,
this, _1, _2), ros::VoidPtr(), &queue_);
srv1_ = rosnode_->advertiseService(aso1);
ros::AdvertiseServiceOptions aso2 =
ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
"off", boost::bind(&GazeboRosVacuumGripper::OffServiceCallback,
this, _1, _2), ros::VoidPtr(), &queue_);
srv2_ = rosnode_->advertiseService(aso2);
// Custom Callback Queue
callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosVacuumGripper::QueueThread,this ) );
// New Mechanism for Updating every World Cycle
// Listen to the update event. This event is broadcast every
// simulation iteration.
update_connection_ = event::Events::ConnectWorldUpdateBegin(
boost::bind(&GazeboRosVacuumGripper::UpdateChild, this));
ROS_INFO("Loaded gazebo_ros_vacuum_gripper");
}