$search

GazeboROSNode Class Reference

List of all members.

Classes

class  ForceJointJob
class  WrenchBodyJob

Public Member Functions

void AdvertiseServices ()
 advertise services
bool applyBodyWrench (gazebo_msgs::ApplyBodyWrench::Request &req, gazebo_msgs::ApplyBodyWrench::Response &res)
bool applyJointEffort (gazebo_msgs::ApplyJointEffort::Request &req, gazebo_msgs::ApplyJointEffort::Response &res)
bool clearBodyWrenches (std::string body_name)
bool clearBodyWrenches (gazebo_msgs::BodyRequest::Request &req, gazebo_msgs::BodyRequest::Response &res)
bool clearJointForces (std::string joint_name)
bool clearJointForces (gazebo_msgs::JointRequest::Request &req, gazebo_msgs::JointRequest::Response &res)
bool deleteModel (gazebo_msgs::DeleteModel::Request &req, gazebo_msgs::DeleteModel::Response &res)
 delete model given name
bool findJointPosition (double &position, std::string name, std::vector< std::string > joint_names, std::vector< double > joint_positions)
void gazeboQueueThread ()
 ros queue thread for this node
 GazeboROSNode ()
void getAllChildrenBodies (std::vector< gazebo::Body * > &bodies, gazebo::Model *model, gazebo::Body *body)
void getAllParentBodies (std::vector< gazebo::Body * > &bodies, gazebo::Model *model, gazebo::Body *body, gazebo::Body *orig_parent_body)
gazebo::Body * getChildBody (gazebo::Joint *joint)
bool getJointProperties (gazebo_msgs::GetJointProperties::Request &req, gazebo_msgs::GetJointProperties::Response &res)
bool getLinkProperties (gazebo_msgs::GetLinkProperties::Request &req, gazebo_msgs::GetLinkProperties::Response &res)
bool getLinkState (gazebo_msgs::GetLinkState::Request &req, gazebo_msgs::GetLinkState::Response &res)
bool getModelProperties (gazebo_msgs::GetModelProperties::Request &req, gazebo_msgs::GetModelProperties::Response &res)
bool getModelState (gazebo_msgs::GetModelState::Request &req, gazebo_msgs::GetModelState::Response &res)
gazebo::Body * getParentBody (gazebo::Joint *joint)
bool getPhysicsProperties (gazebo_msgs::GetPhysicsProperties::Request &req, gazebo_msgs::GetPhysicsProperties::Response &res)
bool getWorldProperties (gazebo_msgs::GetWorldProperties::Request &req, gazebo_msgs::GetWorldProperties::Response &res)
bool inBodies (gazebo::Body *body, std::vector< gazebo::Body * > bodies)
void onLinkStatesConnect ()
void onLinkStatesDisconnect ()
void onModelStatesConnect ()
void onModelStatesDisconnect ()
bool pausePhysics (std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
bool resetSimulation (std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
bool resetWorld (std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
void rotateBodyAndChildren (gazebo::Body *body1, gazebo::Vector3 anchor, gazebo::Vector3 axis, double dangle, bool update_children=true)
bool setJointProperties (gazebo_msgs::SetJointProperties::Request &req, gazebo_msgs::SetJointProperties::Response &res)
bool setLinkProperties (gazebo_msgs::SetLinkProperties::Request &req, gazebo_msgs::SetLinkProperties::Response &res)
bool setLinkState (gazebo_msgs::SetLinkState::Request &req, gazebo_msgs::SetLinkState::Response &res)
bool setModelConfiguration (gazebo_msgs::SetModelConfiguration::Request &req, gazebo_msgs::SetModelConfiguration::Response &res)
void setModelJointPositions (gazebo::Model *gazebo_model, std::vector< std::string > joint_names, std::vector< double > joint_positions)
bool setModelState (gazebo_msgs::SetModelState::Request &req, gazebo_msgs::SetModelState::Response &res)
bool setPhysicsProperties (gazebo_msgs::SetPhysicsProperties::Request &req, gazebo_msgs::SetPhysicsProperties::Response &res)
void slideBodyAndChildren (gazebo::Body *body1, gazebo::Vector3 anchor, gazebo::Vector3 axis, double dposition, bool update_children=true)
bool spawnGazeboModel (gazebo_msgs::SpawnModel::Request &req, gazebo_msgs::SpawnModel::Response &res)
bool spawnURDFModel (gazebo_msgs::SpawnModel::Request &req, gazebo_msgs::SpawnModel::Response &res)
void transformWrench (gazebo::Vector3 &target_force, gazebo::Vector3 &target_torque, gazebo::Vector3 reference_force, gazebo::Vector3 reference_torque, gazebo::Pose3d target_to_reference)
 shift wrench from reference frame to target frame assume wrench is defined in
bool unpausePhysics (std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
void updateLinkState (const gazebo_msgs::LinkState::ConstPtr &link_state)
void updateModelState (const gazebo_msgs::ModelState::ConstPtr &model_state)
 ~GazeboROSNode ()

Private Member Functions

void forceJointSchedulerSlot ()
gazebo::Joint * getJointByName (std::string joint_name)
 get joint by name
bool IsColladaData (const std::string &data)
bool IsGazeboModelXML (std::string model_xml)
bool IsURDF (std::string model_xml)
 utilites for checking incoming string URDF/XML/Param
void PhysicsReconfigureCallback (gazebo::PhysicsConfig &config, uint32_t level)
void PhysicsReconfigureNode ()
void publishLinkStates ()
void publishModelStates ()
void publishSimTime ()
void SetupTransform (btTransform &transform, geometry_msgs::Pose pose)
void SetupTransform (btTransform &transform, urdf::Pose pose)
 utility for transforming in SetModelConfiguration
void wrenchBodySchedulerSlot ()

Private Attributes

ros::ServiceServer apply_body_wrench_service_
ros::ServiceServer apply_joint_effort_service_
ros::ServiceServer clear_body_wrenches_service_
ros::ServiceServer clear_joint_forces_service_
ros::ServiceServer delete_model_service_
std::vector
< GazeboROSNode::ForceJointJob * > 
force_joint_jobs
boost::thread * gazebo_callback_queue_thread_
ros::CallbackQueue gazebo_queue_
ros::ServiceServer get_joint_properties_service_
ros::ServiceServer get_link_properties_service_
ros::ServiceServer get_link_state_service_
ros::ServiceServer get_model_properties_service_
ros::ServiceServer get_model_state_service_
ros::ServiceServer get_physics_properties_service_
ros::ServiceServer get_world_properties_service_
double lastForceJointUpdateTime
double lastWrenchBodyUpdateTime
boost::mutex lock_
 A mutex to lock access to fields that are used in ROS message callbacks.
ros::ServiceServer pause_physics_service_
ros::ServiceClient physics_reconfigure_get_client_
bool physics_reconfigure_initialized_
ros::ServiceClient physics_reconfigure_set_client_
boost::thread * physics_reconfigure_thread_
ros::Publisher pub_clock_
ros::Publisher pub_gazebo_paused_
ros::Publisher pub_link_states_
int pub_link_states_connection_count_
ros::Publisher pub_model_states_
int pub_model_states_connection_count_
ros::ServiceServer reset_simulation_service_
ros::ServiceServer reset_world_service_
ros::NodeHandle rosnode_
ros::ServiceServer set_joint_properties_service_
ros::ServiceServer set_link_properties_service_
ros::ServiceServer set_link_state_service_
ros::Subscriber set_link_state_topic_
ros::ServiceServer set_model_configuration_service_
ros::ServiceServer set_model_state_service_
ros::Subscriber set_model_state_topic_
ros::ServiceServer set_physics_properties_service_
ros::ServiceServer spawn_urdf_gazebo_service_
ros::ServiceServer spawn_urdf_model_service_
tf::TransformBroadcaster tfbr
 broadcast transform between all models and the gazebo inertial "world" frame
ros::ServiceServer unpause_physics_service_
std::vector
< GazeboROSNode::WrenchBodyJob * > 
wrench_body_jobs
std::string xmlPrefix_
 prefix and suffix used for conversion from URDF to Gazebo XML
std::string xmlSuffix_

Detailed Description

Definition at line 303 of file gazeboros.cpp.


Constructor & Destructor Documentation

GazeboROSNode::GazeboROSNode (  )  [inline]

setup custom callback queue

start a thread for the physics dynamic reconfigure node

advertise all services

Definition at line 306 of file gazeboros.cpp.

GazeboROSNode::~GazeboROSNode (  )  [inline]

Definition at line 336 of file gazeboros.cpp.


Member Function Documentation

void GazeboROSNode::AdvertiseServices (  )  [inline]

advertise services

Definition at line 394 of file gazeboros.cpp.

bool GazeboROSNode::applyBodyWrench ( gazebo_msgs::ApplyBodyWrench::Request req,
gazebo_msgs::ApplyBodyWrench::Response res 
) [inline]

shift wrench to body frame if a non-zero reference point is given

Todo:
: to be more general, should we make the reference point a reference pose?
Todo:
: FIXME map is really wrong, need to use tf here somehow

Definition at line 1980 of file gazeboros.cpp.

bool GazeboROSNode::applyJointEffort ( gazebo_msgs::ApplyJointEffort::Request req,
gazebo_msgs::ApplyJointEffort::Response res 
) [inline]

Definition at line 1469 of file gazeboros.cpp.

bool GazeboROSNode::clearBodyWrenches ( std::string  body_name  )  [inline]

Definition at line 1572 of file gazeboros.cpp.

bool GazeboROSNode::clearBodyWrenches ( gazebo_msgs::BodyRequest::Request req,
gazebo_msgs::BodyRequest::Response res 
) [inline]

Definition at line 1568 of file gazeboros.cpp.

bool GazeboROSNode::clearJointForces ( std::string  joint_name  )  [inline]

Definition at line 1543 of file gazeboros.cpp.

bool GazeboROSNode::clearJointForces ( gazebo_msgs::JointRequest::Request req,
gazebo_msgs::JointRequest::Response res 
) [inline]

Definition at line 1539 of file gazeboros.cpp.

bool GazeboROSNode::deleteModel ( gazebo_msgs::DeleteModel::Request req,
gazebo_msgs::DeleteModel::Response res 
) [inline]

delete model given name

Definition at line 852 of file gazeboros.cpp.

bool GazeboROSNode::findJointPosition ( double &  position,
std::string  name,
std::vector< std::string >  joint_names,
std::vector< double >  joint_positions 
) [inline]

Definition at line 1810 of file gazeboros.cpp.

void GazeboROSNode::forceJointSchedulerSlot (  )  [inline, private]

Definition at line 2369 of file gazeboros.cpp.

void GazeboROSNode::gazeboQueueThread (  )  [inline]

ros queue thread for this node

Definition at line 372 of file gazeboros.cpp.

void GazeboROSNode::getAllChildrenBodies ( std::vector< gazebo::Body * > &  bodies,
gazebo::Model *  model,
gazebo::Body *  body 
) [inline]

Definition at line 1754 of file gazeboros.cpp.

void GazeboROSNode::getAllParentBodies ( std::vector< gazebo::Body * > &  bodies,
gazebo::Model *  model,
gazebo::Body *  body,
gazebo::Body *  orig_parent_body 
) [inline]

Definition at line 1779 of file gazeboros.cpp.

gazebo::Body* GazeboROSNode::getChildBody ( gazebo::Joint *  joint  )  [inline]

Definition at line 1681 of file gazeboros.cpp.

gazebo::Joint* GazeboROSNode::getJointByName ( std::string  joint_name  )  [inline, private]

get joint by name

Definition at line 2290 of file gazeboros.cpp.

bool GazeboROSNode::getJointProperties ( gazebo_msgs::GetJointProperties::Request req,
gazebo_msgs::GetJointProperties::Response res 
) [inline]

Todo:
: FIXME

Definition at line 1059 of file gazeboros.cpp.

bool GazeboROSNode::getLinkProperties ( gazebo_msgs::GetLinkProperties::Request req,
gazebo_msgs::GetLinkProperties::Response res 
) [inline]

Todo:
: FIXME

Definition at line 1097 of file gazeboros.cpp.

bool GazeboROSNode::getLinkState ( gazebo_msgs::GetLinkState::Request req,
gazebo_msgs::GetLinkState::Response res 
) [inline]

Todo:
: FIXME map is really wrong, need to use tf here somehow

Definition at line 1140 of file gazeboros.cpp.

bool GazeboROSNode::getModelProperties ( gazebo_msgs::GetModelProperties::Request req,
gazebo_msgs::GetModelProperties::Response res 
) [inline]

Definition at line 988 of file gazeboros.cpp.

bool GazeboROSNode::getModelState ( gazebo_msgs::GetModelState::Request req,
gazebo_msgs::GetModelState::Response res 
) [inline]

Todo:
: FIXME map is really wrong, need to use tf here somehow

Definition at line 915 of file gazeboros.cpp.

gazebo::Body* GazeboROSNode::getParentBody ( gazebo::Joint *  joint  )  [inline]

Definition at line 1692 of file gazeboros.cpp.

bool GazeboROSNode::getPhysicsProperties ( gazebo_msgs::GetPhysicsProperties::Request req,
gazebo_msgs::GetPhysicsProperties::Response res 
) [inline]

Definition at line 1269 of file gazeboros.cpp.

bool GazeboROSNode::getWorldProperties ( gazebo_msgs::GetWorldProperties::Request req,
gazebo_msgs::GetWorldProperties::Response res 
) [inline]

Definition at line 1043 of file gazeboros.cpp.

bool GazeboROSNode::inBodies ( gazebo::Body *  body,
std::vector< gazebo::Body * >  bodies 
) [inline]

Definition at line 1803 of file gazeboros.cpp.

bool GazeboROSNode::IsColladaData ( const std::string &  data  )  [inline, private]

Definition at line 2275 of file gazeboros.cpp.

bool GazeboROSNode::IsGazeboModelXML ( std::string  model_xml  )  [inline, private]

Definition at line 2279 of file gazeboros.cpp.

bool GazeboROSNode::IsURDF ( std::string  model_xml  )  [inline, private]

utilites for checking incoming string URDF/XML/Param

Definition at line 2266 of file gazeboros.cpp.

void GazeboROSNode::onLinkStatesConnect (  )  [inline]

Definition at line 604 of file gazeboros.cpp.

void GazeboROSNode::onLinkStatesDisconnect (  )  [inline]

Definition at line 616 of file gazeboros.cpp.

void GazeboROSNode::onModelStatesConnect (  )  [inline]

Definition at line 610 of file gazeboros.cpp.

void GazeboROSNode::onModelStatesDisconnect (  )  [inline]

Definition at line 624 of file gazeboros.cpp.

bool GazeboROSNode::pausePhysics ( std_srvs::Empty::Request &  req,
std_srvs::Empty::Response &  res 
) [inline]

Definition at line 1523 of file gazeboros.cpp.

void GazeboROSNode::PhysicsReconfigureCallback ( gazebo::PhysicsConfig &  config,
uint32_t  level 
) [inline, private]

Definition at line 2139 of file gazeboros.cpp.

void GazeboROSNode::PhysicsReconfigureNode (  )  [inline, private]

Definition at line 2212 of file gazeboros.cpp.

void GazeboROSNode::publishLinkStates (  )  [inline, private]

Definition at line 2419 of file gazeboros.cpp.

void GazeboROSNode::publishModelStates (  )  [inline, private]

Definition at line 2463 of file gazeboros.cpp.

void GazeboROSNode::publishSimTime (  )  [inline, private]

Definition at line 2404 of file gazeboros.cpp.

bool GazeboROSNode::resetSimulation ( std_srvs::Empty::Request &  req,
std_srvs::Empty::Response &  res 
) [inline]

Definition at line 1507 of file gazeboros.cpp.

bool GazeboROSNode::resetWorld ( std_srvs::Empty::Request &  req,
std_srvs::Empty::Response &  res 
) [inline]

Definition at line 1515 of file gazeboros.cpp.

void GazeboROSNode::rotateBodyAndChildren ( gazebo::Body *  body1,
gazebo::Vector3  anchor,
gazebo::Vector3  axis,
double  dangle,
bool  update_children = true 
) [inline]

Definition at line 1702 of file gazeboros.cpp.

bool GazeboROSNode::setJointProperties ( gazebo_msgs::SetJointProperties::Request req,
gazebo_msgs::SetJointProperties::Response res 
) [inline]

Todo:
: FIXME: current settings only allows for setting of 1DOF joints (e.g. HingeJoint and SliderJoint) correctly.

Definition at line 1299 of file gazeboros.cpp.

bool GazeboROSNode::setLinkProperties ( gazebo_msgs::SetLinkProperties::Request req,
gazebo_msgs::SetLinkProperties::Response res 
) [inline]

Definition at line 1206 of file gazeboros.cpp.

bool GazeboROSNode::setLinkState ( gazebo_msgs::SetLinkState::Request req,
gazebo_msgs::SetLinkState::Response res 
) [inline]

Todo:
: FIXME map is really wrong, unless using tf here somehow

Definition at line 1829 of file gazeboros.cpp.

bool GazeboROSNode::setModelConfiguration ( gazebo_msgs::SetModelConfiguration::Request req,
gazebo_msgs::SetModelConfiguration::Response res 
) [inline]

Definition at line 1598 of file gazeboros.cpp.

void GazeboROSNode::setModelJointPositions ( gazebo::Model *  gazebo_model,
std::vector< std::string >  joint_names,
std::vector< double >  joint_positions 
) [inline]

Definition at line 1626 of file gazeboros.cpp.

bool GazeboROSNode::setModelState ( gazebo_msgs::SetModelState::Request req,
gazebo_msgs::SetModelState::Response res 
) [inline]

Todo:
: FIXME map is really wrong, need to use tf here somehow

Definition at line 1349 of file gazeboros.cpp.

bool GazeboROSNode::setPhysicsProperties ( gazebo_msgs::SetPhysicsProperties::Request req,
gazebo_msgs::SetPhysicsProperties::Response res 
) [inline]

Definition at line 1235 of file gazeboros.cpp.

void GazeboROSNode::SetupTransform ( btTransform &  transform,
geometry_msgs::Pose  pose 
) [inline, private]

Definition at line 2255 of file gazeboros.cpp.

void GazeboROSNode::SetupTransform ( btTransform &  transform,
urdf::Pose  pose 
) [inline, private]

utility for transforming in SetModelConfiguration

Definition at line 2249 of file gazeboros.cpp.

void GazeboROSNode::slideBodyAndChildren ( gazebo::Body *  body1,
gazebo::Vector3  anchor,
gazebo::Vector3  axis,
double  dposition,
bool  update_children = true 
) [inline]

Definition at line 1729 of file gazeboros.cpp.

bool GazeboROSNode::spawnGazeboModel ( gazebo_msgs::SpawnModel::Request req,
gazebo_msgs::SpawnModel::Response res 
) [inline]

Todo:
: FIXME map is really wrong, need to use tf here somehow

STRIP DECLARATION <? ... xml version="1.0" ... ?> from model_xml

Todo:
: does tinyxml have functionality for this?
Todo:
: should gazebo take care of the declaration?

poll and wait, verify that the model is spawned within Hardcoded 60 seconds

Definition at line 687 of file gazeboros.cpp.

bool GazeboROSNode::spawnURDFModel ( gazebo_msgs::SpawnModel::Request req,
gazebo_msgs::SpawnModel::Response res 
) [inline]

STRIP DECLARATION <? ... xml version="1.0" ... ?> from model_xml

Todo:
: does tinyxml have functionality for this?
Todo:
: should gazebo take care of the declaration?

Definition at line 633 of file gazeboros.cpp.

void GazeboROSNode::transformWrench ( gazebo::Vector3 &  target_force,
gazebo::Vector3 &  target_torque,
gazebo::Vector3  reference_force,
gazebo::Vector3  reference_torque,
gazebo::Pose3d  target_to_reference 
) [inline]

shift wrench from reference frame to target frame assume wrench is defined in

Definition at line 1964 of file gazeboros.cpp.

bool GazeboROSNode::unpausePhysics ( std_srvs::Empty::Request &  req,
std_srvs::Empty::Response &  res 
) [inline]

Definition at line 1531 of file gazeboros.cpp.

void GazeboROSNode::updateLinkState ( const gazebo_msgs::LinkState::ConstPtr link_state  )  [inline]

Todo:
: FIXME map is really wrong, unless using tf here somehow

Definition at line 1898 of file gazeboros.cpp.

void GazeboROSNode::updateModelState ( const gazebo_msgs::ModelState::ConstPtr model_state  )  [inline]

Definition at line 1413 of file gazeboros.cpp.

void GazeboROSNode::wrenchBodySchedulerSlot (  )  [inline, private]

Definition at line 2330 of file gazeboros.cpp.


Member Data Documentation

Definition at line 2111 of file gazeboros.cpp.

Definition at line 2114 of file gazeboros.cpp.

Definition at line 2122 of file gazeboros.cpp.

Definition at line 2121 of file gazeboros.cpp.

Definition at line 2101 of file gazeboros.cpp.

Definition at line 2323 of file gazeboros.cpp.

Definition at line 2097 of file gazeboros.cpp.

Definition at line 2096 of file gazeboros.cpp.

Definition at line 2105 of file gazeboros.cpp.

Definition at line 2106 of file gazeboros.cpp.

Definition at line 2107 of file gazeboros.cpp.

Definition at line 2103 of file gazeboros.cpp.

Definition at line 2102 of file gazeboros.cpp.

Definition at line 2110 of file gazeboros.cpp.

Definition at line 2104 of file gazeboros.cpp.

Definition at line 2326 of file gazeboros.cpp.

Definition at line 2325 of file gazeboros.cpp.

boost::mutex GazeboROSNode::lock_ [private]

A mutex to lock access to fields that are used in ROS message callbacks.

Definition at line 2242 of file gazeboros.cpp.

Definition at line 2119 of file gazeboros.cpp.

Definition at line 2137 of file gazeboros.cpp.

Definition at line 2134 of file gazeboros.cpp.

Definition at line 2136 of file gazeboros.cpp.

Definition at line 2133 of file gazeboros.cpp.

Definition at line 2125 of file gazeboros.cpp.

Definition at line 2128 of file gazeboros.cpp.

Definition at line 2126 of file gazeboros.cpp.

Definition at line 2129 of file gazeboros.cpp.

Definition at line 2127 of file gazeboros.cpp.

Definition at line 2130 of file gazeboros.cpp.

Definition at line 2117 of file gazeboros.cpp.

Definition at line 2118 of file gazeboros.cpp.

Definition at line 2095 of file gazeboros.cpp.

Definition at line 2112 of file gazeboros.cpp.

Definition at line 2108 of file gazeboros.cpp.

Definition at line 2116 of file gazeboros.cpp.

Definition at line 2123 of file gazeboros.cpp.

Definition at line 2115 of file gazeboros.cpp.

Definition at line 2113 of file gazeboros.cpp.

Definition at line 2124 of file gazeboros.cpp.

Definition at line 2109 of file gazeboros.cpp.

Definition at line 2100 of file gazeboros.cpp.

Definition at line 2099 of file gazeboros.cpp.

broadcast transform between all models and the gazebo inertial "world" frame

Definition at line 2239 of file gazeboros.cpp.

Definition at line 2120 of file gazeboros.cpp.

Definition at line 2322 of file gazeboros.cpp.

std::string GazeboROSNode::xmlPrefix_ [private]

prefix and suffix used for conversion from URDF to Gazebo XML

Definition at line 2245 of file gazeboros.cpp.

std::string GazeboROSNode::xmlSuffix_ [private]

Definition at line 2246 of file gazeboros.cpp.


The documentation for this class was generated from the following file:
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends


gazebo
Author(s): Nate Koenig, Andrew Howard, with contributions from many others. See web page for a full credits llist.
autogenerated on Sat Mar 2 13:39:08 2013