Skip to content

Commit 76a9261

Browse files
fmauchfmauch
committed
Use hierarchical parameter naming
1 parent ddf5724 commit 76a9261

File tree

3 files changed

+61
-56
lines changed

3 files changed

+61
-56
lines changed

‎joint_trajectory_controller/src/joint_trajectory_controller.cpp‎

Lines changed: 25 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -113,9 +113,9 @@ JointTrajectoryController::command_interface_configuration() const
113113
conf.names.push_back(joint_name + "/" + interface_type);
114114
}
115115
}
116-
if (!params_.speed_scaling_command_interface_name.empty())
116+
if (!params_.speed_scaling.command_interface.empty())
117117
{
118-
conf.names.push_back(params_.speed_scaling_command_interface_name);
118+
conf.names.push_back(params_.speed_scaling.command_interface);
119119
}
120120
return conf;
121121
}
@@ -133,9 +133,9 @@ JointTrajectoryController::state_interface_configuration() const
133133
conf.names.push_back(joint_name + "/" + interface_type);
134134
}
135135
}
136-
if (!params_.speed_scaling_state_interface_name.empty())
136+
if (!params_.speed_scaling.state_interface.empty())
137137
{
138-
conf.names.push_back(params_.speed_scaling_state_interface_name);
138+
conf.names.push_back(params_.speed_scaling.state_interface);
139139
}
140140
return conf;
141141
}
@@ -982,23 +982,23 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
982982
"~/speed_scaling_input", qos,
983983
[&](const SpeedScalingMsg & msg) { set_scaling_factor(msg.factor); });
984984
RCLCPP_INFO(
985-
logger, "Setting initial scaling factor to %2f", params_.scaling_factor_initial_default);
986-
scaling_factor_ = params_.scaling_factor_initial_default;
985+
logger, "Setting initial scaling factor to %2f",
986+
params_.speed_scaling.initial_scaling_factor);
987+
scaling_factor_ = params_.speed_scaling.initial_scaling_factor;
987988
}
988989
else
989990
{
990991
RCLCPP_WARN_EXPRESSION(
991-
logger,
992-
params.scaling_factor_initial_default != 1.0,
992+
logger, params_.speed_scaling.initial_scaling_factor != 1.0,
993993
"Speed scaling is currently only supported for position interfaces. If you want to make use "
994994
"of speed scaling, please only use a position interface when configuring this controller.");
995995
scaling_factor_ = 1.0;
996996
}
997-
if (!params_.speed_scaling_state_interface_name.empty())
997+
if (!params_.speed_scaling.state_interface.empty())
998998
{
999999
RCLCPP_INFO(
10001000
logger, "Using scaling state from the hardware from interface %s.",
1001-
params_.speed_scaling_state_interface_name.c_str());
1001+
params_.speed_scaling.state_interface.c_str());
10021002
}
10031003
else
10041004
{
@@ -1033,11 +1033,11 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
10331033
default_tolerances_ = get_segment_tolerances(logger, params_);
10341034

10351035
// Set scaling interfaces
1036-
if (!params_.speed_scaling_state_interface_name.empty())
1036+
if (!params_.speed_scaling.state_interface.empty())
10371037
{
10381038
auto it = std::find_if(
10391039
state_interfaces_.begin(), state_interfaces_.end(), [&](auto & interface)
1040-
{ return (interface.get_name() == params_.speed_scaling_state_interface_name); });
1040+
{ return (interface.get_name() == params_.speed_scaling.state_interface); });
10411041
if (it != state_interfaces_.end())
10421042
{
10431043
scaling_state_interface_ = *it;
@@ -1046,15 +1046,15 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
10461046
{
10471047
RCLCPP_ERROR(
10481048
logger, "Did not find speed scaling interface '%s' in state interfaces.",
1049-
params_.speed_scaling_state_interface_name.c_str());
1049+
params_.speed_scaling.state_interface.c_str());
10501050
return CallbackReturn::ERROR;
10511051
}
10521052
}
1053-
if (!params_.speed_scaling_command_interface_name.empty())
1053+
if (!params_.speed_scaling.command_interface.empty())
10541054
{
10551055
auto it = std::find_if(
10561056
command_interfaces_.begin(), command_interfaces_.end(), [&](auto & interface)
1057-
{ return (interface.get_name() == params_.speed_scaling_command_interface_name); });
1057+
{ return (interface.get_name() == params_.speed_scaling.command_interface); });
10581058
if (it != command_interfaces_.end())
10591059
{
10601060
scaling_command_interface_ = *it;
@@ -1063,7 +1063,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
10631063
{
10641064
RCLCPP_ERROR(
10651065
logger, "Did not find speed scaling interface '%s' in command interfaces.",
1066-
params_.speed_scaling_command_interface_name.c_str());
1066+
params_.speed_scaling.command_interface.c_str());
10671067
return CallbackReturn::ERROR;
10681068
}
10691069
}
@@ -1774,13 +1774,15 @@ bool JointTrajectoryController::set_scaling_factor(double scaling_factor)
17741774
scaling_factor);
17751775
}
17761776
scaling_factor_.store(scaling_factor);
1777-
if (params_.speed_scaling_command_interface_name.empty() && !params_.speed_scaling_state_interface_name.empty())
1778-
{
1779-
RCLCPP_WARN_ONCE(
1780-
get_node()->get_logger(),
1781-
"Setting the scaling factor while only one-way communication with the hardware is setup. "
1782-
"This will likely get overwritten by the hardware again. If available, please also setup "
1783-
"the speed_scaling_command_interface_name");
1777+
if (
1778+
params_.speed_scaling.command_interface.empty() &&
1779+
!params_.speed_scaling.state_interface.empty())
1780+
{
1781+
RCLCPP_WARN_ONCE(
1782+
get_node()->get_logger(),
1783+
"Setting the scaling factor while only one-way communication with the hardware is setup. "
1784+
"This will likely get overwritten by the hardware again. If available, please also setup "
1785+
"the speed_scaling_command_interface_name");
17841786
}
17851787
else
17861788
{

‎joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml‎

Lines changed: 24 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -46,27 +46,32 @@ joint_trajectory_controller:
4646
"joint_trajectory_controller::state_interface_type_combinations": null,
4747
}
4848
}
49-
scaling_factor_initial_default: {
50-
type: double,
51-
default_value: 1.0,
52-
read_only: true,
53-
description: "The initial value of the scaling factor if no exchange with hardware takes place.",
54-
validation: {
55-
gt_eq<>: 0.0,
49+
speed_scaling: {
50+
51+
initial_scaling_factor: {
52+
type: double,
53+
default_value: 1.0,
54+
read_only: true,
55+
description: "The initial value of the scaling factor if no exchange with hardware takes place.",
56+
validation: {
57+
gt_eq<>: 0.0,
58+
}
59+
},
60+
61+
state_interface: {
62+
type: string,
63+
default_value: "",
64+
read_only: true,
65+
description: "Fully qualified name of the speed scaling state interface name"
66+
},
67+
68+
command_interface: {
69+
type: string,
70+
default_value: "",
71+
read_only: true,
72+
description: "Command interface name used for setting the speed scaling factor on the hardware."
5673
}
5774
}
58-
speed_scaling_state_interface_name: {
59-
type: string,
60-
default_value: "",
61-
read_only: true,
62-
description: "Fully qualified name of the speed scaling state interface name"
63-
}
64-
speed_scaling_command_interface_name: {
65-
type: string,
66-
default_value: "",
67-
read_only: true,
68-
description: "Command interface name used for setting the speed scaling factor on the hardware."
69-
}
7075
allow_partial_joints_goal: {
7176
type: bool,
7277
default_value: false,

‎joint_trajectory_controller/test/test_trajectory_controller.cpp‎

Lines changed: 12 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -2697,7 +2697,7 @@ TEST_F(TrajectoryControllerTest, scaling_factor_from_param)
26972697
double initial_factor = 0.123;
26982698
rclcpp::executors::MultiThreadedExecutor executor;
26992699
std::vector<rclcpp::Parameter> params = {
2700-
rclcpp::Parameter("scaling_factor_initial_default", initial_factor),
2700+
rclcpp::Parameter("speed_scaling/initial_scaling_factor", initial_factor),
27012701
};
27022702
SetUpAndActivateTrajectoryController(executor, params);
27032703
subscribeToState(executor);
@@ -2714,8 +2714,8 @@ TEST_F(
27142714
double initial_factor = 0.123;
27152715
rclcpp::executors::MultiThreadedExecutor executor;
27162716
std::vector<rclcpp::Parameter> params = {
2717-
rclcpp::Parameter("scaling_factor_initial_default", initial_factor),
2718-
rclcpp::Parameter("speed_scaling_state_interface_name", "idontexist"),
2717+
rclcpp::Parameter("speed_scaling/initial_scaling_factor", initial_factor),
2718+
rclcpp::Parameter("speed_scaling/state_interface", "idontexist"),
27192719
};
27202720
SetUpTrajectoryController(executor, params);
27212721

@@ -2732,8 +2732,8 @@ TEST_F(
27322732
double initial_factor = 0.123;
27332733
rclcpp::executors::MultiThreadedExecutor executor;
27342734
std::vector<rclcpp::Parameter> params = {
2735-
rclcpp::Parameter("scaling_factor_initial_default", initial_factor),
2736-
rclcpp::Parameter("speed_scaling_command_interface_name", "idontexist"),
2735+
rclcpp::Parameter("speed_scaling/initial_scaling_factor", initial_factor),
2736+
rclcpp::Parameter("speed_scaling/command_interface", "idontexist"),
27372737
};
27382738
SetUpTrajectoryController(executor, params);
27392739

@@ -2749,8 +2749,8 @@ TEST_F(TrajectoryControllerTest, scaling_state_interface_sets_value)
27492749
double initial_factor = 0.123;
27502750
rclcpp::executors::MultiThreadedExecutor executor;
27512751
std::vector<rclcpp::Parameter> params = {
2752-
rclcpp::Parameter("scaling_factor_initial_default", initial_factor),
2753-
rclcpp::Parameter("speed_scaling_state_interface_name", "speed_scaling/speed_scaling_factor"),
2752+
rclcpp::Parameter("speed_scaling/initial_scaling_factor", initial_factor),
2753+
rclcpp::Parameter("speed_scaling/state_interface", "speed_scaling/speed_scaling_factor"),
27542754
};
27552755
SetUpAndActivateTrajectoryController(executor, params);
27562756

@@ -2783,10 +2783,9 @@ TEST_F(TrajectoryControllerTest, scaling_command_interface_sets_value)
27832783
double initial_factor = 0.123;
27842784
rclcpp::executors::MultiThreadedExecutor executor;
27852785
std::vector<rclcpp::Parameter> params = {
2786-
rclcpp::Parameter("scaling_factor_initial_default", initial_factor),
2787-
rclcpp::Parameter("speed_scaling_state_interface_name", "speed_scaling/speed_scaling_factor"),
2788-
rclcpp::Parameter(
2789-
"speed_scaling_command_interface_name", "speed_scaling/target_speed_fraction_cmd"),
2786+
rclcpp::Parameter("speed_scaling/initial_scaling_factor", initial_factor),
2787+
rclcpp::Parameter("speed_scaling/state_interface", "speed_scaling/speed_scaling_factor"),
2788+
rclcpp::Parameter("speed_scaling/command_interface", "speed_scaling/target_speed_fraction_cmd"),
27902789
};
27912790
SetUpAndActivateTrajectoryController(executor, params);
27922791

@@ -2819,9 +2818,8 @@ TEST_F(TrajectoryControllerTest, activate_with_scaling_interfaces)
28192818
{
28202819
rclcpp::executors::MultiThreadedExecutor executor;
28212820
std::vector<rclcpp::Parameter> params = {
2822-
rclcpp::Parameter("speed_scaling_state_interface_name", "speed_scaling/speed_scaling_factor"),
2823-
rclcpp::Parameter(
2824-
"speed_scaling_command_interface_name", "speed_scaling/target_speed_fraction_cmd"),
2821+
rclcpp::Parameter("speed_scaling/state_interface", "speed_scaling/speed_scaling_factor"),
2822+
rclcpp::Parameter("speed_scaling/command_interface", "speed_scaling/target_speed_fraction_cmd"),
28252823
};
28262824
SetUpTrajectoryController(executor, params);
28272825

0 commit comments

Comments
 (0)