Skip to content

Commit c44c410

Browse files
committed
Removed last remnants of joint_state_controller
1 parent 1e1990d commit c44c410

File tree

2 files changed

+7
-10
lines changed

2 files changed

+7
-10
lines changed

ur_controllers/include/ur_controllers/speed_scaling_state_controller.h

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -62,10 +62,10 @@ class SpeedScalingStateController : public controller_interface::ControllerInter
6262
on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
6363

6464
protected:
65-
bool init_joint_data();
65+
bool init_sensor_data();
6666

6767
protected:
68-
std::vector<std::string> joint_names_;
68+
std::vector<std::string> sensor_names_;
6969
rclcpp::Time last_publish_time_;
7070
double publish_rate_;
7171

ur_controllers/src/speed_scaling_state_controller.cpp

Lines changed: 5 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -53,9 +53,6 @@ class State;
5353
namespace ur_controllers
5454
{
5555
const auto kUninitializedValue = std::numeric_limits<double>::quiet_NaN();
56-
using hardware_interface::HW_IF_EFFORT;
57-
using hardware_interface::HW_IF_POSITION;
58-
using hardware_interface::HW_IF_VELOCITY;
5956

6057
SpeedScalingStateController::SpeedScalingStateController()
6158
{
@@ -103,7 +100,7 @@ SpeedScalingStateController::on_configure(const rclcpp_lifecycle::State& /*previ
103100
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
104101
SpeedScalingStateController::on_activate(const rclcpp_lifecycle::State& /*previous_state*/)
105102
{
106-
if (!init_joint_data())
103+
if (!init_sensor_data())
107104
{
108105
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
109106
}
@@ -134,7 +131,7 @@ bool has_any_key(const std::unordered_map<std::string, T>& map, const std::vecto
134131
return found_key;
135132
}
136133

137-
bool SpeedScalingStateController::init_joint_data()
134+
bool SpeedScalingStateController::init_sensor_data()
138135
{
139136
// loop in reverse order, this maintains the order of values at retrieval time
140137
for (auto si = state_interfaces_.crbegin(); si != state_interfaces_.crend(); si++)
@@ -155,7 +152,7 @@ bool SpeedScalingStateController::init_joint_data()
155152
const auto& interfaces_and_values = name_ifv.second;
156153
if (has_any_key(interfaces_and_values, { "speed_scaling_factor" }))
157154
{
158-
joint_names_.push_back(name_ifv.first);
155+
sensor_names_.push_back(name_ifv.first);
159156
}
160157
}
161158

@@ -189,9 +186,9 @@ controller_interface::return_type SpeedScalingStateController::update()
189186
state_interface.get_interface_name().c_str(), state_interface.get_value());
190187
}
191188

192-
for (auto i = 0ul; i < joint_names_.size(); ++i)
189+
for (auto i = 0ul; i < sensor_names_.size(); ++i)
193190
{
194-
speed_scaling_state_msg_.data = get_value(name_if_value_mapping_, joint_names_[i], "speed_scaling_factor");
191+
speed_scaling_state_msg_.data = get_value(name_if_value_mapping_, sensor_names_[i], "speed_scaling_factor");
195192
}
196193

197194
// publish

0 commit comments

Comments
 (0)