56 this->
world_ = _parent->GetWorld();
60 if (_sdf->HasElement(
"robotNamespace"))
61 this->
robot_namespace_ = _sdf->GetElement(
"robotNamespace")->Get<std::string>() +
"/";
63 if (!_sdf->HasElement(
"bodyName"))
65 ROS_FATAL_NAMED(
"f3d",
"f3d plugin missing <bodyName>, cannot proceed");
69 this->
link_name_ = _sdf->GetElement(
"bodyName")->Get<std::string>();
78 if (!_sdf->HasElement(
"topicName"))
80 ROS_FATAL_NAMED(
"f3d",
"f3d plugin missing <topicName>, cannot proceed");
84 this->
topic_name_ = _sdf->GetElement(
"topicName")->Get<std::string>();
86 if (!_sdf->HasElement(
"frameName"))
88 ROS_INFO_NAMED(
"f3d",
"f3d plugin missing <frameName>, defaults to world");
93 this->
frame_name_ = _sdf->GetElement(
"frameName")->Get<std::string>();
103 <<
"Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
152 ignition::math::Vector3d torque;
153 ignition::math::Vector3d force;
156 #if GAZEBO_MAJOR_VERSION >= 8 157 force = this->
link_->WorldForce();
158 torque = this->
link_->WorldTorque();
160 force = this->
link_->GetWorldForce().Ign();
161 torque = this->
link_->GetWorldTorque().Ign();
167 #if GAZEBO_MAJOR_VERSION >= 8 183 this->
lock_.unlock();
192 static const double timeout = 0.01;
boost::thread callback_queue_thread_
#define ROS_INFO_NAMED(name,...)
void publish(const boost::shared_ptr< M > &message) const
ROSCPP_DECL bool isInitialized()
std::string topic_name_
ROS WrenchStamped topic name.
void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
Load the controller.
std::string resolve(const std::string &prefix, const std::string &frame_name)
physics::LinkPtr link_
A pointer to the Gazebo Body.
GZ_REGISTER_MODEL_PLUGIN(GazeboRosF3D)
int f3d_connect_count_
: keep track of number of connections
std::string frame_name_
ROS frame transform name to use in the image message header. This should be simply map since the retu...
GazeboRosF3D()
Constructor.
ros::NodeHandle * rosnode_
A pointer to the ROS node. A node will be instantiated if it does not exist.
std::string link_name_
store bodyname
virtual void UpdateChild()
Update the controller.
#define ROS_FATAL_STREAM_NAMED(name, args)
event::ConnectionPtr update_connection_
ros::CallbackQueue queue_
std::string robot_namespace_
for setting ROS name space
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool getParam(const std::string &key, std::string &s) const
#define ROS_FATAL_NAMED(name,...)
boost::mutex lock_
A mutex to lock access to fields that are used in message callbacks.
boost::shared_ptr< void > VoidPtr
geometry_msgs::WrenchStamped wrench_msg_
ROS WrenchStamped message.
virtual ~GazeboRosF3D()
Destructor.