feat: debbuging - commands are received

include/gazebo_coldgas_thruster_plugin.h

@@ -53,7 +53,7 @@
 namespace gazebo {
 // Default values
 static const std::string kDefaultNamespace = "";
-static const std::string kDefaultCommandSubTopic = "/gazebo/command/motor_speed";
+static const std::string kDefaultCommandSubTopic = "/gazebo/command/actuator_outputs";
 
 typedef const boost::shared_ptr<const mav_msgs::msgs::CommandMotorSpeed> CommandMotorSpeedPtr;
 

include/gazebo_mavlink_interface.h

@@ -106,6 +106,7 @@ static const std::string kDefaultNamespace = "";
 // This just proxies the motor commands from command/motor_speed to the single motors via internal
 // ConsPtr passing, such that the original commands don't have to go n_motors-times over the wire.
 static const std::string kDefaultMotorVelocityReferencePubTopic = "/gazebo/command/motor_speed";
+static const std::string kDefaultActuatorOutputsReferencePubTopic = "/gazebo/command/actuator_outputs";
 
 static const std::string kDefaultImuTopic = "/imu";
 static const std::string kDefaultOpticalFlowTopic = "/px4flow/link/opticalFlow";
@@ -145,6 +146,7 @@ class GazeboMavlinkInterface : public ModelPlugin {
   void OnUpdate(const common::UpdateInfo&  /*_info*/);
 
 private:
+  bool pub_actuator_outputs_{false};
   bool received_first_actuator_{false};
   Eigen::VectorXd input_reference_;
 
@@ -154,11 +156,13 @@ class GazeboMavlinkInterface : public ModelPlugin {
 
   std::string namespace_{kDefaultNamespace};
   std::string motor_velocity_reference_pub_topic_{kDefaultMotorVelocityReferencePubTopic};
+  std::string actuator_outputs_pub_topic_{kDefaultMotorVelocityReferencePubTopic};
   std::string mavlink_control_sub_topic_;
   std::string link_name_;
 
   transport::NodePtr node_handle_;
   transport::PublisherPtr motor_velocity_reference_pub_;
+  transport::PublisherPtr actuator_outputs_reference_pub_;
   transport::SubscriberPtr mav_control_sub_;
 
   physics::ModelPtr model_{};

src/gazebo_camera_manager_plugin.cpp

@@ -639,8 +639,7 @@ void CameraManagerPlugin::_handle_camera_info(const mavlink_message_t *pMsg, str
         0,                         // lens_id
         camera_capabilities,       // CAP_FLAGS
         0,                         // Camera Definition Version
-        uri,                       // URI
-        0                          // uint8_t gimbal_device_id
+        uri                       // URI
     );
     _send_mavlink_message(&msg, srcaddr);
 }

src/gazebo_coldgas_thruster_plugin.cpp

@@ -52,27 +52,37 @@ void GazeboColdGasThrusterPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr
   if (_sdf->HasElement("robotNamespace"))
     namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
   else
-    gzerr << "[gazebo_motor_model] Please specify a robotNamespace.\n";
+    gzerr << "[gazebo_thruster_model] Please specify a robotNamespace.\n";
+
+  std::cout << "Robot namespace: " << namespace_ << std::endl;
   node_handle_ = transport::NodePtr(new transport::Node());
   node_handle_->Init(namespace_);
 
   if (_sdf->HasElement("linkName"))
     link_name_ = _sdf->GetElement("linkName")->Get<std::string>();
   else
-    gzerr << "[gazebo_motor_model] Please specify a linkName of the rotor.\n";
+    gzerr << "[gazebo_thruster_model] Please specify a linkName of the thruster.\n";
+
+  // removing this for loop breaks it - maybe race condition?
+  for (auto link : model_->GetLinks()) {
+    std::cout << "Link name: " << link->GetName() << std::endl;
+  }
+
   link_ = model_->GetLink(link_name_);
+  std::cout << "Parsed link name: " << link_name_ << std::endl;
   if (link_ == NULL)
-    gzthrow("[gazebo_motor_model] Couldn't find specified link \"" << link_name_ << "\".");
+    gzthrow("[gazebo_thruster_model] Couldn't find specified link \"" << link_name_ << "\".");
 
 
-  if (_sdf->HasElement("motorNumber"))
-    motor_number_ = _sdf->GetElement("motorNumber")->Get<int>();
+  if (_sdf->HasElement("thrusterNumber"))
+    motor_number_ = _sdf->GetElement("thrusterNumber")->Get<int>();
   else
-    gzerr << "[gazebo_motor_model] Please specify a motorNumber.\n";
+    gzerr << "[gazebo_thruster_model] Please specify a thrusterNumber.\n";
   getSdfParam<std::string>(_sdf, "commandSubTopic", command_sub_topic_, command_sub_topic_);
   getSdfParam<double>(_sdf, "pwmFrequency", pwm_frequency_, 10.0);
   getSdfParam<double>(_sdf, "maxThrust", max_thrust_, 1.4);
 
+
   // Listen to the update event. This event is broadcast every
   // simulation iteration.
   updateConnection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboColdGasThrusterPlugin::OnUpdate, this, _1));
@@ -102,7 +112,6 @@ void GazeboColdGasThrusterPlugin::VelocityCallback(CommandMotorSpeedPtr &rot_vel
 void GazeboColdGasThrusterPlugin::UpdateForcesAndMoments(const double &duty_cycle, const double &period) {
   // Thrust is only generated uring the duty cycle
   double force = duty_cycle > period ? max_thrust_ : 0.0;
-  // std::cout << "duty cycle: " << duty_cycle << " period: " << period << " thrust: " << force << std::endl;
   link_->AddRelativeForce(ignition::math::Vector3d(0, 0, force));
 }
 

src/gazebo_gimbal_controller_plugin.cpp

@@ -682,8 +682,7 @@ void GimbalControllerPlugin::SendGimbalDeviceInformation()
     pitchMin,
     pitchMax,
     yawMin,
-    yawMax,
-    0 /*gimbal_device_id*/);
+    yawMax);
   SendMavlinkMessage(msg);
 }
 
@@ -737,8 +736,7 @@ void GimbalControllerPlugin::SendGimbalDeviceAttitudeStatus()
     angularVelocity.Z(),
     failureFlags,
     NAN, // per mavlink spec - NAN if unknown
-    NAN, // per mavlink spec - NAN if unknown
-    0 /*gimbal_device_id*/);
+    NAN);
   SendMavlinkMessage(msg);
 }
 

src/gazebo_mavlink_interface.cpp

@@ -205,6 +205,11 @@ void GazeboMavlinkInterface::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf
 
   getSdfParam<std::string>(_sdf, "motorSpeedCommandPubTopic", motor_velocity_reference_pub_topic_,
       motor_velocity_reference_pub_topic_);
+  if (_sdf->HasElement("actuatorOutputsCommandPubTopic")) {
+    getSdfParam<std::string>(_sdf, "actuatorOutputsCommandPubTopic", actuator_outputs_pub_topic_,
+    actuator_outputs_pub_topic_);
+    pub_actuator_outputs_ = true;
+  }
   getSdfParam<std::string>(_sdf, "imuSubTopic", imu_sub_topic_, imu_sub_topic_);
   getSdfParam<std::string>(_sdf, "visionSubTopic", vision_sub_topic_, vision_sub_topic_);
   getSdfParam<std::string>(_sdf, "opticalFlowSubTopic",
@@ -237,10 +242,6 @@ void GazeboMavlinkInterface::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf
         int index = channel->Get<int>("input_index");
         if (index < n_out_max)
         {
-          input_offset_[index] = channel->Get<double>("input_offset");
-          input_scaling_[index] = channel->Get<double>("input_scaling");
-          zero_position_disarmed_[index] = channel->Get<double>("zero_position_disarmed");
-          zero_position_armed_[index] = channel->Get<double>("zero_position_armed");
           if (channel->HasElement("joint_control_type"))
           {
             joint_control_type_[index] = channel->Get<std::string>("joint_control_type");
@@ -251,6 +252,17 @@ void GazeboMavlinkInterface::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf
             joint_control_type_[index] = "velocity";
           }
 
+          if (joint_control_type_[index] == "bypass") {
+            channel = channel->GetNextElement("channel");
+            continue;
+          }
+
+          // handle velocity/position/... types
+          input_offset_[index] = channel->Get<double>("input_offset");
+          input_scaling_[index] = channel->Get<double>("input_scaling");
+          zero_position_disarmed_[index] = channel->Get<double>("zero_position_disarmed");
+          zero_position_armed_[index] = channel->Get<double>("zero_position_armed");
+
           // start gz transport node handle
           if (joint_control_type_[index] == "position_gztopic")
           {
@@ -453,6 +465,9 @@ void GazeboMavlinkInterface::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf
 
   // Publish gazebo's motor_speed message
   motor_velocity_reference_pub_ = node_handle_->Advertise<mav_msgs::msgs::CommandMotorSpeed>("~/" + model_->GetName() + motor_velocity_reference_pub_topic_, 1);
+
+  // Publish unalduterated actuator outputs
+  actuator_outputs_reference_pub_ = node_handle_->Advertise<mav_msgs::msgs::CommandMotorSpeed>("~/" + model_->GetName() + actuator_outputs_pub_topic_, 1);
 
 #if GAZEBO_MAJOR_VERSION >= 9
   last_time_ = world_->SimTime();
@@ -1183,19 +1198,28 @@ void GazeboMavlinkInterface::handle_actuator_controls() {
   // Read Input References
   input_reference_.resize(n_out_max);
 
+  // Parse received actuator controls
+  mav_msgs::msgs::CommandMotorSpeed actuator_outputs_msg;
   Eigen::VectorXd actuator_controls = mavlink_interface_->GetActuatorControls();
   if (actuator_controls.size() < n_out_max) return; //TODO: Handle this properly
   for (int i = 0; i < input_reference_.size(); i++) {
+    // do usual joint control
     if (armed) {
       input_reference_[i] = (actuator_controls[input_index_[i]] + input_offset_[i])
           * input_scaling_[i] + zero_position_armed_[i];
-      // std::cout << input_reference_ << ", ";
     } else {
       input_reference_[i] = zero_position_disarmed_[i];
-      // std::cout << input_reference_ << ", ";
+    }
+
+    // additional actions for bypass joint mode
+    if (pub_actuator_outputs_) {
+      actuator_outputs_msg.add_motor_speed(actuator_controls[input_index_[i]]);
     }
   }
-  // std::cout << "Input Reference: " << input_reference_.transpose() << std::endl;
+
+  // publish if timestamp is good
+  actuator_outputs_reference_pub_->Publish(actuator_outputs_msg);
+
   received_first_actuator_ = mavlink_interface_->GetReceivedFirstActuator();
 }