Merge branch 'wide_angle_camera' of github.com:ignitionrobotics/ign-gazebo into wide_angle_camera

Author: WilliamLewww

src/systems/thruster/Thruster.cc

@@ -45,6 +45,16 @@ using namespace systems;
 
 class ignition::gazebo::systems::ThrusterPrivateData
 {
+  /// \brief The mode of operation
+  public: enum OperationMode {
+    /// \brief Takes in a force commmand and spins the propeller at an
+    /// appropriate rate.
+    ForceCmd = 0,
+    /// \brief Takes in angular velocity commands in radians per second and
+    /// calculates the appropriate force.
+    AngVelCmd
+  } opmode = OperationMode::ForceCmd;
+
   /// \brief Mutex for read/write access to class
   public: std::mutex mtx;
 
@@ -55,25 +65,28 @@ class ignition::gazebo::systems::ThrusterPrivateData
   public: double propellerAngVel = 0.0;
 
   /// \brief The link entity which will spin
-  public: ignition::gazebo::Entity linkEntity;
+  public: Entity linkEntity;
 
   /// \brief Axis along which the propeller spins. Expressed in the joint
   /// frame. Addume this doesn't change during simulation.
-  public: ignition::math::Vector3d jointAxis;
+  public: math::Vector3d jointAxis;
 
   /// \brief Joint pose in the child link frame. Assume this doesn't change
   /// during the simulation.
   public: math::Pose3d jointPose;
 
   /// \brief Propeller koint entity
-  public: ignition::gazebo::Entity jointEntity;
+  public: Entity jointEntity;
 
   /// \brief ignition node for handling transport
-  public: ignition::transport::Node node;
+  public: transport::Node node;
+
+  /// \brief Publisher for feedback of data
+  public: transport::Node::Publisher pub;
 
   /// \brief The PID which controls the propeller. This isn't used if
   /// velocityControl is true.
-  public: ignition::math::PID propellerController;
+  public: math::PID propellerController;
 
   /// \brief Velocity Control mode - this disables the propellerController
   /// and writes the angular velocity directly to the joint. default: false
@@ -100,10 +113,18 @@ class ignition::gazebo::systems::ThrusterPrivateData
   /// \brief callback for handling thrust update
   public: void OnCmdThrust(const ignition::msgs::Double &_msg);
 
+  /// \brief callback for handling angular velocity update
+  public: void OnCmdAngVel(const ignition::msgs::Double &_msg);
+
   /// \brief function which computes angular velocity from thrust
   /// \param[in] _thrust Thrust in N
   /// \return Angular velocity in rad/s
   public: double ThrustToAngularVec(double _thrust);
+
+  /// \brief function which computers thrust from angular velocity
+  /// \param[in] _angVel Angular Velocity in rad/s
+  /// \return Thrust in Newtons
+  public: double AngularVelToThrust(double _angVel);
 };
 
 /////////////////////////////////////////////////
@@ -158,6 +179,14 @@ void Thruster::Configure(
     this->dataPtr->fluidDensity = _sdf->Get<double>("fluid_density");
   }
 
+  // Get the operation mode
+  if (_sdf->HasElement("use_angvel_cmd"))
+  {
+    this->dataPtr->opmode = _sdf->Get<bool>("use_angvel_cmd") ?
+      ThrusterPrivateData::OperationMode::AngVelCmd :
+      ThrusterPrivateData::OperationMode::ForceCmd;
+  }
+
   this->dataPtr->jointEntity = model.JointByName(_ecm, jointName);
   if (kNullEntity == this->dataPtr->jointEntity)
   {
@@ -173,34 +202,65 @@ void Thruster::Configure(
   this->dataPtr->jointPose = _ecm.Component<components::Pose>(
       this->dataPtr->jointEntity)->Data();
 
-  // Keeping cmd_pos for backwards compatibility
-  // TODO(chapulina) Deprecate cmd_pos, because the commands aren't positions
-  std::string thrusterTopicOld = ignition::transport::TopicUtils::AsValidTopic(
-    "/model/" + ns + "/joint/" + jointName + "/cmd_pos");
-
-  this->dataPtr->node.Subscribe(
-    thrusterTopicOld,
-    &ThrusterPrivateData::OnCmdThrust,
-    this->dataPtr.get());
-
-  // Subscribe to force commands
-  std::string thrusterTopic = ignition::transport::TopicUtils::AsValidTopic(
-    "/model/" + ns + "/joint/" + jointName + "/cmd_thrust");
-
-  this->dataPtr->node.Subscribe(
-    thrusterTopic,
-    &ThrusterPrivateData::OnCmdThrust,
-    this->dataPtr.get());
-
-  ignmsg << "Thruster listening to commands in [" << thrusterTopic << "]"
-         << std::endl;
-
   // Get link entity
   auto childLink =
       _ecm.Component<ignition::gazebo::components::ChildLinkName>(
       this->dataPtr->jointEntity);
   this->dataPtr->linkEntity = model.LinkByName(_ecm, childLink->Data());
 
+  if (this->dataPtr->opmode == ThrusterPrivateData::OperationMode::ForceCmd)
+  {
+    // Keeping cmd_pos for backwards compatibility
+    // TODO(chapulina) Deprecate cmd_pos, because the commands aren't positions
+    std::string thrusterTopicOld =
+      ignition::transport::TopicUtils::AsValidTopic(
+        "/model/" + ns + "/joint/" + jointName + "/cmd_pos");
+
+    this->dataPtr->node.Subscribe(
+      thrusterTopicOld,
+      &ThrusterPrivateData::OnCmdThrust,
+      this->dataPtr.get());
+
+    // Subscribe to force commands
+    std::string thrusterTopic = ignition::transport::TopicUtils::AsValidTopic(
+      "/model/" + ns + "/joint/" + jointName + "/cmd_thrust");
+
+    this->dataPtr->node.Subscribe(
+      thrusterTopic,
+      &ThrusterPrivateData::OnCmdThrust,
+      this->dataPtr.get());
+
+    ignmsg << "Thruster listening to commands in [" << thrusterTopic << "]"
+          << std::endl;
+
+    std::string feedbackTopic = ignition::transport::TopicUtils::AsValidTopic(
+      "/model/" + ns + "/joint/" + jointName + "/ang_vel");
+    this->dataPtr->pub = this->dataPtr->node.Advertise<msgs::Double>(
+      feedbackTopic
+    );
+  }
+  else
+  {
+    igndbg << "Using angular velocity mode" << std::endl;
+    // Subscribe to angvel commands
+    std::string thrusterTopic = ignition::transport::TopicUtils::AsValidTopic(
+      "/model/" + ns + "/joint/" + jointName + "/cmd_vel");
+
+    this->dataPtr->node.Subscribe(
+      thrusterTopic,
+      &ThrusterPrivateData::OnCmdAngVel,
+      this->dataPtr.get());
+
+    ignmsg << "Thruster listening to commands in [" << thrusterTopic << "]"
+          << std::endl;
+
+    std::string feedbackTopic = ignition::transport::TopicUtils::AsValidTopic(
+      "/model/" + ns + "/joint/" + jointName + "/force");
+    this->dataPtr->pub = this->dataPtr->node.Advertise<msgs::Double>(
+      feedbackTopic
+    );
+  }
+
   // Create necessary components if not present.
   enableComponent<components::AngularVelocity>(_ecm, this->dataPtr->linkEntity);
   enableComponent<components::WorldAngularVelocity>(_ecm,
@@ -265,6 +325,19 @@ void ThrusterPrivateData::OnCmdThrust(const ignition::msgs::Double &_msg)
   this->propellerAngVel = this->ThrustToAngularVec(this->thrust);
 }
 
+/////////////////////////////////////////////////
+void ThrusterPrivateData::OnCmdAngVel(const ignition::msgs::Double &_msg)
+{
+  std::lock_guard<std::mutex> lock(mtx);
+  this->propellerAngVel =
+    ignition::math::clamp(ignition::math::fixnan(_msg.data()),
+      this->cmdMin, this->cmdMax);
+
+  // Thrust is proportional to the Rotation Rate squared
+  // See Thor I Fossen's  "Guidance and Control of ocean vehicles" p. 246
+  this->thrust = this->AngularVelToThrust(this->propellerAngVel);
+}
+
 /////////////////////////////////////////////////
 double ThrusterPrivateData::ThrustToAngularVec(double _thrust)
 {
@@ -280,6 +353,15 @@ double ThrusterPrivateData::ThrustToAngularVec(double _thrust)
   return propAngularVelocity;
 }
 
+/////////////////////////////////////////////////
+double ThrusterPrivateData::AngularVelToThrust(double _angVel)
+{
+  // Thrust is proportional to the Rotation Rate squared
+  // See Thor I Fossen's  "Guidance and Control of ocean vehicles" p. 246
+  return this->thrustCoefficient * pow(this->propellerDiameter, 4)
+    * abs(_angVel) * _angVel * this->fluidDensity;
+}
+
 /////////////////////////////////////////////////
 void Thruster::PreUpdate(
   const ignition::gazebo::UpdateInfo &_info,
@@ -307,6 +389,7 @@ void Thruster::PreUpdate(
     desiredPropellerAngVel = this->dataPtr->propellerAngVel;
   }
 
+  msgs::Double angvel;
   // PID control
   double torque = 0.0;
   if (!this->dataPtr->velocityControl)
@@ -318,6 +401,7 @@ void Thruster::PreUpdate(
       torque = this->dataPtr->propellerController.Update(angularError,
           _info.dt);
     }
+    angvel.set_data(currentAngular);
   }
   // Velocity control
   else
@@ -334,8 +418,19 @@ void Thruster::PreUpdate(
     {
       velocityComp->Data()[0] = desiredPropellerAngVel;
     }
+    angvel.set_data(desiredPropellerAngVel);
   }
 
+  if (this->dataPtr->opmode == ThrusterPrivateData::OperationMode::ForceCmd)
+  {
+    this->dataPtr->pub.Publish(angvel);
+  }
+  else
+  {
+    msgs::Double force;
+    force.set_data(desiredThrust);
+    this->dataPtr->pub.Publish(force);
+  }
   // Force: thrust
   // Torque: propeller rotation, if using PID
   link.AddWorldWrench(
@@ -350,4 +445,3 @@ IGNITION_ADD_PLUGIN(
   Thruster::ISystemPreUpdate)
 
 IGNITION_ADD_PLUGIN_ALIAS(Thruster, "ignition::gazebo::systems::Thruster")
-

src/systems/thruster/Thruster.hh

@@ -34,16 +34,28 @@ namespace systems
 
   /// \brief This plugin simulates a maritime thruster for
   /// boats and underwater vehicles. It uses the equations described in Fossen's
-  /// "Guidance and Control of Ocean Vehicles" in page 246. This plugin takes in
+  /// "Guidance and Control of Ocean Vehicles" in page 246. This plugin has two
+  /// modes of operation. In the default mode it takes in a
   /// force in Newtons and applies it to the thruster. It also calculates the
   /// theoretical angular velocity of the blades and spins them accordingly.
+  /// Alternatively, one may send angular velocity commands to calculate the
+  /// force to be applied using the said equation. In the default mode the
+  /// plugin will publish angular velocity in radians per second on
+  /// `/model/{ns}/joint/{joint_name}/ang_vel` as an ignition.msgs.double. If
+  /// <use_angvel_cmd> is set to true it publishes force in Newtons instead to
+  /// `/model/{ns}/joint/{joint_name}/force`.
   ///
   /// ## System Parameters
   /// - <namespace> - The namespace in which the robot exists. The plugin will
-  ///   listen on the topic `/model/{namespace}/joint/{joint_name}/cmd_thrust`.
+  ///   listen on the topic `/model/{namespace}/joint/{joint_name}/cmd_thrust`or
+  ///   `/model/{namespace}/joint/{joint_name}/cmd_vel` depending on the mode of
+  ///   operation.
   ///   [Optional]
   /// - <joint_name> - This is the joint in the model which corresponds to the
   ///   propeller. [Required]
+  /// - <use_angvel_cmd> - Default false, if set to true will make the thruster
+  ///   plugin accept commands in angular velocity in radians per seconds in
+  ///   terms of newtons. [Optional, Boolean, Default True]
   /// - <fluid_density> - The fluid density of the liquid in which the thruster
   ///   is operating in. [Optional, kg/m^3, defaults to 1000 kg/m^3]
   /// - <propeller_diameter> - The diameter of the propeller in meters.

test/integration/thruster.cc

@@ -46,15 +46,19 @@ class ThrusterTest : public InternalFixture<::testing::Test>
   /// \param[in] _density Fluid density
   /// \param[in] _diameter Propeller diameter
   /// \param[in] _baseTol Base tolerance for most quantities
+  /// \param[in] _useAngVelCmd Send commands in angular velocity instead of
+  /// force
+  /// \param[in] _mass Mass of the body being propelled.
   public: void TestWorld(const std::string &_world,
       const std::string &_namespace, double _coefficient, double _density,
-      double _diameter, double _baseTol);
+      double _diameter, double _baseTol, bool _useAngVelCmd = false,
+      double _mass = 100.1);
 };
 
 //////////////////////////////////////////////////
 void ThrusterTest::TestWorld(const std::string &_world,
     const std::string &_namespace, double _coefficient, double _density,
-    double _diameter, double _baseTol)
+    double _diameter, double _baseTol, bool _useAngVelCmd, double _mass)
 {
   // Start server
   ServerConfig serverConfig;
@@ -121,8 +125,17 @@ void ThrusterTest::TestWorld(const std::string &_world,
 
   // Publish command and check that vehicle moved
   transport::Node node;
+  std::string cmdTopic;
+  if (!_useAngVelCmd)
+  {
+    cmdTopic = "/model/" + _namespace + "/joint/propeller_joint/cmd_thrust";
+  }
+  else
+  {
+    cmdTopic = "/model/" + _namespace + "/joint/propeller_joint/cmd_vel";
+  }
   auto pub = node.Advertise<msgs::Double>(
-      "/model/" + _namespace + "/joint/propeller_joint/cmd_thrust");
+      cmdTopic);
 
   int sleep{0};
   int maxSleep{30};
@@ -134,8 +147,21 @@ void ThrusterTest::TestWorld(const std::string &_world,
   EXPECT_TRUE(pub.HasConnections());
 
   double force{300.0};
+
+  // See Thor I Fossen's  "Guidance and Control of ocean vehicles" p. 246
+  // omega = sqrt(thrust /
+  //     (fluid_density * thrust_coefficient * propeller_diameter ^ 4))
+  auto omega = sqrt(force / (_density * _coefficient * pow(_diameter, 4)));
+
   msgs::Double msg;
-  msg.set_data(force);
+  if(!_useAngVelCmd)
+  {
+    msg.set_data(force);
+  }
+  else
+  {
+    msg.set_data(omega);
+  }
   pub.Publish(msg);
 
   // Check movement
@@ -155,13 +181,12 @@ void ThrusterTest::TestWorld(const std::string &_world,
   // s = a * t^2 / 2
   // F = m * 2 * s / t^2
   // s = F * t^2 / 2m
-  double mass{100.1};
   double xTol{1e-2};
   for (unsigned int i = 0; i < modelPoses.size(); ++i)
   {
     auto pose = modelPoses[i];
     auto time = dt * i;
-    EXPECT_NEAR(force * time * time / (2 * mass), pose.Pos().X(), xTol);
+    EXPECT_NEAR(force * time * time / (2 * _mass), pose.Pos().X(), xTol);
     EXPECT_NEAR(0.0, pose.Pos().Y(), _baseTol);
     EXPECT_NEAR(0.0, pose.Pos().Z(), _baseTol);
     EXPECT_NEAR(0.0, pose.Rot().Pitch(), _baseTol);
@@ -175,10 +200,6 @@ void ThrusterTest::TestWorld(const std::string &_world,
       EXPECT_NEAR(0.0, pose.Rot().Roll(), _baseTol);
   }
 
-  // See Thor I Fossen's  "Guidance and Control of ocean vehicles" p. 246
-  // omega = sqrt(thrust /
-  //     (fluid_density * thrust_coefficient * propeller_diameter ^ 4))
-  auto omega = sqrt(force / (_density * _coefficient * pow(_diameter, 4)));
   double omegaTol{1e-1};
   for (unsigned int i = 0; i < propellerAngVels.size(); ++i)
   {
@@ -193,6 +214,16 @@ void ThrusterTest::TestWorld(const std::string &_world,
   }
 }
 
+/////////////////////////////////////////////////
+TEST_F(ThrusterTest, AngVelCmdControl)
+{
+  auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH),
+      "test", "worlds", "thruster_ang_vel_cmd.sdf");
+
+  //  Tolerance is high because the joint command disturbs the vehicle body
+  this->TestWorld(world, "custom", 0.005, 950, 0.2, 1e-2, true, 100.01);
+}
+
 /////////////////////////////////////////////////
 TEST_F(ThrusterTest, PIDControl)
 {