gazebo_ros_bumper.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: Bumper controller
20  * Author: Nate Koenig
21  * Date: 09 Sept. 2008
22  */
23 
24 #include <map>
25 #include <string>
26 
27 #include <gazebo/physics/World.hh>
28 #include <gazebo/physics/HingeJoint.hh>
29 #include <gazebo/physics/Contact.hh>
30 #include <gazebo/sensors/Sensor.hh>
31 #include <sdf/sdf.hh>
32 #include <sdf/Param.hh>
33 #include <gazebo/common/Exception.hh>
34 #include <gazebo/sensors/SensorTypes.hh>
35 #include <ignition/math/Pose3.hh>
36 #include <ignition/math/Quaternion.hh>
37 #include <ignition/math/Vector3.hh>
38 
39 #include <tf/tf.h>
40 
43 
44 namespace gazebo
45 {
46 // Register this plugin with the simulator
47 GZ_REGISTER_SENSOR_PLUGIN(GazeboRosBumper)
48 
49 // Constructor
51 GazeboRosBumper::GazeboRosBumper() : SensorPlugin()
52 {
53 }
54 
56 // Destructor
58 {
59  this->rosnode_->shutdown();
60  this->callback_queue_thread_.join();
61 
62  delete this->rosnode_;
63 }
64 
66 // Load the controller
67 void GazeboRosBumper::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
68 {
70  this->parentSensor = dynamic_pointer_cast<sensors::ContactSensor>(_parent);
71  if (!this->parentSensor)
72  {
73  ROS_ERROR_NAMED("bumper", "Contact sensor parent is not of type ContactSensor");
74  return;
75  }
76 
77  this->robot_namespace_ = "";
78  if (_sdf->HasElement("robotNamespace"))
79  this->robot_namespace_ =
80  _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
81 
82  // "publishing contact/collisions to this topic name: "
83  // << this->bumper_topic_name_ << std::endl;
84  this->bumper_topic_name_ = "bumper_states";
85  if (_sdf->HasElement("bumperTopicName"))
86  this->bumper_topic_name_ =
87  _sdf->GetElement("bumperTopicName")->Get<std::string>();
88 
89  // "transform contact/collisions pose, forces to this body (link) name: "
90  // << this->frame_name_ << std::endl;
91  if (!_sdf->HasElement("frameName"))
92  {
93  ROS_INFO_NAMED("bumper", "bumper plugin missing <frameName>, defaults to world");
94  this->frame_name_ = "world";
95  }
96  else
97  this->frame_name_ = _sdf->GetElement("frameName")->Get<std::string>();
98 
99  // Make sure the ROS node for Gazebo has already been initialized
100  if (!ros::isInitialized())
101  {
102  ROS_FATAL_STREAM_NAMED("bumper", "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  this->contact_pub_ = this->rosnode_->advertise<gazebo_msgs::ContactsState>(
115  std::string(this->bumper_topic_name_), 1);
116 
117  // Initialize
118  // start custom queue for contact bumper
119  this->callback_queue_thread_ = boost::thread(
120  boost::bind(&GazeboRosBumper::ContactQueueThread, this));
121 
122  // Listen to the update event. This event is broadcast every
123  // simulation iteration.
124  this->update_connection_ = this->parentSensor->ConnectUpdated(
125  boost::bind(&GazeboRosBumper::OnContact, this));
126 
127  // Make sure the parent sensor is active.
128  this->parentSensor->SetActive(true);
129 }
130 
132 // Update the controller
134 {
135  if (this->contact_pub_.getNumSubscribers() <= 0)
136  return;
137 
138  msgs::Contacts contacts;
139  contacts = this->parentSensor->Contacts();
141  this->contact_state_msg_.header.frame_id = this->frame_name_;
142  this->contact_state_msg_.header.stamp = ros::Time(contacts.time().sec(),
143  contacts.time().nsec());
144 
145 /*
148 
149  // if frameName specified is "world", "/map" or "map" report back
150  // inertial values in the gazebo world
151  physics::LinkPtr myFrame;
152  if (myFrame == NULL && this->frame_name_ != "world" &&
153  this->frame_name_ != "/map" && this->frame_name_ != "map")
154  {
155  // lock in case a model is being spawned
156  boost::recursive_mutex::scoped_lock lock(
157  *Simulator::Instance()->GetMRMutex());
158  // look through all models in the world, search for body
159  // name that matches frameName
160  physics::Model_V all_models = World::Instance()->Models();
161  for (physics::Model_V::iterator iter = all_models.begin();
162  iter != all_models.end(); iter++)
163  {
164  if (*iter) myFrame =
165  boost::dynamic_pointer_cast<physics::Link>((*iter)->GetLink(this->frame_name_));
166  if (myFrame) break;
167  }
168 
169  // not found
170  if (!myFrame)
171  {
172  ROS_INFO_NAMED("bumper", "gazebo_ros_bumper plugin: frameName: %s does not exist"
173  " yet, will not publish\n",this->frame_name_.c_str());
174  return;
175  }
176  }
177 */
178  // get reference frame (body(link)) pose and subtract from it to get
179  // relative force, torque, position and normal vectors
180  ignition::math::Pose3d pose, frame_pose;
181  ignition::math::Quaterniond rot, frame_rot;
182  ignition::math::Vector3d pos, frame_pos;
183  /*
184  if (myFrame)
185  {
186  frame_pose = myFrame->WorldPose(); //-this->myBody->GetCoMPose();
187  frame_pos = frame_pose.Pos();
188  frame_rot = frame_pose.Rot();
189  }
190  else
191  */
192  {
193  // no specific frames specified, use identity pose, keeping
194  // relative frame at inertial origin
195  frame_pos = ignition::math::Vector3d(0, 0, 0);
196  frame_rot = ignition::math::Quaterniond(1, 0, 0, 0); // gazebo u,x,y,z == identity
197  frame_pose = ignition::math::Pose3d(frame_pos, frame_rot);
198  }
199 
200 
201 
202  // set contact states size
203  this->contact_state_msg_.states.clear();
204 
205  // GetContacts returns all contacts on the collision body
206  unsigned int contactsPacketSize = contacts.contact_size();
207  for (unsigned int i = 0; i < contactsPacketSize; ++i)
208  {
209 
210  // For each collision contact
211  // Create a ContactState
212  gazebo_msgs::ContactState state;
214  gazebo::msgs::Contact contact = contacts.contact(i);
215 
216  state.collision1_name = contact.collision1();
217  state.collision2_name = contact.collision2();
218  std::ostringstream stream;
219  stream << "Debug: i:(" << i << "/" << contactsPacketSize
220  << ") my geom:" << state.collision1_name
221  << " other geom:" << state.collision2_name
222  << " time:" << ros::Time(contact.time().sec(), contact.time().nsec())
223  << std::endl;
224  state.info = stream.str();
225 
226  state.wrenches.clear();
227  state.contact_positions.clear();
228  state.contact_normals.clear();
229  state.depths.clear();
230 
231  // sum up all wrenches for each DOF
232  geometry_msgs::Wrench total_wrench;
233  total_wrench.force.x = 0;
234  total_wrench.force.y = 0;
235  total_wrench.force.z = 0;
236  total_wrench.torque.x = 0;
237  total_wrench.torque.y = 0;
238  total_wrench.torque.z = 0;
239 
240  unsigned int contactGroupSize = contact.position_size();
241  for (unsigned int j = 0; j < contactGroupSize; ++j)
242  {
243  // loop through individual contacts between collision1 and collision2
244  // gzerr << j << " Position:"
245  // << contact.position(j).x() << " "
246  // << contact.position(j).y() << " "
247  // << contact.position(j).z() << "\n";
248  // gzerr << " Normal:"
249  // << contact.normal(j).x() << " "
250  // << contact.normal(j).y() << " "
251  // << contact.normal(j).z() << "\n";
252  // gzerr << " Depth:" << contact.depth(j) << "\n";
253 
254  // Get force, torque and rotate into user specified frame.
255  // frame_rot is identity if world is used (default for now)
256  ignition::math::Vector3d force = frame_rot.RotateVectorReverse(ignition::math::Vector3d(
257  contact.wrench(j).body_1_wrench().force().x(),
258  contact.wrench(j).body_1_wrench().force().y(),
259  contact.wrench(j).body_1_wrench().force().z()));
260  ignition::math::Vector3d torque = frame_rot.RotateVectorReverse(ignition::math::Vector3d(
261  contact.wrench(j).body_1_wrench().torque().x(),
262  contact.wrench(j).body_1_wrench().torque().y(),
263  contact.wrench(j).body_1_wrench().torque().z()));
264 
265  // set wrenches
266  geometry_msgs::Wrench wrench;
267  wrench.force.x = force.X();
268  wrench.force.y = force.Y();
269  wrench.force.z = force.Z();
270  wrench.torque.x = torque.X();
271  wrench.torque.y = torque.Y();
272  wrench.torque.z = torque.Z();
273  state.wrenches.push_back(wrench);
274 
275  total_wrench.force.x += wrench.force.x;
276  total_wrench.force.y += wrench.force.y;
277  total_wrench.force.z += wrench.force.z;
278  total_wrench.torque.x += wrench.torque.x;
279  total_wrench.torque.y += wrench.torque.y;
280  total_wrench.torque.z += wrench.torque.z;
281 
282  // transform contact positions into relative frame
283  // set contact positions
284  ignition::math::Vector3d position = frame_rot.RotateVectorReverse(
285  ignition::math::Vector3d(contact.position(j).x(),
286  contact.position(j).y(),
287  contact.position(j).z()) - frame_pos);
288  geometry_msgs::Vector3 contact_position;
289  contact_position.x = position.X();
290  contact_position.y = position.Y();
291  contact_position.z = position.Z();
292  state.contact_positions.push_back(contact_position);
293 
294  // rotate normal into user specified frame.
295  // frame_rot is identity if world is used.
296  ignition::math::Vector3d normal = frame_rot.RotateVectorReverse(
297  ignition::math::Vector3d(contact.normal(j).x(),
298  contact.normal(j).y(),
299  contact.normal(j).z()));
300  // set contact normals
301  geometry_msgs::Vector3 contact_normal;
302  contact_normal.x = normal.X();
303  contact_normal.y = normal.Y();
304  contact_normal.z = normal.Z();
305  state.contact_normals.push_back(contact_normal);
306 
307  // set contact depth, interpenetration
308  state.depths.push_back(contact.depth(j));
309  }
310 
311  state.total_wrench = total_wrench;
312  this->contact_state_msg_.states.push_back(state);
313  }
314 
316 }
317 
318 
320 // Put laser data to the interface
322 {
323  static const double timeout = 0.01;
324 
325  while (this->rosnode_->ok())
326  {
328  }
329 }
330 }
#define ROS_INFO_NAMED(name,...)
std::string bumper_topic_name_
set topic name of broadcast
void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
Load the plugin.
void publish(const boost::shared_ptr< M > &message) const
ROSCPP_DECL bool isInitialized()
std::string resolve(const std::string &prefix, const std::string &frame_name)
ros::CallbackQueue contact_queue_
boost::thread callback_queue_thread_
#define ROS_FATAL_STREAM_NAMED(name, args)
std::string robot_namespace_
for setting ROS name space
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
event::ConnectionPtr update_connection_
uint32_t getNumSubscribers() const
void OnContact()
Update the controller.
A Bumper controller.
bool getParam(const std::string &key, std::string &s) const
#define ROS_ERROR_NAMED(name,...)
bool ok() const
#define GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST
ros::NodeHandle * rosnode_
pointer to ros node
sensors::ContactSensorPtr parentSensor
gazebo_msgs::ContactsState contact_state_msg_
broadcast some string for now.


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