Skip to content

Commit aacd77e

Browse files
Tomas BacaTomas Baca
Tomas Baca
authored and
Tomas Baca
committed
+ midair activation
1 parent 16af66e commit aacd77e

File tree

3 files changed

+75
-60
lines changed

3 files changed

+75
-60
lines changed

‎CMakeLists.txt‎

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -49,6 +49,7 @@ set(LIBRARIES
4949
MrsUavControllers_MpcController
5050
MrsUavControllers_Se3Controller
5151
MrsUavControllers_FailsafeController
52+
MrsUavControllers_MidairActivationController
5253
)
5354

5455
foreach(DEP IN LISTS DEPENDENCIES)
@@ -104,6 +105,19 @@ ament_target_dependencies(MrsUavControllers_FailsafeController
104105

105106
target_compile_definitions(MrsUavControllers_FailsafeController PRIVATE USE_ROS_TIMER=${USE_ROS_TIMER})
106107

108+
# Midair Activation Controller
109+
110+
add_library(MrsUavControllers_MidairActivationController SHARED
111+
src/midair_activation_controller.cpp
112+
src/common.cpp
113+
)
114+
115+
ament_target_dependencies(MrsUavControllers_MidairActivationController
116+
${DEPENDENCIES}
117+
)
118+
119+
target_compile_definitions(MrsUavControllers_MidairActivationController PRIVATE USE_ROS_TIMER=${USE_ROS_TIMER})
120+
107121
## --------------------------------------------------------------
108122
## | export plugins |
109123
## --------------------------------------------------------------

‎plugins.xml‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -15,3 +15,9 @@
1515
<description>FailsafeController</description>
1616
</class>
1717
</library>
18+
19+
<library path="MrsUavControllers_MidairActivationController">
20+
<class type="mrs_uav_controllers::midair_activation_controller::MidairActivationController" base_class_type="mrs_uav_managers::Controller">
21+
<description>MidairActivationController</description>
22+
</class>
23+
</library>

‎src/midair_activation_controller.cpp‎

Lines changed: 55 additions & 60 deletions
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,6 @@
11
/* includes //{ */
22

3-
#include <ros/ros.h>
4-
#include <ros/package.h>
3+
#include <rclcpp/rclcpp.hpp>
54

65
#include <common.h>
76

@@ -12,6 +11,8 @@
1211
#include <mrs_lib/mutex.h>
1312
#include <mrs_lib/param_loader.h>
1413

14+
#include <ament_index_cpp/get_package_share_directory.hpp>
15+
1516
//}
1617

1718
namespace mrs_uav_controllers
@@ -25,27 +26,27 @@ namespace midair_activation_controller
2526
class MidairActivationController : public mrs_uav_managers::Controller {
2627

2728
public:
28-
bool initialize(const ros::NodeHandle &nh, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers,
29-
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, std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers);
3030

3131
bool activate(const ControlOutput &last_control_output);
3232

3333
void deactivate(void);
3434

35-
void updateInactive(const mrs_msgs::UavState &uav_state, const std::optional<mrs_msgs::TrackerCommand> &tracker_command);
35+
void updateInactive(const mrs_msgs::msg::UavState &uav_state, const std::optional<mrs_msgs::msg::TrackerCommand> &tracker_command);
3636

37-
ControlOutput updateActive(const mrs_msgs::UavState &uav_state, const mrs_msgs::TrackerCommand &tracker_command);
37+
ControlOutput updateActive(const mrs_msgs::msg::UavState &uav_state, const mrs_msgs::msg::TrackerCommand &tracker_command);
3838

39-
const mrs_msgs::ControllerStatus getStatus();
39+
const mrs_msgs::msg::ControllerStatus getStatus();
4040

41-
void switchOdometrySource(const mrs_msgs::UavState &new_uav_state);
41+
void switchOdometrySource(const mrs_msgs::msg::UavState &new_uav_state);
4242

4343
void resetDisturbanceEstimators(void);
4444

45-
const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd);
45+
const std::shared_ptr<mrs_msgs::srv::DynamicsConstraintsSrv::Response> setConstraints(const std::shared_ptr<mrs_msgs::srv::DynamicsConstraintsSrv::Request> &constraints);
4646

4747
private:
48-
ros::NodeHandle nh_;
48+
rclcpp::Node::SharedPtr node_;
49+
rclcpp::Clock::SharedPtr clock_;
4950

5051
bool is_initialized_ = false;
5152
bool is_active_ = false;
@@ -55,8 +56,8 @@ class MidairActivationController : public mrs_uav_managers::Controller {
5556

5657
// | ------------------------ uav state ----------------------- |
5758

58-
mrs_msgs::UavState uav_state_;
59-
std::mutex mutex_uav_state_;
59+
mrs_msgs::msg::UavState uav_state_;
60+
std::mutex mutex_uav_state_;
6061

6162
// | --------------------- thrust control --------------------- |
6263

@@ -72,7 +73,7 @@ class MidairActivationController : public mrs_uav_managers::Controller {
7273

7374
// | ------------------------ routines ------------------------ |
7475

75-
double getHeadingSafely(const mrs_msgs::UavState &uav_state, const mrs_msgs::TrackerCommand &tracker_command);
76+
double getHeadingSafely(const mrs_msgs::msg::UavState &uav_state, const mrs_msgs::msg::TrackerCommand &tracker_command);
7677
};
7778

7879
//}
@@ -83,46 +84,44 @@ class MidairActivationController : public mrs_uav_managers::Controller {
8384

8485
/* initialize() //{ */
8586

86-
bool MidairActivationController::initialize(const ros::NodeHandle &nh, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers,
87-
std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers) {
87+
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) {
8888

89-
nh_ = nh;
89+
node_ = node;
90+
clock_ = node->get_clock();
9091

9192
common_handlers_ = common_handlers;
9293
private_handlers_ = private_handlers;
9394

9495
_uav_mass_ = common_handlers->getMass();
9596

96-
ros::Time::waitForValid();
97-
9897
// | ---------- loading params using the parent's nh ---------- |
9998

10099
private_handlers->parent_param_loader->loadParamReusable("enable_profiler", _profiler_enabled_);
101100

102101
if (!private_handlers->parent_param_loader->loadedSuccessfully()) {
103-
ROS_ERROR("[MidairActivationController]: Could not load all parameters!");
102+
RCLCPP_ERROR(node_->get_logger(), "[MidairActivationController]: Could not load all parameters!");
104103
return false;
105104
}
106105

107106
// | -------------------- loading my params ------------------- |
108107

109-
private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/private/midair_activation_controller.yaml");
110-
private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/public/midair_activation_controller.yaml");
108+
private_handlers->param_loader->addYamlFile(ament_index_cpp::get_package_share_directory("mrs_uav_controllers") + "/config/private/midair_activation_controller.yaml");
109+
private_handlers->param_loader->addYamlFile(ament_index_cpp::get_package_share_directory("mrs_uav_controllers") + "/config/public/midair_activation_controller.yaml");
111110

112111
if (!private_handlers->param_loader->loadedSuccessfully()) {
113-
ROS_ERROR("[MidairActivationController]: Could not load all parameters!");
112+
RCLCPP_ERROR(node_->get_logger(), "[MidairActivationController]: Could not load all parameters!");
114113
return false;
115114
}
116115

117116
uav_mass_difference_ = 0;
118117

119118
// | ------------------------ profiler ------------------------ |
120119

121-
profiler_ = mrs_lib::Profiler(common_handlers->parent_nh, "MidairActivationController", _profiler_enabled_);
120+
profiler_ = mrs_lib::Profiler(common_handlers->parent_node, "MidairActivationController", _profiler_enabled_);
122121

123122
// | ----------------------- finish init ---------------------- |
124123

125-
ROS_INFO("[MidairActivationController]: initialized");
124+
RCLCPP_INFO(node_->get_logger(), "[MidairActivationController]: initialized");
126125

127126
is_initialized_ = true;
128127

@@ -135,15 +134,15 @@ bool MidairActivationController::initialize(const ros::NodeHandle &nh, std::shar
135134

136135
bool MidairActivationController::activate([[maybe_unused]] const ControlOutput &last_control_output) {
137136

138-
ROS_INFO("[MidairActivationController]: activating");
137+
RCLCPP_INFO(node_->get_logger(), "[MidairActivationController]: activating");
139138

140139
auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
141140

142141
hover_throttle_ = mrs_lib::quadratic_throttle_model::forceToThrottle(common_handlers_->throttle_model, _uav_mass_ * common_handlers_->g);
143142

144143
is_active_ = true;
145144

146-
ROS_INFO("[MidairActivationController]: activated, hover throttle %.2f", hover_throttle_);
145+
RCLCPP_INFO(node_->get_logger(), "[MidairActivationController]: activated, hover throttle %.2f", hover_throttle_);
147146

148147
return true;
149148
}
@@ -157,15 +156,14 @@ void MidairActivationController::deactivate(void) {
157156
is_active_ = false;
158157
uav_mass_difference_ = 0;
159158

160-
ROS_INFO("[MidairActivationController]: deactivated");
159+
RCLCPP_INFO(node_->get_logger(), "[MidairActivationController]: deactivated");
161160
}
162161

163162
//}
164163

165164
/* updateInactive() //{ */
166165

167-
void MidairActivationController::updateInactive(const mrs_msgs::UavState & uav_state,
168-
[[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand> &tracker_command) {
166+
void MidairActivationController::updateInactive(const mrs_msgs::msg::UavState &uav_state, [[maybe_unused]] const std::optional<mrs_msgs::msg::TrackerCommand> &tracker_command) {
169167

170168
mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
171169
}
@@ -174,12 +172,10 @@ void MidairActivationController::updateInactive(const mrs_msgs::UavState &
174172

175173
/* //{ updateWhenAcctive() */
176174

177-
MidairActivationController::ControlOutput MidairActivationController::updateActive(const mrs_msgs::UavState & uav_state,
178-
const mrs_msgs::TrackerCommand &tracker_command) {
175+
MidairActivationController::ControlOutput MidairActivationController::updateActive(const mrs_msgs::msg::UavState &uav_state, const mrs_msgs::msg::TrackerCommand &tracker_command) {
179176

180177
mrs_lib::Routine profiler_routine = profiler_.createRoutine("update");
181-
mrs_lib::ScopeTimer timer =
182-
mrs_lib::ScopeTimer("MidairActivationController::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
178+
mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer(node_, "MidairActivationController::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
183179

184180
mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
185181

@@ -197,7 +193,7 @@ MidairActivationController::ControlOutput MidairActivationController::updateActi
197193

198194
if (!highest_modality) {
199195

200-
ROS_ERROR_THROTTLE(1.0, "[MidairActivationController]: output modalities are empty! This error should never appear.");
196+
RCLCPP_ERROR_THROTTLE(node_->get_logger(), *clock_, 1000, "[MidairActivationController]: output modalities are empty! This error should never appear.");
201197

202198
return control_output;
203199
}
@@ -206,9 +202,9 @@ MidairActivationController::ControlOutput MidairActivationController::updateActi
206202

207203
case common::POSITION: {
208204

209-
mrs_msgs::HwApiPositionCmd cmd;
205+
mrs_msgs::msg::HwApiPositionCmd cmd;
210206

211-
cmd.header.stamp = ros::Time::now();
207+
cmd.header.stamp = clock_->now();
212208
cmd.header.frame_id = uav_state.header.frame_id;
213209

214210
cmd.position.x = uav_state.pose.position.x;
@@ -224,9 +220,9 @@ MidairActivationController::ControlOutput MidairActivationController::updateActi
224220

225221
case common::VELOCITY_HDG: {
226222

227-
mrs_msgs::HwApiVelocityHdgCmd cmd;
223+
mrs_msgs::msg::HwApiVelocityHdgCmd cmd;
228224

229-
cmd.header.stamp = ros::Time::now();
225+
cmd.header.stamp = clock_->now();
230226
cmd.header.frame_id = uav_state.header.frame_id;
231227

232228
cmd.velocity.x = uav_state.velocity.linear.x;
@@ -242,9 +238,9 @@ MidairActivationController::ControlOutput MidairActivationController::updateActi
242238

243239
case common::VELOCITY_HDG_RATE: {
244240

245-
mrs_msgs::HwApiVelocityHdgRateCmd cmd;
241+
mrs_msgs::msg::HwApiVelocityHdgRateCmd cmd;
246242

247-
cmd.header.stamp = ros::Time::now();
243+
cmd.header.stamp = clock_->now();
248244
cmd.header.frame_id = uav_state.header.frame_id;
249245

250246
cmd.velocity.x = uav_state.velocity.linear.x;
@@ -260,9 +256,9 @@ MidairActivationController::ControlOutput MidairActivationController::updateActi
260256

261257
case common::ACCELERATION_HDG: {
262258

263-
mrs_msgs::HwApiAccelerationHdgCmd cmd;
259+
mrs_msgs::msg::HwApiAccelerationHdgCmd cmd;
264260

265-
cmd.header.stamp = ros::Time::now();
261+
cmd.header.stamp = clock_->now();
266262
cmd.header.frame_id = uav_state.header.frame_id;
267263

268264
cmd.acceleration.x = uav_state.acceleration.linear.x;
@@ -278,9 +274,9 @@ MidairActivationController::ControlOutput MidairActivationController::updateActi
278274

279275
case common::ACCELERATION_HDG_RATE: {
280276

281-
mrs_msgs::HwApiAccelerationHdgRateCmd cmd;
277+
mrs_msgs::msg::HwApiAccelerationHdgRateCmd cmd;
282278

283-
cmd.header.stamp = ros::Time::now();
279+
cmd.header.stamp = clock_->now();
284280
cmd.header.frame_id = uav_state.header.frame_id;
285281

286282
cmd.acceleration.x = uav_state.acceleration.linear.x;
@@ -296,9 +292,9 @@ MidairActivationController::ControlOutput MidairActivationController::updateActi
296292

297293
case common::ATTITUDE: {
298294

299-
mrs_msgs::HwApiAttitudeCmd cmd;
295+
mrs_msgs::msg::HwApiAttitudeCmd cmd;
300296

301-
cmd.stamp = ros::Time::now();
297+
cmd.stamp = clock_->now();
302298

303299
cmd.orientation = uav_state.pose.orientation;
304300
cmd.throttle = hover_throttle_;
@@ -310,9 +306,9 @@ MidairActivationController::ControlOutput MidairActivationController::updateActi
310306

311307
case common::ATTITUDE_RATE: {
312308

313-
mrs_msgs::HwApiAttitudeRateCmd cmd;
309+
mrs_msgs::msg::HwApiAttitudeRateCmd cmd;
314310

315-
cmd.stamp = ros::Time::now();
311+
cmd.stamp = clock_->now();
316312

317313
cmd.body_rate.x = 0;
318314
cmd.body_rate.y = 0;
@@ -327,9 +323,9 @@ MidairActivationController::ControlOutput MidairActivationController::updateActi
327323

328324
case common::CONTROL_GROUP: {
329325

330-
mrs_msgs::HwApiControlGroupCmd cmd;
326+
mrs_msgs::msg::HwApiControlGroupCmd cmd;
331327

332-
cmd.stamp = ros::Time::now();
328+
cmd.stamp = clock_->now();
333329

334330
cmd.roll = 0;
335331
cmd.pitch = 0;
@@ -343,9 +339,9 @@ MidairActivationController::ControlOutput MidairActivationController::updateActi
343339

344340
case common::ACTUATORS_CMD: {
345341

346-
mrs_msgs::HwApiActuatorCmd cmd;
342+
mrs_msgs::msg::HwApiActuatorCmd cmd;
347343

348-
cmd.stamp = ros::Time::now();
344+
cmd.stamp = clock_->now();
349345

350346
for (int i = 0; i < common_handlers_->throttle_model.n_motors; i++) {
351347
cmd.motors.push_back(hover_throttle_);
@@ -364,9 +360,9 @@ MidairActivationController::ControlOutput MidairActivationController::updateActi
364360

365361
/* getStatus() //{ */
366362

367-
const mrs_msgs::ControllerStatus MidairActivationController::getStatus() {
363+
const mrs_msgs::msg::ControllerStatus MidairActivationController::getStatus() {
368364

369-
mrs_msgs::ControllerStatus controller_status;
365+
mrs_msgs::msg::ControllerStatus controller_status;
370366

371367
controller_status.active = is_active_;
372368

@@ -377,7 +373,7 @@ const mrs_msgs::ControllerStatus MidairActivationController::getStatus() {
377373

378374
/* switchOdometrySource() //{ */
379375

380-
void MidairActivationController::switchOdometrySource([[maybe_unused]] const mrs_msgs::UavState &new_uav_state) {
376+
void MidairActivationController::switchOdometrySource([[maybe_unused]] const mrs_msgs::msg::UavState &new_uav_state) {
381377
}
382378

383379
//}
@@ -391,17 +387,16 @@ void MidairActivationController::resetDisturbanceEstimators(void) {
391387

392388
/* setConstraints() //{ */
393389

394-
const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr MidairActivationController::setConstraints([
395-
[maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &constraints) {
390+
const std::shared_ptr<mrs_msgs::srv::DynamicsConstraintsSrv::Response> MidairActivationController::setConstraints([[maybe_unused]] const std::shared_ptr<mrs_msgs::srv::DynamicsConstraintsSrv::Request> &constraints) {
396391

397-
return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse());
392+
return nullptr;
398393
}
399394

400395
//}
401396

402397
/* getHeadingSafely() //{ */
403398

404-
double MidairActivationController::getHeadingSafely(const mrs_msgs::UavState &uav_state, const mrs_msgs::TrackerCommand &tracker_command) {
399+
double MidairActivationController::getHeadingSafely(const mrs_msgs::msg::UavState &uav_state, const mrs_msgs::msg::TrackerCommand &tracker_command) {
405400

406401
try {
407402
return mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
@@ -428,5 +423,5 @@ double MidairActivationController::getHeadingSafely(const mrs_msgs::UavState &ua
428423

429424
} // namespace mrs_uav_controllers
430425

431-
#include <pluginlib/class_list_macros.h>
426+
#include <pluginlib/class_list_macros.hpp>
432427
PLUGINLIB_EXPORT_CLASS(mrs_uav_controllers::midair_activation_controller::MidairActivationController, mrs_uav_managers::Controller)

0 commit comments

Comments
 (0)