gazebo_ros_force.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2013 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 
18 /*
19  Desc: GazeboRosForce plugin for manipulating objects in Gazebo
20  Author: John Hsu
21  Date: 24 Sept 2008
22  */
23 
24 #include <algorithm>
25 #include <assert.h>
26 
28 
29 namespace gazebo
30 {
31 GZ_REGISTER_MODEL_PLUGIN(GazeboRosForce);
32 
34 // Constructor
36 {
37  this->wrench_msg_.force.x = 0;
38  this->wrench_msg_.force.y = 0;
39  this->wrench_msg_.force.z = 0;
40  this->wrench_msg_.torque.x = 0;
41  this->wrench_msg_.torque.y = 0;
42  this->wrench_msg_.torque.z = 0;
43 }
44 
46 // Destructor
48 {
49  this->update_connection_.reset();
50 
51  // Custom Callback Queue
52  this->queue_.clear();
53  this->queue_.disable();
54  this->rosnode_->shutdown();
55  this->callback_queue_thread_.join();
56 
57  delete this->rosnode_;
58 }
59 
61 // Load the controller
62 void GazeboRosForce::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
63 {
64  // Get the world name.
65  this->world_ = _model->GetWorld();
66 
67  // load parameters
68  this->robot_namespace_ = "";
69  if (_sdf->HasElement("robotNamespace"))
70  this->robot_namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
71 
72  if (!_sdf->HasElement("bodyName"))
73  {
74  ROS_FATAL_NAMED("force", "force plugin missing <bodyName>, cannot proceed");
75  return;
76  }
77  else
78  this->link_name_ = _sdf->GetElement("bodyName")->Get<std::string>();
79 
80  this->link_ = _model->GetLink(this->link_name_);
81  if (!this->link_)
82  {
83  ROS_FATAL_NAMED("force", "gazebo_ros_force plugin error: link named: %s does not exist\n",this->link_name_.c_str());
84  return;
85  }
86 
87  if (!_sdf->HasElement("topicName"))
88  {
89  ROS_FATAL_NAMED("force", "force plugin missing <topicName>, cannot proceed");
90  return;
91  }
92  else
93  this->topic_name_ = _sdf->GetElement("topicName")->Get<std::string>();
94 
95 
96  // Make sure the ROS node for Gazebo has already been initialized
97  if (!ros::isInitialized())
98  {
99  ROS_FATAL_STREAM_NAMED("force", "A ROS node for Gazebo has not been initialized, unable to load plugin. "
100  << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
101  return;
102  }
103 
104  this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);
105 
106  // Custom Callback Queue
107  ros::SubscribeOptions so = ros::SubscribeOptions::create<geometry_msgs::Wrench>(
108  this->topic_name_,1,
109  boost::bind( &GazeboRosForce::UpdateObjectForce,this,_1),
110  ros::VoidPtr(), &this->queue_);
111  this->sub_ = this->rosnode_->subscribe(so);
112 
113  // Custom Callback Queue
114  this->callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosForce::QueueThread,this ) );
115 
116  // New Mechanism for Updating every World Cycle
117  // Listen to the update event. This event is broadcast every
118  // simulation iteration.
119  this->update_connection_ = event::Events::ConnectWorldUpdateBegin(
120  boost::bind(&GazeboRosForce::UpdateChild, this));
121 }
122 
124 // Update the controller
125 void GazeboRosForce::UpdateObjectForce(const geometry_msgs::Wrench::ConstPtr& _msg)
126 {
127  this->wrench_msg_.force.x = _msg->force.x;
128  this->wrench_msg_.force.y = _msg->force.y;
129  this->wrench_msg_.force.z = _msg->force.z;
130  this->wrench_msg_.torque.x = _msg->torque.x;
131  this->wrench_msg_.torque.y = _msg->torque.y;
132  this->wrench_msg_.torque.z = _msg->torque.z;
133 }
134 
136 // Update the controller
138 {
139  this->lock_.lock();
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();
145 }
146 
147 // Custom Callback Queue
149 // custom callback queue thread
151 {
152  static const double timeout = 0.01;
153 
154  while (this->rosnode_->ok())
155  {
156  this->queue_.callAvailable(ros::WallDuration(timeout));
157  }
158 }
159 
160 }
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
#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.
bool ok() const
boost::shared_ptr< void > VoidPtr
boost::thread callback_queue_thread_
Thead object for the running callback Thread.


gazebo_plugins
Author(s): John Hsu
autogenerated on Tue Mar 26 2019 02:31:27