math::Box getModelBox(physics::ModelPtr const& model)
{
math::Vector3 mins, maxs;
bool inited = false;
if(model->HasType(physics::Base::LINK))
return model->GetBoundingBox();
int maxK = model->GetChildCount();
for(int k = 0; k < maxK; k++)
getBaseBox(model->GetChild(k).get(), maxs, mins, inited);
math::Box box;
box.max = maxs;
box.min = mins;
return box;
}
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);
}