@@ -122,11 +122,15 @@ JointTrajectoryController::state_interface_configuration() const
122122controller_interface::return_type JointTrajectoryController::update (
123123 const rclcpp::Time & time, const rclcpp::Duration & period)
124124{
125- if (state_interfaces_.back ().get_name () == params_.speed_scaling_interface_name ) {
125+ if (state_interfaces_.back ().get_name () == params_.speed_scaling_interface_name )
126+ {
126127 scaling_factor_ = state_interfaces_.back ().get_value ();
127- } else {
128- RCLCPP_ERROR (get_node ()->get_logger (), " Speed scaling interface (%s) not found in hardware interface." ,
129- params_.speed_scaling_interface_name .c_str ());
128+ }
129+ else
130+ {
131+ RCLCPP_ERROR (
132+ get_node ()->get_logger (), " Speed scaling interface (%s) not found in hardware interface." ,
133+ params_.speed_scaling_interface_name .c_str ());
130134 }
131135
132136 if (get_state ().id () == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
@@ -204,7 +208,8 @@ controller_interface::return_type JointTrajectoryController::update(
204208 rcl_duration_value_t t_period = (time_data.time - time_data_.readFromRT ()->time ).nanoseconds ();
205209 time_data.period = rclcpp::Duration::from_nanoseconds (scaling_factor_ * t_period);
206210 time_data.uptime = time_data_.readFromRT ()->uptime + time_data.period ;
207- rclcpp::Time traj_time = time_data_.readFromRT ()->uptime + rclcpp::Duration::from_nanoseconds (t_period);
211+ rclcpp::Time traj_time =
212+ time_data_.readFromRT ()->uptime + rclcpp::Duration::from_nanoseconds (t_period);
208213 time_data_.writeFromNonRT (time_data);
209214
210215 bool first_sample = false ;
0 commit comments