#include <gazebo_ros_api_plugin.h>

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 (gazebo_msgs::BodyRequest::Request &req, gazebo_msgs::BodyRequest::Response &res)
bool	clearBodyWrenches (std::string body_name)
bool	clearJointForces (gazebo_msgs::JointRequest::Request &req, gazebo_msgs::JointRequest::Response &res)
bool	clearJointForces (std::string joint_name)
bool	deleteModel (gazebo_msgs::DeleteModel::Request &req, gazebo_msgs::DeleteModel::Response &res)
	delete model given name
void	gazeboQueueThread ()
	ros queue thread for this node
	GazeboRosApiPlugin ()
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)
bool	getPhysicsProperties (gazebo_msgs::GetPhysicsProperties::Request &req, gazebo_msgs::GetPhysicsProperties::Response &res)
bool	getWorldProperties (gazebo_msgs::GetWorldProperties::Request &req, gazebo_msgs::GetWorldProperties::Response &res)
void	Load (int argc, char **argv)
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)
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)
bool	setModelState (gazebo_msgs::SetModelState::Request &req, gazebo_msgs::SetModelState::Response &res)
bool	setPhysicsProperties (gazebo_msgs::SetPhysicsProperties::Request &req, gazebo_msgs::SetPhysicsProperties::Response &res)
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	spin ()
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)
	~GazeboRosApiPlugin ()
Private Member Functions
void	forceJointSchedulerSlot ()
bool	IsGazeboModelXML (std::string model_xml)
bool	IsSDF (std::string model_xml)
bool	IsURDF (std::string model_xml)
	utilites for checking incoming string URDF/XML/Param
void	LoadGazeboRosApiPlugin (std::string _worldName)
void	OnResponse (ConstResponsePtr &_response)
void	PhysicsReconfigureCallback (gazebo::PhysicsConfig &config, uint32_t level)
void	PhysicsReconfigureNode ()
void	publishLinkStates ()
void	publishModelStates ()
void	publishSimTime (const boost::shared_ptr< gazebo::msgs::WorldStatistics const > &msg)
void	publishSimTime ()
bool	spawnAndConform (TiXmlDocument &gazebo_model_xml, std::string model_name, gazebo_msgs::SpawnModel::Response &res)
void	stripXmlDeclaration (std::string &model_xml)
void	transformWrench (gazebo::math::Vector3 &target_force, gazebo::math::Vector3 &target_torque, gazebo::math::Vector3 reference_force, gazebo::math::Vector3 reference_torque, gazebo::math::Pose target_to_reference)
	shift wrench from reference frame to target frame assume wrench is defined in
void	updateGazeboSDFModelPose (TiXmlDocument &gazebo_model_xml, gazebo::math::Vector3 initial_xyz, gazebo::math::Quaternion initial_q)
void	updateGazeboSDFName (TiXmlDocument &gazebo_model_xml, std::string model_name)
void	updateGazeboXmlModelPose (TiXmlDocument &gazebo_model_xml, gazebo::math::Vector3 initial_xyz, gazebo::math::Quaternion initial_q)
void	updateGazeboXmlName (TiXmlDocument &gazebo_model_xml, std::string model_name)
void	updateURDFModelPose (TiXmlDocument &gazebo_model_xml, gazebo::math::Vector3 initial_xyz, gazebo::math::Quaternion initial_q)
void	updateURDFName (TiXmlDocument &gazebo_model_xml, std::string model_name)
void	walkChildAddRobotNamespace (TiXmlNode *robot_xml)
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_
gazebo::transport::PublisherPtr	factory_pub_
std::vector < GazeboRosApiPlugin::ForceJointJob * >	force_joint_jobs
gazebo::event::ConnectionPtr	force_update_event_
boost::thread *	gazebo_callback_queue_thread_
ros::CallbackQueue	gazebo_queue_
gazebo::transport::NodePtr	gazebonode_
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_
gazebo::event::ConnectionPtr	load_gazebo_ros_api_plugin_event_
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_link_states_
int	pub_link_states_connection_count_
gazebo::event::ConnectionPtr	pub_link_states_event_
ros::Publisher	pub_model_states_
int	pub_model_states_connection_count_
gazebo::event::ConnectionPtr	pub_model_states_event_
gazebo::transport::PublisherPtr	request_pub_
ros::ServiceServer	reset_simulation_service_
ros::ServiceServer	reset_world_service_
gazebo::transport::SubscriberPtr	response_sub_
std::string	robot_namespace_
boost::thread *	ros_spin_thread_
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_
gazebo::transport::SubscriberPtr	stat_sub_
gazebo::event::ConnectionPtr	time_update_event_
ros::ServiceServer	unpause_physics_service_
gazebo::physics::WorldPtr	world
bool	world_created_
std::vector < GazeboRosApiPlugin::WrenchBodyJob * >	wrench_body_jobs
gazebo::event::ConnectionPtr	wrench_update_event_
std::string	xmlPrefix_
std::string	xmlSuffix_

Detailed Description

Definition at line 125 of file gazebo_ros_api_plugin.h.

Constructor & Destructor Documentation

gazebo::GazeboRosApiPlugin::GazeboRosApiPlugin ( )

Definition at line 34 of file gazebo_ros_api_plugin.cpp.

gazebo::GazeboRosApiPlugin::~GazeboRosApiPlugin ( )

Definition at line 40 of file gazebo_ros_api_plugin.cpp.

Member Function Documentation

void gazebo::GazeboRosApiPlugin::AdvertiseServices ( )

advertise services

Definition at line 166 of file gazebo_ros_api_plugin.cpp.

bool gazebo::GazeboRosApiPlugin::applyBodyWrench	(	gazebo_msgs::ApplyBodyWrench::Request &	req,
		gazebo_msgs::ApplyBodyWrench::Response &	res
	)

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 1415 of file gazebo_ros_api_plugin.cpp.

bool gazebo::GazeboRosApiPlugin::applyJointEffort	(	gazebo_msgs::ApplyJointEffort::Request &	req,
		gazebo_msgs::ApplyJointEffort::Response &	res
	)

Definition at line 1149 of file gazebo_ros_api_plugin.cpp.

bool gazebo::GazeboRosApiPlugin::clearBodyWrenches	(	gazebo_msgs::BodyRequest::Request &	req,
		gazebo_msgs::BodyRequest::Response &	res
	)

Definition at line 1242 of file gazebo_ros_api_plugin.cpp.

bool gazebo::GazeboRosApiPlugin::clearBodyWrenches ( std::string body_name )

Definition at line 1246 of file gazebo_ros_api_plugin.cpp.

bool gazebo::GazeboRosApiPlugin::clearJointForces	(	gazebo_msgs::JointRequest::Request &	req,
		gazebo_msgs::JointRequest::Response &	res
	)

Definition at line 1213 of file gazebo_ros_api_plugin.cpp.

bool gazebo::GazeboRosApiPlugin::clearJointForces ( std::string joint_name )

Definition at line 1217 of file gazebo_ros_api_plugin.cpp.

bool gazebo::GazeboRosApiPlugin::deleteModel	(	gazebo_msgs::DeleteModel::Request &	req,
		gazebo_msgs::DeleteModel::Response &	res
	)

delete model given name

Definition at line 577 of file gazebo_ros_api_plugin.cpp.

void gazebo::GazeboRosApiPlugin::forceJointSchedulerSlot ( ) [private]

Definition at line 1608 of file gazebo_ros_api_plugin.cpp.

void gazebo::GazeboRosApiPlugin::gazeboQueueThread ( )

ros queue thread for this node

Definition at line 157 of file gazebo_ros_api_plugin.cpp.

bool gazebo::GazeboRosApiPlugin::getJointProperties	(	gazebo_msgs::GetJointProperties::Request &	req,
		gazebo_msgs::GetJointProperties::Response &	res
	)

Todo:: : FIXME

Definition at line 794 of file gazebo_ros_api_plugin.cpp.

bool gazebo::GazeboRosApiPlugin::getLinkProperties	(	gazebo_msgs::GetLinkProperties::Request &	req,
		gazebo_msgs::GetLinkProperties::Response &	res
	)

Todo:: : validate

Definition at line 831 of file gazebo_ros_api_plugin.cpp.

bool gazebo::GazeboRosApiPlugin::getLinkState	(	gazebo_msgs::GetLinkState::Request &	req,
		gazebo_msgs::GetLinkState::Response &	res
	)

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

Definition at line 872 of file gazebo_ros_api_plugin.cpp.

bool gazebo::GazeboRosApiPlugin::getModelProperties	(	gazebo_msgs::GetModelProperties::Request &	req,
		gazebo_msgs::GetModelProperties::Response &	res
	)

Definition at line 716 of file gazebo_ros_api_plugin.cpp.

bool gazebo::GazeboRosApiPlugin::getModelState	(	gazebo_msgs::GetModelState::Request &	req,
		gazebo_msgs::GetModelState::Response &	res
	)

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

Definition at line 642 of file gazebo_ros_api_plugin.cpp.

bool gazebo::GazeboRosApiPlugin::getPhysicsProperties	(	gazebo_msgs::GetPhysicsProperties::Request &	req,
		gazebo_msgs::GetPhysicsProperties::Response &	res
	)

Definition at line 998 of file gazebo_ros_api_plugin.cpp.

bool gazebo::GazeboRosApiPlugin::getWorldProperties	(	gazebo_msgs::GetWorldProperties::Request &	req,
		gazebo_msgs::GetWorldProperties::Response &	res
	)

Definition at line 779 of file gazebo_ros_api_plugin.cpp.

bool gazebo::GazeboRosApiPlugin::IsGazeboModelXML ( std::string model_xml ) [private]

Definition at line 1548 of file gazebo_ros_api_plugin.cpp.

bool gazebo::GazeboRosApiPlugin::IsSDF ( std::string model_xml ) [private]

Definition at line 1558 of file gazebo_ros_api_plugin.cpp.

bool gazebo::GazeboRosApiPlugin::IsURDF ( std::string model_xml ) [private]

utilites for checking incoming string URDF/XML/Param

Definition at line 1539 of file gazebo_ros_api_plugin.cpp.

void gazebo::GazeboRosApiPlugin::Load	(	int	argc,
		char **	argv
	)

setup custom callback queue

start a thread for the physics dynamic reconfigure node

Definition at line 81 of file gazebo_ros_api_plugin.cpp.

void gazebo::GazeboRosApiPlugin::LoadGazeboRosApiPlugin ( std::string _worldName ) [private]

advertise all services

Definition at line 102 of file gazebo_ros_api_plugin.cpp.

void gazebo::GazeboRosApiPlugin::onLinkStatesConnect ( )

Definition at line 373 of file gazebo_ros_api_plugin.cpp.

void gazebo::GazeboRosApiPlugin::onLinkStatesDisconnect ( )

Definition at line 385 of file gazebo_ros_api_plugin.cpp.

void gazebo::GazeboRosApiPlugin::onModelStatesConnect ( )

Definition at line 379 of file gazebo_ros_api_plugin.cpp.

void gazebo::GazeboRosApiPlugin::onModelStatesDisconnect ( )

Definition at line 395 of file gazebo_ros_api_plugin.cpp.

void gazebo::GazeboRosApiPlugin::OnResponse ( ConstResponsePtr & _response ) [private]

Definition at line 151 of file gazebo_ros_api_plugin.cpp.

bool gazebo::GazeboRosApiPlugin::pausePhysics	(	std_srvs::Empty::Request &	req,
		std_srvs::Empty::Response &	res
	)

Definition at line 1197 of file gazebo_ros_api_plugin.cpp.

void gazebo::GazeboRosApiPlugin::PhysicsReconfigureCallback	(	gazebo::PhysicsConfig &	config,
		uint32_t	level
	)		`[private]`

Definition at line 1743 of file gazebo_ros_api_plugin.cpp.

void gazebo::GazeboRosApiPlugin::PhysicsReconfigureNode ( ) [private]

Definition at line 1816 of file gazebo_ros_api_plugin.cpp.

void gazebo::GazeboRosApiPlugin::publishLinkStates ( ) [private]

Definition at line 1661 of file gazebo_ros_api_plugin.cpp.

void gazebo::GazeboRosApiPlugin::publishModelStates ( ) [private]

Definition at line 1708 of file gazebo_ros_api_plugin.cpp.

void gazebo::GazeboRosApiPlugin::publishSimTime ( const boost::shared_ptr< gazebo::msgs::WorldStatistics const > & msg ) [private]

Definition at line 1640 of file gazebo_ros_api_plugin.cpp.

void gazebo::GazeboRosApiPlugin::publishSimTime ( ) [private]

Definition at line 1649 of file gazebo_ros_api_plugin.cpp.

bool gazebo::GazeboRosApiPlugin::resetSimulation	(	std_srvs::Empty::Request &	req,
		std_srvs::Empty::Response &	res
	)

Definition at line 1181 of file gazebo_ros_api_plugin.cpp.

bool gazebo::GazeboRosApiPlugin::resetWorld	(	std_srvs::Empty::Request &	req,
		std_srvs::Empty::Response &	res
	)

Definition at line 1189 of file gazebo_ros_api_plugin.cpp.

bool gazebo::GazeboRosApiPlugin::setJointProperties	(	gazebo_msgs::SetJointProperties::Request &	req,
		gazebo_msgs::SetJointProperties::Response &	res
	)

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

Definition at line 1027 of file gazebo_ros_api_plugin.cpp.

bool gazebo::GazeboRosApiPlugin::setLinkProperties	(	gazebo_msgs::SetLinkProperties::Request &	req,
		gazebo_msgs::SetLinkProperties::Response &	res
	)

Definition at line 939 of file gazebo_ros_api_plugin.cpp.

bool gazebo::GazeboRosApiPlugin::setLinkState	(	gazebo_msgs::SetLinkState::Request &	req,
		gazebo_msgs::SetLinkState::Response &	res
	)

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

Definition at line 1319 of file gazebo_ros_api_plugin.cpp.

bool gazebo::GazeboRosApiPlugin::setModelConfiguration	(	gazebo_msgs::SetModelConfiguration::Request &	req,
		gazebo_msgs::SetModelConfiguration::Response &	res
	)

Definition at line 1274 of file gazebo_ros_api_plugin.cpp.

bool gazebo::GazeboRosApiPlugin::setModelState	(	gazebo_msgs::SetModelState::Request &	req,
		gazebo_msgs::SetModelState::Response &	res
	)

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

Definition at line 1074 of file gazebo_ros_api_plugin.cpp.

bool gazebo::GazeboRosApiPlugin::setPhysicsProperties	(	gazebo_msgs::SetPhysicsProperties::Request &	req,
		gazebo_msgs::SetPhysicsProperties::Response &	res
	)

Definition at line 966 of file gazebo_ros_api_plugin.cpp.

bool gazebo::GazeboRosApiPlugin::spawnAndConform	(	TiXmlDocument &	gazebo_model_xml,
		std::string	model_name,
		gazebo_msgs::SpawnModel::Response &	res
	)		`[private]`

FIXME: should change publish to direct invocation World::LoadModel() and/or change the poll for Model existence to common::Events based check.

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

Definition at line 2068 of file gazebo_ros_api_plugin.cpp.

bool gazebo::GazeboRosApiPlugin::spawnGazeboModel	(	gazebo_msgs::SpawnModel::Request &	req,
		gazebo_msgs::SpawnModel::Response &	res
	)

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

Todo:: : if (!this->robot_namespace_.empty())

Todo:: : this->addRobotNamespace(gazebo_model_xml);

Definition at line 475 of file gazebo_ros_api_plugin.cpp.

bool gazebo::GazeboRosApiPlugin::spawnURDFModel	(	gazebo_msgs::SpawnModel::Request &	req,
		gazebo_msgs::SpawnModel::Response &	res
	)

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 406 of file gazebo_ros_api_plugin.cpp.

void gazebo::GazeboRosApiPlugin::spin ( )

Definition at line 1527 of file gazebo_ros_api_plugin.cpp.

void gazebo::GazeboRosApiPlugin::stripXmlDeclaration ( std::string & model_xml ) [private]

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 1843 of file gazebo_ros_api_plugin.cpp.

void gazebo::GazeboRosApiPlugin::transformWrench	(	gazebo::math::Vector3 &	target_force,
		gazebo::math::Vector3 &	target_torque,
		gazebo::math::Vector3	reference_force,
		gazebo::math::Vector3	reference_torque,
		gazebo::math::Pose	target_to_reference
	)		`[private]`

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

Definition at line 1399 of file gazebo_ros_api_plugin.cpp.

bool gazebo::GazeboRosApiPlugin::unpausePhysics	(	std_srvs::Empty::Request &	req,
		std_srvs::Empty::Response &	res
	)

Definition at line 1205 of file gazebo_ros_api_plugin.cpp.

void gazebo::GazeboRosApiPlugin::updateGazeboSDFModelPose	(	TiXmlDocument &	gazebo_model_xml,
		gazebo::math::Vector3	initial_xyz,
		gazebo::math::Quaternion	initial_q
	)		`[private]`

Definition at line 1917 of file gazebo_ros_api_plugin.cpp.

void gazebo::GazeboRosApiPlugin::updateGazeboSDFName	(	TiXmlDocument &	gazebo_model_xml,
		std::string	model_name
	)		`[private]`

Definition at line 1951 of file gazebo_ros_api_plugin.cpp.

void gazebo::GazeboRosApiPlugin::updateGazeboXmlModelPose	(	TiXmlDocument &	gazebo_model_xml,
		gazebo::math::Vector3	initial_xyz,
		gazebo::math::Quaternion	initial_q
	)		`[private]`

Definition at line 1859 of file gazebo_ros_api_plugin.cpp.

void gazebo::GazeboRosApiPlugin::updateGazeboXmlName	(	TiXmlDocument &	gazebo_model_xml,
		std::string	model_name
	)		`[private]`

Definition at line 1897 of file gazebo_ros_api_plugin.cpp.

void gazebo::GazeboRosApiPlugin::updateLinkState ( const gazebo_msgs::LinkState::ConstPtr & link_state )

Definition at line 1387 of file gazebo_ros_api_plugin.cpp.

void gazebo::GazeboRosApiPlugin::updateModelState ( const gazebo_msgs::ModelState::ConstPtr & model_state )

Definition at line 1139 of file gazebo_ros_api_plugin.cpp.

void gazebo::GazeboRosApiPlugin::updateURDFModelPose	(	TiXmlDocument &	gazebo_model_xml,
		gazebo::math::Vector3	initial_xyz,
		gazebo::math::Quaternion	initial_q
	)		`[private]`