Skip to content

Commit

Permalink
updated agains new param loader
Browse files Browse the repository at this point in the history
  • Loading branch information
@klaxalk
klaxalk committed Oct 13, 2023
1 parent 5d3b68a commit 3e13721
Show file tree
Hide file tree
Showing 21 changed files with 451 additions and 163 deletions.
2 changes: 1 addition & 1 deletion config/private/control_manager.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,7 +65,7 @@ mrs_uav_managers:
pitch: 20.0
yaw: 10.0

tilt: deg(60)
tilt: 60.0 # [deg]

scope_timer:

Expand Down
6 changes: 3 additions & 3 deletions config/public/constraint_manager/constraints.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@ mrs_uav_managers:
pitch: 60.0
yaw: 10.0

tilt: deg(60)
tilt: 60.0 # [deg]

medium:

Expand Down Expand Up @@ -70,7 +70,7 @@ mrs_uav_managers:
pitch: 60.0
yaw: 10.0

tilt: deg(60)
tilt: 60.0 # [deg]

fast:

Expand Down Expand Up @@ -105,4 +105,4 @@ mrs_uav_managers:
pitch: 60.0
yaw: 10.0

tilt: deg(60)
tilt: 60.0 # [deg]
8 changes: 4 additions & 4 deletions config/public/control_manager.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,20 +21,20 @@ mrs_uav_managers:

eland:
enabled: true
limit: deg(75) # [rad]
limit: 75.0 # [deg]

disarm:
enabled: true
limit: deg(75) # [rad]
limit: 75.0 # [deg]

tilt_error_disarm:
enabled: true
timeout: 0.5 # [s] # for how long the error has to be present to disarm
error_threshold: deg(20) # [rad]
error_threshold: 20.0 # [deg]

yaw_error_eland:
enabled: true
limit: deg(90) # [rad]
limit: 90.0 # [deg]

# tracker used for emergency hover routine
ehover_tracker: "LandoffTracker"
Expand Down
8 changes: 5 additions & 3 deletions include/mrs_uav_managers/control_manager/private_handlers.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@
#define CONTROL_MANAGER_PRIVATE_HANDLERS_H

#include <string>
#include <mrs_lib/param_loader.h>

namespace mrs_uav_managers
{
Expand All @@ -13,9 +14,10 @@ typedef boost::function<bool(const std::string)> loadConfigFile_t;

struct PrivateHandlers_t
{
loadConfigFile_t loadConfigFile;
std::string name_space;
std::string runtime_name;
loadConfigFile_t loadConfigFile;
std::unique_ptr<mrs_lib::ParamLoader> param_loader;
std::string name_space;
std::string runtime_name;
};

} // namespace control_manager
Expand Down
4 changes: 3 additions & 1 deletion include/mrs_uav_managers/estimation_manager/private_handlers.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@
#define ESTIMATION_MANAGER_PRIVATE_HANDLERS_H

#include <string>
#include <mrs_lib/param_loader.h>

namespace mrs_uav_managers
{
Expand All @@ -13,7 +14,8 @@ typedef boost::function<bool(const std::string)> loadConfigFile_t;

struct PrivateHandlers_t
{
loadConfigFile_t loadConfigFile;
std::shared_ptr<mrs_lib::ParamLoader> param_loader;
loadConfigFile_t loadConfigFile;
};

} // namespace estimation_manager
Expand Down
23 changes: 11 additions & 12 deletions include/transform_manager/tf_mapping_origin.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
#include <mrs_lib/subscribe_handler.h>
#include <mrs_lib/transform_broadcaster.h>
#include <mrs_lib/attitude_converter.h>
#include <mrs_lib/publisher_handler.h>

#include <nav_msgs/Odometry.h>
#include <geometry_msgs/QuaternionStamped.h>
Expand All @@ -26,29 +27,27 @@ class TfMappingOrigin {

public:
/*//{ constructor */
TfMappingOrigin(ros::NodeHandle nh, const std::shared_ptr<mrs_lib::TransformBroadcaster>& broadcaster,
TfMappingOrigin(ros::NodeHandle nh, std::shared_ptr<mrs_lib::ParamLoader> param_loader, const std::shared_ptr<mrs_lib::TransformBroadcaster>& broadcaster,
const std::shared_ptr<estimation_manager::CommonHandlers_t> ch)
: nh_(nh), broadcaster_(broadcaster), ch_(ch) {

ROS_INFO("[%s]: initializing", getPrintName().c_str());


mrs_lib::ParamLoader param_loader(nh_, getPrintName());

const std::string yaml_prefix = "mrs_uav_managers/transform_manager/mapping_origin_tf/";

/*//{ load mapping origin parameters */
param_loader.loadParam(yaml_prefix + "debug_prints", debug_prints_);
param_loader.loadParam(yaml_prefix + "lateral_topic", lateral_topic_);
param_loader.loadParam(yaml_prefix + "altitude_topic", altitude_topic_);
param_loader.loadParam(yaml_prefix + "orientation_topic", orientation_topic_);
param_loader.loadParam(yaml_prefix + "inverted", tf_inverted_);
param_loader.loadParam(yaml_prefix + "custom_frame_id/enabled", custom_frame_id_enabled_);
param_loader->loadParam(yaml_prefix + "debug_prints", debug_prints_);
param_loader->loadParam(yaml_prefix + "lateral_topic", lateral_topic_);
param_loader->loadParam(yaml_prefix + "altitude_topic", altitude_topic_);
param_loader->loadParam(yaml_prefix + "orientation_topic", orientation_topic_);
param_loader->loadParam(yaml_prefix + "inverted", tf_inverted_);
param_loader->loadParam(yaml_prefix + "custom_frame_id/enabled", custom_frame_id_enabled_);

if (custom_frame_id_enabled_) {
param_loader.loadParam(yaml_prefix + "custom_frame_id/frame_id", custom_frame_id_);
param_loader->loadParam(yaml_prefix + "custom_frame_id/frame_id", custom_frame_id_);
}

if (!param_loader.loadedSuccessfully()) {
if (!param_loader->loadedSuccessfully()) {
ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", getPrintName().c_str());
ros::shutdown();
}
Expand Down
37 changes: 19 additions & 18 deletions include/transform_manager/tf_source.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,66 +35,67 @@ class TfSource {

public:
/*//{ constructor */
TfSource(const std::string& name, ros::NodeHandle nh, const std::shared_ptr<mrs_lib::TransformBroadcaster>& broadcaster,
const std::shared_ptr<estimation_manager::CommonHandlers_t> ch, const bool is_utm_source)
TfSource(const std::string& name, ros::NodeHandle nh, std::shared_ptr<mrs_lib::ParamLoader> param_loader,
const std::shared_ptr<mrs_lib::TransformBroadcaster>& broadcaster, const std::shared_ptr<estimation_manager::CommonHandlers_t> ch,
const bool is_utm_source)
: name_(name), nh_(nh), broadcaster_(broadcaster), ch_(ch), is_utm_source_(is_utm_source) {

ROS_INFO("[%s]: initializing", getPrintName().c_str());

if (name != "dummy") {

/*//{ load parameters */
mrs_lib::ParamLoader param_loader(nh_, getPrintName());

const std::string yaml_prefix = "mrs_uav_managers/transform_manager/";

std::string odom_topic, attitude_topic, ns;

param_loader.loadParam(yaml_prefix + getName() + "/odom_topic", odom_topic);
param_loader.loadParam(yaml_prefix + getName() + "/custom_frame_id/enabled", custom_frame_id_enabled_, false);
param_loader->loadParam(yaml_prefix + getName() + "/odom_topic", odom_topic);
param_loader->loadParam(yaml_prefix + getName() + "/custom_frame_id/enabled", custom_frame_id_enabled_, false);
if (custom_frame_id_enabled_) {
param_loader.loadParam(yaml_prefix + getName() + "/custom_frame_id/frame_id", custom_frame_id_);
param_loader->loadParam(yaml_prefix + getName() + "/custom_frame_id/frame_id", custom_frame_id_);
}
param_loader.loadParam(yaml_prefix + getName() + "/tf_from_attitude/enabled", tf_from_attitude_enabled_);
param_loader->loadParam(yaml_prefix + getName() + "/tf_from_attitude/enabled", tf_from_attitude_enabled_);
if (tf_from_attitude_enabled_) {
param_loader.loadParam(yaml_prefix + getName() + "/tf_from_attitude/attitude_topic", attitude_topic);
param_loader->loadParam(yaml_prefix + getName() + "/tf_from_attitude/attitude_topic", attitude_topic);
}
param_loader.loadParam(yaml_prefix + getName() + "/namespace", ns);
param_loader->loadParam(yaml_prefix + getName() + "/namespace", ns);
full_topic_odom_ = "/" + ch_->uav_name + "/" + ns + "/" + odom_topic;
full_topic_attitude_ = "/" + ch_->uav_name + "/" + ns + "/" + attitude_topic;
param_loader.loadParam(yaml_prefix + getName() + "/inverted", is_inverted_);
param_loader.loadParam(yaml_prefix + getName() + "/republish_in_frames", republish_in_frames_);
param_loader->loadParam(yaml_prefix + getName() + "/inverted", is_inverted_);
param_loader->loadParam(yaml_prefix + getName() + "/republish_in_frames", republish_in_frames_);

/* coordinate frames origins //{ */
param_loader.loadParam(yaml_prefix + getName() + "/utm_based", is_utm_based_);
/* param_loader.loadParam(yaml_prefix + getName() + "/publish_local_tf", publish_local_tf_); */
param_loader->loadParam(yaml_prefix + getName() + "/utm_based", is_utm_based_);
/* param_loader->loadParam(yaml_prefix + getName() + "/publish_local_tf", publish_local_tf_); */

/*//{ utm source */
if (is_utm_based_) {
std::string utm_origin_parent_frame_id;
param_loader.loadParam(yaml_prefix + "utm_origin_tf/parent", utm_origin_parent_frame_id);
param_loader->loadParam(yaml_prefix + "utm_origin_tf/parent", utm_origin_parent_frame_id);
ns_utm_origin_parent_frame_id_ = ch_->uav_name + "/" + utm_origin_parent_frame_id;

std::string utm_origin_child_frame_id;
param_loader.loadParam(yaml_prefix + "utm_origin_tf/child", utm_origin_child_frame_id);
param_loader->loadParam(yaml_prefix + "utm_origin_tf/child", utm_origin_child_frame_id);
ns_utm_origin_child_frame_id_ = ch_->uav_name + "/" + utm_origin_child_frame_id;
}
/*//}*/

/*//{ world source */
if (is_utm_based_) {
std::string world_origin_parent_frame_id;
param_loader.loadParam(yaml_prefix + "world_origin_tf/parent", world_origin_parent_frame_id);
param_loader->loadParam(yaml_prefix + "world_origin_tf/parent", world_origin_parent_frame_id);
ns_world_origin_parent_frame_id_ = ch_->uav_name + "/" + world_origin_parent_frame_id;

std::string world_origin_child_frame_id;
param_loader.loadParam(yaml_prefix + "world_origin_tf/child", world_origin_child_frame_id);
param_loader->loadParam(yaml_prefix + "world_origin_tf/child", world_origin_child_frame_id);
ns_world_origin_child_frame_id_ = ch_->uav_name + "/" + world_origin_child_frame_id;
}
/*//}*/

//}

if (!param_loader.loadedSuccessfully()) {
if (!param_loader->loadedSuccessfully()) {
ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", getPrintName().c_str());
ros::shutdown();
}
Expand Down
19 changes: 12 additions & 7 deletions launch/constraint_manager.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,13 +35,18 @@
<node pkg="nodelet" type="nodelet" name="constraint_manager" args="$(arg nodelet) mrs_uav_managers/ConstraintManager $(arg nodelet_manager) $(arg bond_suffix)" output="screen" launch-prefix="$(arg launch_prefix)">

<!-- Load the default param files -->
<rosparam file="$(find mrs_uav_managers)/config/private/constraint_manager/constraint_manager.yaml" />
<rosparam file="$(find mrs_uav_managers)/config/public/constraint_manager/constraint_manager.yaml" />
<rosparam file="$(find mrs_uav_managers)/config/public/constraint_manager/constraints.yaml" />

<!-- Load a user param file -->
<rosparam if="$(eval not arg('platform_config') == '')" file="$(arg platform_config)" />
<rosparam if="$(eval not arg('custom_config') == '')" file="$(arg custom_config)" />
<param name="private_config" value="$(find mrs_uav_managers)/config/private/constraint_manager/constraint_manager.yaml" />
<param name="public_config" value="$(find mrs_uav_managers)/config/public/constraint_manager/constraint_manager.yaml" />
<param name="public_constraints" value="$(find mrs_uav_managers)/config/public/constraint_manager/constraints.yaml" />

<!-- Load user param files -->
<param if="$(eval arg('platform_config') == '')" name="platform_config" value="" />
<param if="$(eval not arg('platform_config') == '' and arg('platform_config')[0] == '/')" name="platform_config" value="$(arg platform_config)" />
<param if="$(eval not arg('platform_config') == '' and not arg('platform_config')[0] == '/')" name="platform_config" value="$(env PWD)/$(arg platform_config)" />

<param if="$(eval arg('custom_config') == '')" name="custom_config" value="" />
<param if="$(eval not arg('custom_config') == '' and arg('custom_config')[0] == '/')" name="custom_config" value="$(arg custom_config)" />
<param if="$(eval not arg('custom_config') == '' and not arg('custom_config')[0] == '/')" name="custom_config" value="$(env PWD)/$(arg custom_config)" />

<param name="enable_profiler" type="bool" value="$(arg PROFILER)" />

Expand Down
25 changes: 14 additions & 11 deletions launch/control_manager.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,16 +44,11 @@
<node pkg="nodelet" type="nodelet" name="control_manager" args="$(arg nodelet) mrs_uav_managers/ControlManager $(arg nodelet_manager) $(arg bond_suffix)" output="screen" launch-prefix="$(arg launch_prefix)">

<!-- Load the default param files -->
<rosparam file="$(find mrs_uav_managers)/config/private/control_manager.yaml" />
<rosparam file="$(find mrs_uav_managers)/config/public/control_manager.yaml" />
<rosparam file="$(find mrs_uav_managers)/config/private/trackers.yaml" />
<rosparam file="$(find mrs_uav_managers)/config/private/controllers.yaml" />
<rosparam file="$(find mrs_uav_managers)/config/public/controllers.yaml" />

<rosparam if="$(eval not arg('platform_config') == '')" file="$(arg platform_config)" />
<rosparam if="$(eval not arg('custom_config') == '')" file="$(arg custom_config)" />
<rosparam if="$(eval not arg('world_config') == '')" file="$(arg world_config)" />
<rosparam if="$(eval not arg('network_config') == '')" file="$(arg network_config)" />
<param name="private_config" value="$(find mrs_uav_managers)/config/private/control_manager.yaml" />
<param name="public_config" value="$(find mrs_uav_managers)/config/public/control_manager.yaml" />
<param name="private_trackers" value="$(find mrs_uav_managers)/config/private/trackers.yaml" />
<param name="private_controllers" value="$(find mrs_uav_managers)/config/private/controllers.yaml" />
<param name="public_controllers" value="$(find mrs_uav_managers)/config/public/controllers.yaml" />

<param name="enable_profiler" type="bool" value="$(arg PROFILER)" />
<param name="uav_name" type="string" value="$(arg UAV_NAME)" />
Expand All @@ -67,10 +62,18 @@
<param if="$(eval not arg('platform_config') == '' and arg('platform_config')[0] == '/')" name="platform_config" value="$(arg platform_config)" />
<param if="$(eval not arg('platform_config') == '' and not arg('platform_config')[0] == '/')" name="platform_config" value="$(env PWD)/$(arg platform_config)" />

<param if="$(eval not arg('custom_config') == '')" name="custom_config" value="$(arg custom_config)" />
<param if="$(eval arg('custom_config') == '')" name="custom_config" value="" />
<param if="$(eval not arg('custom_config') == '' and arg('custom_config')[0] == '/')" name="custom_config" value="$(arg custom_config)" />
<param if="$(eval not arg('custom_config') == '' and not arg('custom_config')[0] == '/')" name="custom_config" value="$(env PWD)/$(arg custom_config)" />

<param if="$(eval arg('world_config') == '')" name="world_config" value="" />
<param if="$(eval not arg('world_config') == '' and arg('world_config')[0] == '/')" name="world_config" value="$(arg world_config)" />
<param if="$(eval not arg('world_config') == '' and not arg('world_config')[0] == '/')" name="world_config" value="$(env PWD)/$(arg world_config)" />

<param if="$(eval arg('network_config') == '')" name="network_config" value="" />
<param if="$(eval not arg('network_config') == '' and arg('network_config')[0] == '/')" name="network_config" value="$(arg network_config)" />
<param if="$(eval not arg('network_config') == '' and not arg('network_config')[0] == '/')" name="network_config" value="$(env PWD)/$(arg network_config)" />

<!-- Trackers' configs -->

<!-- Subscribers -->
Expand Down
21 changes: 9 additions & 12 deletions launch/estimation_manager.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,27 +36,24 @@
<param name="enable_profiler" value="$(arg PROFILER)" />
<param name="uav_name" value="$(arg UAV_NAME)" />


<!-- Load the default param files -->
<rosparam file="$(find mrs_uav_managers)/config/private/estimation_manager/estimation_manager.yaml" command="load" />
<rosparam file="$(find mrs_uav_managers)/config/public/estimation_manager/estimation_manager.yaml" command="load" />
<rosparam file="$(find mrs_uav_managers)/config/public/estimators.yaml" command="load" />
<rosparam file="$(find mrs_uav_managers)/config/public/active_estimators.yaml" command="load" />

<rosparam if="$(eval not arg('world_config') == '')" file="$(arg world_config)" />
<param name="private_config" value="$(find mrs_uav_managers)/config/private/estimation_manager/estimation_manager.yaml" />
<param name="public_config" value="$(find mrs_uav_managers)/config/public/estimation_manager/estimation_manager.yaml" />
<param name="estimators_config" value="$(find mrs_uav_managers)/config/public/estimators.yaml" />
<param name="active_estimators_config" value="$(find mrs_uav_managers)/config/public/active_estimators.yaml" />

<!-- Load the platform param files -->
<rosparam if="$(eval not arg('platform_config') == '')" file="$(arg platform_config)" />
<param if="$(eval arg('platform_config') == '')" name="platform_config" value="" />
<param if="$(eval not arg('platform_config') == '' and arg('platform_config')[0] == '/')" name="platform_config" value="$(arg platform_config)" />
<param if="$(eval not arg('platform_config') == '' and not arg('platform_config')[0] == '/')" name="platform_config" value="$(env PWD)/$(arg platform_config)" />

<!-- Load the custom param files -->
<rosparam if="$(eval not arg('custom_config') == '')" file="$(arg custom_config)" />
<param if="$(eval not arg('custom_config') == '')" name="custom_config" value="$(arg custom_config)" />
<param if="$(eval arg('custom_config') == '')" name="custom_config" value="" />
<param if="$(eval not arg('custom_config') == '' and arg('custom_config')[0] == '/')" name="custom_config" value="$(arg custom_config)" />
<param if="$(eval not arg('custom_config') == '' and not arg('custom_config')[0] == '/')" name="custom_config" value="$(env PWD)/$(arg custom_config)" />

<param if="$(eval arg('world_config') == '')" name="world_config" value="" />
<param if="$(eval not arg('world_config') == '' and arg('world_config')[0] == '/')" name="world_config" value="$(arg world_config)" />
<param if="$(eval not arg('world_config') == '' and not arg('world_config')[0] == '/')" name="world_config" value="$(env PWD)/$(arg world_config)" />

<!-- common -->
<remap from="~control_input_in" to="control_manager/estimator_input" />
<remap from="~imu_in" to="hw_api/imu" />
Expand Down
Loading

0 comments on commit 3e13721

Please sign in to comment.