From d69430b11656409a7cf686c9dbdd4d6b3b24946e Mon Sep 17 00:00:00 2001 From: Shivesh Khaitan Date: Thu, 18 Jul 2019 15:43:17 +0530 Subject: [PATCH 1/3] [ros2] Port vacuum gripper to ROS2 --- .../gazebo_ros_vacuum_gripper.h | 161 ----------- .../src/gazebo_ros_vacuum_gripper.cpp | 259 ------------------ gazebo_plugins/CMakeLists.txt | 15 + .../gazebo_ros_vacuum_gripper.hpp | 83 ++++++ gazebo_plugins/package.xml | 1 + .../src/gazebo_ros_vacuum_gripper.cpp | 221 +++++++++++++++ gazebo_plugins/test/CMakeLists.txt | 2 + .../test/test_gazebo_ros_vacuum_gripper.cpp | 97 +++++++ .../worlds/gazebo_ros_vacuum_gripper.world | 110 ++++++++ .../gazebo_ros_vacuum_gripper_demo.world | 188 +++++++++++++ 10 files changed, 717 insertions(+), 420 deletions(-) delete mode 100644 .ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_vacuum_gripper.h delete mode 100644 .ros1_unported/gazebo_plugins/src/gazebo_ros_vacuum_gripper.cpp create mode 100644 gazebo_plugins/include/gazebo_plugins/gazebo_ros_vacuum_gripper.hpp create mode 100644 gazebo_plugins/src/gazebo_ros_vacuum_gripper.cpp create mode 100644 gazebo_plugins/test/test_gazebo_ros_vacuum_gripper.cpp create mode 100644 gazebo_plugins/test/worlds/gazebo_ros_vacuum_gripper.world create mode 100644 gazebo_plugins/worlds/gazebo_ros_vacuum_gripper_demo.world diff --git a/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_vacuum_gripper.h b/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_vacuum_gripper.h deleted file mode 100644 index b9e6a090a..000000000 --- a/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_vacuum_gripper.h +++ /dev/null @@ -1,161 +0,0 @@ -/* - * Copyright (C) 2015-2016 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ -/* - Desc: GazeboVacuumGripper plugin for manipulating objects in Gazebo - * Author: Kentaro Wada - * Date: 7 Dec 2015 - */ - -#ifndef GAZEBO_ROS_VACUUM_GRIPPER_HH -#define GAZEBO_ROS_VACUUM_GRIPPER_HH - -#include - -// Custom Callback Queue -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include -#include - - -namespace gazebo -{ -/// @addtogroup gazebo_dynamic_plugins Gazebo ROS Dynamic Plugins -/// @{ -/** \defgroup GazeboRosVacuumGripper Plugin XML Reference and Example - - \brief Ros Vacuum Gripper Plugin. - - This is a Plugin that collects data from a ROS topic and applies wrench to a body accordingly. - - Example Usage: - - left_end_effector will be the power point - - revolute type joint is necessary (fixed joint disappears on gazebo and plugin can't find the joint and link) - \verbatim - - 0 - - - - - - - - - - - - - - - - - - - - - - - - /robot/left_vacuum_gripper - left_end_effector - grasping - - - \endverbatim - - -\{ -*/ - - -class GazeboRosVacuumGripper : public ModelPlugin -{ - /// \brief Constructor - public: GazeboRosVacuumGripper(); - - /// \brief Destructor - public: virtual ~GazeboRosVacuumGripper(); - - // Documentation inherited - protected: void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); - - // Documentation inherited - protected: virtual void UpdateChild(); - - /// \brief The custom callback queue thread function. - private: void QueueThread(); - - private: bool OnServiceCallback(std_srvs::Empty::Request &req, - std_srvs::Empty::Response &res); - private: bool OffServiceCallback(std_srvs::Empty::Request &req, - std_srvs::Empty::Response &res); - - private: bool status_; - - private: physics::ModelPtr parent_; - - /// \brief A pointer to the gazebo world. - private: physics::WorldPtr world_; - - /// \brief A pointer to the Link, where force is applied - private: physics::LinkPtr link_; - - /// \brief A pointer to the ROS node. A node will be instantiated if it does not exist. - private: ros::NodeHandle* rosnode_; - - /// \brief A mutex to lock access to fields that are used in ROS message callbacks - private: boost::mutex lock_; - private: ros::Publisher pub_; - private: ros::ServiceServer srv1_; - private: ros::ServiceServer srv2_; - - /// \brief ROS Wrench topic name inputs - private: std::string topic_name_; - private: std::string service_name_; - /// \brief The Link this plugin is attached to, and will exert forces on. - private: std::string link_name_; - - /// \brief for setting ROS name space - private: std::string robot_namespace_; - - // Custom Callback Queue - private: ros::CallbackQueue queue_; - /// \brief Thead object for the running callback Thread. - private: boost::thread callback_queue_thread_; - - // Pointer to the update event connection - private: event::ConnectionPtr update_connection_; - - /// \brief: keep track of number of connections - private: int connect_count_; - private: void Connect(); - private: void Disconnect(); -}; -/** \} */ -/// @} -} -#endif diff --git a/.ros1_unported/gazebo_plugins/src/gazebo_ros_vacuum_gripper.cpp b/.ros1_unported/gazebo_plugins/src/gazebo_ros_vacuum_gripper.cpp deleted file mode 100644 index fee4d3822..000000000 --- a/.ros1_unported/gazebo_plugins/src/gazebo_ros_vacuum_gripper.cpp +++ /dev/null @@ -1,259 +0,0 @@ -/* - * Copyright 2015 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ - -/* - Desc: GazeboVacuumGripper plugin for manipulating objects in Gazebo - Author: Kentaro Wada - Date: 7 Dec 2015 - */ - -#include -#include - -#include -#include - - -namespace gazebo -{ -GZ_REGISTER_MODEL_PLUGIN(GazeboRosVacuumGripper); - -//////////////////////////////////////////////////////////////////////////////// -// Constructor -GazeboRosVacuumGripper::GazeboRosVacuumGripper() -{ - connect_count_ = 0; - status_ = false; -} - -//////////////////////////////////////////////////////////////////////////////// -// Destructor -GazeboRosVacuumGripper::~GazeboRosVacuumGripper() -{ - update_connection_.reset(); - - // Custom Callback Queue - queue_.clear(); - queue_.disable(); - rosnode_->shutdown(); - callback_queue_thread_.join(); - - delete rosnode_; -} - -//////////////////////////////////////////////////////////////////////////////// -// Load the controller -void GazeboRosVacuumGripper::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) -{ - ROS_INFO_NAMED("vacuum_gripper", "Loading gazebo_ros_vacuum_gripper"); - - // Set attached model; - parent_ = _model; - - // Get the world name. - world_ = _model->GetWorld(); - - // load parameters - robot_namespace_ = ""; - if (_sdf->HasElement("robotNamespace")) - robot_namespace_ = _sdf->GetElement("robotNamespace")->Get() + "/"; - - if (!_sdf->HasElement("bodyName")) - { - ROS_FATAL_NAMED("vacuum_gripper", "vacuum_gripper plugin missing , cannot proceed"); - return; - } - else - link_name_ = _sdf->GetElement("bodyName")->Get(); - - link_ = _model->GetLink(link_name_); - if (!link_) - { - std::string found; - physics::Link_V links = _model->GetLinks(); - for (size_t i = 0; i < links.size(); i++) { - found += std::string(" ") + links[i]->GetName(); - } - ROS_FATAL_NAMED("vacuum_gripper", "gazebo_ros_vacuum_gripper plugin error: link named: %s does not exist", link_name_.c_str()); - ROS_FATAL_NAMED("vacuum_gripper", "gazebo_ros_vacuum_gripper plugin error: You should check it exists and is not connected with fixed joint"); - ROS_FATAL_NAMED("vacuum_gripper", "gazebo_ros_vacuum_gripper plugin error: Found links are: %s", found.c_str()); - return; - } - - if (!_sdf->HasElement("topicName")) - { - ROS_FATAL_NAMED("vacuum_gripper", "vacuum_gripper plugin missing , cannot proceed"); - return; - } - else - topic_name_ = _sdf->GetElement("topicName")->Get(); - - // Make sure the ROS node for Gazebo has already been initialized - if (!ros::isInitialized()) - { - ROS_FATAL_STREAM_NAMED("vacuum_gripper", "A ROS node for Gazebo has not been initialized, unable to load plugin. " - << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); - return; - } - - rosnode_ = new ros::NodeHandle(robot_namespace_); - - // Custom Callback Queue - ros::AdvertiseOptions ao = ros::AdvertiseOptions::create( - topic_name_, 1, - boost::bind(&GazeboRosVacuumGripper::Connect, this), - boost::bind(&GazeboRosVacuumGripper::Disconnect, this), - ros::VoidPtr(), &queue_); - pub_ = rosnode_->advertise(ao); - - // Custom Callback Queue - ros::AdvertiseServiceOptions aso1 = - ros::AdvertiseServiceOptions::create( - "on", boost::bind(&GazeboRosVacuumGripper::OnServiceCallback, - this, _1, _2), ros::VoidPtr(), &queue_); - srv1_ = rosnode_->advertiseService(aso1); - ros::AdvertiseServiceOptions aso2 = - ros::AdvertiseServiceOptions::create( - "off", boost::bind(&GazeboRosVacuumGripper::OffServiceCallback, - this, _1, _2), ros::VoidPtr(), &queue_); - srv2_ = rosnode_->advertiseService(aso2); - - // Custom Callback Queue - callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosVacuumGripper::QueueThread,this ) ); - - // New Mechanism for Updating every World Cycle - // Listen to the update event. This event is broadcast every - // simulation iteration. - update_connection_ = event::Events::ConnectWorldUpdateBegin( - boost::bind(&GazeboRosVacuumGripper::UpdateChild, this)); - - ROS_INFO_NAMED("vacuum_gripper", "Loaded gazebo_ros_vacuum_gripper"); -} - -bool GazeboRosVacuumGripper::OnServiceCallback(std_srvs::Empty::Request &req, - std_srvs::Empty::Response &res) -{ - if (status_) { - ROS_WARN_NAMED("vacuum_gripper", "gazebo_ros_vacuum_gripper: already status is 'on'"); - } else { - status_ = true; - ROS_INFO_NAMED("vacuum_gripper", "gazebo_ros_vacuum_gripper: status: off -> on"); - } - return true; -} -bool GazeboRosVacuumGripper::OffServiceCallback(std_srvs::Empty::Request &req, - std_srvs::Empty::Response &res) -{ - if (status_) { - status_ = false; - ROS_INFO_NAMED("vacuum_gripper", "gazebo_ros_vacuum_gripper: status: on -> off"); - } else { - ROS_WARN_NAMED("vacuum_gripper", "gazebo_ros_vacuum_gripper: already status is 'off'"); - } - return true; -} - -//////////////////////////////////////////////////////////////////////////////// -// Update the controller -void GazeboRosVacuumGripper::UpdateChild() -{ - std_msgs::Bool grasping_msg; - grasping_msg.data = false; - if (!status_) { - pub_.publish(grasping_msg); - return; - } - // apply force - lock_.lock(); -#if GAZEBO_MAJOR_VERSION >= 8 - ignition::math::Pose3d parent_pose = link_->WorldPose(); - physics::Model_V models = world_->Models(); -#else - ignition::math::Pose3d parent_pose = link_->GetWorldPose().Ign(); - physics::Model_V models = world_->GetModels(); -#endif - for (size_t i = 0; i < models.size(); i++) { - if (models[i]->GetName() == link_->GetName() || - models[i]->GetName() == parent_->GetName()) - { - continue; - } - physics::Link_V links = models[i]->GetLinks(); - for (size_t j = 0; j < links.size(); j++) { -#if GAZEBO_MAJOR_VERSION >= 8 - ignition::math::Pose3d link_pose = links[j]->WorldPose(); -#else - ignition::math::Pose3d link_pose = links[j]->GetWorldPose().Ign(); -#endif - ignition::math::Pose3d diff = parent_pose - link_pose; - double norm = diff.Pos().Length(); - if (norm < 0.05) { -#if GAZEBO_MAJOR_VERSION >= 8 - links[j]->SetLinearVel(link_->WorldLinearVel()); - links[j]->SetAngularVel(link_->WorldAngularVel()); -#else - links[j]->SetLinearVel(link_->GetWorldLinearVel()); - links[j]->SetAngularVel(link_->GetWorldAngularVel()); -#endif - double norm_force = 1 / norm; - if (norm < 0.01) { - // apply friction like force - // TODO(unknown): should apply friction actually - link_pose.Set(parent_pose.Pos(), link_pose.Rot()); - links[j]->SetWorldPose(link_pose); - } - if (norm_force > 20) { - norm_force = 20; // max_force - } - ignition::math::Vector3d force = norm_force * diff.Pos().Normalize(); - links[j]->AddForce(force); - grasping_msg.data = true; - } - } - } - pub_.publish(grasping_msg); - lock_.unlock(); -} - -// Custom Callback Queue -//////////////////////////////////////////////////////////////////////////////// -// custom callback queue thread -void GazeboRosVacuumGripper::QueueThread() -{ - static const double timeout = 0.01; - - while (rosnode_->ok()) - { - queue_.callAvailable(ros::WallDuration(timeout)); - } -} - -//////////////////////////////////////////////////////////////////////////////// -// Someone subscribes to me -void GazeboRosVacuumGripper::Connect() -{ - this->connect_count_++; -} - -//////////////////////////////////////////////////////////////////////////////// -// Someone subscribes to me -void GazeboRosVacuumGripper::Disconnect() -{ - this->connect_count_--; -} - -} // namespace gazebo diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt index 7f5a56c64..78deec139 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -22,6 +22,7 @@ find_package(nav_msgs REQUIRED) find_package(rclcpp REQUIRED) find_package(sensor_msgs REQUIRED) find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) @@ -201,6 +202,19 @@ target_link_libraries(gazebo_ros_elevator ) ament_export_libraries(gazebo_ros_elevator) +# gazebo_ros_vacuum_gripper +add_library(gazebo_ros_vacuum_gripper SHARED + src/gazebo_ros_vacuum_gripper.cpp +) +ament_target_dependencies(gazebo_ros_vacuum_gripper + "gazebo_ros" + "gazebo_dev" + "rclcpp" + "std_msgs" + "std_srvs" +) +ament_export_libraries(gazebo_ros_vacuum_gripper) + ament_export_include_directories(include) ament_export_dependencies(rclcpp) ament_export_dependencies(gazebo_dev) @@ -229,6 +243,7 @@ install(TARGETS gazebo_ros_p3d gazebo_ros_template gazebo_ros_tricycle_drive + gazebo_ros_vacuum_gripper gazebo_ros_video ARCHIVE DESTINATION lib LIBRARY DESTINATION lib diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_vacuum_gripper.hpp b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_vacuum_gripper.hpp new file mode 100644 index 000000000..a9d8db480 --- /dev/null +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_vacuum_gripper.hpp @@ -0,0 +1,83 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GAZEBO_PLUGINS__GAZEBO_ROS_VACUUM_GRIPPER_HPP_ +#define GAZEBO_PLUGINS__GAZEBO_ROS_VACUUM_GRIPPER_HPP_ + +#include + +#include + +namespace gazebo_plugins +{ +class GazeboRosVacuumGripperPrivate; + +/// Vacuum Gripper plugin for attracting entities around the model like vacuum +/* + * \author Kentaro Wada + * + * \date 7 Dec 2015 + */ +/** + Example Usage: + \code{.xml} + + + + + + /demo + + + switch:=custom_switch + grasping:=custom_grasping + + + + link + + + 10.0 + 0.01 + + + 2 + + + ground_plane + wall + + + \endcode +*/ +class GazeboRosVacuumGripper : public gazebo::ModelPlugin +{ +public: + /// Constructor + GazeboRosVacuumGripper(); + + /// Destructor + ~GazeboRosVacuumGripper(); + +protected: + // Documentation inherited + void Load(gazebo::physics::ModelPtr _model, sdf::ElementPtr _sdf) override; + +private: + /// Private data pointer + std::unique_ptr impl_; +}; +} // namespace gazebo_plugins + +#endif // GAZEBO_PLUGINS__GAZEBO_ROS_VACUUM_GRIPPER_HPP_ diff --git a/gazebo_plugins/package.xml b/gazebo_plugins/package.xml index 63f313961..89672fb6b 100644 --- a/gazebo_plugins/package.xml +++ b/gazebo_plugins/package.xml @@ -26,6 +26,7 @@ nav_msgs sensor_msgs std_msgs + std_srvs tf2_geometry_msgs tf2_ros diff --git a/gazebo_plugins/src/gazebo_ros_vacuum_gripper.cpp b/gazebo_plugins/src/gazebo_ros_vacuum_gripper.cpp new file mode 100644 index 000000000..4f26da483 --- /dev/null +++ b/gazebo_plugins/src/gazebo_ros_vacuum_gripper.cpp @@ -0,0 +1,221 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * \brief Vacuum Gripper plugin for attracting entities around the model like vacuum + * + * \author Kentaro Wada + * + * \date 7 Dec 2015 + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace gazebo_plugins +{ +class GazeboRosVacuumGripperPrivate +{ +public: + /// Callback to be called at every simulation iteration. + void OnUpdate(); + + /// \brief Function to switch the gripper on/off. + /// \param[in] req Request + /// \param[out] res Response + void OnSwitch( + std_srvs::srv::SetBool::Request::SharedPtr req, + std_srvs::srv::SetBool::Response::SharedPtr res); + + /// A pointer to the GazeboROS node. + gazebo_ros::Node::SharedPtr ros_node_; + + /// Publisher for gripper action status + rclcpp::Publisher::SharedPtr pub_; + + /// Service for gripper switch + rclcpp::Service::SharedPtr service_; + + /// Connection to event called at every world iteration. + gazebo::event::ConnectionPtr update_connection_; + + /// Pointer to world. + gazebo::physics::WorldPtr world_; + + /// Pointer to link. + gazebo::physics::LinkPtr link_; + + /// Protect variables accessed on callbacks. + std::mutex lock_; + + /// True if gripper is on. + bool status_; + + /// Entities not affected by gripper. + std::unordered_set fixed_; + + /// Max distance to apply force. + double max_distance_; + + /// Min distance to apply force. + double min_distance_; + + /// Max applied force. + double max_force_; +}; + +GazeboRosVacuumGripper::GazeboRosVacuumGripper() +: impl_(std::make_unique()) +{ +} + +GazeboRosVacuumGripper::~GazeboRosVacuumGripper() +{ +} + +void GazeboRosVacuumGripper::Load(gazebo::physics::ModelPtr _model, sdf::ElementPtr _sdf) +{ + impl_->world_ = _model->GetWorld(); + + // Initialize ROS node + impl_->ros_node_ = gazebo_ros::Node::Get(_sdf); + + if (_sdf->HasElement("link_name")) { + auto link = _sdf->Get("link_name"); + impl_->link_ = _model->GetLink(link); + if (!impl_->link_) { + RCLCPP_ERROR(impl_->ros_node_->get_logger(), "Link [%s] not found. Aborting", link.c_str()); + impl_->ros_node_.reset(); + return; + } + } else { + RCLCPP_ERROR(impl_->ros_node_->get_logger(), "Please specify . Aborting."); + } + + impl_->max_distance_ = _sdf->Get("max_distance", 0.05).first; + impl_->min_distance_ = _sdf->Get("min_distance", 0.01).first; + impl_->max_force_ = _sdf->Get("max_force", 20).first; + + if (_sdf->HasElement("fixed")) { + for (auto fixed = _sdf->GetElement("fixed"); fixed != nullptr; + fixed = fixed->GetNextElement("fixed")) + { + auto name = fixed->Get(); + impl_->fixed_.insert(name); + RCLCPP_INFO(impl_->ros_node_->get_logger(), + "Model/Link [%s] exempted from gripper force", name.c_str()); + } + } + impl_->fixed_.insert(_model->GetName()); + impl_->fixed_.insert(impl_->link_->GetName()); + + // Initialize publisher + impl_->pub_ = impl_->ros_node_->create_publisher( + "grasping", rclcpp::QoS(rclcpp::KeepLast(1))); + + RCLCPP_INFO(impl_->ros_node_->get_logger(), + "Advertise gripper status on [%s]", impl_->pub_->get_topic_name()); + + // Initialize service + impl_->service_ = impl_->ros_node_->create_service("switch", + std::bind(&GazeboRosVacuumGripperPrivate::OnSwitch, impl_.get(), + std::placeholders::_1, std::placeholders::_2)); + + RCLCPP_INFO(impl_->ros_node_->get_logger(), + "Advertise gripper switch service on [%s]", impl_->service_->get_service_name()); + + // Listen to the update event (broadcast every simulation iteration) + impl_->update_connection_ = gazebo::event::Events::ConnectWorldUpdateBegin( + std::bind(&GazeboRosVacuumGripperPrivate::OnUpdate, impl_.get())); +} + +void GazeboRosVacuumGripperPrivate::OnUpdate() +{ + std_msgs::msg::Bool grasping_msg; + if (!status_) { + pub_->publish(grasping_msg); + return; + } + + std::lock_guard lock(lock_); + + ignition::math::Pose3d parent_pose = link_->WorldPose(); + gazebo::physics::Model_V models = world_->Models(); + + for (auto & model : models) { + if (fixed_.find(model->GetName()) != fixed_.end()) { + continue; + } + gazebo::physics::Link_V links = model->GetLinks(); + for (auto & link : links) { + ignition::math::Pose3d link_pose = link->WorldPose(); + ignition::math::Pose3d diff = parent_pose - link_pose; + double norm = diff.Pos().Length(); + + if (norm < max_distance_) { + link->SetLinearVel(link_->WorldLinearVel()); + link->SetAngularVel(link_->WorldAngularVel()); + + double norm_force = 1 / norm; + if (norm < min_distance_) { + link_pose.Set(parent_pose.Pos(), link_pose.Rot()); + link->SetWorldPose(link_pose); + } + + norm_force = std::min(norm_force, max_force_); // max_force + + ignition::math::Vector3d force = norm_force * diff.Pos().Normalize(); + link->AddForce(force); + grasping_msg.data = true; + } + } + } + pub_->publish(grasping_msg); +} + +void GazeboRosVacuumGripperPrivate::OnSwitch( + std_srvs::srv::SetBool::Request::SharedPtr req, + std_srvs::srv::SetBool::Response::SharedPtr res) +{ + res->success = false; + if (req->data) { + if (!status_) { + status_ = true; + res->success = true; + } else { + RCLCPP_WARN(ros_node_->get_logger(), "Gripper is already on"); + } + } else { + if (status_) { + status_ = false; + res->success = true; + } else { + RCLCPP_WARN(ros_node_->get_logger(), "Gripper is already off"); + } + } +} + +GZ_REGISTER_MODEL_PLUGIN(GazeboRosVacuumGripper) +} // namespace gazebo_plugins diff --git a/gazebo_plugins/test/CMakeLists.txt b/gazebo_plugins/test/CMakeLists.txt index ab54dd2c4..1492a4b5a 100644 --- a/gazebo_plugins/test/CMakeLists.txt +++ b/gazebo_plugins/test/CMakeLists.txt @@ -21,6 +21,7 @@ set(tests test_gazebo_ros_p3d test_gazebo_ros_ray_sensor test_gazebo_ros_tricycle_drive + test_gazebo_ros_vacuum_gripper ) if(ENABLE_DISPLAY_TESTS) @@ -55,6 +56,7 @@ foreach(test ${tests}) "rclcpp" "sensor_msgs" "std_msgs" + "std_srvs" ) endforeach() diff --git a/gazebo_plugins/test/test_gazebo_ros_vacuum_gripper.cpp b/gazebo_plugins/test/test_gazebo_ros_vacuum_gripper.cpp new file mode 100644 index 000000000..28fd2da6a --- /dev/null +++ b/gazebo_plugins/test/test_gazebo_ros_vacuum_gripper.cpp @@ -0,0 +1,97 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include + +#define tol 10e-2 + +class GazeboRosVacuumGripperTest : public gazebo::ServerFixture +{ +}; + +TEST_F(GazeboRosVacuumGripperTest, VacuumGripperServiceTest) +{ + // Load test world and start paused + this->Load("worlds/gazebo_ros_vacuum_gripper.world", true); + + // World + auto world = gazebo::physics::get_world(); + ASSERT_NE(nullptr, world); + + // Box + auto box = world->ModelByName("box"); + ASSERT_NE(nullptr, box); + + // Ball1 + auto ball1 = world->ModelByName("ball1"); + ASSERT_NE(nullptr, ball1); + + // Ball2 + auto ball2 = world->ModelByName("ball2"); + ASSERT_NE(nullptr, ball2); + + // Check plugin was loaded + world->Step(100); + EXPECT_EQ(1u, box->GetPluginCount()); + + // Check ball1 position + EXPECT_NEAR(0.6, ball1->WorldPose().Pos().X(), tol); + EXPECT_NEAR(0.0, ball1->WorldPose().Pos().Y(), tol); + + // Check ball2 position + EXPECT_NEAR(-0.6, ball2->WorldPose().Pos().X(), tol); + EXPECT_NEAR(0.0, ball2->WorldPose().Pos().Y(), tol); + + // Create ROS publisher + auto node = std::make_shared("gazebo_ros_vacuum_gripper_test"); + ASSERT_NE(nullptr, node); + + auto client = node->create_client("test/switch_test"); + ASSERT_NE(nullptr, client); + EXPECT_TRUE(client->wait_for_service(std::chrono::seconds(1))); + + auto request = std::make_shared(); + request->data = true; + auto response_future = client->async_send_request(request); + EXPECT_EQ(rclcpp::executor::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, response_future)); + + unsigned int sleep = 0; + unsigned int max_sleep = 200; + while (sleep < max_sleep && + (ball1->WorldPose().Pos().X() > 0.1 || ball2->WorldPose().Pos().Y() < -0.1)) + { + gazebo::common::Time::MSleep(100); + world->Step(100); + sleep++; + } + + // Check balls moved + EXPECT_LT(sleep, max_sleep); + EXPECT_LT(ball1->WorldPose().Pos().X(), 0.1); + EXPECT_NE(ball1->WorldPose().Pos().Y(), 0); + EXPECT_GT(ball2->WorldPose().Pos().X(), -0.1); + EXPECT_NE(ball2->WorldPose().Pos().Y(), 0); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/gazebo_plugins/test/worlds/gazebo_ros_vacuum_gripper.world b/gazebo_plugins/test/worlds/gazebo_ros_vacuum_gripper.world new file mode 100644 index 000000000..08f33b1eb --- /dev/null +++ b/gazebo_plugins/test/worlds/gazebo_ros_vacuum_gripper.world @@ -0,0 +1,110 @@ + + + + + + model://ground_plane + + + + 0 0 0.04 0 0 0 + + + + + 0.08 0.08 0.08 + + + + + + + 0.08 0.08 0.08 + + + + + + + + + /test + switch:=switch_test + grasping:=grasping_test + + + link + 10.0 + 0.01 + 2 + + ground_plane + + + + + + 0.6 0 0.04 0 0 0 + + + 0.026 + + 1.664e-5 + 1.664e-5 + 1.664e-5 + 0 + 0 + 0 + + + + + + 0.04 + + + + + + + 0.04 + + + + + + + + + -0.6 0 0.04 0 0 0 + + + 0.026 + + 1.664e-5 + 1.664e-5 + 1.664e-5 + 0 + 0 + 0 + + + + + + 0.04 + + + + + + + 0.04 + + + + + + + + diff --git a/gazebo_plugins/worlds/gazebo_ros_vacuum_gripper_demo.world b/gazebo_plugins/worlds/gazebo_ros_vacuum_gripper_demo.world new file mode 100644 index 000000000..aa0e53ca6 --- /dev/null +++ b/gazebo_plugins/worlds/gazebo_ros_vacuum_gripper_demo.world @@ -0,0 +1,188 @@ + + + + + + + model://ground_plane + + + + model://sun + + + + 0 0 0.04 0 0 0 + + + + + 0.08 0.08 0.08 + + + + + + + 0.08 0.08 0.08 + + + + + + + + + /demo + switch:=switch_demo + grasping:=grasping_demo + + + link + 10.0 + 0.01 + 2 + + ground_plane + + + + + + 0 -0.8 0.04 0 0 0 + + + 0.026 + + 1.664e-5 + 1.664e-5 + 1.664e-5 + 0 + 0 + 0 + + + + + + 0.04 + + + + + + + 0.04 + + + + + + + + + 0.8 0 0.04 0 0 0 + + + 0.026 + + 1.664e-5 + 1.664e-5 + 1.664e-5 + 0 + 0 + 0 + + + + + + 0.04 + + + + + + + 0.04 + + + + + + + + -0.8 0 0.04 0 0 0 + + + 0.026 + + 1.664e-5 + 1.664e-5 + 1.664e-5 + 0 + 0 + 0 + + + + + + 0.04 + + + + + + + 0.04 + + + + + + + + 0 0.8 0.04 0 0 0 + + + 0.026 + + 1.664e-5 + 1.664e-5 + 1.664e-5 + 0 + 0 + 0 + + + + + + 0.04 + + + + + + + 0.04 + + + + + + + + From 436a7fb969ba572a644d9c335ea6b62c83f18928 Mon Sep 17 00:00:00 2001 From: Shivesh Khaitan Date: Fri, 9 Aug 2019 21:54:31 +0530 Subject: [PATCH 2/3] Fix gripper forces --- .../gazebo_ros_vacuum_gripper.hpp | 7 ---- .../src/gazebo_ros_vacuum_gripper.cpp | 33 ++----------------- .../worlds/gazebo_ros_vacuum_gripper.world | 3 -- .../gazebo_ros_vacuum_gripper_demo.world | 3 -- 4 files changed, 2 insertions(+), 44 deletions(-) diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_vacuum_gripper.hpp b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_vacuum_gripper.hpp index a9d8db480..c3eb8f7b5 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_vacuum_gripper.hpp +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_vacuum_gripper.hpp @@ -47,13 +47,6 @@ class GazeboRosVacuumGripperPrivate; link - - 10.0 - 0.01 - - - 2 - ground_plane wall diff --git a/gazebo_plugins/src/gazebo_ros_vacuum_gripper.cpp b/gazebo_plugins/src/gazebo_ros_vacuum_gripper.cpp index 4f26da483..b9a41de8a 100644 --- a/gazebo_plugins/src/gazebo_ros_vacuum_gripper.cpp +++ b/gazebo_plugins/src/gazebo_ros_vacuum_gripper.cpp @@ -75,15 +75,6 @@ class GazeboRosVacuumGripperPrivate /// Entities not affected by gripper. std::unordered_set fixed_; - - /// Max distance to apply force. - double max_distance_; - - /// Min distance to apply force. - double min_distance_; - - /// Max applied force. - double max_force_; }; GazeboRosVacuumGripper::GazeboRosVacuumGripper() @@ -114,10 +105,6 @@ void GazeboRosVacuumGripper::Load(gazebo::physics::ModelPtr _model, sdf::Element RCLCPP_ERROR(impl_->ros_node_->get_logger(), "Please specify . Aborting."); } - impl_->max_distance_ = _sdf->Get("max_distance", 0.05).first; - impl_->min_distance_ = _sdf->Get("min_distance", 0.01).first; - impl_->max_force_ = _sdf->Get("max_force", 20).first; - if (_sdf->HasElement("fixed")) { for (auto fixed = _sdf->GetElement("fixed"); fixed != nullptr; fixed = fixed->GetNextElement("fixed")) @@ -172,24 +159,8 @@ void GazeboRosVacuumGripperPrivate::OnUpdate() for (auto & link : links) { ignition::math::Pose3d link_pose = link->WorldPose(); ignition::math::Pose3d diff = parent_pose - link_pose; - double norm = diff.Pos().Length(); - - if (norm < max_distance_) { - link->SetLinearVel(link_->WorldLinearVel()); - link->SetAngularVel(link_->WorldAngularVel()); - - double norm_force = 1 / norm; - if (norm < min_distance_) { - link_pose.Set(parent_pose.Pos(), link_pose.Rot()); - link->SetWorldPose(link_pose); - } - - norm_force = std::min(norm_force, max_force_); // max_force - - ignition::math::Vector3d force = norm_force * diff.Pos().Normalize(); - link->AddForce(force); - grasping_msg.data = true; - } + link->SetForce(link_pose.Rot().RotateVector((parent_pose - link_pose).Pos()).Normalize()); + grasping_msg.data = true; } } pub_->publish(grasping_msg); diff --git a/gazebo_plugins/test/worlds/gazebo_ros_vacuum_gripper.world b/gazebo_plugins/test/worlds/gazebo_ros_vacuum_gripper.world index 08f33b1eb..d56ab83b1 100644 --- a/gazebo_plugins/test/worlds/gazebo_ros_vacuum_gripper.world +++ b/gazebo_plugins/test/worlds/gazebo_ros_vacuum_gripper.world @@ -34,9 +34,6 @@ link - 10.0 - 0.01 - 2 ground_plane diff --git a/gazebo_plugins/worlds/gazebo_ros_vacuum_gripper_demo.world b/gazebo_plugins/worlds/gazebo_ros_vacuum_gripper_demo.world index aa0e53ca6..010f8fe35 100644 --- a/gazebo_plugins/worlds/gazebo_ros_vacuum_gripper_demo.world +++ b/gazebo_plugins/worlds/gazebo_ros_vacuum_gripper_demo.world @@ -50,9 +50,6 @@ link - 10.0 - 0.01 - 2 ground_plane From fa40b22d6c0446dbae3907d3b65b2dd9196a86b4 Mon Sep 17 00:00:00 2001 From: Shivesh Khaitan Date: Wed, 14 Aug 2019 14:21:27 +0530 Subject: [PATCH 3/3] Add option to set max_distance Change SetForce -> Add Force --- .../gazebo_plugins/gazebo_ros_vacuum_gripper.hpp | 3 +++ gazebo_plugins/src/gazebo_ros_vacuum_gripper.cpp | 10 +++++++++- .../test/worlds/gazebo_ros_vacuum_gripper.world | 2 ++ .../worlds/gazebo_ros_vacuum_gripper_demo.world | 2 ++ 4 files changed, 16 insertions(+), 1 deletion(-) diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_vacuum_gripper.hpp b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_vacuum_gripper.hpp index c3eb8f7b5..ca26f5c8e 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_vacuum_gripper.hpp +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_vacuum_gripper.hpp @@ -47,6 +47,9 @@ class GazeboRosVacuumGripperPrivate; link + + 10.0 + ground_plane wall diff --git a/gazebo_plugins/src/gazebo_ros_vacuum_gripper.cpp b/gazebo_plugins/src/gazebo_ros_vacuum_gripper.cpp index b9a41de8a..c3102f840 100644 --- a/gazebo_plugins/src/gazebo_ros_vacuum_gripper.cpp +++ b/gazebo_plugins/src/gazebo_ros_vacuum_gripper.cpp @@ -75,6 +75,9 @@ class GazeboRosVacuumGripperPrivate /// Entities not affected by gripper. std::unordered_set fixed_; + + /// Max distance to apply force. + double max_distance_; }; GazeboRosVacuumGripper::GazeboRosVacuumGripper() @@ -105,6 +108,8 @@ void GazeboRosVacuumGripper::Load(gazebo::physics::ModelPtr _model, sdf::Element RCLCPP_ERROR(impl_->ros_node_->get_logger(), "Please specify . Aborting."); } + impl_->max_distance_ = _sdf->Get("max_distance", 0.05).first; + if (_sdf->HasElement("fixed")) { for (auto fixed = _sdf->GetElement("fixed"); fixed != nullptr; fixed = fixed->GetNextElement("fixed")) @@ -159,7 +164,10 @@ void GazeboRosVacuumGripperPrivate::OnUpdate() for (auto & link : links) { ignition::math::Pose3d link_pose = link->WorldPose(); ignition::math::Pose3d diff = parent_pose - link_pose; - link->SetForce(link_pose.Rot().RotateVector((parent_pose - link_pose).Pos()).Normalize()); + if (diff.Pos().Length() > max_distance_) { + continue; + } + link->AddForce(link_pose.Rot().RotateVector((parent_pose - link_pose).Pos()).Normalize()); grasping_msg.data = true; } } diff --git a/gazebo_plugins/test/worlds/gazebo_ros_vacuum_gripper.world b/gazebo_plugins/test/worlds/gazebo_ros_vacuum_gripper.world index d56ab83b1..4937201b7 100644 --- a/gazebo_plugins/test/worlds/gazebo_ros_vacuum_gripper.world +++ b/gazebo_plugins/test/worlds/gazebo_ros_vacuum_gripper.world @@ -35,6 +35,8 @@ link + 5.0 + ground_plane diff --git a/gazebo_plugins/worlds/gazebo_ros_vacuum_gripper_demo.world b/gazebo_plugins/worlds/gazebo_ros_vacuum_gripper_demo.world index 010f8fe35..08c4e092e 100644 --- a/gazebo_plugins/worlds/gazebo_ros_vacuum_gripper_demo.world +++ b/gazebo_plugins/worlds/gazebo_ros_vacuum_gripper_demo.world @@ -51,6 +51,8 @@ link + 5.0 + ground_plane