Fix Gimbal plugin to Gazebo 9+

CMakeLists.txt

@@ -39,9 +39,8 @@ link_directories(
 set (plugins_single_header
         ArduPilotPlugin
         ArduCopterIRLockPlugin
-	ArduCopterPlugin
-        # GimbalSmall2dPlugin
-	# WindPlugin
+        ArduCopterPlugin
+        GimbalSmall2dPlugin
         )
 
 add_library(ArduCopterIRLockPlugin SHARED src/ArduCopterIRLockPlugin.cc)
@@ -53,12 +52,10 @@ target_link_libraries(ArduPilotPlugin ${GAZEBO_LIBRARIES})
 add_library(ArduCopterPlugin SHARED src/ArduCopterPlugin.cc)
 target_link_libraries(ArduCopterPlugin ${GAZEBO_LIBRARIES})
 
-if("${GAZEBO_VERSION}" VERSION_LESS "8.0")
-    add_library(GimbalSmall2dPlugin SHARED src/GimbalSmall2dPlugin.cc)
-    target_link_libraries(GimbalSmall2dPlugin ${GAZEBO_LIBRARIES})
-    install(TARGETS GimbalSmall2dPlugin DESTINATION ${GAZEBO_PLUGIN_PATH})
-endif()
+add_library(GimbalSmall2dPlugin SHARED src/GimbalSmall2dPlugin.cc)
+target_link_libraries(GimbalSmall2dPlugin ${GAZEBO_LIBRARIES})
 
 install(TARGETS ArduCopterIRLockPlugin DESTINATION ${GAZEBO_PLUGIN_PATH})
 install(TARGETS ArduPilotPlugin DESTINATION ${GAZEBO_PLUGIN_PATH})
 install(TARGETS ArduCopterPlugin DESTINATION ${GAZEBO_PLUGIN_PATH})
+install(TARGETS GimbalSmall2dPlugin DESTINATION ${GAZEBO_PLUGIN_PATH})

include/gazebo_gimbal_controller_plugin.hh

@@ -0,0 +1,161 @@
+/*
+ * Copyright (C) 2012-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: A basic gimbal controller
+ * Author: John Hsu
+ */
+
+#ifndef _GAZEBO_GIMBAL_CONTROLLER_PLUGIN_HH_
+#define _GAZEBO_GIMBAL_CONTROLLER_PLUGIN_HH_
+
+#include <atomic>
+#include <string>
+#include <vector>
+#include <mutex>
+#include <memory>
+#include <optional>
+#include <thread>
+
+#include <gazebo/common/PID.hh>
+#include <gazebo/common/Plugin.hh>
+#include <gazebo/physics/physics.hh>
+#include <gazebo/transport/transport.hh>
+#include <gazebo/util/system.hh>
+#include <gazebo/sensors/sensors.hh>
+#include <ignition/math.hh>
+#include <mavlink/v2.0/common/mavlink.h>
+
+namespace gazebo
+{
+  // Default PID gains
+  static double kPIDPitchP = 5.0;
+  static double kPIDPitchI = 0.0;
+  static double kPIDPitchD = 0.0;
+  static double kPIDPitchIMax = 0.0;
+  static double kPIDPitchIMin = 0.0;
+  static double kPIDPitchCmdMax = 0.3;
+  static double kPIDPitchCmdMin = -0.3;
+
+  static double kPIDRollP = 5.0;
+  static double kPIDRollI = 0.0;
+  static double kPIDRollD = 0.0;
+  static double kPIDRollIMax = 0.0;
+  static double kPIDRollIMin = 0.0;
+  static double kPIDRollCmdMax = 0.3;
+  static double kPIDRollCmdMin = -0.3;
+
+  static double kPIDYawP = 1.0;
+  static double kPIDYawI = 0.0;
+  static double kPIDYawD = 0.0;
+  static double kPIDYawIMax = 0.0;
+  static double kPIDYawIMin = 0.0;
+  static double kPIDYawCmdMax = 1.0;
+  static double kPIDYawCmdMin = -1.0;
+
+  // Default rotation directions
+  static double kRollDir = -1.0;
+  static double kPitchDir = -1.0;
+  static double kYawDir = 1.0;
+
+  class GAZEBO_VISIBLE GimbalControllerPlugin : public ModelPlugin
+  {
+    public: GimbalControllerPlugin();
+    public: ~GimbalControllerPlugin();
+
+    public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
+
+    public: virtual void Init();
+
+    private: void OnUpdate();
+
+    private: bool InitUdp();
+    private: void SendHeartbeat();
+    private: void SendGimbalDeviceInformation();
+    private: void SendGimbalDeviceAttitudeStatus();
+    private: void SendResult(uint8_t target_sysid, uint8_t target_compid, uint16_t command, MAV_RESULT result);
+    private: void SendMavlinkMessage(const mavlink_message_t& msg);
+    private: void RxThread();
+    private: void HandleMessage(const mavlink_message_t& msg);
+    private: void HandleCommandLong(const mavlink_message_t& msg);
+    private: void HandleGimbalDeviceSetAttitude(const mavlink_message_t& msg);
+    private: void HandleAutopilotStateForGimbalDevice(const mavlink_message_t& msg);
+    private: void HandleRequestMessage(uint8_t target_sysid, uint8_t target_compid, const mavlink_command_long_t& command_long);
+    private: void HandleSetMessageInterval(uint8_t target_sysid, uint8_t target_compid, const mavlink_command_long_t& command_long);
+
+    private: static std::optional<double> calcSetpoint(double dt, double lastSetpoint, double newSetpoint, double newRateSetpoint);
+
+    private: std::mutex cmd_mutex;
+
+    private: sdf::ElementPtr sdf;
+
+    private: std::vector<event::ConnectionPtr> connections;
+
+    private: physics::ModelPtr model;
+
+    /// \brief yaw camera
+    private: physics::JointPtr yawJoint;
+
+    /// \brief camera roll joint
+    private: physics::JointPtr rollJoint;
+
+    /// \brief camera pitch joint
+    private: physics::JointPtr pitchJoint;
+
+    private: sensors::ImuSensorPtr cameraImuSensor;
+    private: double vehicleYawRad {0.0};
+    private: std::string status;
+
+    private: double rDir;
+    private: double pDir;
+    private: double yDir;
+
+    private: std::mutex setpointMutex {};
+    private: double lastRollSetpoint {0.0};
+    private: double lastPitchSetpoint {0.0};
+    private: double lastYawSetpoint {0.0};
+    private: double rollSetpoint {0.0};
+    private: double pitchSetpoint {0.0};
+    private: double yawSetpoint {0.0};
+    private: bool yawLock {false};
+    private: double rollRateSetpoint {NAN};
+    private: double pitchRateSetpoint {NAN};
+    private: double yawRateSetpoint {NAN};
+
+    private: transport::NodePtr node;
+
+    private: common::PID pitchPid;
+    private: common::PID rollPid;
+    private: common::PID yawPid;
+    private: common::Time lastUpdateTime;
+
+    private: std::unique_ptr<std::thread> rxThread {};
+    private: std::atomic<bool> shouldExit {false};
+    private: int sock;
+    private: struct sockaddr_in myaddr;
+    private: common::Time lastHeartbeatSentTime;
+    private: const double heartbeatIntervalS {1.0};
+    private: common::Time lastAttitudeStatusSentTime;
+
+    private: const double defaultAttitudeStatusIntervalS {0.1};
+    private: double attitudeStatusIntervalS {defaultAttitudeStatusIntervalS};
+    private: bool sendingAttitudeStatus {true};
+
+    private: static constexpr uint8_t ourSysid {1};
+    private: static constexpr uint8_t ourCompid {MAV_COMP_ID_GIMBAL};
+    private: static constexpr uint8_t mavlinkChannel {MAVLINK_COMM_3};
+  };
+}
+#endif

src/GimbalSmall2dPlugin.cc

@@ -32,13 +32,15 @@ class gazebo::GimbalSmall2dPluginPrivate
 {
   /// \brief Callback when a command string is received.
   /// \param[in] _msg Mesage containing the command string
-  public: void OnStringMsg(ConstGzStringPtr &_msg);
+  public: void TiltOnStringMsg(ConstGzStringPtr &_msg);
+  public: void PanOnStringMsg(ConstGzStringPtr &_msg);
 
   /// \brief A list of event connections
   public: std::vector<event::ConnectionPtr> connections;
 
   /// \brief Subscriber to the gimbal command topic
-  public: transport::SubscriberPtr sub;
+  public: transport::SubscriberPtr tiltSub;
+  public: transport::SubscriberPtr rollSub;
 
   /// \brief Publisher to the gimbal status topic
   public: transport::PublisherPtr pub;
@@ -49,8 +51,12 @@ class gazebo::GimbalSmall2dPluginPrivate
   /// \brief Joint for tilting the gimbal
   public: physics::JointPtr tiltJoint;
 
+  /// \brief Joint for rolling the gimbal
+  public: physics::JointPtr rollJoint;
+
   /// \brief Command that updates the gimbal tilt angle
-  public: double command = IGN_PI_2;
+  public: double tiltSetpoint = -IGN_PI_2;
+  public: double rollSetpoint = 0.0;
 
   /// \brief Pointer to the transport node
   public: transport::NodePtr node;
@@ -66,7 +72,6 @@ class gazebo::GimbalSmall2dPluginPrivate
 GimbalSmall2dPlugin::GimbalSmall2dPlugin()
   : dataPtr(new GimbalSmall2dPluginPrivate)
 {
-  this->dataPtr->pid.Init(1, 0, 0, 0, 0, 1.0, -1.0);
 }
 
 /////////////////////////////////////////////////
@@ -75,40 +80,69 @@ void GimbalSmall2dPlugin::Load(physics::ModelPtr _model,
 {
   this->dataPtr->model = _model;
 
-  std::string jointName = "tilt_joint";
-  if (_sdf->HasElement("joint"))
+  std::string tiltJointName = "tilt_joint";
+  if (_sdf->HasElement("tilt_joint"))
+  {
+    tiltJointName = _sdf->Get<std::string>("tilt_joint");
+  }
+  this->dataPtr->tiltJoint = this->dataPtr->model->GetJoint(tiltJointName);
+
+  std::string rollJointName = "roll_joint";
+  if (_sdf->HasElement("roll_joint"))
+  {
+    rollJointName = _sdf->Get<std::string>("roll_joint");
+  }
+  this->dataPtr->rollJoint = this->dataPtr->model->GetJoint(rollJointName);
+
+  std::vector<double> PID = {2, 0, 0};
+  if (_sdf->HasElement("pid"))
   {
-    jointName = _sdf->Get<std::string>("joint");
+    std::string PID_string = _sdf->Get<std::string>("pid");
+    std::stringstream ss(PID_string);
+
+    ss >> PID[0];
+    ss >> PID[1];
+    ss >> PID[2];
   }
-  this->dataPtr->tiltJoint = this->dataPtr->model->GetJoint(jointName);
+
+  this->dataPtr->pid.Init(PID[0], PID[1], PID[2], 0, 0, 1.0, -1.0);
+
   if (!this->dataPtr->tiltJoint)
   {
-    std::string scopedJointName = _model->GetScopedName() + "::" + jointName;
-    gzwarn << "joint [" << jointName
+    std::string scopedJointName = _model->GetScopedName() + "::" + tiltJointName;
+    gzwarn << "joint [" << tiltJointName
            << "] not found, trying again with scoped joint name ["
            << scopedJointName << "]\n";
     this->dataPtr->tiltJoint = this->dataPtr->model->GetJoint(scopedJointName);
   }
   if (!this->dataPtr->tiltJoint)
   {
     gzerr << "GimbalSmall2dPlugin::Load ERROR! Can't get joint '"
-          << jointName << "' " << endl;
+          << tiltJointName << "' " << endl;
   }
 }
 
 /////////////////////////////////////////////////
 void GimbalSmall2dPlugin::Init()
 {
   this->dataPtr->node = transport::NodePtr(new transport::Node());
-  this->dataPtr->node->Init(this->dataPtr->model->GetWorld()->GetName());
-
-  this->dataPtr->lastUpdateTime =
-    this->dataPtr->model->GetWorld()->GetSimTime();
+  #if GAZEBO_MAJOR_VERSION >= 9
+    this->dataPtr->node->Init(this->dataPtr->model->GetWorld()->Name());
+    this->dataPtr->lastUpdateTime = this->dataPtr->model->GetWorld()->SimTime();
+  #else
+    this->dataPtr->node->Init(this->dataPtr->model->GetWorld()->GetName());
+    this->dataPtr->lastUpdateTime = this->dataPtr->model->GetWorld()->GetSimTime();
+  #endif
 
   std::string topic = std::string("~/") +  this->dataPtr->model->GetName() +
     "/gimbal_tilt_cmd";
-  this->dataPtr->sub = this->dataPtr->node->Subscribe(topic,
-      &GimbalSmall2dPluginPrivate::OnStringMsg, this->dataPtr.get());
+  this->dataPtr->tiltSub = this->dataPtr->node->Subscribe(topic,
+      &GimbalSmall2dPluginPrivate::TiltOnStringMsg, this->dataPtr.get());
+
+  topic = std::string("~/") +  this->dataPtr->model->GetName() +
+    "/gimbal_roll_cmd";
+  this->dataPtr->rollSub = this->dataPtr->node->Subscribe(topic,
+      &GimbalSmall2dPluginPrivate::PanOnStringMsg, this->dataPtr.get());
 
   this->dataPtr->connections.push_back(event::Events::ConnectWorldUpdateBegin(
           std::bind(&GimbalSmall2dPlugin::OnUpdate, this)));
@@ -121,9 +155,14 @@ void GimbalSmall2dPlugin::Init()
 }
 
 /////////////////////////////////////////////////
-void GimbalSmall2dPluginPrivate::OnStringMsg(ConstGzStringPtr &_msg)
+void GimbalSmall2dPluginPrivate::TiltOnStringMsg(ConstGzStringPtr &_msg)
+{
+  this->tiltSetpoint = atof(_msg->data().c_str());
+}
+
+void GimbalSmall2dPluginPrivate::PanOnStringMsg(ConstGzStringPtr &_msg)
 {
-  this->command = atof(_msg->data().c_str());
+  this->rollSetpoint = atof(_msg->data().c_str());
 }
 
 /////////////////////////////////////////////////
@@ -132,9 +171,16 @@ void GimbalSmall2dPlugin::OnUpdate()
   if (!this->dataPtr->tiltJoint)
     return;
 
-  double angle = this->dataPtr->tiltJoint->GetAngle(0).Radian();
+  #if GAZEBO_MAJOR_VERSION >= 9
+    double tiltAngle = this->dataPtr->tiltJoint->Position(0);
+    double rollAngle = this->dataPtr->rollJoint->Position(0);
+    common::Time time = this->dataPtr->model->GetWorld()->SimTime();
+  #else
+    double tiltAngle = this->dataPtr->tiltJoint->GetAngle(0).Radian();
+    double rollAngle = this->dataPtr->rollJoint->GetAngle(0).Radian();
+    common::Time time = this->dataPtr->model->GetWorld()->GetSimTime();
+  #endif
 
-  common::Time time = this->dataPtr->model->GetWorld()->GetSimTime();
   if (time < this->dataPtr->lastUpdateTime)
   {
     this->dataPtr->lastUpdateTime = time;
@@ -143,9 +189,16 @@ void GimbalSmall2dPlugin::OnUpdate()
   else if (time > this->dataPtr->lastUpdateTime)
   {
     double dt = (this->dataPtr->lastUpdateTime - time).Double();
-    double error = angle - this->dataPtr->command;
-    double force = this->dataPtr->pid.Update(error, dt);
+
+    double tiltError = tiltAngle - this->dataPtr->tiltSetpoint;
+    double rollError = rollAngle - this->dataPtr->rollSetpoint;
+
+    double force = this->dataPtr->pid.Update(tiltError, dt);
     this->dataPtr->tiltJoint->SetForce(0, force);
+
+    force = this->dataPtr->pid.Update(rollError, dt);
+    this->dataPtr->rollJoint->SetForce(0, force);
+
     this->dataPtr->lastUpdateTime = time;
   }
 
@@ -154,7 +207,7 @@ void GimbalSmall2dPlugin::OnUpdate()
   {
     i = 0;
     std::stringstream ss;
-    ss << angle;
+    ss << "Tilt: " << tiltAngle << "Roll: " << rollAngle;
     gazebo::msgs::GzString m;
     m.set_data(ss.str());
     this->dataPtr->pub->Publish(m);