gazebo_ros_f3d.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: Force Feed Back Ground Truth
20  Author: Sachin Chitta and John Hsu
21  Date: 1 June 2008
22  */
23 
25 #include <tf/tf.h>
26 
27 namespace gazebo
28 {
29 GZ_REGISTER_MODEL_PLUGIN(GazeboRosF3D);
30 
32 // Constructor
34 {
35  this->f3d_connect_count_ = 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 GazeboRosF3D::Load( physics::ModelPtr _parent, sdf::ElementPtr _sdf )
54 {
55  // Get the world name.
56  this->world_ = _parent->GetWorld();
57 
58  // load parameters
59  this->robot_namespace_ = "";
60  if (_sdf->HasElement("robotNamespace"))
61  this->robot_namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
62 
63  if (!_sdf->HasElement("bodyName"))
64  {
65  ROS_FATAL_NAMED("f3d", "f3d plugin missing <bodyName>, cannot proceed");
66  return;
67  }
68  else
69  this->link_name_ = _sdf->GetElement("bodyName")->Get<std::string>();
70 
71  this->link_ = _parent->GetLink(this->link_name_);
72  if (!this->link_)
73  {
74  ROS_FATAL_NAMED("f3d", "gazebo_ros_f3d plugin error: bodyName: %s does not exist\n",this->link_name_.c_str());
75  return;
76  }
77 
78  if (!_sdf->HasElement("topicName"))
79  {
80  ROS_FATAL_NAMED("f3d", "f3d plugin missing <topicName>, cannot proceed");
81  return;
82  }
83  else
84  this->topic_name_ = _sdf->GetElement("topicName")->Get<std::string>();
85 
86  if (!_sdf->HasElement("frameName"))
87  {
88  ROS_INFO_NAMED("f3d", "f3d plugin missing <frameName>, defaults to world");
89  this->frame_name_ = "world";
90  }
91  else
92  {
93  this->frame_name_ = _sdf->GetElement("frameName")->Get<std::string>();
94  // todo: frameName not used
95  ROS_INFO_NAMED("f3d", "f3d plugin specifies <frameName> [%s], not used, default to world",this->frame_name_.c_str());
96  }
97 
98 
99  // Make sure the ROS node for Gazebo has already been initialized
100  if (!ros::isInitialized())
101  {
102  ROS_FATAL_STREAM_NAMED("f3d", "A ROS node for Gazebo has not been initialized, unable to load plugin. "
103  << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
104  return;
105  }
106 
107  this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);
108 
109  // resolve tf prefix
110  std::string prefix;
111  this->rosnode_->getParam(std::string("tf_prefix"), prefix);
112  this->frame_name_ = tf::resolve(prefix, this->frame_name_);
113 
114  // Custom Callback Queue
115  ros::AdvertiseOptions ao = ros::AdvertiseOptions::create<geometry_msgs::WrenchStamped>(
116  this->topic_name_,1,
117  boost::bind( &GazeboRosF3D::F3DConnect,this),
118  boost::bind( &GazeboRosF3D::F3DDisconnect,this), ros::VoidPtr(), &this->queue_);
119  this->pub_ = this->rosnode_->advertise(ao);
120 
121  // Custom Callback Queue
122  this->callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosF3D::QueueThread,this ) );
123 
124  // New Mechanism for Updating every World Cycle
125  // Listen to the update event. This event is broadcast every
126  // simulation iteration.
127  this->update_connection_ = event::Events::ConnectWorldUpdateBegin(
128  boost::bind(&GazeboRosF3D::UpdateChild, this));
129 }
130 
132 // Someone subscribes to me
134 {
135  this->f3d_connect_count_++;
136 }
137 
139 // Someone subscribes to me
141 {
142  this->f3d_connect_count_--;
143 }
144 
146 // Update the controller
148 {
149  if (this->f3d_connect_count_ == 0)
150  return;
151 
152  ignition::math::Vector3d torque;
153  ignition::math::Vector3d force;
154 
155  // get force and torque on body
156 #if GAZEBO_MAJOR_VERSION >= 8
157  force = this->link_->WorldForce();
158  torque = this->link_->WorldTorque();
159 #else
160  force = this->link_->GetWorldForce().Ign();
161  torque = this->link_->GetWorldTorque().Ign();
162 #endif
163 
164  this->lock_.lock();
165  // copy data into wrench message
166  this->wrench_msg_.header.frame_id = this->frame_name_;
167 #if GAZEBO_MAJOR_VERSION >= 8
168  this->wrench_msg_.header.stamp.sec = (this->world_->SimTime()).sec;
169  this->wrench_msg_.header.stamp.nsec = (this->world_->SimTime()).nsec;
170 #else
171  this->wrench_msg_.header.stamp.sec = (this->world_->GetSimTime()).sec;
172  this->wrench_msg_.header.stamp.nsec = (this->world_->GetSimTime()).nsec;
173 #endif
174 
175  this->wrench_msg_.wrench.force.x = force.X();
176  this->wrench_msg_.wrench.force.y = force.Y();
177  this->wrench_msg_.wrench.force.z = force.Z();
178  this->wrench_msg_.wrench.torque.x = torque.X();
179  this->wrench_msg_.wrench.torque.y = torque.Y();
180  this->wrench_msg_.wrench.torque.z = torque.Z();
181 
182  this->pub_.publish(this->wrench_msg_);
183  this->lock_.unlock();
184 
185 }
186 
187 // Custom Callback Queue
189 // custom callback queue thread
191 {
192  static const double timeout = 0.01;
193 
194  while (this->rosnode_->ok())
195  {
196  this->queue_.callAvailable(ros::WallDuration(timeout));
197  }
198 }
199 
200 }
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.
bool ok() const
ros::Publisher pub_
boost::shared_ptr< void > VoidPtr
geometry_msgs::WrenchStamped wrench_msg_
ROS WrenchStamped message.
physics::WorldPtr world_
virtual ~GazeboRosF3D()
Destructor.


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