C++ ModelPtr::GetChild方法代码示例

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);
}