From 9d64fcf5bd24b96339d23f7eb017b34a1e0e9b46 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 26 Jul 2018 18:22:02 -0700 Subject: [PATCH 1/5] move gazebo_ros_force files --- .../include/gazebo_plugins/gazebo_ros_force.hpp | 0 .../gazebo_plugins => gazebo_plugins}/src/gazebo_ros_force.cpp | 0 2 files changed, 0 insertions(+), 0 deletions(-) rename .ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_force.h => gazebo_plugins/include/gazebo_plugins/gazebo_ros_force.hpp (100%) rename {.ros1_unported/gazebo_plugins => gazebo_plugins}/src/gazebo_ros_force.cpp (100%) diff --git a/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_force.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_force.hpp similarity index 100% rename from .ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_force.h rename to gazebo_plugins/include/gazebo_plugins/gazebo_ros_force.hpp diff --git a/.ros1_unported/gazebo_plugins/src/gazebo_ros_force.cpp b/gazebo_plugins/src/gazebo_ros_force.cpp similarity index 100% rename from .ros1_unported/gazebo_plugins/src/gazebo_ros_force.cpp rename to gazebo_plugins/src/gazebo_ros_force.cpp From d8ce2522f4c708abd48b66f49b6bd5d9f304b3cf Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 26 Jul 2018 19:21:48 -0700 Subject: [PATCH 2/5] Convert plugin and add test world --- gazebo_plugins/CMakeLists.txt | 23 ++ .../gazebo_plugins/gazebo_ros_force.hpp | 163 +++++-------- .../gazebo_plugins/gazebo_ros_template.hpp | 2 +- gazebo_plugins/package.xml | 2 + gazebo_plugins/src/gazebo_ros_force.cpp | 216 +++++++----------- .../test/worlds/gazebo_ros_force.world | 38 +++ 6 files changed, 209 insertions(+), 235 deletions(-) create mode 100644 gazebo_plugins/test/worlds/gazebo_ros_force.world diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt index 5fbeac4a1..9f161ffc2 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -14,12 +14,30 @@ endif() find_package(ament_cmake REQUIRED) find_package(gazebo_dev REQUIRED) find_package(gazebo_ros REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(rclcpp REQUIRED) include_directories(include ${gazebo_ros_INCLUDE_DIRS} + ${geometry_msgs_INCLUDE_DIRS} ) +# gazebo_ros_force +add_library(gazebo_ros_force SHARED + src/gazebo_ros_force.cpp +) +target_link_libraries(gazebo_ros_force + ${gazebo_ros_LIBRARIES} + ${geometry_msgs_LIBRARIES} +) +ament_target_dependencies(gazebo_ros_force + "rclcpp" + "gazebo_dev" + "gazebo_ros" + "geometry_msgs" +) +ament_export_libraries(gazebo_ros_force) + # gazebo_ros_template add_library(gazebo_ros_template SHARED src/gazebo_ros_template.cpp @@ -46,6 +64,11 @@ ament_package() install(DIRECTORY include/ DESTINATION include) +install(TARGETS gazebo_ros_force + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + install(TARGETS gazebo_ros_template ARCHIVE DESTINATION lib LIBRARY DESTINATION lib diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_force.hpp b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_force.hpp index 1091bfa16..e7d9cc11a 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_force.hpp +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_force.hpp @@ -1,126 +1,83 @@ -/* - * Copyright (C) 2012-2014 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: A dynamic controller plugin that performs generic force interface. - * Author: John Hsu - * Date: 24 Sept 2008 - */ - -#ifndef GAZEBO_ROS_FORCE_HH -#define GAZEBO_ROS_FORCE_HH - -#include +// Copyright 2012 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_FORCE_HPP_ +#define GAZEBO_PLUGINS__GAZEBO_ROS_FORCE_HPP_ -// Custom Callback Queue -#include -#include -#include - -#include -#include -#include - -#include -#include #include -#include +#include +#include -namespace gazebo +namespace gazebo_plugins { -/// @addtogroup gazebo_dynamic_plugins Gazebo ROS Dynamic Plugins -/// @{ -/** \defgroup GazeboRosForce Plugin XML Reference and Example - - \brief Ros Force Plugin. +class GazeboRosForcePrivate; - This is a Plugin that collects data from a ROS topic and applies wrench to a body accordingly. +/// This plugin collects data from a ROS topic and applies wrench to a link accordingly. +/** + \details The last received force will be continuously added to the link at every simulation iteration. + Send an empty / zero message to stop applying a force. Example Usage: \verbatim - - - box_body - box_force - - - \endverbatim + -\{ -*/ + -/** - . + + force_test_node -*/ + + /test -class GazeboRosForce : public ModelPlugin -{ - /// \brief Constructor - public: GazeboRosForce(); + + gazebo_ros_force:=force_test - /// \brief Destructor - public: virtual ~GazeboRosForce(); + - // Documentation inherited - protected: void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); - - // Documentation inherited - protected: virtual void UpdateChild(); - - /// \brief call back when a Wrench message is published - /// \param[in] _msg The Incoming ROS message representing the new force to exert. - private: void UpdateObjectForce(const geometry_msgs::Wrench::ConstPtr& _msg); - - /// \brief The custom callback queue thread function. - private: void QueueThread(); - - /// \brief A pointer to the gazebo world. - private: physics::WorldPtr world_; + + link - /// \brief A pointer to the Link, where force is applied - private: physics::LinkPtr link_; + + \endverbatim +*/ - /// \brief A pointer to the ROS node. A node will be instantiated if it does not exist. - private: ros::NodeHandle* rosnode_; - private: ros::Subscriber sub_; +class GazeboRosForce : public gazebo::ModelPlugin +{ +public: + /// Constructor + GazeboRosForce(); - /// \brief A mutex to lock access to fields that are used in ROS message callbacks - private: boost::mutex lock_; + /// Destructor + virtual ~GazeboRosForce(); - /// \brief ROS Wrench topic name inputs - private: std::string topic_name_; - /// \brief The Link this plugin is attached to, and will exert forces on. - private: std::string link_name_; +protected: + // Documentation inherited + void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) override; - /// \brief for setting ROS name space - private: std::string robot_namespace_; + /// Optional callback to be called at every simulation iteration. + virtual void OnUpdate(); - // Custom Callback Queue - private: ros::CallbackQueue queue_; - /// \brief Thead object for the running callback Thread. - private: boost::thread callback_queue_thread_; - /// \brief Container for the wrench force that this plugin exerts on the body. - private: geometry_msgs::Wrench wrench_msg_; +private: + /// Callback when a ROS Wrench message is received + /// \param[in] msg The Incoming ROS message representing the new force to + /// exert. + void OnRosWrenchMsg(const geometry_msgs::msg::Wrench::SharedPtr msg); - // Pointer to the update event connection - private: event::ConnectionPtr update_connection_; + /// Private data pointer + std::unique_ptr impl_; }; -/** \} */ -/// @} -} -#endif +} // namespace gazebo_plugins + +#endif // GAZEBO_PLUGINS__GAZEBO_ROS_FORCE_HPP_ diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_template.hpp b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_template.hpp index 4e257e72d..58d09cb9b 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_template.hpp +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_template.hpp @@ -42,7 +42,7 @@ class GazeboRosTemplate : public gazebo::ModelPlugin /// such as `gazebo::sensors::SensorPtr`, `gazebo::physics::WorldPtr`, /// `gazebo::rendering::VisualPtr`, etc. /// \param[in] sdf SDF element containing user-defined parameters. - void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf); + void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) override; protected: /// Optional callback to be called at every simulation iteration. diff --git a/gazebo_plugins/package.xml b/gazebo_plugins/package.xml index fbde6569a..fecc0a143 100644 --- a/gazebo_plugins/package.xml +++ b/gazebo_plugins/package.xml @@ -21,10 +21,12 @@ gazebo_dev gazebo_ros + geometry_msgs rclcpp gazebo_dev gazebo_ros + geometry_msgs rclcpp ament_lint_auto diff --git a/gazebo_plugins/src/gazebo_ros_force.cpp b/gazebo_plugins/src/gazebo_ros_force.cpp index 769f14e42..eb628576f 100644 --- a/gazebo_plugins/src/gazebo_ros_force.cpp +++ b/gazebo_plugins/src/gazebo_ros_force.cpp @@ -1,160 +1,114 @@ -/* - * Copyright 2013 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: GazeboRosForce plugin for manipulating objects in Gazebo - Author: John Hsu - Date: 24 Sept 2008 - */ - -#include -#include - -#include - -namespace gazebo +// Copyright 2013 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 +#include +#include + +#include +#include + +namespace gazebo_plugins { -GZ_REGISTER_MODEL_PLUGIN(GazeboRosForce); +class GazeboRosForcePrivate +{ +public: + /// A pointer to the Link, where force is applied + gazebo::physics::LinkPtr link_; + + /// A pointer to the GazeboROS node. + gazebo_ros::Node::SharedPtr ros_node_; + + /// Wrench subscriber + rclcpp::Subscription::SharedPtr wrench_sub_; + + /// Container for the wrench force that this plugin exerts on the body. + geometry_msgs::msg::Wrench wrench_msg_; + + // Pointer to the update event connection + gazebo::event::ConnectionPtr update_connection_; +}; -//////////////////////////////////////////////////////////////////////////////// -// Constructor GazeboRosForce::GazeboRosForce() +: impl_(std::make_unique()) { - this->wrench_msg_.force.x = 0; - this->wrench_msg_.force.y = 0; - this->wrench_msg_.force.z = 0; - this->wrench_msg_.torque.x = 0; - this->wrench_msg_.torque.y = 0; - this->wrench_msg_.torque.z = 0; + impl_->wrench_msg_.force.x = 0.0; + impl_->wrench_msg_.force.y = 0.0; + impl_->wrench_msg_.force.z = 0.0; + impl_->wrench_msg_.torque.x = 0.0; + impl_->wrench_msg_.torque.y = 0.0; + impl_->wrench_msg_.torque.z = 0.0; } -//////////////////////////////////////////////////////////////////////////////// -// Destructor GazeboRosForce::~GazeboRosForce() { - this->update_connection_.reset(); - - // Custom Callback Queue - this->queue_.clear(); - this->queue_.disable(); - this->rosnode_->shutdown(); - this->callback_queue_thread_.join(); - - delete this->rosnode_; } -//////////////////////////////////////////////////////////////////////////////// -// Load the controller -void GazeboRosForce::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) +void GazeboRosForce::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) { - // Get the world name. - this->world_ = _model->GetWorld(); - - // load parameters - this->robot_namespace_ = ""; - if (_sdf->HasElement("robotNamespace")) - this->robot_namespace_ = _sdf->GetElement("robotNamespace")->Get() + "/"; + auto logger = rclcpp::get_logger("gazebo_ros_force"); - if (!_sdf->HasElement("bodyName")) - { - ROS_FATAL_NAMED("force", "force plugin missing , cannot proceed"); + // Target link + if (!sdf->HasElement("link_name")) { + RCLCPP_ERROR(logger, "Force plugin missing , cannot proceed"); return; } - else - this->link_name_ = _sdf->GetElement("bodyName")->Get(); - this->link_ = _model->GetLink(this->link_name_); - if (!this->link_) - { - ROS_FATAL_NAMED("force", "gazebo_ros_force plugin error: link named: %s does not exist\n",this->link_name_.c_str()); - return; - } + auto link_name = sdf->GetElement("link_name")->Get(); - if (!_sdf->HasElement("topicName")) - { - ROS_FATAL_NAMED("force", "force plugin missing , cannot proceed"); + impl_->link_ = model->GetLink(link_name); + if (!impl_->link_) { + RCLCPP_ERROR(logger, "Link named: %s does not exist\n", link_name.c_str()); return; } - else - this->topic_name_ = _sdf->GetElement("topicName")->Get(); + // Subscribe to wrench messages + impl_->ros_node_ = gazebo_ros::Node::Create("gazebo_ros_force", sdf); - // Make sure the ROS node for Gazebo has already been initialized - if (!ros::isInitialized()) - { - ROS_FATAL_STREAM_NAMED("force", "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; - } - - this->rosnode_ = new ros::NodeHandle(this->robot_namespace_); - - // Custom Callback Queue - ros::SubscribeOptions so = ros::SubscribeOptions::create( - this->topic_name_,1, - boost::bind( &GazeboRosForce::UpdateObjectForce,this,_1), - ros::VoidPtr(), &this->queue_); - this->sub_ = this->rosnode_->subscribe(so); - - // Custom Callback Queue - this->callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosForce::QueueThread,this ) ); - - // New Mechanism for Updating every World Cycle - // Listen to the update event. This event is broadcast every - // simulation iteration. - this->update_connection_ = event::Events::ConnectWorldUpdateBegin( - boost::bind(&GazeboRosForce::UpdateChild, this)); -} + impl_->wrench_sub_ = impl_->ros_node_->create_subscription( + "gazebo_ros_force", std::bind(&GazeboRosForce::OnRosWrenchMsg, this, + std::placeholders::_1)); -//////////////////////////////////////////////////////////////////////////////// -// Update the controller -void GazeboRosForce::UpdateObjectForce(const geometry_msgs::Wrench::ConstPtr& _msg) -{ - this->wrench_msg_.force.x = _msg->force.x; - this->wrench_msg_.force.y = _msg->force.y; - this->wrench_msg_.force.z = _msg->force.z; - this->wrench_msg_.torque.x = _msg->torque.x; - this->wrench_msg_.torque.y = _msg->torque.y; - this->wrench_msg_.torque.z = _msg->torque.z; + // Callback on every iteration + impl_->update_connection_ = gazebo::event::Events::ConnectWorldUpdateBegin( + std::bind(&GazeboRosForce::OnUpdate, this)); } -//////////////////////////////////////////////////////////////////////////////// -// Update the controller -void GazeboRosForce::UpdateChild() +void GazeboRosForce::OnRosWrenchMsg(const geometry_msgs::msg::Wrench::SharedPtr msg) { - this->lock_.lock(); - ignition::math::Vector3d force(this->wrench_msg_.force.x,this->wrench_msg_.force.y,this->wrench_msg_.force.z); - ignition::math::Vector3d torque(this->wrench_msg_.torque.x,this->wrench_msg_.torque.y,this->wrench_msg_.torque.z); - this->link_->AddForce(force); - this->link_->AddTorque(torque); - this->lock_.unlock(); + impl_->wrench_msg_.force.x = msg->force.x; + impl_->wrench_msg_.force.y = msg->force.y; + impl_->wrench_msg_.force.z = msg->force.z; + impl_->wrench_msg_.torque.x = msg->torque.x; + impl_->wrench_msg_.torque.y = msg->torque.y; + impl_->wrench_msg_.torque.z = msg->torque.z; } -// Custom Callback Queue -//////////////////////////////////////////////////////////////////////////////// -// custom callback queue thread -void GazeboRosForce::QueueThread() +void GazeboRosForce::OnUpdate() { - static const double timeout = 0.01; - - while (this->rosnode_->ok()) - { - this->queue_.callAvailable(ros::WallDuration(timeout)); - } + ignition::math::Vector3d force(impl_->wrench_msg_.force.x, + impl_->wrench_msg_.force.y, + impl_->wrench_msg_.force.z); + ignition::math::Vector3d torque(impl_->wrench_msg_.torque.x, + impl_->wrench_msg_.torque.y, + impl_->wrench_msg_.torque.z); + impl_->link_->AddForce(force); + impl_->link_->AddTorque(torque); } -} +GZ_REGISTER_MODEL_PLUGIN(GazeboRosForce) +} // namespace gazebo_plugins diff --git a/gazebo_plugins/test/worlds/gazebo_ros_force.world b/gazebo_plugins/test/worlds/gazebo_ros_force.world new file mode 100644 index 000000000..538e87df7 --- /dev/null +++ b/gazebo_plugins/test/worlds/gazebo_ros_force.world @@ -0,0 +1,38 @@ + + + + + model://ground_plane + + + model://sun + + + 0 0 0.5 0 0 0 + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + + force_test_node + /test + gazebo_ros_force:=force_test + + link + + + + From 3e1a82b485c4f8c06d7671e0e1ba2cab7f0c2a60 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Fri, 27 Jul 2018 15:40:58 -0700 Subject: [PATCH 3/5] conversions --- gazebo_plugins/src/gazebo_ros_force.cpp | 21 ++---- gazebo_ros/CMakeLists.txt | 7 +- gazebo_ros/include/gazebo_ros/conversions.hpp | 69 +++++++++++++++++++ gazebo_ros/test/CMakeLists.txt | 1 + gazebo_ros/test/test_conversions.cpp | 42 +++++++++++ 5 files changed, 124 insertions(+), 16 deletions(-) create mode 100644 gazebo_ros/include/gazebo_ros/conversions.hpp create mode 100644 gazebo_ros/test/test_conversions.cpp diff --git a/gazebo_plugins/src/gazebo_ros_force.cpp b/gazebo_plugins/src/gazebo_ros_force.cpp index eb628576f..653d7bc5b 100644 --- a/gazebo_plugins/src/gazebo_ros_force.cpp +++ b/gazebo_plugins/src/gazebo_ros_force.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #include #include @@ -46,12 +47,10 @@ class GazeboRosForcePrivate GazeboRosForce::GazeboRosForce() : impl_(std::make_unique()) { - impl_->wrench_msg_.force.x = 0.0; - impl_->wrench_msg_.force.y = 0.0; - impl_->wrench_msg_.force.z = 0.0; - impl_->wrench_msg_.torque.x = 0.0; - impl_->wrench_msg_.torque.y = 0.0; - impl_->wrench_msg_.torque.z = 0.0; + impl_->wrench_msg_.force = + gazebo_ros::Convert(ignition::math::Vector3d::Zero); + impl_->wrench_msg_.torque = + gazebo_ros::Convert(ignition::math::Vector3d::Zero); } GazeboRosForce::~GazeboRosForce() @@ -100,14 +99,8 @@ void GazeboRosForce::OnRosWrenchMsg(const geometry_msgs::msg::Wrench::SharedPtr void GazeboRosForce::OnUpdate() { - ignition::math::Vector3d force(impl_->wrench_msg_.force.x, - impl_->wrench_msg_.force.y, - impl_->wrench_msg_.force.z); - ignition::math::Vector3d torque(impl_->wrench_msg_.torque.x, - impl_->wrench_msg_.torque.y, - impl_->wrench_msg_.torque.z); - impl_->link_->AddForce(force); - impl_->link_->AddTorque(torque); + impl_->link_->AddForce(gazebo_ros::Convert(impl_->wrench_msg_.force)); + impl_->link_->AddTorque(gazebo_ros::Convert(impl_->wrench_msg_.torque)); } GZ_REGISTER_MODEL_PLUGIN(GazeboRosForce) diff --git a/gazebo_ros/CMakeLists.txt b/gazebo_ros/CMakeLists.txt index 1ec293700..361a7098f 100644 --- a/gazebo_ros/CMakeLists.txt +++ b/gazebo_ros/CMakeLists.txt @@ -16,11 +16,14 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(gazebo_dev REQUIRED) -include_directories(include) +include_directories( + include + ${gazebo_dev_INCLUDE_DIRS} +) add_library(gazebo_ros_node SHARED - src/node.cpp src/executor.cpp + src/node.cpp ) ament_target_dependencies(gazebo_ros_node "rclcpp" diff --git a/gazebo_ros/include/gazebo_ros/conversions.hpp b/gazebo_ros/include/gazebo_ros/conversions.hpp new file mode 100644 index 000000000..76f36514e --- /dev/null +++ b/gazebo_ros/include/gazebo_ros/conversions.hpp @@ -0,0 +1,69 @@ +// Copyright 2018 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_ROS__CONVERSIONS_HPP_ +#define GAZEBO_ROS__CONVERSIONS_HPP_ + +#include +#include + +namespace gazebo_ros +{ +/// Generic conversion from a ROS geometry vector message to another type. +/// \param[in] in Input message. +/// \return Conversion result +/// \tparam OUT Output type +template +OUT Convert(const geometry_msgs::msg::Vector3 & in) +{ + return OUT(); +} + +/// \brief Specialized conversion from a ROS vector message to an Ignition Math vector. +/// \param[in] msg ROS message to convert. +/// \return An Ignition Math vector. +template<> +ignition::math::Vector3d Convert(const geometry_msgs::msg::Vector3 & msg) +{ + ignition::math::Vector3d vec; + vec.X(msg.x); + vec.Y(msg.y); + vec.Z(msg.z); + return vec; +} + +/// Generic conversion from an Ignition Math vector to another type. +/// \param[in] in Input vector. +/// \return Conversion result +/// \tparam OUT Output type +template +OUT Convert(const ignition::math::Vector3d & in) +{ + return OUT(); +} + +/// \brief Specialized conversion from an Ignition Math vector to a ROS message. +/// \param[in] vec Ignition vector to convert. +/// \return ROS geometry vector message +template<> +geometry_msgs::msg::Vector3 Convert(const ignition::math::Vector3d & vec) +{ + geometry_msgs::msg::Vector3 msg; + msg.x = vec.X(); + msg.y = vec.Y(); + msg.z = vec.Z(); + return msg; +} +} // namespace gazebo_ros +#endif // GAZEBO_ROS__CONVERSIONS_HPP_ diff --git a/gazebo_ros/test/CMakeLists.txt b/gazebo_ros/test/CMakeLists.txt index f320b2224..8fe04c469 100644 --- a/gazebo_ros/test/CMakeLists.txt +++ b/gazebo_ros/test/CMakeLists.txt @@ -25,6 +25,7 @@ file(COPY worlds DESTINATION .) # Tests set(tests + test_conversions test_plugins ) diff --git a/gazebo_ros/test/test_conversions.cpp b/gazebo_ros/test/test_conversions.cpp new file mode 100644 index 000000000..0edc2ca74 --- /dev/null +++ b/gazebo_ros/test/test_conversions.cpp @@ -0,0 +1,42 @@ +// Copyright 2018 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 + +TEST(TestConversions, RosVectorIgnVector) +{ + // Ignition + ignition::math::Vector3d vec(1.0, 2.0, 3.0); + + // Ign to ROS + auto msg = gazebo_ros::Convert(vec); + + EXPECT_EQ(1.0, msg.x); + EXPECT_EQ(2.0, msg.y); + EXPECT_EQ(3.0, msg.z); + + // ROS to Ign + vec = gazebo_ros::Convert(msg); + + EXPECT_EQ(1.0, vec.X()); + EXPECT_EQ(2.0, vec.Y()); + EXPECT_EQ(3.0, vec.Z()); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From 03964645c4abf174d3da4977ef93b6f5fca748a5 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Fri, 27 Jul 2018 17:04:06 -0700 Subject: [PATCH 4/5] Add test using ServerFixture --- gazebo_plugins/CMakeLists.txt | 1 + gazebo_plugins/test/CMakeLists.txt | 30 +++++++ gazebo_plugins/test/test_gazebo_ros_force.cpp | 80 +++++++++++++++++++ .../test/worlds/gazebo_ros_force.world | 2 +- .../worlds/gazebo_ros_force_demo.world | 45 +++++++++++ 5 files changed, 157 insertions(+), 1 deletion(-) create mode 100644 gazebo_plugins/test/CMakeLists.txt create mode 100644 gazebo_plugins/test/test_gazebo_ros_force.cpp create mode 100644 gazebo_plugins/worlds/gazebo_ros_force_demo.world diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt index 9f161ffc2..61ee028c4 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -55,6 +55,7 @@ ament_export_dependencies(gazebo_dev) ament_export_dependencies(gazebo_ros) if(BUILD_TESTING) + add_subdirectory(test) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() endif() diff --git a/gazebo_plugins/test/CMakeLists.txt b/gazebo_plugins/test/CMakeLists.txt new file mode 100644 index 000000000..2baebd282 --- /dev/null +++ b/gazebo_plugins/test/CMakeLists.txt @@ -0,0 +1,30 @@ +find_package(ament_cmake_gtest REQUIRED) + +# Worlds +file(COPY worlds DESTINATION .) + +# Tests +set(tests + test_gazebo_ros_force +) + +foreach(test ${tests}) + ament_add_gtest(${test} + ${test}.cpp + WORKING_DIRECTORY + ${CMAKE_CURRENT_BINARY_DIR} + # Long timeout because has to run gazebo several times + TIMEOUT + 120 + ) + target_link_libraries(${test} + gazebo_test_fixture + ) + ament_target_dependencies(${test} + "rclcpp" + "gazebo_dev" + "gazebo_ros" + "geometry_msgs" + ) +endforeach() + diff --git a/gazebo_plugins/test/test_gazebo_ros_force.cpp b/gazebo_plugins/test/test_gazebo_ros_force.cpp new file mode 100644 index 000000000..286c42973 --- /dev/null +++ b/gazebo_plugins/test/test_gazebo_ros_force.cpp @@ -0,0 +1,80 @@ +// Copyright 2018 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-10 + +class GazeboRosForceTest : public gazebo::ServerFixture +{ +}; + +TEST_F(GazeboRosForceTest, ApplyForceTorque) +{ + // Load test world and start paused + this->Load("worlds/gazebo_ros_force.world", true); + + // World + auto world = gazebo::physics::get_world(); + ASSERT_NE(nullptr, world); + + // Box + auto box = world->ModelByName("box"); + ASSERT_NE(nullptr, box); + + // Check plugin was loaded + world->Step(100); + EXPECT_EQ(1u, box->GetPluginCount()); + + // Check box is at world origin + EXPECT_NEAR(0.0, box->WorldPose().Pos().X(), tol); + EXPECT_NEAR(0.0, box->WorldPose().Rot().Z(), tol); + + // Create ROS publisher + auto node = std::make_shared("gazebo_ros_force_test"); + ASSERT_NE(nullptr, node); + + auto pub = node->create_publisher("test/force_test"); + + // Apply force on X + auto msg = geometry_msgs::msg::Wrench(); + msg.force.x = 10.0; + msg.torque.z = 10.0; + pub->publish(msg); + + // Wait until box moves + unsigned int sleep = 0; + unsigned int max_sleep = 30; + while (sleep < max_sleep && abs(box->WorldPose().Pos().X()) < tol) { + world->Step(100); + gazebo::common::Time::MSleep(100); + sleep++; + } + + // Check box moved + EXPECT_LT(sleep, max_sleep); + EXPECT_LT(0.0, box->WorldPose().Pos().X()); + EXPECT_LT(0.0, box->WorldPose().Rot().Z()); +} + +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_force.world b/gazebo_plugins/test/worlds/gazebo_ros_force.world index 538e87df7..699ccc8b8 100644 --- a/gazebo_plugins/test/worlds/gazebo_ros_force.world +++ b/gazebo_plugins/test/worlds/gazebo_ros_force.world @@ -25,7 +25,7 @@ - + force_test_node /test diff --git a/gazebo_plugins/worlds/gazebo_ros_force_demo.world b/gazebo_plugins/worlds/gazebo_ros_force_demo.world new file mode 100644 index 000000000..7d8af8713 --- /dev/null +++ b/gazebo_plugins/worlds/gazebo_ros_force_demo.world @@ -0,0 +1,45 @@ + + + + + + model://ground_plane + + + model://sun + + + 0 0 0.5 0 0 0 + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + + force_demo_node + /demo + gazebo_ros_force:=force_demo + + link + + + + From 0cc0e06f4aaca8b9cabd025dde907402afc9fc7a Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Fri, 27 Jul 2018 18:08:56 -0700 Subject: [PATCH 5/5] PR feedback --- gazebo_plugins/CMakeLists.txt | 14 ++++---------- .../include/gazebo_plugins/gazebo_ros_force.hpp | 4 ++-- gazebo_plugins/src/gazebo_ros_force.cpp | 4 ---- gazebo_plugins/test/test_gazebo_ros_force.cpp | 7 ++++--- gazebo_ros/CMakeLists.txt | 10 ++++------ 5 files changed, 14 insertions(+), 25 deletions(-) diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt index 61ee028c4..a1fc730cf 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -26,10 +26,6 @@ include_directories(include add_library(gazebo_ros_force SHARED src/gazebo_ros_force.cpp ) -target_link_libraries(gazebo_ros_force - ${gazebo_ros_LIBRARIES} - ${geometry_msgs_LIBRARIES} -) ament_target_dependencies(gazebo_ros_force "rclcpp" "gazebo_dev" @@ -65,12 +61,10 @@ ament_package() install(DIRECTORY include/ DESTINATION include) -install(TARGETS gazebo_ros_force - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin) - -install(TARGETS gazebo_ros_template +install( + TARGETS + gazebo_ros_force + gazebo_ros_template ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin) diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_force.hpp b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_force.hpp index e7d9cc11a..1d279a96c 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_force.hpp +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_force.hpp @@ -30,7 +30,7 @@ class GazeboRosForcePrivate; Send an empty / zero message to stop applying a force. Example Usage: - \verbatim + \code{.xml} @@ -50,7 +50,7 @@ class GazeboRosForcePrivate; link - \endverbatim + \endcode */ class GazeboRosForce : public gazebo::ModelPlugin diff --git a/gazebo_plugins/src/gazebo_ros_force.cpp b/gazebo_plugins/src/gazebo_ros_force.cpp index 653d7bc5b..ffe04c3b5 100644 --- a/gazebo_plugins/src/gazebo_ros_force.cpp +++ b/gazebo_plugins/src/gazebo_ros_force.cpp @@ -47,10 +47,6 @@ class GazeboRosForcePrivate GazeboRosForce::GazeboRosForce() : impl_(std::make_unique()) { - impl_->wrench_msg_.force = - gazebo_ros::Convert(ignition::math::Vector3d::Zero); - impl_->wrench_msg_.torque = - gazebo_ros::Convert(ignition::math::Vector3d::Zero); } GazeboRosForce::~GazeboRosForce() diff --git a/gazebo_plugins/test/test_gazebo_ros_force.cpp b/gazebo_plugins/test/test_gazebo_ros_force.cpp index 286c42973..9fec0650d 100644 --- a/gazebo_plugins/test/test_gazebo_ros_force.cpp +++ b/gazebo_plugins/test/test_gazebo_ros_force.cpp @@ -58,9 +58,10 @@ TEST_F(GazeboRosForceTest, ApplyForceTorque) pub->publish(msg); // Wait until box moves + double target_dist{0.1}; unsigned int sleep = 0; unsigned int max_sleep = 30; - while (sleep < max_sleep && abs(box->WorldPose().Pos().X()) < tol) { + while (sleep < max_sleep && box->WorldPose().Pos().X() < target_dist) { world->Step(100); gazebo::common::Time::MSleep(100); sleep++; @@ -68,8 +69,8 @@ TEST_F(GazeboRosForceTest, ApplyForceTorque) // Check box moved EXPECT_LT(sleep, max_sleep); - EXPECT_LT(0.0, box->WorldPose().Pos().X()); - EXPECT_LT(0.0, box->WorldPose().Rot().Z()); + EXPECT_LT(target_dist, box->WorldPose().Pos().X()); + EXPECT_LT(target_dist, box->WorldPose().Rot().Z()); } int main(int argc, char ** argv) diff --git a/gazebo_ros/CMakeLists.txt b/gazebo_ros/CMakeLists.txt index 361a7098f..99a4849c6 100644 --- a/gazebo_ros/CMakeLists.txt +++ b/gazebo_ros/CMakeLists.txt @@ -55,12 +55,10 @@ ament_package() install(DIRECTORY include/ DESTINATION include) -install(TARGETS gazebo_ros_node - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin) - -install(TARGETS gazebo_ros_init +install( + TARGETS + gazebo_ros_node + gazebo_ros_init ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin)