From 0ea51140b68ae8f78963f185c5646ed7881631bc Mon Sep 17 00:00:00 2001
From: Alex Fernandes Neves <alexfneves@gmail.com>
Date: Wed, 14 Aug 2019 16:48:19 -0300
Subject: [PATCH 1/5] Adding option to select the frame where the force will be
 applied

A new parameter was added on the plugin with the options 'world' and 'link' frame.
The default value is 'world'.
Internally the AddRelativeForce() and torque functions are used instead of the AddForce() when the body option is selected.
---
 .../gazebo_plugins/gazebo_ros_force.hpp       |  6 +++++
 gazebo_plugins/src/gazebo_ros_force.cpp       | 27 ++++++++++++++++++-
 .../test/worlds/gazebo_ros_force.world        |  5 ++--
 .../worlds/gazebo_ros_force_demo.world        |  6 +++++
 4 files changed, 41 insertions(+), 3 deletions(-)

diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_force.hpp b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_force.hpp
index ae77e5cd6..27290dc4c 100644
--- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_force.hpp
+++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_force.hpp
@@ -46,6 +46,9 @@ class GazeboRosForcePrivate;
       <!-- Name of link within model which will receive the force -->
       <link_name>link</link_name>
 
+      <!-- Frame where the force/torque will be applied (options: world; body)-->
+      <force_frame>link</force_frame>
+
     </plugin>
   \endcode
 */
@@ -66,6 +69,9 @@ class GazeboRosForce : public gazebo::ModelPlugin
   /// Optional callback to be called at every simulation iteration.
   virtual void OnUpdate();
 
+  /// Optional callback to be called at every simulation iteration when the force is applied on the body frame.
+  virtual void OnUpdateRelative();
+
 private:
   /// Callback when a ROS Wrench message is received
   /// \param[in] msg The Incoming ROS message representing the new force to
diff --git a/gazebo_plugins/src/gazebo_ros_force.cpp b/gazebo_plugins/src/gazebo_ros_force.cpp
index 3abfe9d0e..21e82b341 100644
--- a/gazebo_plugins/src/gazebo_ros_force.cpp
+++ b/gazebo_plugins/src/gazebo_ros_force.cpp
@@ -72,6 +72,24 @@ void GazeboRosForce::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf)
     return;
   }
 
+  // Force frame
+  bool force_on_world_frame;
+  if (!sdf->HasElement("force_frame")) {
+    RCLCPP_WARN(logger, "Force plugin missing <force_frame> wasn't set, therefore it's been set as 'world'. The other option is 'link'.");
+    force_on_world_frame = true;
+  }
+  else {
+    auto force_frame = sdf->GetElement("force_frame")->Get<std::string>();
+    if (force_frame == "world")
+      force_on_world_frame = true;
+    else if (force_frame == "link")
+      force_on_world_frame = false;
+    else {
+      RCLCPP_ERROR(logger, "Force plugin <force_frame> can only be 'world' or 'link'");
+      return;
+    }
+  }
+
   // Subscribe to wrench messages
   impl_->ros_node_ = gazebo_ros::Node::Get(sdf);
 
@@ -81,7 +99,8 @@ void GazeboRosForce::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf)
 
   // Callback on every iteration
   impl_->update_connection_ = gazebo::event::Events::ConnectWorldUpdateBegin(
-    std::bind(&GazeboRosForce::OnUpdate, this));
+    force_on_world_frame ? std::bind(&GazeboRosForce::OnUpdate, this)
+      : std::bind(&GazeboRosForce::OnUpdateRelative, this));
 }
 
 void GazeboRosForce::OnRosWrenchMsg(const geometry_msgs::msg::Wrench::SharedPtr msg)
@@ -100,5 +119,11 @@ void GazeboRosForce::OnUpdate()
   impl_->link_->AddTorque(gazebo_ros::Convert<ignition::math::Vector3d>(impl_->wrench_msg_.torque));
 }
 
+void GazeboRosForce::OnUpdateRelative()
+{
+  impl_->link_->AddRelativeForce(gazebo_ros::Convert<ignition::math::Vector3d>(impl_->wrench_msg_.force));
+  impl_->link_->AddRelativeTorque(gazebo_ros::Convert<ignition::math::Vector3d>(impl_->wrench_msg_.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
index 74ba8d8fc..03a53e0b4 100644
--- a/gazebo_plugins/test/worlds/gazebo_ros_force.world
+++ b/gazebo_plugins/test/worlds/gazebo_ros_force.world
@@ -9,7 +9,7 @@
     </include>
     <model name="box">
       <pose>0 0 0.5 0 0 0</pose>
-      <link name="link">
+      <link name="box_link">
         <collision name="collision">
           <geometry>
             <box>
@@ -30,7 +30,8 @@
           <namespace>/test</namespace>
           <argument>gazebo_ros_force:=force_test</argument>
         </ros>
-        <link_name>link</link_name>
+        <link_name>box_link</link_name>
+        <force_frame>world</force_frame>
       </plugin>
     </model>
   </world>
diff --git a/gazebo_plugins/worlds/gazebo_ros_force_demo.world b/gazebo_plugins/worlds/gazebo_ros_force_demo.world
index ba09d7532..3c480ae15 100644
--- a/gazebo_plugins/worlds/gazebo_ros_force_demo.world
+++ b/gazebo_plugins/worlds/gazebo_ros_force_demo.world
@@ -2,6 +2,11 @@
 <!--
   Gazebo ROS force plugin demo
 
+  If you want the force to be applied on the world frame, use
+    <force_frame>world</force_frame>
+  If you want the force to be applied on the body frame, use
+    <force_frame>body</force_frame>
+
   Try for example:
 
   ros2 topic pub -1 /demo/force_demo geometry_msgs/Wrench "force: {x: 10.0}"
@@ -38,6 +43,7 @@
           <argument>gazebo_ros_force:=force_demo</argument>
         </ros>
         <link_name>link</link_name>
+        <force_frame>world</force_frame>
       </plugin>
     </model>
   </world>

From 8a1ab8ae397a1958b301151ccd7bdf169b3305f8 Mon Sep 17 00:00:00 2001
From: Alex Fernandes Neves <alexfneves@gmail.com>
Date: Fri, 16 Aug 2019 10:09:03 -0300
Subject: [PATCH 2/5] Modifying force test for the 'world' frame, and adding
 test for the force on the 'link' frame

The new world file starts with the box rotated.
---
 gazebo_plugins/test/test_gazebo_ros_force.cpp | 131 +++++++++++-------
 .../test/worlds/gazebo_ros_force_link.world   |  38 +++++
 .../worlds/gazebo_ros_force_demo.world        |   4 +-
 3 files changed, 120 insertions(+), 53 deletions(-)
 create mode 100644 gazebo_plugins/test/worlds/gazebo_ros_force_link.world

diff --git a/gazebo_plugins/test/test_gazebo_ros_force.cpp b/gazebo_plugins/test/test_gazebo_ros_force.cpp
index 22a39a31a..579239a13 100644
--- a/gazebo_plugins/test/test_gazebo_ros_force.cpp
+++ b/gazebo_plugins/test/test_gazebo_ros_force.cpp
@@ -20,67 +20,96 @@
 
 #define tol 10e-10
 
+enum AXIS
+{
+  X, Y, Z
+};
+
 class GazeboRosForceTest : public gazebo::ServerFixture
 {
+public:
+  void ApplyForceTorque(AXIS force_axis, double init_rotation = 0)
+  {
+    // 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(init_rotation, box->WorldPose().Rot().Yaw(), tol);
+
+    // Create ROS publisher
+    auto node = std::make_shared<rclcpp::Node>("gazebo_ros_force_test");
+    ASSERT_NE(nullptr, node);
+
+    auto pub = node->create_publisher<geometry_msgs::msg::Wrench>(
+      "test/force_test", rclcpp::QoS(rclcpp::KeepLast(1)));
+
+    // Wait for subscriber to come up
+    unsigned int sleep = 0;
+    unsigned int max_sleep = 30;
+    while (sleep < max_sleep && pub->get_subscription_count() == 0u) {
+      gazebo::common::Time::MSleep(100);
+      sleep++;
+    }
+    EXPECT_LT(0u, pub->get_subscription_count());
+    EXPECT_LT(sleep, max_sleep);
+
+    // Apply force
+    auto msg = geometry_msgs::msg::Wrench();
+    switch (force_axis)
+    {
+    case AXIS::Y:
+      msg.force.y = 10.0;
+      break;
+    case AXIS::Z:
+      msg.force.z = 10.0;
+      break;
+    default:
+      msg.force.x = 10.0;
+      break;
+    }
+    msg.torque.z = 10.0;
+    pub->publish(msg);
+
+    // Wait until box moves
+    sleep = 0;
+    double target_dist{0.1};
+    while (sleep < max_sleep && box->WorldPose().Pos().X() < target_dist) {
+      world->Step(100);
+      gazebo::common::Time::MSleep(100);
+      sleep++;
+    }
+
+    // Check box moved
+    EXPECT_LT(sleep, max_sleep);
+    EXPECT_LT(target_dist, box->WorldPose().Pos().X());
+    EXPECT_LT(target_dist + init_rotation, box->WorldPose().Rot().Yaw());
+  }
 };
 
-TEST_F(GazeboRosForceTest, ApplyForceTorque)
+TEST_F(GazeboRosForceTest, ApplyForceTorqueWorld)
 {
   // 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<rclcpp::Node>("gazebo_ros_force_test");
-  ASSERT_NE(nullptr, node);
-
-  auto pub = node->create_publisher<geometry_msgs::msg::Wrench>(
-    "test/force_test", rclcpp::QoS(rclcpp::KeepLast(1)));
+  this->ApplyForceTorque(AXIS::X);
+}
 
-  // Wait for subscriber to come up
-  unsigned int sleep = 0;
-  unsigned int max_sleep = 30;
-  while (sleep < max_sleep && pub->get_subscription_count() == 0u) {
-    gazebo::common::Time::MSleep(100);
-    sleep++;
-  }
-  EXPECT_LT(0u, pub->get_subscription_count());
-  EXPECT_LT(sleep, max_sleep);
-
-  // 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
-  sleep = 0;
-  double target_dist{0.1};
-  while (sleep < max_sleep && box->WorldPose().Pos().X() < target_dist) {
-    world->Step(100);
-    gazebo::common::Time::MSleep(100);
-    sleep++;
-  }
+TEST_F(GazeboRosForceTest, ApplyForceTorqueLink)
+{
+  // Load test world and start paused
+  this->Load("worlds/gazebo_ros_force_link.world", true);
 
-  // Check box moved
-  EXPECT_LT(sleep, max_sleep);
-  EXPECT_LT(target_dist, box->WorldPose().Pos().X());
-  EXPECT_LT(target_dist, box->WorldPose().Rot().Z());
+  this->ApplyForceTorque(AXIS::Y, -M_PI/2);
 }
 
 int main(int argc, char ** argv)
diff --git a/gazebo_plugins/test/worlds/gazebo_ros_force_link.world b/gazebo_plugins/test/worlds/gazebo_ros_force_link.world
new file mode 100644
index 000000000..d92c10c37
--- /dev/null
+++ b/gazebo_plugins/test/worlds/gazebo_ros_force_link.world
@@ -0,0 +1,38 @@
+<?xml version="1.0"?>
+<sdf version="1.6">
+  <world name="default">
+    <include>
+      <uri>model://ground_plane</uri>
+    </include>
+    <include>
+      <uri>model://sun</uri>
+    </include>
+    <model name="box">
+      <pose>0 0 0.5 0 0 -1.5707963267948966</pose>
+      <link name="box_link">
+        <collision name="collision">
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+        </visual>
+      </link>
+      <plugin name="gazebo_ros_force" filename="libgazebo_ros_force.so">
+        <ros>
+          <namespace>/test</namespace>
+          <argument>gazebo_ros_force:=force_test</argument>
+        </ros>
+        <link_name>box_link</link_name>
+        <force_frame>link</force_frame>
+      </plugin>
+    </model>
+  </world>
+</sdf>
diff --git a/gazebo_plugins/worlds/gazebo_ros_force_demo.world b/gazebo_plugins/worlds/gazebo_ros_force_demo.world
index 3c480ae15..a1d81c782 100644
--- a/gazebo_plugins/worlds/gazebo_ros_force_demo.world
+++ b/gazebo_plugins/worlds/gazebo_ros_force_demo.world
@@ -4,8 +4,8 @@
 
   If you want the force to be applied on the world frame, use
     <force_frame>world</force_frame>
-  If you want the force to be applied on the body frame, use
-    <force_frame>body</force_frame>
+  If you want the force to be applied on the link frame, use
+    <force_frame>link</force_frame>
 
   Try for example:
 

From 09b07f6c476f86fc36b664f3bfff61c9cf5a14d4 Mon Sep 17 00:00:00 2001
From: Alex Fernandes Neves <alexfneves@gmail.com>
Date: Sat, 17 Aug 2019 12:44:00 -0300
Subject: [PATCH 3/5] Fix cpplint and uncrustify on force plugin files

---
 .../gazebo_plugins/gazebo_ros_force.hpp       |  3 ++-
 gazebo_plugins/src/gazebo_ros_force.cpp       | 22 ++++++++++--------
 gazebo_plugins/test/test_gazebo_ros_force.cpp | 23 +++++++++----------
 3 files changed, 25 insertions(+), 23 deletions(-)

diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_force.hpp b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_force.hpp
index 27290dc4c..e49204aee 100644
--- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_force.hpp
+++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_force.hpp
@@ -69,7 +69,8 @@ class GazeboRosForce : public gazebo::ModelPlugin
   /// Optional callback to be called at every simulation iteration.
   virtual void OnUpdate();
 
-  /// Optional callback to be called at every simulation iteration when the force is applied on the body frame.
+  /// Optional callback to be called at every simulation iteration when the force
+  /// is applied on the body frame.
   virtual void OnUpdateRelative();
 
 private:
diff --git a/gazebo_plugins/src/gazebo_ros_force.cpp b/gazebo_plugins/src/gazebo_ros_force.cpp
index 21e82b341..f256c43c3 100644
--- a/gazebo_plugins/src/gazebo_ros_force.cpp
+++ b/gazebo_plugins/src/gazebo_ros_force.cpp
@@ -75,16 +75,16 @@ void GazeboRosForce::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf)
   // Force frame
   bool force_on_world_frame;
   if (!sdf->HasElement("force_frame")) {
-    RCLCPP_WARN(logger, "Force plugin missing <force_frame> wasn't set, therefore it's been set as 'world'. The other option is 'link'.");
+    RCLCPP_WARN(logger, "Force plugin missing <force_frame> wasn't set,"
+      "therefore it's been set as 'world'. The other option is 'link'.");
     force_on_world_frame = true;
-  }
-  else {
+  } else {
     auto force_frame = sdf->GetElement("force_frame")->Get<std::string>();
-    if (force_frame == "world")
+    if (force_frame == "world") {
       force_on_world_frame = true;
-    else if (force_frame == "link")
+    } else if (force_frame == "link") {
       force_on_world_frame = false;
-    else {
+    } else {
       RCLCPP_ERROR(logger, "Force plugin <force_frame> can only be 'world' or 'link'");
       return;
     }
@@ -99,8 +99,8 @@ void GazeboRosForce::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf)
 
   // Callback on every iteration
   impl_->update_connection_ = gazebo::event::Events::ConnectWorldUpdateBegin(
-    force_on_world_frame ? std::bind(&GazeboRosForce::OnUpdate, this)
-      : std::bind(&GazeboRosForce::OnUpdateRelative, this));
+    force_on_world_frame ? std::bind(&GazeboRosForce::OnUpdate, this) :
+    std::bind(&GazeboRosForce::OnUpdateRelative, this));
 }
 
 void GazeboRosForce::OnRosWrenchMsg(const geometry_msgs::msg::Wrench::SharedPtr msg)
@@ -121,8 +121,10 @@ void GazeboRosForce::OnUpdate()
 
 void GazeboRosForce::OnUpdateRelative()
 {
-  impl_->link_->AddRelativeForce(gazebo_ros::Convert<ignition::math::Vector3d>(impl_->wrench_msg_.force));
-  impl_->link_->AddRelativeTorque(gazebo_ros::Convert<ignition::math::Vector3d>(impl_->wrench_msg_.torque));
+  impl_->link_->AddRelativeForce(
+    gazebo_ros::Convert<ignition::math::Vector3d>(impl_->wrench_msg_.force));
+  impl_->link_->AddRelativeTorque(
+    gazebo_ros::Convert<ignition::math::Vector3d>(impl_->wrench_msg_.torque));
 }
 
 GZ_REGISTER_MODEL_PLUGIN(GazeboRosForce)
diff --git a/gazebo_plugins/test/test_gazebo_ros_force.cpp b/gazebo_plugins/test/test_gazebo_ros_force.cpp
index 579239a13..e5350edeb 100644
--- a/gazebo_plugins/test/test_gazebo_ros_force.cpp
+++ b/gazebo_plugins/test/test_gazebo_ros_force.cpp
@@ -65,17 +65,16 @@ class GazeboRosForceTest : public gazebo::ServerFixture
 
     // Apply force
     auto msg = geometry_msgs::msg::Wrench();
-    switch (force_axis)
-    {
-    case AXIS::Y:
-      msg.force.y = 10.0;
-      break;
-    case AXIS::Z:
-      msg.force.z = 10.0;
-      break;
-    default:
-      msg.force.x = 10.0;
-      break;
+    switch (force_axis) {
+      case AXIS::Y:
+        msg.force.y = 10.0;
+        break;
+      case AXIS::Z:
+        msg.force.z = 10.0;
+        break;
+      default:
+        msg.force.x = 10.0;
+        break;
     }
     msg.torque.z = 10.0;
     pub->publish(msg);
@@ -109,7 +108,7 @@ TEST_F(GazeboRosForceTest, ApplyForceTorqueLink)
   // Load test world and start paused
   this->Load("worlds/gazebo_ros_force_link.world", true);
 
-  this->ApplyForceTorque(AXIS::Y, -M_PI/2);
+  this->ApplyForceTorque(AXIS::Y, -M_PI / 2);
 }
 
 int main(int argc, char ** argv)

From 0f50c5dc9a38e8f71bdb9c21173b45b3c66f97f4 Mon Sep 17 00:00:00 2001
From: Alex Fernandes Neves <alexfneves@gmail.com>
Date: Thu, 22 Aug 2019 10:34:21 -0300
Subject: [PATCH 4/5] Removing OnUpdateRelative() from the force plugin

This function could potentially break the ABI, therefore is been removed.
---
 .../gazebo_plugins/gazebo_ros_force.hpp       |  4 ---
 gazebo_plugins/src/gazebo_ros_force.cpp       | 34 ++++++++++---------
 2 files changed, 18 insertions(+), 20 deletions(-)

diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_force.hpp b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_force.hpp
index e49204aee..4b4b12978 100644
--- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_force.hpp
+++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_force.hpp
@@ -69,10 +69,6 @@ class GazeboRosForce : public gazebo::ModelPlugin
   /// Optional callback to be called at every simulation iteration.
   virtual void OnUpdate();
 
-  /// Optional callback to be called at every simulation iteration when the force
-  /// is applied on the body frame.
-  virtual void OnUpdateRelative();
-
 private:
   /// Callback when a ROS Wrench message is received
   /// \param[in] msg The Incoming ROS message representing the new force to
diff --git a/gazebo_plugins/src/gazebo_ros_force.cpp b/gazebo_plugins/src/gazebo_ros_force.cpp
index f256c43c3..62e5dfb5d 100644
--- a/gazebo_plugins/src/gazebo_ros_force.cpp
+++ b/gazebo_plugins/src/gazebo_ros_force.cpp
@@ -43,6 +43,9 @@ class GazeboRosForcePrivate
 
   // Pointer to the update event connection
   gazebo::event::ConnectionPtr update_connection_;
+
+  /// Indicates that the force should be applied on the world frame instead of the body frame
+  bool force_on_world_frame_;
 };
 
 GazeboRosForce::GazeboRosForce()
@@ -73,17 +76,16 @@ void GazeboRosForce::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf)
   }
 
   // Force frame
-  bool force_on_world_frame;
   if (!sdf->HasElement("force_frame")) {
     RCLCPP_WARN(logger, "Force plugin missing <force_frame> wasn't set,"
       "therefore it's been set as 'world'. The other option is 'link'.");
-    force_on_world_frame = true;
+    impl_->force_on_world_frame_ = true;
   } else {
     auto force_frame = sdf->GetElement("force_frame")->Get<std::string>();
     if (force_frame == "world") {
-      force_on_world_frame = true;
+      impl_->force_on_world_frame_ = true;
     } else if (force_frame == "link") {
-      force_on_world_frame = false;
+      impl_->force_on_world_frame_ = false;
     } else {
       RCLCPP_ERROR(logger, "Force plugin <force_frame> can only be 'world' or 'link'");
       return;
@@ -99,8 +101,7 @@ void GazeboRosForce::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf)
 
   // Callback on every iteration
   impl_->update_connection_ = gazebo::event::Events::ConnectWorldUpdateBegin(
-    force_on_world_frame ? std::bind(&GazeboRosForce::OnUpdate, this) :
-    std::bind(&GazeboRosForce::OnUpdateRelative, this));
+    std::bind(&GazeboRosForce::OnUpdate, this));
 }
 
 void GazeboRosForce::OnRosWrenchMsg(const geometry_msgs::msg::Wrench::SharedPtr msg)
@@ -115,16 +116,17 @@ void GazeboRosForce::OnRosWrenchMsg(const geometry_msgs::msg::Wrench::SharedPtr
 
 void GazeboRosForce::OnUpdate()
 {
-  impl_->link_->AddForce(gazebo_ros::Convert<ignition::math::Vector3d>(impl_->wrench_msg_.force));
-  impl_->link_->AddTorque(gazebo_ros::Convert<ignition::math::Vector3d>(impl_->wrench_msg_.torque));
-}
-
-void GazeboRosForce::OnUpdateRelative()
-{
-  impl_->link_->AddRelativeForce(
-    gazebo_ros::Convert<ignition::math::Vector3d>(impl_->wrench_msg_.force));
-  impl_->link_->AddRelativeTorque(
-    gazebo_ros::Convert<ignition::math::Vector3d>(impl_->wrench_msg_.torque));
+  if (impl_->force_on_world_frame_) {
+    impl_->link_->AddForce(
+      gazebo_ros::Convert<ignition::math::Vector3d>(impl_->wrench_msg_.force));
+    impl_->link_->AddTorque(
+      gazebo_ros::Convert<ignition::math::Vector3d>(impl_->wrench_msg_.torque));
+  } else {
+    impl_->link_->AddRelativeForce(
+      gazebo_ros::Convert<ignition::math::Vector3d>(impl_->wrench_msg_.force));
+    impl_->link_->AddRelativeTorque(
+      gazebo_ros::Convert<ignition::math::Vector3d>(impl_->wrench_msg_.torque));
+  }
 }
 
 GZ_REGISTER_MODEL_PLUGIN(GazeboRosForce)

From 95fbd6c4560a6d2f7dbef560d9555532e2aa0fff Mon Sep 17 00:00:00 2001
From: Louise Poubel <louise@openrobotics.org>
Date: Thu, 22 Aug 2019 18:32:51 -0700
Subject: [PATCH 5/5] body -> link, warn -> info, more examples

Signed-off-by: Louise Poubel <louise@openrobotics.org>
---
 .../gazebo_plugins/gazebo_ros_force.hpp       |  2 +-
 gazebo_plugins/src/gazebo_ros_force.cpp       |  4 +-
 .../worlds/gazebo_ros_force_demo.world        | 39 +++++++++++++++++--
 3 files changed, 38 insertions(+), 7 deletions(-)

diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_force.hpp b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_force.hpp
index 4b4b12978..917aff506 100644
--- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_force.hpp
+++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_force.hpp
@@ -46,7 +46,7 @@ class GazeboRosForcePrivate;
       <!-- Name of link within model which will receive the force -->
       <link_name>link</link_name>
 
-      <!-- Frame where the force/torque will be applied (options: world; body)-->
+      <!-- Frame where the force/torque will be applied (options: world; link)-->
       <force_frame>link</force_frame>
 
     </plugin>
diff --git a/gazebo_plugins/src/gazebo_ros_force.cpp b/gazebo_plugins/src/gazebo_ros_force.cpp
index 62e5dfb5d..09af745da 100644
--- a/gazebo_plugins/src/gazebo_ros_force.cpp
+++ b/gazebo_plugins/src/gazebo_ros_force.cpp
@@ -44,7 +44,7 @@ class GazeboRosForcePrivate
   // Pointer to the update event connection
   gazebo::event::ConnectionPtr update_connection_;
 
-  /// Indicates that the force should be applied on the world frame instead of the body frame
+  /// Indicates that the force should be applied on the world frame instead of the link frame
   bool force_on_world_frame_;
 };
 
@@ -77,7 +77,7 @@ void GazeboRosForce::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf)
 
   // Force frame
   if (!sdf->HasElement("force_frame")) {
-    RCLCPP_WARN(logger, "Force plugin missing <force_frame> wasn't set,"
+    RCLCPP_INFO(logger, "Force plugin missing <force_frame> wasn't set,"
       "therefore it's been set as 'world'. The other option is 'link'.");
     impl_->force_on_world_frame_ = true;
   } else {
diff --git a/gazebo_plugins/worlds/gazebo_ros_force_demo.world b/gazebo_plugins/worlds/gazebo_ros_force_demo.world
index a1d81c782..1fdc2d211 100644
--- a/gazebo_plugins/worlds/gazebo_ros_force_demo.world
+++ b/gazebo_plugins/worlds/gazebo_ros_force_demo.world
@@ -9,7 +9,11 @@
 
   Try for example:
 
-  ros2 topic pub -1 /demo/force_demo geometry_msgs/Wrench "force: {x: 10.0}"
+  ros2 topic pub -1 /demo/world/force_demo geometry_msgs/Wrench "force: {x: 10.0}"
+
+  and
+
+  ros2 topic pub -1 /demo/link/force_demo geometry_msgs/Wrench "force: {x: 10.0}"
 -->
 <sdf version="1.6">
   <world name="default">
@@ -19,8 +23,8 @@
     <include>
       <uri>model://sun</uri>
     </include>
-    <model name="box">
-      <pose>0 0 0.5 0 0 0</pose>
+    <model name="force_on_world_frame">
+      <pose>0 3 0.5 0 0 1.57</pose>
       <link name="link">
         <collision name="collision">
           <geometry>
@@ -39,12 +43,39 @@
       </link>
       <plugin name="gazebo_ros_force" filename="libgazebo_ros_force.so">
         <ros>
-          <namespace>/demo</namespace>
+          <namespace>/demo/world</namespace>
           <argument>gazebo_ros_force:=force_demo</argument>
         </ros>
         <link_name>link</link_name>
         <force_frame>world</force_frame>
       </plugin>
     </model>
+    <model name="force_on_link_frame">
+      <pose>0 -3 0.5 0 0 1.57</pose>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+        </visual>
+      </link>
+      <plugin name="gazebo_ros_force" filename="libgazebo_ros_force.so">
+        <ros>
+          <namespace>/demo/link</namespace>
+          <argument>gazebo_ros_force:=force_demo</argument>
+        </ros>
+        <link_name>link</link_name>
+        <force_frame>link</force_frame>
+      </plugin>
+    </model>
   </world>
 </sdf>