gazebo_ros_vacuum_gripper.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2015 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: GazeboVacuumGripper plugin for manipulating objects in Gazebo
20  Author: Kentaro Wada
21  Date: 7 Dec 2015
22  */
23 
24 #include <algorithm>
25 #include <assert.h>
26 
27 #include <std_msgs/Bool.h>
29 
30 
31 namespace gazebo
32 {
33 GZ_REGISTER_MODEL_PLUGIN(GazeboRosVacuumGripper);
34 
36 // Constructor
38 {
39  connect_count_ = 0;
40  status_ = false;
41 }
42 
44 // Destructor
46 {
47  update_connection_.reset();
48 
49  // Custom Callback Queue
50  queue_.clear();
51  queue_.disable();
52  rosnode_->shutdown();
54 
55  delete rosnode_;
56 }
57 
59 // Load the controller
60 void GazeboRosVacuumGripper::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
61 {
62  ROS_INFO_NAMED("vacuum_gripper", "Loading gazebo_ros_vacuum_gripper");
63 
64  // Set attached model;
65  parent_ = _model;
66 
67  // Get the world name.
68  world_ = _model->GetWorld();
69 
70  // load parameters
71  robot_namespace_ = "";
72  if (_sdf->HasElement("robotNamespace"))
73  robot_namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
74 
75  if (!_sdf->HasElement("bodyName"))
76  {
77  ROS_FATAL_NAMED("vacuum_gripper", "vacuum_gripper plugin missing <bodyName>, cannot proceed");
78  return;
79  }
80  else
81  link_name_ = _sdf->GetElement("bodyName")->Get<std::string>();
82 
83  link_ = _model->GetLink(link_name_);
84  if (!link_)
85  {
86  std::string found;
87  physics::Link_V links = _model->GetLinks();
88  for (size_t i = 0; i < links.size(); i++) {
89  found += std::string(" ") + links[i]->GetName();
90  }
91  ROS_FATAL_NAMED("vacuum_gripper", "gazebo_ros_vacuum_gripper plugin error: link named: %s does not exist", link_name_.c_str());
92  ROS_FATAL_NAMED("vacuum_gripper", "gazebo_ros_vacuum_gripper plugin error: You should check it exists and is not connected with fixed joint");
93  ROS_FATAL_NAMED("vacuum_gripper", "gazebo_ros_vacuum_gripper plugin error: Found links are: %s", found.c_str());
94  return;
95  }
96 
97  if (!_sdf->HasElement("topicName"))
98  {
99  ROS_FATAL_NAMED("vacuum_gripper", "vacuum_gripper plugin missing <serviceName>, cannot proceed");
100  return;
101  }
102  else
103  topic_name_ = _sdf->GetElement("topicName")->Get<std::string>();
104 
105  // Make sure the ROS node for Gazebo has already been initialized
106  if (!ros::isInitialized())
107  {
108  ROS_FATAL_STREAM_NAMED("vacuum_gripper", "A ROS node for Gazebo has not been initialized, unable to load plugin. "
109  << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
110  return;
111  }
112 
114 
115  // Custom Callback Queue
116  ros::AdvertiseOptions ao = ros::AdvertiseOptions::create<std_msgs::Bool>(
117  topic_name_, 1,
118  boost::bind(&GazeboRosVacuumGripper::Connect, this),
119  boost::bind(&GazeboRosVacuumGripper::Disconnect, this),
120  ros::VoidPtr(), &queue_);
121  pub_ = rosnode_->advertise(ao);
122 
123  // Custom Callback Queue
125  ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
127  this, _1, _2), ros::VoidPtr(), &queue_);
130  ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
131  "off", boost::bind(&GazeboRosVacuumGripper::OffServiceCallback,
132  this, _1, _2), ros::VoidPtr(), &queue_);
134 
135  // Custom Callback Queue
136  callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosVacuumGripper::QueueThread,this ) );
137 
138  // New Mechanism for Updating every World Cycle
139  // Listen to the update event. This event is broadcast every
140  // simulation iteration.
141  update_connection_ = event::Events::ConnectWorldUpdateBegin(
142  boost::bind(&GazeboRosVacuumGripper::UpdateChild, this));
143 
144  ROS_INFO_NAMED("vacuum_gripper", "Loaded gazebo_ros_vacuum_gripper");
145 }
146 
147 bool GazeboRosVacuumGripper::OnServiceCallback(std_srvs::Empty::Request &req,
148  std_srvs::Empty::Response &res)
149 {
150  if (status_) {
151  ROS_WARN_NAMED("vacuum_gripper", "gazebo_ros_vacuum_gripper: already status is 'on'");
152  } else {
153  status_ = true;
154  ROS_INFO_NAMED("vacuum_gripper", "gazebo_ros_vacuum_gripper: status: off -> on");
155  }
156  return true;
157 }
158 bool GazeboRosVacuumGripper::OffServiceCallback(std_srvs::Empty::Request &req,
159  std_srvs::Empty::Response &res)
160 {
161  if (status_) {
162  status_ = false;
163  ROS_INFO_NAMED("vacuum_gripper", "gazebo_ros_vacuum_gripper: status: on -> off");
164  } else {
165  ROS_WARN_NAMED("vacuum_gripper", "gazebo_ros_vacuum_gripper: already status is 'off'");
166  }
167  return true;
168 }
169 
171 // Update the controller
173 {
174  std_msgs::Bool grasping_msg;
175  grasping_msg.data = false;
176  if (!status_) {
177  pub_.publish(grasping_msg);
178  return;
179  }
180  // apply force
181  lock_.lock();
182 #if GAZEBO_MAJOR_VERSION >= 8
183  ignition::math::Pose3d parent_pose = link_->WorldPose();
184  physics::Model_V models = world_->Models();
185 #else
186  ignition::math::Pose3d parent_pose = link_->GetWorldPose().Ign();
187  physics::Model_V models = world_->GetModels();
188 #endif
189  for (size_t i = 0; i < models.size(); i++) {
190  if (models[i]->GetName() == link_->GetName() ||
191  models[i]->GetName() == parent_->GetName())
192  {
193  continue;
194  }
195  physics::Link_V links = models[i]->GetLinks();
196  for (size_t j = 0; j < links.size(); j++) {
197 #if GAZEBO_MAJOR_VERSION >= 8
198  ignition::math::Pose3d link_pose = links[j]->WorldPose();
199 #else
200  ignition::math::Pose3d link_pose = links[j]->GetWorldPose().Ign();
201 #endif
202  ignition::math::Pose3d diff = parent_pose - link_pose;
203  double norm = diff.Pos().Length();
204  if (norm < 0.05) {
205 #if GAZEBO_MAJOR_VERSION >= 8
206  links[j]->SetLinearVel(link_->WorldLinearVel());
207  links[j]->SetAngularVel(link_->WorldAngularVel());
208 #else
209  links[j]->SetLinearVel(link_->GetWorldLinearVel());
210  links[j]->SetAngularVel(link_->GetWorldAngularVel());
211 #endif
212  double norm_force = 1 / norm;
213  if (norm < 0.01) {
214  // apply friction like force
215  // TODO(unknown): should apply friction actually
216  link_pose.Set(parent_pose.Pos(), link_pose.Rot());
217  links[j]->SetWorldPose(link_pose);
218  }
219  if (norm_force > 20) {
220  norm_force = 20; // max_force
221  }
222  ignition::math::Vector3d force = norm_force * diff.Pos().Normalize();
223  links[j]->AddForce(force);
224  grasping_msg.data = true;
225  }
226  }
227  }
228  pub_.publish(grasping_msg);
229  lock_.unlock();
230 }
231 
232 // Custom Callback Queue
234 // custom callback queue thread
236 {
237  static const double timeout = 0.01;
238 
239  while (rosnode_->ok())
240  {
242  }
243 }
244 
246 // Someone subscribes to me
248 {
249  this->connect_count_++;
250 }
251 
253 // Someone subscribes to me
255 {
256  this->connect_count_--;
257 }
258 
259 } // namespace gazebo
void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
boost::thread callback_queue_thread_
Thead object for the running callback Thread.
std::string robot_namespace_
for setting ROS name space
#define ROS_INFO_NAMED(name,...)
#define ROS_WARN_NAMED(name,...)
ROSCPP_DECL bool isInitialized()
void QueueThread()
The custom callback queue thread function.
physics::WorldPtr world_
A pointer to the gazebo world.
boost::mutex lock_
A mutex to lock access to fields that are used in ROS message callbacks.
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
bool OnServiceCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
GZ_REGISTER_MODEL_PLUGIN(GazeboRosF3D)
void publish(const boost::shared_ptr< M > &message) const
physics::LinkPtr link_
A pointer to the Link, where force is applied.
#define ROS_FATAL_STREAM_NAMED(name, args)
std::string link_name_
The Link this plugin is attached to, and will exert forces on.
int connect_count_
: keep track of number of connections
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool OffServiceCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
#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


gazebo_plugins
Author(s): John Hsu
autogenerated on Wed Aug 24 2022 02:47:52