Skip to content

Commit 5b72abc

Browse files
Tomas BacaTomas Baca
Tomas Baca
authored and
Tomas Baca
committed
formatting, minor changes
1 parent ef7404a commit 5b72abc

File tree

4 files changed

+41
-23
lines changed

4 files changed

+41
-23
lines changed

‎src/common.cpp‎

Lines changed: 16 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -28,7 +28,8 @@ Eigen::Vector3d orientationError(const Eigen::Matrix3d& R, const Eigen::Matrix3d
2828

2929
/* sanitizeDesiredForce() //{ */
3030

31-
std::optional<Eigen::Vector3d> sanitizeDesiredForce(const rclcpp::Node::SharedPtr& node, const Eigen::Vector3d& input, const double& tilt_safety_limit, const double& tilt_saturation, const std::string& node_name) {
31+
std::optional<Eigen::Vector3d> sanitizeDesiredForce(const rclcpp::Node::SharedPtr& node, const Eigen::Vector3d& input, const double& tilt_safety_limit,
32+
const double& tilt_saturation, const std::string& node_name) {
3233

3334
// calculate the force in spherical coordinates
3435
double theta = acos(input(2));
@@ -42,7 +43,8 @@ std::optional<Eigen::Vector3d> sanitizeDesiredForce(const rclcpp::Node::SharedPt
4243

4344
if (tilt_safety_limit > 1e-3 && std::abs(theta) > tilt_safety_limit) {
4445

45-
RCLCPP_ERROR(node->get_logger(), "[%s]: the produced tilt angle (%.2f deg) would be over the failsafe limit (%.2f deg), returning null", node_name.c_str(), (180.0 / M_PI) * theta, (180.0 / M_PI) * tilt_safety_limit);
46+
RCLCPP_ERROR(node->get_logger(), "[%s]: the produced tilt angle (%.2f deg) would be over the failsafe limit (%.2f deg), returning null", node_name.c_str(),
47+
(180.0 / M_PI) * theta, (180.0 / M_PI) * tilt_safety_limit);
4648
RCLCPP_ERROR_STREAM(node->get_logger(), "[" << node_name << "]: f = [" << input.transpose() << "]");
4749

4850
return {};
@@ -51,7 +53,8 @@ std::optional<Eigen::Vector3d> sanitizeDesiredForce(const rclcpp::Node::SharedPt
5153
// saturate the angle
5254

5355
if (tilt_saturation > 1e-3 && std::abs(theta) > tilt_saturation) {
54-
RCLCPP_WARN_THROTTLE(node->get_logger(), *node->get_clock(), 1000, "[%s]: tilt is being saturated, desired: %.2f deg, saturated %.2f deg", node_name.c_str(), (theta / M_PI) * 180.0, (tilt_saturation / M_PI) * 180.0);
56+
RCLCPP_WARN_THROTTLE(node->get_logger(), *node->get_clock(), 1000, "[%s]: tilt is being saturated, desired: %.2f deg, saturated %.2f deg",
57+
node_name.c_str(), (theta / M_PI) * 180.0, (tilt_saturation / M_PI) * 180.0);
5558
theta = tilt_saturation;
5659
}
5760

@@ -65,7 +68,8 @@ std::optional<Eigen::Vector3d> sanitizeDesiredForce(const rclcpp::Node::SharedPt
6568

6669
/* so3transform() //{ */
6770

68-
Eigen::Matrix3d so3transform(const rclcpp::Node::SharedPtr& node, const Eigen::Vector3d& body_z, const ::Eigen::Vector3d& heading, const bool& preserve_heading) {
71+
Eigen::Matrix3d so3transform(const rclcpp::Node::SharedPtr& node, const Eigen::Vector3d& body_z, const ::Eigen::Vector3d& heading,
72+
const bool& preserve_heading) {
6973

7074
Eigen::Vector3d body_z_normed = body_z.normalized();
7175

@@ -227,7 +231,10 @@ std::optional<double> extractThrottle(const mrs_uav_managers::Controller::Contro
227231

228232
/* attitudeController() //{ */
229233

230-
std::optional<mrs_msgs::msg::HwApiAttitudeRateCmd> attitudeController(const rclcpp::Node::SharedPtr& node, const mrs_msgs::msg::UavState& uav_state, const mrs_msgs::msg::HwApiAttitudeCmd& reference, const Eigen::Vector3d& ff_rate, const Eigen::Vector3d& rate_saturation, const Eigen::Vector3d& gains, const bool& parasitic_heading_rate_compensation) {
234+
std::optional<mrs_msgs::msg::HwApiAttitudeRateCmd> attitudeController(const rclcpp::Node::SharedPtr& node, const mrs_msgs::msg::UavState& uav_state,
235+
const mrs_msgs::msg::HwApiAttitudeCmd& reference, const Eigen::Vector3d& ff_rate,
236+
const Eigen::Vector3d& rate_saturation, const Eigen::Vector3d& gains,
237+
const bool& parasitic_heading_rate_compensation) {
231238

232239
Eigen::Matrix3d R = mrs_lib::AttitudeConverter(uav_state.pose.orientation);
233240
Eigen::Matrix3d Rd = mrs_lib::AttitudeConverter(reference.orientation);
@@ -307,7 +314,8 @@ std::optional<mrs_msgs::msg::HwApiAttitudeRateCmd> attitudeController(const rclc
307314

308315
/* attitudeRateController() //{ */
309316

310-
std::optional<mrs_msgs::msg::HwApiControlGroupCmd> attitudeRateController(const rclcpp::Node::SharedPtr& node, const mrs_msgs::msg::UavState& uav_state, const mrs_msgs::msg::HwApiAttitudeRateCmd& reference, const Eigen::Vector3d& gains) {
317+
std::optional<mrs_msgs::msg::HwApiControlGroupCmd> attitudeRateController(const rclcpp::Node::SharedPtr& node, const mrs_msgs::msg::UavState& uav_state,
318+
const mrs_msgs::msg::HwApiAttitudeRateCmd& reference, const Eigen::Vector3d& gains) {
311319

312320
Eigen::Vector3d des_rate(reference.body_rate.x, reference.body_rate.y, reference.body_rate.z);
313321
Eigen::Vector3d cur_rate(uav_state.velocity.angular.x, uav_state.velocity.angular.y, uav_state.velocity.angular.z);
@@ -331,7 +339,8 @@ std::optional<mrs_msgs::msg::HwApiControlGroupCmd> attitudeRateController(const
331339

332340
/* actuatorMixer() //{ */
333341

334-
mrs_msgs::msg::HwApiActuatorCmd actuatorMixer(const rclcpp::Node::SharedPtr& node, const mrs_msgs::msg::HwApiControlGroupCmd& ctrl_group_cmd, const Eigen::MatrixXd& mixer) {
342+
mrs_msgs::msg::HwApiActuatorCmd actuatorMixer(const rclcpp::Node::SharedPtr& node, const mrs_msgs::msg::HwApiControlGroupCmd& ctrl_group_cmd,
343+
const Eigen::MatrixXd& mixer) {
335344

336345
Eigen::Vector4d ctrl_group(ctrl_group_cmd.roll, ctrl_group_cmd.pitch, ctrl_group_cmd.yaw, ctrl_group_cmd.throttle);
337346

‎src/midair_activation_controller.cpp‎

Lines changed: 19 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -26,7 +26,8 @@ namespace midair_activation_controller
2626
class MidairActivationController : public mrs_uav_managers::Controller {
2727

2828
public:
29-
bool initialize(const rclcpp::Node::SharedPtr &node, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers, std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers);
29+
bool initialize(const rclcpp::Node::SharedPtr &node, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers,
30+
std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers);
3031

3132
void destroy();
3233

@@ -44,7 +45,8 @@ class MidairActivationController : public mrs_uav_managers::Controller {
4445

4546
void resetDisturbanceEstimators(void);
4647

47-
const std::shared_ptr<mrs_msgs::srv::DynamicsConstraintsSrv::Response> setConstraints(const std::shared_ptr<mrs_msgs::srv::DynamicsConstraintsSrv::Request> &constraints);
48+
const std::shared_ptr<mrs_msgs::srv::DynamicsConstraintsSrv::Response> setConstraints(
49+
const std::shared_ptr<mrs_msgs::srv::DynamicsConstraintsSrv::Request> &constraints);
4850

4951
private:
5052
rclcpp::Node::SharedPtr node_;
@@ -86,7 +88,9 @@ class MidairActivationController : public mrs_uav_managers::Controller {
8688

8789
/* initialize() //{ */
8890

89-
bool MidairActivationController::initialize(const rclcpp::Node::SharedPtr &node, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers, std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers) {
91+
bool MidairActivationController::initialize(const rclcpp::Node::SharedPtr &node,
92+
std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers,
93+
std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers) {
9094

9195
node_ = node;
9296
clock_ = node->get_clock();
@@ -107,8 +111,10 @@ bool MidairActivationController::initialize(const rclcpp::Node::SharedPtr &node,
107111

108112
// | -------------------- loading my params ------------------- |
109113

110-
private_handlers->param_loader->addYamlFile(ament_index_cpp::get_package_share_directory("mrs_uav_controllers") + "/config/private/midair_activation_controller.yaml");
111-
private_handlers->param_loader->addYamlFile(ament_index_cpp::get_package_share_directory("mrs_uav_controllers") + "/config/public/midair_activation_controller.yaml");
114+
private_handlers->param_loader->addYamlFile(ament_index_cpp::get_package_share_directory("mrs_uav_controllers") +
115+
"/config/private/midair_activation_controller.yaml");
116+
private_handlers->param_loader->addYamlFile(ament_index_cpp::get_package_share_directory("mrs_uav_controllers") +
117+
"/config/public/midair_activation_controller.yaml");
112118

113119
if (!private_handlers->param_loader->loadedSuccessfully()) {
114120
RCLCPP_ERROR(node_->get_logger(), "[MidairActivationController]: Could not load all parameters!");
@@ -135,7 +141,6 @@ bool MidairActivationController::initialize(const rclcpp::Node::SharedPtr &node,
135141
/* destroy() //{ */
136142

137143
void MidairActivationController::destroy() {
138-
139144
}
140145

141146
//}
@@ -173,7 +178,8 @@ void MidairActivationController::deactivate(void) {
173178

174179
/* updateInactive() //{ */
175180

176-
void MidairActivationController::updateInactive(const mrs_msgs::msg::UavState &uav_state, [[maybe_unused]] const std::optional<mrs_msgs::msg::TrackerCommand> &tracker_command) {
181+
void MidairActivationController::updateInactive(const mrs_msgs::msg::UavState &uav_state,
182+
[[maybe_unused]] const std::optional<mrs_msgs::msg::TrackerCommand> &tracker_command) {
177183

178184
mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
179185
}
@@ -182,10 +188,12 @@ void MidairActivationController::updateInactive(const mrs_msgs::msg::UavState &u
182188

183189
/* //{ updateWhenAcctive() */
184190

185-
MidairActivationController::ControlOutput MidairActivationController::updateActive(const mrs_msgs::msg::UavState &uav_state, const mrs_msgs::msg::TrackerCommand &tracker_command) {
191+
MidairActivationController::ControlOutput MidairActivationController::updateActive(const mrs_msgs::msg::UavState &uav_state,
192+
const mrs_msgs::msg::TrackerCommand &tracker_command) {
186193

187194
mrs_lib::Routine profiler_routine = profiler_.createRoutine("update");
188-
mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer(node_, "MidairActivationController::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
195+
mrs_lib::ScopeTimer timer =
196+
mrs_lib::ScopeTimer(node_, "MidairActivationController::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
189197

190198
mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
191199

@@ -397,7 +405,8 @@ void MidairActivationController::resetDisturbanceEstimators(void) {
397405

398406
/* setConstraints() //{ */
399407

400-
const std::shared_ptr<mrs_msgs::srv::DynamicsConstraintsSrv::Response> MidairActivationController::setConstraints([[maybe_unused]] const std::shared_ptr<mrs_msgs::srv::DynamicsConstraintsSrv::Request> &constraints) {
408+
const std::shared_ptr<mrs_msgs::srv::DynamicsConstraintsSrv::Response> MidairActivationController::setConstraints(
409+
[[maybe_unused]] const std::shared_ptr<mrs_msgs::srv::DynamicsConstraintsSrv::Request> &constraints) {
401410

402411
return nullptr;
403412
}

‎src/mpc_controller.cpp‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -119,7 +119,7 @@ class MpcController : public mrs_uav_managers::Controller {
119119
mrs_msgs::msg::UavState uav_state_;
120120
std::mutex mutex_uav_state_;
121121

122-
// | --------------- dynamic reconfigure server --------------- |
122+
// | ------------------- dynamic reconfigure ------------------ |
123123

124124
std::shared_ptr<mrs_lib::DynparamMgr> dynparam_mgr_;
125125

@@ -434,7 +434,7 @@ bool MpcController::initialize(const rclcpp::Node::SharedPtr &node, std::shared_
434434
mrs_lib::SubscriberHandlerOptions shopts;
435435

436436
shopts.node = node_;
437-
shopts.node_name = "Se3Controller";
437+
shopts.node_name = "MpcController";
438438
shopts.no_message_timeout = mrs_lib::no_timeout;
439439
shopts.threadsafe = true;
440440
shopts.autostart = true;
@@ -494,7 +494,7 @@ bool MpcController::initialize(const rclcpp::Node::SharedPtr &node, std::shared_
494494
// | --------------------- service servers -------------------- |
495495

496496
ss_set_integral_terms_ = mrs_lib::ServiceServerHandler<std_srvs::srv::SetBool>(
497-
node_, "set_integral_terms_in", std::bind(&MpcController::callbackSetIntegralTerms, this, std::placeholders::_1, std::placeholders::_2), cbkgrp_ss_);
497+
node_, "~/set_integral_terms", std::bind(&MpcController::callbackSetIntegralTerms, this, std::placeholders::_1, std::placeholders::_2), cbkgrp_ss_);
498498

499499
// | ------------------------- timers ------------------------- |
500500

@@ -862,7 +862,7 @@ const std::shared_ptr<mrs_msgs::srv::DynamicsConstraintsSrv::Response> MpcContro
862862
// | controllers |
863863
// --------------------------------------------------------------
864864

865-
/* Mpc() //{ */
865+
/* MPC() //{ */
866866

867867
void MpcController::MPC(const mrs_msgs::msg::UavState &uav_state, const mrs_msgs::msg::TrackerCommand &tracker_command, const double &dt,
868868
const common::CONTROL_OUTPUT &output_modality) {

‎src/se3_controller.cpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -119,7 +119,7 @@ class Se3Controller : public mrs_uav_managers::Controller {
119119
mrs_msgs::msg::UavState uav_state_;
120120
std::mutex mutex_uav_state_;
121121

122-
// | --------------- dynamic reconfigure server --------------- |
122+
// | ------------------- dynamic reconfigure ------------------ |
123123

124124
std::shared_ptr<mrs_lib::DynparamMgr> dynparam_mgr_;
125125

@@ -397,7 +397,7 @@ bool Se3Controller::initialize(const rclcpp::Node::SharedPtr& node, std::shared_
397397
Iw_w_ = Eigen::Vector2d::Zero(2);
398398
Ib_b_ = Eigen::Vector2d::Zero(2);
399399

400-
// | --------------- dynamic reconfigure server --------------- |
400+
// | ------------------- dynamic reconfigure ------------------ |
401401

402402
dynparam_mgr_->register_param("horizontal.kpxy", &drs_params_.kpxy, gains_.kpxy, mrs_lib::DynparamMgr::range_t<double>(0.0, 40.0));
403403
dynparam_mgr_->register_param("horizontal.kvxy", &drs_params_.kvxy, gains_.kvxy, mrs_lib::DynparamMgr::range_t<double>(0.0, 40.0));

0 commit comments

Comments
 (0)