gazebo_ros_ft_sensor.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2014 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  * Desc: Force Torque Sensor Plugin
19  * Author: Francisco Suarez-Ruiz
20  * Date: 5 June 2014
21  */
22 
24 #include <tf/tf.h>
25 
26 namespace gazebo
27 {
28 GZ_REGISTER_MODEL_PLUGIN(GazeboRosFT);
29 
31 // Constructor
33 {
34  this->ft_connect_count_ = 0;
35  this->seed = 0;
36 }
37 
39 // Destructor
41 {
42  this->update_connection_.reset();
43  // Custom Callback Queue
44  this->queue_.clear();
45  this->queue_.disable();
46  this->rosnode_->shutdown();
47  this->callback_queue_thread_.join();
48  delete this->rosnode_;
49 }
50 
52 // Load the controller
53 void GazeboRosFT::Load( physics::ModelPtr _model, sdf::ElementPtr _sdf )
54 {
55  // Save pointers
56  this->model_ = _model;
57  this->world_ = this->model_->GetWorld();
58 
59  // load parameters
60  this->robot_namespace_ = "";
61  if (_sdf->HasElement("robotNamespace"))
62  this->robot_namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
63 
64  if (!_sdf->HasElement("jointName"))
65  {
66  ROS_FATAL_NAMED("ft_sensor", "ft_sensor plugin missing <jointName>, cannot proceed");
67  return;
68  }
69  else
70  this->joint_name_ = _sdf->GetElement("jointName")->Get<std::string>();
71 
72  this->joint_ = this->model_->GetJoint(this->joint_name_);
73  if (!this->joint_)
74  {
75  ROS_FATAL_NAMED("ft_sensor", "gazebo_ros_ft_sensor plugin error: jointName: %s does not exist\n",this->joint_name_.c_str());
76  return;
77  }
78 
79  this->parent_link_ = this->joint_->GetParent();
80  this->child_link_ = this->joint_->GetChild();
81  this->frame_name_ = this->child_link_->GetName();
82 
83  ROS_INFO_NAMED("ft_sensor", "ft_sensor plugin reporting wrench values to the frame [%s]", this->frame_name_.c_str());
84 
85  if (!_sdf->HasElement("topicName"))
86  {
87  ROS_FATAL_NAMED("ft_sensor", "ft_sensor plugin missing <topicName>, cannot proceed");
88  return;
89  }
90  else
91  this->topic_name_ = _sdf->GetElement("topicName")->Get<std::string>();
92 
93  if (!_sdf->HasElement("gaussianNoise"))
94  {
95  ROS_INFO_NAMED("ft_sensor", "imu plugin missing <gaussianNoise>, defaults to 0.0");
96  this->gaussian_noise_ = 0.0;
97  }
98  else
99  this->gaussian_noise_ = _sdf->Get<double>("gaussianNoise");
100 
101  if (!_sdf->HasElement("updateRate"))
102  {
103  ROS_DEBUG_NAMED("ft_sensor", "ft_sensor plugin missing <updateRate>, defaults to 0.0"
104  " (as fast as possible)");
105  this->update_rate_ = 0;
106  }
107  else
108  this->update_rate_ = _sdf->GetElement("updateRate")->Get<double>();
109 
110  // Make sure the ROS node for Gazebo has already been initialized
111  if (!ros::isInitialized())
112  {
113  ROS_FATAL_STREAM_NAMED("ft_sensor", "A ROS node for Gazebo has not been initialized, unable to load plugin. "
114  << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
115  return;
116  }
117 
118  this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);
119 
120  // resolve tf prefix
121  std::string prefix;
122  this->rosnode_->getParam(std::string("tf_prefix"), prefix);
123  this->frame_name_ = tf::resolve(prefix, this->frame_name_);
124 
125  // Custom Callback Queue
126  ros::AdvertiseOptions ao = ros::AdvertiseOptions::create<geometry_msgs::WrenchStamped>(
127  this->topic_name_,1,
128  boost::bind( &GazeboRosFT::FTConnect,this),
129  boost::bind( &GazeboRosFT::FTDisconnect,this), ros::VoidPtr(), &this->queue_);
130  this->pub_ = this->rosnode_->advertise(ao);
131 
132  // Custom Callback Queue
133  this->callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosFT::QueueThread,this ) );
134 
135  // New Mechanism for Updating every World Cycle
136  // Listen to the update event. This event is broadcast every
137  // simulation iteration.
138  this->update_connection_ = event::Events::ConnectWorldUpdateBegin(
139  boost::bind(&GazeboRosFT::UpdateChild, this));
140 }
141 
143 // Someone subscribes to me
145 {
146  this->ft_connect_count_++;
147 }
148 
150 // Someone subscribes to me
152 {
153  this->ft_connect_count_--;
154 }
155 
157 // Update the controller
159 {
160 #if GAZEBO_MAJOR_VERSION >= 8
161  common::Time cur_time = this->world_->SimTime();
162 #else
163  common::Time cur_time = this->world_->GetSimTime();
164 #endif
165 
166  // rate control
167  if (this->update_rate_ > 0 &&
168  (cur_time-this->last_time_).Double() < (1.0/this->update_rate_))
169  return;
170 
171  if (this->ft_connect_count_ == 0)
172  return;
173 
174  physics::JointWrench wrench;
175  ignition::math::Vector3d torque;
176  ignition::math::Vector3d force;
177 
178  // FIXME: Should include options for diferent frames and measure directions
179  // E.g: https://bitbucket.org/osrf/gazebo/raw/default/gazebo/sensors/ForceTorqueSensor.hh
180  // Get force torque at the joint
181  // The wrench is reported in the CHILD <frame>
182  // The <measure_direction> is child_to_parent
183  wrench = this->joint_->GetForceTorque(0);
184 #if GAZEBO_MAJOR_VERSION >= 8
185  force = wrench.body2Force;
186  torque = wrench.body2Torque;
187 #else
188  force = wrench.body2Force.Ign();
189  torque = wrench.body2Torque.Ign();
190 #endif
191 
192 
193  this->lock_.lock();
194  // copy data into wrench message
195  this->wrench_msg_.header.frame_id = this->frame_name_;
196 #if GAZEBO_MAJOR_VERSION >= 8
197  this->wrench_msg_.header.stamp.sec = (this->world_->SimTime()).sec;
198  this->wrench_msg_.header.stamp.nsec = (this->world_->SimTime()).nsec;
199 #else
200  this->wrench_msg_.header.stamp.sec = (this->world_->GetSimTime()).sec;
201  this->wrench_msg_.header.stamp.nsec = (this->world_->GetSimTime()).nsec;
202 #endif
203 
204  this->wrench_msg_.wrench.force.x = force.X() + this->GaussianKernel(0, this->gaussian_noise_);
205  this->wrench_msg_.wrench.force.y = force.Y() + this->GaussianKernel(0, this->gaussian_noise_);
206  this->wrench_msg_.wrench.force.z = force.Z() + this->GaussianKernel(0, this->gaussian_noise_);
207  this->wrench_msg_.wrench.torque.x = torque.X() + this->GaussianKernel(0, this->gaussian_noise_);
208  this->wrench_msg_.wrench.torque.y = torque.Y() + this->GaussianKernel(0, this->gaussian_noise_);
209  this->wrench_msg_.wrench.torque.z = torque.Z() + this->GaussianKernel(0, this->gaussian_noise_);
210 
211  this->pub_.publish(this->wrench_msg_);
212  this->lock_.unlock();
213 
214  // save last time stamp
215  this->last_time_ = cur_time;
216 }
217 
219 // Utility for adding noise
220 double GazeboRosFT::GaussianKernel(double mu, double sigma)
221 {
222  // using Box-Muller transform to generate two independent standard
223  // normally disbributed normal variables see wikipedia
224 
225  // normalized uniform random variable
226  double U = static_cast<double>(rand_r(&this->seed)) /
227  static_cast<double>(RAND_MAX);
228 
229  // normalized uniform random variable
230  double V = static_cast<double>(rand_r(&this->seed)) /
231  static_cast<double>(RAND_MAX);
232 
233  double X = sqrt(-2.0 * ::log(U)) * cos(2.0*M_PI * V);
234  // double Y = sqrt(-2.0 * ::log(U)) * sin(2.0*M_PI * V);
235 
236  // there are 2 indep. vars, we'll just use X
237  // scale to our mu and sigma
238  X = sigma * X + mu;
239  return X;
240 }
241 
242 // Custom Callback Queue
244 // custom callback queue thread
246 {
247  static const double timeout = 0.01;
248 
249  while (this->rosnode_->ok())
250  {
251  this->queue_.callAvailable(ros::WallDuration(timeout));
252  }
253 }
254 
255 }
virtual void UpdateChild()
Update the controller.
#define ROS_INFO_NAMED(name,...)
std::string topic_name_
ROS WrenchStamped topic name.
virtual ~GazeboRosFT()
Destructor.
int ft_connect_count_
: keep track of number of connections
void publish(const boost::shared_ptr< M > &message) const
std::string robot_namespace_
for setting ROS name space
physics::JointPtr joint_
A pointer to the Gazebo joint.
geometry_msgs::WrenchStamped wrench_msg_
ROS WrenchStamped message.
void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
Load the controller.
ROSCPP_DECL bool isInitialized()
ros::CallbackQueue queue_
ros::NodeHandle * rosnode_
A pointer to the ROS node. A node will be instantiated if it does not exist.
double GaussianKernel(double mu, double sigma)
Gaussian noise generator.
boost::mutex lock_
A mutex to lock access to fields that are used in message callbacks.
boost::thread callback_queue_thread_
std::string resolve(const std::string &prefix, const std::string &frame_name)
double gaussian_noise_
Gaussian noise.
#define ROS_DEBUG_NAMED(name,...)
GZ_REGISTER_MODEL_PLUGIN(GazeboRosF3D)
physics::WorldPtr world_
A pointer to the Gazebo world.
#define ROS_FATAL_STREAM_NAMED(name, args)
event::ConnectionPtr update_connection_
physics::ModelPtr model_
A pointer to the Gazebo model.
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
physics::LinkPtr parent_link_
A pointer to the Gazebo parent link.
bool getParam(const std::string &key, std::string &s) const
#define ROS_FATAL_NAMED(name,...)
physics::LinkPtr child_link_
A pointer to the Gazebo child link.
common::Time last_time_
save last_time
bool ok() const
std::string joint_name_
store bodyname
std::string frame_name_
ROS frame transform name to use in the image message header. FIXME: extract link name directly...
boost::shared_ptr< void > VoidPtr


gazebo_plugins
Author(s): John Hsu
autogenerated on Tue Apr 6 2021 02:19:39