02412       /*                                                             */
    02413       /***************************************************************/
    02414       this->[pub_clock_](classGazeboROSNode.html#aed02a543ac7934fc64049963e08fe30f).[publish](../../../api/roscpp/html/classros_1_1Publisher.html#af4e0009d55b5a7729fd71c3ffdf502cd)(ros_time_);
    02415     }
    02416 
    [02419](classGazeboROSNode.html#abdcedd88754decfd73b04c1e9d7f5023)     void [publishLinkStates](classGazeboROSNode.html#abdcedd88754decfd73b04c1e9d7f5023)()
    02420     {
    02421       [gazebo_msgs::LinkStates](../../gazebo_msgs/html/structgazebo__msgs_1_1LinkStates__.html) link_states;
    02422       // fill link_states
    02423       const std::vector<gazebo::Model*> models = gazebo::World::Instance()->GetModels();
    02424       for (std::vector<gazebo::Model*>::const_iterator miter = models.begin(); miter != models.end(); miter++)
    02425       {
    02426         const std::vector< gazebo::Entity* > entities = (*miter)->GetChildren();
    02427         for (std::vector< gazebo::Entity* >::const_iterator eiter = entities.begin();eiter!=entities.end();eiter++)
    02428         {
    02429           gazebo::Body* body = dynamic_cast<gazebo::Body*>(*eiter);
    02430           if (body)
    02431           {
    02432             link_states.[name](../../gazebo_msgs/html/structgazebo__msgs_1_1LinkStates__.html#a2021ec7cffcc1905c069c01c8ea91db4).push_back(body->GetCompleteScopedName());
    02433             [geometry_msgs::Pose](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Pose__.html) pose;
    02434             gazebo::Pose3d  body_pose = body->GetWorldPose(); // - this->myBody->GetCoMPose();
    02435             gazebo::Vector3 pos = body_pose.pos;
    02436             gazebo::Quatern rot = body_pose.rot;
    02437             pose.[position](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Pose__.html#a27067d1772d85a856b0e4ed4690a80b4).[x](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Point__.html#acd9420e6916084b2a9c09019bfda1708) = pos.x;
    02438             pose.[position](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Pose__.html#a27067d1772d85a856b0e4ed4690a80b4).[y](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Point__.html#a46a84ff5a3e5817ab5693abda66fccb0) = pos.y;
    02439             pose.[position](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Pose__.html#a27067d1772d85a856b0e4ed4690a80b4).[z](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Point__.html#a061f0093e927a95998f35ceceb5cebda) = pos.z;
    02440             pose.[orientation](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Pose__.html#af8c1d34a44a7628ea9c3e571339f5a28).[w](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Quaternion__.html#a440791d268a65607ecbac21e0ed153cf) = rot.u;
    02441             pose.[orientation](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Pose__.html#af8c1d34a44a7628ea9c3e571339f5a28).[x](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Quaternion__.html#a11f3b4e2df587ce516fd2a8140c1c7c5) = rot.x;
    02442             pose.[orientation](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Pose__.html#af8c1d34a44a7628ea9c3e571339f5a28).[y](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Quaternion__.html#a601c978178311ce3a618afb38c0b6b98) = rot.y;
    02443             pose.[orientation](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Pose__.html#af8c1d34a44a7628ea9c3e571339f5a28).[z](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Quaternion__.html#ae2caceb9fc4b2fd01acd8419aa7ebc2e) = rot.z;
    02444             link_states.[pose](../../gazebo_msgs/html/structgazebo__msgs_1_1LinkStates__.html#ac4a273bbbd38b32c7df56006efc450a1).push_back(pose);
    02445             gazebo::Vector3 linear_vel  = body->GetWorldLinearVel();
    02446             gazebo::Vector3 angular_vel = body->GetWorldAngularVel();
    02447             [geometry_msgs::Twist](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Twist__.html) twist;
    02448             twist.[linear](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Twist__.html#a394500332ac20028528c37103e1bf8e5).[x](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Vector3__.html#abf9dabaf651196aee50fd14bf3b643be) = linear_vel.x;
    02449             twist.[linear](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Twist__.html#a394500332ac20028528c37103e1bf8e5).[y](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Vector3__.html#aaea58ad929c43960a06f728d97132404) = linear_vel.y;
    02450             twist.[linear](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Twist__.html#a394500332ac20028528c37103e1bf8e5).[z](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Vector3__.html#a4cde719326e6c3688ab50b51ab4708b1) = linear_vel.z;
    02451             twist.[angular](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Twist__.html#aa0cb518e509179290aa793228ef01f6b).[x](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Vector3__.html#abf9dabaf651196aee50fd14bf3b643be) = angular_vel.x;
    02452             twist.[angular](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Twist__.html#aa0cb518e509179290aa793228ef01f6b).[y](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Vector3__.html#aaea58ad929c43960a06f728d97132404) = angular_vel.y;
    02453             twist.[angular](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Twist__.html#aa0cb518e509179290aa793228ef01f6b).[z](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Vector3__.html#a4cde719326e6c3688ab50b51ab4708b1) = angular_vel.z;