+ setMass() and setGrondZ() services

include/mrs_multirotor_simulator/uav_system/multirotor_model.hpp

@@ -373,10 +373,6 @@ void MultirotorModel::operator()(const MultirotorModel::InternalState& x, Multir
 void MultirotorModel::setParams(const MultirotorModel::ModelParams& params) {
 
   params_ = params;
-
-  initializeState();
-
-  updateInternalState();
 }
 
 //}

include/mrs_multirotor_simulator/uav_system/uav_system.hpp

@@ -62,6 +62,8 @@ class UavSystem {
   MultirotorModel::State       getState(void);
   MultirotorModel::ModelParams getParams(void);
 
+  void setParams(const MultirotorModel::ModelParams& params);
+
   Eigen::Vector3d getImuAcceleration(void);
 
   void setMixerParams(const Mixer::Params& params);
@@ -392,6 +394,17 @@ MultirotorModel::ModelParams UavSystem::getParams(void) {
 
 //}
 
+/* setParams() //{ */
+
+void UavSystem::setParams(const MultirotorModel::ModelParams& params) {
+
+  multirotor_model_.setParams(params);
+
+  initializeControllers();
+}
+
+//}
+
 /* getMixerAllocation() //{ */
 
 Eigen::MatrixXd UavSystem::getMixerAllocation(void) {

include/mrs_multirotor_simulator/uav_system_ros.h

@@ -17,6 +17,7 @@
 #include <sensor_msgs/Imu.h>
 #include <sensor_msgs/Range.h>
 #include <nav_msgs/Odometry.h>
+#include <mrs_msgs/Float64Srv.h>
 
 #include <mrs_msgs/HwApiActuatorCmd.h>
 #include <mrs_msgs/HwApiControlGroupCmd.h>
@@ -121,6 +122,20 @@ class UavSystemRos {
   mrs_lib::SubscribeHandler<mrs_msgs::HwApiVelocityHdgCmd>         sh_velocity_hdg_cmd_;
   mrs_lib::SubscribeHandler<mrs_msgs::HwApiPositionCmd>            sh_position_cmd_;
   mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand>              sh_tracker_cmd_;
+
+  // | --------------------- service servers -------------------- |
+
+  ros::ServiceServer service_server_set_mass_;
+
+  ros::ServiceServer service_server_set_ground_z_;
+
+  bool callbackSetMass(mrs_msgs::Float64Srv::Request& req, mrs_msgs::Float64Srv::Response& res);
+
+  bool callbackSetGroundZ(mrs_msgs::Float64Srv::Request& req, mrs_msgs::Float64Srv::Response& res);
+
+  // | ------------------------ routines ------------------------ |
+
+  void calculateInertia(MultirotorModel::ModelParams& params);
 };
 
 }  // namespace mrs_multirotor_simulator

launch/multirotor_simulator.launch

@@ -50,17 +50,6 @@
     <remap from="~clock_out" to="/clock" />
     <remap from="~uav_poses_out" to="~uav_poses" />
 
-    <remap from="~actuator_cmd_in" to="~actuator_cmd" />
-    <remap from="~control_group_cmd_in" to="~control_group_cmd" />
-    <remap from="~attitude_rate_cmd_in" to="~attitude_rate_cmd" />
-    <remap from="~attitude_cmd_in" to="~attitude_cmd" />
-    <remap from="~acceleration_cmd_in" to="~acceleration_cmd" />
-    <remap from="~velocity_cmd_in" to="~velocity_cmd" />
-    <remap from="~position_cmd_in" to="~position_cmd" />
-
-    <remap from="~arm_in" to="~arm" />
-    <remap from="~offboard_in" to="~offboard" />
-
   </node>
 
 </launch>

src/uav_system_ros.cpp

@@ -93,13 +93,7 @@ UavSystemRos::UavSystemRos(ros::NodeHandle &nh, const std::string uav_name) {
     spawn_heading += randd(-3.14, 3.14);
   }
 
-  // create the inertia matrix
-  model_params_.J = Eigen::Matrix3d::Zero();
-  model_params_.J(0, 0) =
-      model_params_.mass * (3.0 * model_params_.arm_length * model_params_.arm_length + model_params_.body_height * model_params_.body_height) / 12.0;
-  model_params_.J(1, 1) =
-      model_params_.mass * (3.0 * model_params_.arm_length * model_params_.arm_length + model_params_.body_height * model_params_.body_height) / 12.0;
-  model_params_.J(2, 2) = (model_params_.mass * model_params_.arm_length * model_params_.arm_length) / 2.0;
+  calculateInertia(model_params_);
 
   model_params_.allocation_matrix = param_loader.loadMatrixDynamic2(type + "/propulsion/allocation_matrix", 4, -1);
 
@@ -214,6 +208,12 @@ UavSystemRos::UavSystemRos(ros::NodeHandle &nh, const std::string uav_name) {
 
   tf_broadcaster_ = std::make_shared<mrs_lib::TransformBroadcaster>();
 
+  // | --------------------- service servers -------------------- |
+
+  service_server_set_mass_ = nh.advertiseService(uav_name + "/set_mass", &UavSystemRos::callbackSetMass, this);
+
+  service_server_set_ground_z_ = nh.advertiseService(uav_name + "/set_ground_z", &UavSystemRos::callbackSetGroundZ, this);
+
   // | ------------------ first model iteration ----------------- |
 
   // * we need to iterate the model first to initialize its state
@@ -657,6 +657,19 @@ double UavSystemRos::randd(double from, double to) {
 
 //}
 
+/* calculateInertia() //{ */
+
+void UavSystemRos::calculateInertia(MultirotorModel::ModelParams &params) {
+
+  // create the inertia matrix
+  params.J       = Eigen::Matrix3d::Zero();
+  params.J(0, 0) = params.mass * (3.0 * params.arm_length * params.arm_length + params.body_height * params.body_height) / 12.0;
+  params.J(1, 1) = params.mass * (3.0 * params.arm_length * params.arm_length + params.body_height * params.body_height) / 12.0;
+  params.J(2, 2) = (params.mass * params.arm_length * params.arm_length) / 2.0;
+}
+
+//}
+
 // | ------------------------ callbacks ----------------------- |
 
 /* callbackActuatorCmd() //{ */
@@ -1008,4 +1021,62 @@ void UavSystemRos::callbackTrackerCmd(const mrs_msgs::TrackerCommand::ConstPtr m
 
 //}
 
+/* callbackSetMass() //{ */
+
+bool UavSystemRos::callbackSetMass(mrs_msgs::Float64Srv::Request &req, mrs_msgs::Float64Srv::Response &res) {
+
+  if (!is_initialized_) {
+    return false;
+  }
+
+  {
+    std::scoped_lock lock(mutex_uav_system_);
+
+    model_params_ = uav_system_.getParams();
+
+    const double original_mass = model_params_.mass;
+
+    model_params_.mass = req.value;
+
+    model_params_.allocation_matrix.row(2) = model_params_.mass * (model_params_.allocation_matrix.row(2) / original_mass);
+
+    calculateInertia(model_params_);
+
+    uav_system_.setParams(model_params_);
+  }
+
+  res.success = true;
+  res.message = "mass set";
+
+  return true;
+}
+
+//}
+
+/* callbackSetGroundZ() //{ */
+
+bool UavSystemRos::callbackSetGroundZ(mrs_msgs::Float64Srv::Request &req, mrs_msgs::Float64Srv::Response &res) {
+
+  if (!is_initialized_) {
+    return false;
+  }
+
+  {
+    std::scoped_lock lock(mutex_uav_system_);
+
+    model_params_ = uav_system_.getParams();
+
+    model_params_.ground_z = req.value;
+
+    uav_system_.setParams(model_params_);
+  }
+
+  res.success = true;
+  res.message = "ground z set";
+
+  return true;
+}
+
+//}
+
 }  // namespace mrs_multirotor_simulator