underwater robot/uuv extension

include/gazebo_uuv_plugin.h

@@ -1,5 +1,7 @@
 /*
- * Copyright 2016 Austin Buchan, Nils Rottmann, TUHH Hamburg, Germany
+ * Copyright 2020 Daniel Duecker, TU Hamburg, Germany
+ * Copyright 2020 Philipp Hastedt, TU Hamburg, Germany
+ * based on prior work by Nils Rottmann and Austin Buchan
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
@@ -14,81 +16,89 @@
  * limitations under the License.
  */
 
-#include <string>
-#include <gazebo/common/common.hh>
-#include <gazebo/common/Plugin.hh>
+
+
+#include <stdio.h>
+#include <boost/bind.hpp>
+#include <Eigen/Eigen>
 #include <gazebo/gazebo.hh>
 #include <gazebo/physics/physics.hh>
-
-#include "common.h"
+#include <gazebo/common/common.hh>
+#include <gazebo/common/Plugin.hh>
+#include <rotors_model/motor_model.hpp>
+#include "CommandMotorSpeed.pb.h"
 #include "gazebo/transport/transport.hh"
 #include "gazebo/msgs/msgs.hh"
-#include "CommandMotorSpeed.pb.h"
+#include "MotorSpeed.pb.h"
+#include "Float.pb.h"
+#include "common.h"
 
-namespace gazebo {
 
-// Default values
-static const std::string kDefaultNamespace = "";
-static const std::string kDefaultCommandSubTopic = "/gazebo/command/motor_speed";
-static const std::string kDefaultLinkName = "base_link";        // the link name of the base hippocampus, see sdf file
+namespace turning_direction {
+const static int CCW = 1;
+const static int CW = -1;
+}
 
+namespace gazebo {
 typedef const boost::shared_ptr<const mav_msgs::msgs::CommandMotorSpeed> CommandMotorSpeedPtr;
 
-// define class GazeboUUVPlugin
-class GazeboUUVPlugin : public ModelPlugin {
+class GazeboUUVPlugin : public MotorModel, public ModelPlugin {
+ public:
+  GazeboUUVPlugin()
+      : ModelPlugin(),
+        MotorModel(){
+  }
+
+  virtual ~GazeboUUVPlugin();
+  virtual void InitializeParams();
+  virtual void Publish();
+ protected:
+  virtual void UpdateForcesAndMoments();
+  virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
+  virtual void OnUpdate(const common::UpdateInfo &);
+
+ private:
+  std::string namespace_;
+  std::string command_sub_topic_;
+  std::string link_base_;
+  std::string link_prop_0_;
+  std::string link_prop_1_;
+  std::string link_prop_2_;
+  std::string link_prop_3_;
+
+  int turning_directions_[4];
+  double motor_commands_[4];
+  double motor_force_constant_;
+  double motor_torque_constant_;
+  double dead_zone_;
+
+  /* Hydro coefficients - FOSSEN 2011 */
+  double X_u_;
+  double Y_v_;
+  double Z_w_;
+  double K_p_;
+  double M_q_;
+  double N_r_;
+
+  double X_udot_;
+  double Y_vdot_;
+  double Z_wdot_;
+  double K_pdot_;
+  double M_qdot_;
+  double N_rdot_;
+
+  transport::NodePtr node_handle_;
+  transport::SubscriberPtr command_sub_;
+  physics::ModelPtr model_;
+  physics::LinkPtr baseLink_;
+  physics::LinkPtr prop0Link_;
+  physics::LinkPtr prop1Link_;
+  physics::LinkPtr prop2Link_;
+  physics::LinkPtr prop3Link_;
+  event::ConnectionPtr updateConnection_;
+  boost::thread callback_queue_thread_;
+  void QueueThread();
+  void VelocityCallback(CommandMotorSpeedPtr &rot_velocities);
 
-  public:
-    GazeboUUVPlugin() :
-      ModelPlugin(),
-      namespace_(kDefaultNamespace),                        // definde namespace, topic and link_name
-      command_sub_topic_(kDefaultCommandSubTopic),
-      link_name_(kDefaultLinkName) {}
-
-    virtual ~GazeboUUVPlugin();
-
-  protected:
-    void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
-    void OnUpdate(const common::UpdateInfo&);
-
-  private:
-    event::ConnectionPtr update_connection_;
-
-    std::string namespace_;
-    std::string command_sub_topic_;
-    std::string link_name_;
-
-    transport::NodePtr node_handle_;
-    transport::SubscriberPtr command_sub_;
-
-    physics::LinkPtr link_;
-    physics::Link_V rotor_links_;
-
-    void CommandCallback(CommandMotorSpeedPtr &command);
-    double command_[4];
-
-    double last_time_;
-    double time_delta_;
-
-    double motor_force_constant_;
-    double motor_torque_constant_;
-
-    double X_u_;
-    double Y_v_;
-    double Z_w_;
-    double K_p_;
-    double M_q_;
-    double N_r_;
-
-    double X_udot_;
-    double Y_vdot_;
-    double Z_wdot_;
-    double K_pdot_;
-    double M_qdot_;
-    double N_rdot_;
-
-    // variables for debugging
-    double time_;
-    double counter_;
 };
-
 }