Skip to content

Commit aec7b5f

Browse files
klaxalkklaxalk
committed
fixed param declaration in mpc controller
1 parent b846089 commit aec7b5f

File tree

4 files changed

+31
-111
lines changed

4 files changed

+31
-111
lines changed

‎CMakeLists.txt‎

Lines changed: 4 additions & 32 deletions
Original file line numberDiff line numberDiff line change
@@ -41,6 +41,7 @@ set(DEPENDENCIES
4141
sensor_msgs
4242
mrs_uav_managers
4343
Eigen3
44+
mrs_mpc_solvers
4445
)
4546

4647
set(LIBRARIES
@@ -56,50 +57,21 @@ include_directories(
5657
${rclcpp_INCLUDE_DIRS}
5758
${mrs_lib_INCLUDE_DIRS}
5859
${mrs_uav_managers_INCLUDE_DIRS}
60+
${mrs_mpc_solvers_INCLUDE_DIRS}
5961
)
6062

61-
# Mpc Solver Library
62-
63-
# Store in CMAKE_DEB_HOST_ARCH var the current build architecture
64-
execute_process(COMMAND
65-
dpkg-architecture
66-
-qDEB_HOST_ARCH
67-
OUTPUT_VARIABLE
68-
CMAKE_DEB_HOST_ARCH
69-
OUTPUT_STRIP_TRAILING_WHITESPACE
70-
)
71-
72-
# deduce the library path based on the system architecture
73-
if(${CMAKE_DEB_HOST_ARCH} MATCHES "armhf")
74-
MESSAGE(FATAL_ERROR "Mising MpcControllerSolver.so for armhf")
75-
elseif(${CMAKE_DEB_HOST_ARCH} MATCHES "i386")
76-
MESSAGE(FATAL_ERROR "Mising MpcControllerSolver.so for i386")
77-
elseif(${CMAKE_DEB_HOST_ARCH} MATCHES "x64")
78-
set(MPC_CONTROLLER_SOLVER_BIN ${PROJECT_SOURCE_DIR}/lib/MpcControllerSolver/x64/libMpcControllerSolver.so)
79-
elseif(${CMAKE_DEB_HOST_ARCH} MATCHES "amd64")
80-
set(MPC_CONTROLLER_SOLVER_BIN ${PROJECT_SOURCE_DIR}/lib/MpcControllerSolver/x64/libMpcControllerSolver.so)
81-
elseif(${CMAKE_DEB_HOST_ARCH} MATCHES "arm64")
82-
set(MPC_CONTROLLER_SOLVER_BIN ${PROJECT_SOURCE_DIR}/lib/MpcControllerSolver/arm64/libMpcControllerSolver.so)
83-
else()
84-
MESSAGE(FATAL_ERROR ${CMAKE_DEB_HOST_ARCH})
85-
MESSAGE(FATAL_ERROR "MpcControllerSolver.so has not been selected, check CMakeLists.txt.")
86-
endif()
87-
88-
# Common estimators
63+
# MPC Controller
8964

9065
add_library(MrsUavControllers_MpcController SHARED
9166
src/mpc_controller.cpp
9267
src/common.cpp
9368
)
9469

9570
ament_target_dependencies(MrsUavControllers_MpcController
71+
mrs_mpc_solvers
9672
${DEPENDENCIES}
9773
)
9874

99-
target_link_libraries(MrsUavControllers_MpcController
100-
${MPC_CONTROLLER_SOLVER_BIN}
101-
)
102-
10375
target_compile_definitions(MrsUavControllers_MpcController PRIVATE USE_ROS_TIMER=${USE_ROS_TIMER})
10476

10577
## --------------------------------------------------------------

‎include/mpc_controller.h‎

Lines changed: 0 additions & 56 deletions
This file was deleted.

‎package.xml‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -30,6 +30,7 @@
3030
<depend>mrs_lib</depend>
3131
<depend>mrs_msgs</depend>
3232
<depend>mrs_uav_managers</depend>
33+
<depend>mrs_mpc_solvers</depend>
3334

3435
<export>
3536
<build_type>ament_cmake</build_type>

‎src/mpc_controller.cpp‎

Lines changed: 26 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,7 @@
77

88
#include <mrs_uav_managers/controller.h>
99

10-
#include <mpc_controller.h>
10+
#include <mrs_mpc_solvers/mpc_controller.h>
1111

1212
#include <std_srvs/srv/set_bool.hpp>
1313

@@ -295,6 +295,8 @@ bool MpcController::initialize(const rclcpp::Node::SharedPtr &node, std::shared_
295295
node_ = node;
296296
clock_ = node->get_clock();
297297

298+
RCLCPP_INFO(node_->get_logger(), "initializing");
299+
298300
common_handlers_ = common_handlers;
299301
private_handlers_ = private_handlers;
300302

@@ -461,10 +463,10 @@ bool MpcController::initialize(const rclcpp::Node::SharedPtr &node, std::shared_
461463

462464
param_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
463465

464-
node_->declare_parameter("horizontal.kiwxy", 0.0, param_desc);
465-
node_->declare_parameter("horizontal.kibxy", 0.0, param_desc);
466-
node_->declare_parameter("horizontal.kiwxy_lim", 0.0, param_desc);
467-
node_->declare_parameter("horizontal.kibxy_lim", 0.0, param_desc);
466+
node_->declare_parameter(node_->get_sub_namespace() + "horizontal.kiwxy", 0.0, param_desc);
467+
node_->declare_parameter(node_->get_sub_namespace() + "horizontal.kibxy", 0.0, param_desc);
468+
node_->declare_parameter(node_->get_sub_namespace() + "horizontal.kiwxy_lim", 0.0, param_desc);
469+
node_->declare_parameter(node_->get_sub_namespace() + "horizontal.kibxy_lim", 0.0, param_desc);
468470
}
469471

470472
{
@@ -479,7 +481,7 @@ bool MpcController::initialize(const rclcpp::Node::SharedPtr &node, std::shared_
479481

480482
param_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
481483

482-
node_->declare_parameter("attitude.kq_roll_pitch", 0.0, param_desc);
484+
node_->declare_parameter(node_->get_sub_namespace() + "attitude.kq_roll_pitch", 0.0, param_desc);
483485
}
484486

485487
{
@@ -494,7 +496,7 @@ bool MpcController::initialize(const rclcpp::Node::SharedPtr &node, std::shared_
494496

495497
param_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
496498

497-
node_->declare_parameter("attitude.kq_yaw", 0.0, param_desc);
499+
node_->declare_parameter(node_->get_sub_namespace() + "attitude.kq_yaw", 0.0, param_desc);
498500
}
499501

500502
{
@@ -509,15 +511,15 @@ bool MpcController::initialize(const rclcpp::Node::SharedPtr &node, std::shared_
509511

510512
param_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
511513

512-
node_->declare_parameter("mass.km", 0.0, param_desc);
514+
node_->declare_parameter(node_->get_sub_namespace() + "mass.km", 0.0, param_desc);
513515
}
514516

515517
{
516518
auto param_desc = rcl_interfaces::msg::ParameterDescriptor{};
517519

518520
param_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
519521

520-
node_->declare_parameter("mass.fuse_acceleration", false, param_desc);
522+
node_->declare_parameter(node_->get_sub_namespace() + "mass.fuse_acceleration", false, param_desc);
521523
}
522524

523525
{
@@ -532,7 +534,7 @@ bool MpcController::initialize(const rclcpp::Node::SharedPtr &node, std::shared_
532534

533535
param_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
534536

535-
node_->declare_parameter("mass.km_lim", 0.0, param_desc);
537+
node_->declare_parameter(node_->get_sub_namespace() + "mass.km_lim", 0.0, param_desc);
536538
}
537539

538540
{
@@ -547,15 +549,15 @@ bool MpcController::initialize(const rclcpp::Node::SharedPtr &node, std::shared_
547549

548550
param_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER;
549551

550-
node_->declare_parameter("preferred_output_mode", 0.0, param_desc);
552+
node_->declare_parameter(node_->get_sub_namespace() + "preferred_output_mode", 0.0, param_desc);
551553
}
552554

553555
{
554556
auto param_desc = rcl_interfaces::msg::ParameterDescriptor{};
555557

556558
param_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
557559

558-
node_->declare_parameter("jerk_feedforward", false, param_desc);
560+
node_->declare_parameter(node_->get_sub_namespace() + "jerk_feedforward", false, param_desc);
559561
}
560562

561563
drs_params_.kiwxy = gains_.kiwxy;
@@ -2292,17 +2294,18 @@ double MpcController::getHeadingSafely(const mrs_msgs::msg::UavState &uav_state,
22922294

22932295
void MpcController::setParamsToServer(const DrsParams_t &drs_params) {
22942296

2295-
node_->set_parameter(rclcpp::Parameter("mass.fuse_acceleration", drs_params.fuse_acceleration));
2296-
node_->set_parameter(rclcpp::Parameter("jerk_feedforward", drs_params.jerk_feedforward));
2297-
node_->set_parameter(rclcpp::Parameter("preferred_output_mode", drs_params.preferred_output_mode));
2298-
node_->set_parameter(rclcpp::Parameter("horizontal.kiwxy", drs_params.kiwxy));
2299-
node_->set_parameter(rclcpp::Parameter("horizontal.kibxy", drs_params.kibxy));
2300-
node_->set_parameter(rclcpp::Parameter("attitude.kq_roll_pitch", drs_params.kq_roll_pitch));
2301-
node_->set_parameter(rclcpp::Parameter("attitude.kq_yaw", drs_params.kq_yaw));
2302-
node_->set_parameter(rclcpp::Parameter("mass.km", drs_params.km));
2303-
node_->set_parameter(rclcpp::Parameter("mass.km_lim", drs_params.km_lim));
2304-
node_->set_parameter(rclcpp::Parameter("mass.kiwxy_lim", drs_params.kiwxy_lim));
2305-
node_->set_parameter(rclcpp::Parameter("mass.kibxy_lim", drs_params.kibxy_lim));
2297+
node_->set_parameter(rclcpp::Parameter(node_->get_sub_namespace() + "mass.fuse_acceleration", drs_params.fuse_acceleration));
2298+
node_->set_parameter(rclcpp::Parameter(node_->get_sub_namespace() + "jerk_feedforward", drs_params.jerk_feedforward));
2299+
node_->set_parameter(rclcpp::Parameter(node_->get_sub_namespace() + "preferred_output_mode", drs_params.preferred_output_mode));
2300+
node_->set_parameter(rclcpp::Parameter(node_->get_sub_namespace() + "horizontal.kiwxy", drs_params.kiwxy));
2301+
node_->set_parameter(rclcpp::Parameter(node_->get_sub_namespace() + "horizontal.kibxy", drs_params.kibxy));
2302+
node_->set_parameter(rclcpp::Parameter(node_->get_sub_namespace() + "horizontal.kiwxy_lim", drs_params.kiwxy_lim));
2303+
node_->set_parameter(rclcpp::Parameter(node_->get_sub_namespace() + "horizontal.kibxy_lim", drs_params.kibxy_lim));
2304+
node_->set_parameter(rclcpp::Parameter(node_->get_sub_namespace() + "attitude.kq_roll_pitch", drs_params.kq_roll_pitch));
2305+
node_->set_parameter(rclcpp::Parameter(node_->get_sub_namespace() + "attitude.kq_yaw", drs_params.kq_yaw));
2306+
node_->set_parameter(rclcpp::Parameter(node_->get_sub_namespace() + "mass.km", drs_params.km));
2307+
node_->set_parameter(rclcpp::Parameter(node_->get_sub_namespace() + "mass.km_lim", drs_params.km_lim));
2308+
node_->set_parameter(rclcpp::Parameter(node_->get_sub_namespace() + "mass.fuse_acceleration", drs_params.fuse_acceleration));
23062309
}
23072310

23082311
//}

0 commit comments

Comments
 (0)