Skip to content

Commit 6c886b5

Browse files
fmauchsaikishor
andauthored
Apply suggestions from code review
Co-authored-by: Sai Kishor Kothakota <saisastra3@gmail.com>
1 parent 76411c5 commit 6c886b5

File tree

2 files changed

+6
-8
lines changed

2 files changed

+6
-8
lines changed

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -283,7 +283,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
283283
void resize_joint_trajectory_point_command(
284284
trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size, double value = 0.0);
285285

286-
bool set_scaling_factor(const double scaling_factor);
286+
bool set_scaling_factor(double scaling_factor);
287287
using SpeedScalingMsg = control_msgs::msg::SpeedScalingFactor;
288288
rclcpp::Subscription<SpeedScalingMsg>::SharedPtr scaling_factor_sub_;
289289
/**

joint_trajectory_controller/src/joint_trajectory_controller.cpp

Lines changed: 5 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -987,8 +987,9 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
987987
}
988988
else
989989
{
990-
RCLCPP_WARN(
990+
RCLCPP_WARN_EXPRESSION(
991991
logger,
992+
params.scaling_factor_initial_default != 1.0,
992993
"Speed scaling is currently only supported for position interfaces. If you want to make use "
993994
"of speed scaling, please only use a position interface when configuring this controller.");
994995
scaling_factor_ = 1.0;
@@ -1756,7 +1757,7 @@ void JointTrajectoryController::resize_joint_trajectory_point_command(
17561757
}
17571758
}
17581759

1759-
bool JointTrajectoryController::set_scaling_factor(const double scaling_factor)
1760+
bool JointTrajectoryController::set_scaling_factor(double scaling_factor)
17601761
{
17611762
if (scaling_factor < 0)
17621763
{
@@ -1773,16 +1774,13 @@ bool JointTrajectoryController::set_scaling_factor(const double scaling_factor)
17731774
scaling_factor);
17741775
}
17751776
scaling_factor_.store(scaling_factor);
1776-
if (params_.speed_scaling_command_interface_name.empty())
1777-
{
1778-
if (!params_.speed_scaling_state_interface_name.empty())
1777+
if (params_.speed_scaling_command_interface_name.empty() && !params_.speed_scaling_state_interface_name.empty())
17791778
{
1780-
RCLCPP_WARN(
1779+
RCLCPP_WARN_ONCE(
17811780
get_node()->get_logger(),
17821781
"Setting the scaling factor while only one-way communication with the hardware is setup. "
17831782
"This will likely get overwritten by the hardware again. If available, please also setup "
17841783
"the speed_scaling_command_interface_name");
1785-
}
17861784
}
17871785
else
17881786
{

0 commit comments

Comments
 (0)