27 #include <std_msgs/Bool.h> 62 ROS_INFO_NAMED(
"vacuum_gripper",
"Loading gazebo_ros_vacuum_gripper");
68 world_ = _model->GetWorld();
72 if (_sdf->HasElement(
"robotNamespace"))
73 robot_namespace_ = _sdf->GetElement(
"robotNamespace")->Get<std::string>() +
"/";
75 if (!_sdf->HasElement(
"bodyName"))
77 ROS_FATAL_NAMED(
"vacuum_gripper",
"vacuum_gripper plugin missing <bodyName>, cannot proceed");
81 link_name_ = _sdf->GetElement(
"bodyName")->Get<std::string>();
87 physics::Link_V links = _model->GetLinks();
88 for (
size_t i = 0; i < links.size(); i++) {
89 found += std::string(
" ") + links[i]->GetName();
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());
97 if (!_sdf->HasElement(
"topicName"))
99 ROS_FATAL_NAMED(
"vacuum_gripper",
"vacuum_gripper plugin missing <serviceName>, cannot proceed");
103 topic_name_ = _sdf->GetElement(
"topicName")->Get<std::string>();
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)");
125 ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
130 ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
144 ROS_INFO_NAMED(
"vacuum_gripper",
"Loaded gazebo_ros_vacuum_gripper");
148 std_srvs::Empty::Response &res)
151 ROS_WARN_NAMED(
"vacuum_gripper",
"gazebo_ros_vacuum_gripper: already status is 'on'");
154 ROS_INFO_NAMED(
"vacuum_gripper",
"gazebo_ros_vacuum_gripper: status: off -> on");
159 std_srvs::Empty::Response &res)
163 ROS_INFO_NAMED(
"vacuum_gripper",
"gazebo_ros_vacuum_gripper: status: on -> off");
165 ROS_WARN_NAMED(
"vacuum_gripper",
"gazebo_ros_vacuum_gripper: already status is 'off'");
174 std_msgs::Bool grasping_msg;
175 grasping_msg.data =
false;
182 #if GAZEBO_MAJOR_VERSION >= 8 183 ignition::math::Pose3d parent_pose =
link_->WorldPose();
184 physics::Model_V models =
world_->Models();
186 ignition::math::Pose3d parent_pose =
link_->GetWorldPose().Ign();
187 physics::Model_V models =
world_->GetModels();
189 for (
size_t i = 0; i < models.size(); i++) {
190 if (models[i]->GetName() ==
link_->GetName() ||
191 models[i]->GetName() ==
parent_->GetName())
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();
200 ignition::math::Pose3d link_pose = links[j]->GetWorldPose().Ign();
202 ignition::math::Pose3d diff = parent_pose - link_pose;
203 double norm = diff.Pos().Length();
205 #if GAZEBO_MAJOR_VERSION >= 8 206 links[j]->SetLinearVel(
link_->WorldLinearVel());
207 links[j]->SetAngularVel(
link_->WorldAngularVel());
209 links[j]->SetLinearVel(
link_->GetWorldLinearVel());
210 links[j]->SetAngularVel(
link_->GetWorldAngularVel());
212 double norm_force = 1 / norm;
216 link_pose.Set(parent_pose.Pos(), link_pose.Rot());
217 links[j]->SetWorldPose(link_pose);
219 if (norm_force > 20) {
222 ignition::math::Vector3d force = norm_force * diff.Pos().Normalize();
223 links[j]->AddForce(force);
224 grasping_msg.data =
true;
237 static const double timeout = 0.01;
physics::ModelPtr parent_
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,...)
void publish(const boost::shared_ptr< M > &message) const
#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.
virtual ~GazeboRosVacuumGripper()
Destructor.
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)
physics::LinkPtr link_
A pointer to the Link, where force is applied.
#define ROS_FATAL_STREAM_NAMED(name, args)
virtual void UpdateChild()
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)
event::ConnectionPtr update_connection_
#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.
GazeboRosVacuumGripper()
Constructor.
ros::CallbackQueue queue_
boost::shared_ptr< void > VoidPtr