65 this->
world_ = _model->GetWorld();
69 if (_sdf->HasElement(
"robotNamespace"))
70 this->
robot_namespace_ = _sdf->GetElement(
"robotNamespace")->Get<std::string>() +
"/";
72 if (!_sdf->HasElement(
"bodyName"))
74 ROS_FATAL_NAMED(
"force",
"force plugin missing <bodyName>, cannot proceed");
78 this->
link_name_ = _sdf->GetElement(
"bodyName")->Get<std::string>();
87 if (!_sdf->HasElement(
"topicName"))
89 ROS_FATAL_NAMED(
"force",
"force plugin missing <topicName>, cannot proceed");
93 this->
topic_name_ = _sdf->GetElement(
"topicName")->Get<std::string>();
100 <<
"Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
140 ignition::math::Vector3d force(this->
wrench_msg_.force.x,this->wrench_msg_.force.y,this->wrench_msg_.force.z);
141 ignition::math::Vector3d torque(this->
wrench_msg_.torque.x,this->wrench_msg_.torque.y,this->wrench_msg_.torque.z);
142 this->
link_->AddForce(force);
143 this->
link_->AddTorque(torque);
144 this->
lock_.unlock();
152 static const double timeout = 0.01;
void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
geometry_msgs::Wrench wrench_msg_
Container for the wrench force that this plugin exerts on the body.
void QueueThread()
The custom callback queue thread function.
ros::CallbackQueue queue_
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
GazeboRosForce()
Constructor.
ROSCPP_DECL bool isInitialized()
boost::mutex lock_
A mutex to lock access to fields that are used in ROS message callbacks.
std::string link_name_
The Link this plugin is attached to, and will exert forces on.
GZ_REGISTER_MODEL_PLUGIN(GazeboRosF3D)
physics::WorldPtr world_
A pointer to the gazebo world.
virtual ~GazeboRosForce()
Destructor.
std::string robot_namespace_
for setting ROS name space
physics::LinkPtr link_
A pointer to the Link, where force is applied.
#define ROS_FATAL_STREAM_NAMED(name, args)
event::ConnectionPtr update_connection_
void UpdateObjectForce(const geometry_msgs::Wrench::ConstPtr &_msg)
call back when a Wrench message is published
virtual void UpdateChild()
#define ROS_FATAL_NAMED(name,...)
std::string topic_name_
ROS Wrench topic name inputs.
ros::NodeHandle * rosnode_
A pointer to the ROS node. A node will be instantiated if it does not exist.
boost::shared_ptr< void > VoidPtr
boost::thread callback_queue_thread_
Thead object for the running callback Thread.