Skip to content

Commit 64b4247

Browse files
livanov93livanov93
committed
Claim individual resources.
1 parent 299b983 commit 64b4247

File tree

2 files changed

+155
-151
lines changed

2 files changed

+155
-151
lines changed

‎ur_controllers/include/ur_controllers/force_torque_sensor_controller.h‎

Lines changed: 18 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -18,32 +18,32 @@
1818

1919
namespace ur_controllers
2020
{
21-
class ForceTorqueStateController : public controller_interface::ControllerInterface
22-
{
23-
public:
24-
ForceTorqueStateController();
21+
class ForceTorqueStateController : public controller_interface::ControllerInterface
22+
{
23+
public:
24+
ForceTorqueStateController();
2525

26-
controller_interface::InterfaceConfiguration command_interface_configuration() const override;
26+
controller_interface::InterfaceConfiguration command_interface_configuration() const override;
2727

28-
controller_interface::InterfaceConfiguration state_interface_configuration() const override;
28+
controller_interface::InterfaceConfiguration state_interface_configuration() const override;
2929

30-
controller_interface::return_type update() override;
30+
controller_interface::return_type update() override;
3131

32-
CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
32+
CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
3333

34-
CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
34+
CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
3535

36-
CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
36+
CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
3737

38-
protected:
39-
bool init_sensor_data();
40-
void init_sensor_state_msg();
38+
protected:
39+
bool init_sensor_data();
40+
void init_sensor_state_msg();
4141

42-
// we store the name of values per axis - compatible interfaces
43-
std::vector<std::string> axis_val_names_;
44-
std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::WrenchStamped>> wrench_state_publisher_;
45-
geometry_msgs::msg::WrenchStamped wrench_state_msg_;
46-
};
42+
// we store the name of values per axis - compatible interfaces
43+
std::vector<std::string> axis_val_names_;
44+
std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::WrenchStamped>> wrench_state_publisher_;
45+
geometry_msgs::msg::WrenchStamped wrench_state_msg_;
46+
};
4747
} // namespace ur_controllers
4848

4949
#endif // UR_CONTROLLERS_FORCE_TORQUE_SENSOR_CONTROLLER_H

‎ur_controllers/src/force_torque_sensor_controller.cpp‎

Lines changed: 137 additions & 133 deletions
Original file line numberDiff line numberDiff line change
@@ -6,167 +6,171 @@
66

77
namespace ur_controllers
88
{
9-
ForceTorqueStateController::ForceTorqueStateController() : controller_interface::ControllerInterface()
9+
ForceTorqueStateController::ForceTorqueStateController() : controller_interface::ControllerInterface()
10+
{
11+
}
12+
13+
controller_interface::InterfaceConfiguration ForceTorqueStateController::command_interface_configuration() const
14+
{
15+
controller_interface::InterfaceConfiguration config;
16+
config.type = controller_interface::interface_configuration_type::NONE;
17+
return config;
18+
}
19+
20+
controller_interface::InterfaceConfiguration ForceTorqueStateController::state_interface_configuration() const
21+
{
22+
controller_interface::InterfaceConfiguration config;
23+
config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
24+
// TODO solve this hardcoded naming
25+
config.names.emplace_back("tcp_fts_sensor/fx");
26+
config.names.emplace_back("tcp_fts_sensor/fy");
27+
config.names.emplace_back("tcp_fts_sensor/fz");
28+
config.names.emplace_back("tcp_fts_sensor/tx");
29+
config.names.emplace_back("tcp_fts_sensor/ty");
30+
config.names.emplace_back("tcp_fts_sensor/tz");
31+
return config;
32+
}
33+
34+
controller_interface::return_type ur_controllers::ForceTorqueStateController::update()
35+
{
36+
geometry_msgs::msg::Vector3 fVec;
37+
geometry_msgs::msg::Vector3 tVec;
38+
39+
// TODO remove these hardcoded names and create better string filtering within state_interfaces_
40+
for (const auto& state_interface : state_interfaces_)
41+
{
42+
if (state_interface.get_interface_name() == "fx")
1043
{
44+
fVec.set__x(state_interface.get_value());
45+
continue;
1146
}
1247

13-
controller_interface::InterfaceConfiguration ForceTorqueStateController::command_interface_configuration() const
48+
if (state_interface.get_interface_name() == "fy")
1449
{
15-
controller_interface::InterfaceConfiguration config;
16-
config.type = controller_interface::interface_configuration_type::NONE;
17-
return config;
50+
fVec.set__y(state_interface.get_value());
51+
continue;
1852
}
1953

20-
controller_interface::InterfaceConfiguration ForceTorqueStateController::state_interface_configuration() const
54+
if (state_interface.get_interface_name() == "fz")
2155
{
22-
controller_interface::InterfaceConfiguration config;
23-
config.type = controller_interface::interface_configuration_type::ALL;
24-
return config;
56+
fVec.set__z(state_interface.get_value());
57+
continue;
2558
}
2659

27-
controller_interface::return_type ur_controllers::ForceTorqueStateController::update()
60+
if (state_interface.get_interface_name() == "tx")
2861
{
29-
geometry_msgs::msg::Vector3 fVec;
30-
geometry_msgs::msg::Vector3 tVec;
31-
32-
// TODO remove these hardcoded names and create better string filtering within state_interfaces_
33-
for (const auto& state_interface : state_interfaces_)
34-
{
35-
if (state_interface.get_name() != "tcp_fts_sensor")
36-
continue;
37-
38-
if (state_interface.get_interface_name() == "fx")
39-
{
40-
fVec.set__x(state_interface.get_value());
41-
continue;
42-
}
43-
44-
if (state_interface.get_interface_name() == "fy")
45-
{
46-
fVec.set__y(state_interface.get_value());
47-
continue;
48-
}
49-
50-
if (state_interface.get_interface_name() == "fz")
51-
{
52-
fVec.set__z(state_interface.get_value());
53-
continue;
54-
}
55-
56-
if (state_interface.get_interface_name() == "tx")
57-
{
58-
tVec.set__x(state_interface.get_value());
59-
continue;
60-
}
61-
62-
if (state_interface.get_interface_name() == "ty")
63-
{
64-
tVec.set__y(state_interface.get_value());
65-
continue;
66-
}
67-
68-
if (state_interface.get_interface_name() == "tz")
69-
{
70-
tVec.set__z(state_interface.get_value());
71-
continue;
72-
}
73-
}
74-
75-
if (!wrench_state_publisher_->is_activated())
76-
{
77-
RCUTILS_LOG_WARN_ONCE_NAMED("publisher", "wrench state publisher is not activated");
78-
return controller_interface::return_type::ERROR;
79-
}
80-
81-
// TODO set frame_id as parameter --> it includes tf listener within controller
82-
wrench_state_msg_.header.stamp = lifecycle_node_->get_clock()->now();
83-
wrench_state_msg_.header.frame_id = "tool0";
84-
85-
// update wrench state message
86-
wrench_state_msg_.wrench.set__force(fVec);
87-
wrench_state_msg_.wrench.set__torque(tVec);
88-
89-
// publish
90-
wrench_state_publisher_->publish(wrench_state_msg_);
91-
92-
return controller_interface::return_type::SUCCESS;
62+
tVec.set__x(state_interface.get_value());
63+
continue;
9364
}
9465

95-
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
96-
ForceTorqueStateController::on_configure(const rclcpp_lifecycle::State& /*previous_state*/)
66+
if (state_interface.get_interface_name() == "ty")
9767
{
98-
try
99-
{
100-
// TODO make topic name a parameter
101-
wrench_state_publisher_ =
102-
lifecycle_node_->create_publisher<geometry_msgs::msg::WrenchStamped>("ft_data", rclcpp::SystemDefaultsQoS());
103-
}
104-
catch (...)
105-
{
106-
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
107-
}
108-
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
68+
tVec.set__y(state_interface.get_value());
69+
continue;
10970
}
11071

111-
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
112-
ForceTorqueStateController::on_activate(const rclcpp_lifecycle::State& /*previous_state*/)
72+
if (state_interface.get_interface_name() == "tz")
11373
{
114-
if (!init_sensor_data())
115-
{
116-
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
117-
}
74+
tVec.set__z(state_interface.get_value());
75+
continue;
76+
}
77+
}
11878

119-
init_sensor_state_msg();
79+
if (!wrench_state_publisher_->is_activated())
80+
{
81+
RCUTILS_LOG_WARN_ONCE_NAMED("publisher", "wrench state publisher is not activated");
82+
return controller_interface::return_type::ERROR;
83+
}
12084

121-
wrench_state_publisher_->on_activate();
85+
// TODO set frame_id as parameter --> it includes tf listener within controller
86+
wrench_state_msg_.header.stamp = lifecycle_node_->get_clock()->now();
87+
wrench_state_msg_.header.frame_id = "tool0";
12288

123-
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
124-
}
89+
// update wrench state message
90+
wrench_state_msg_.wrench.set__force(fVec);
91+
wrench_state_msg_.wrench.set__torque(tVec);
12592

126-
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
127-
ForceTorqueStateController::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/)
128-
{
129-
wrench_state_publisher_->on_deactivate();
93+
// publish
94+
wrench_state_publisher_->publish(wrench_state_msg_);
13095

131-
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
132-
}
96+
return controller_interface::return_type::SUCCESS;
97+
}
13398

134-
template <typename T>
135-
bool has_any_key(const std::unordered_map<std::string, T>& map, const std::vector<std::string>& keys)
136-
{
137-
bool found_key = false;
138-
for (const auto& key_item : map)
139-
{
140-
const auto& key = key_item.first;
141-
if (std::find(keys.cbegin(), keys.cend(), key) != keys.cend())
142-
{
143-
found_key = true;
144-
break;
145-
}
146-
}
147-
return found_key;
148-
}
99+
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
100+
ForceTorqueStateController::on_configure(const rclcpp_lifecycle::State& /*previous_state*/)
101+
{
102+
try
103+
{
104+
// TODO make topic name a parameter
105+
wrench_state_publisher_ =
106+
lifecycle_node_->create_publisher<geometry_msgs::msg::WrenchStamped>("ft_data", rclcpp::SystemDefaultsQoS());
107+
}
108+
catch (...)
109+
{
110+
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
111+
}
112+
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
113+
}
114+
115+
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
116+
ForceTorqueStateController::on_activate(const rclcpp_lifecycle::State& /*previous_state*/)
117+
{
118+
if (!init_sensor_data())
119+
{
120+
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
121+
}
122+
123+
init_sensor_state_msg();
124+
125+
wrench_state_publisher_->on_activate();
126+
127+
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
128+
}
129+
130+
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
131+
ForceTorqueStateController::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/)
132+
{
133+
wrench_state_publisher_->on_deactivate();
134+
135+
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
136+
}
149137

150-
bool ForceTorqueStateController::init_sensor_data()
138+
template <typename T>
139+
bool has_any_key(const std::unordered_map<std::string, T>& map, const std::vector<std::string>& keys)
140+
{
141+
bool found_key = false;
142+
for (const auto& key_item : map)
143+
{
144+
const auto& key = key_item.first;
145+
if (std::find(keys.cbegin(), keys.cend(), key) != keys.cend())
151146
{
152-
bool has_fts_sensor = false;
153-
// loop in reverse order, this maintains the order of values at retrieval time
154-
for (auto si = state_interfaces_.crbegin(); si != state_interfaces_.crend(); si++)
155-
{
156-
// TODO remove hardcoded naming
157-
if (si->get_name() == "tcp_fts_sensor")
158-
{
159-
has_fts_sensor = true;
160-
axis_val_names_.push_back(si->get_interface_name());
161-
}
162-
}
163-
return has_fts_sensor;
147+
found_key = true;
148+
break;
164149
}
150+
}
151+
return found_key;
152+
}
165153

166-
void ForceTorqueStateController::init_sensor_state_msg()
154+
bool ForceTorqueStateController::init_sensor_data()
155+
{
156+
bool has_fts_sensor = false;
157+
// loop in reverse order, this maintains the order of values at retrieval time
158+
for (auto si = state_interfaces_.crbegin(); si != state_interfaces_.crend(); si++)
159+
{
160+
// TODO remove hardcoded naming
161+
if (si->get_name() == "tcp_fts_sensor")
167162
{
168-
// default initialization for wrenchstamped state message
163+
has_fts_sensor = true;
164+
axis_val_names_.push_back(si->get_interface_name());
169165
}
166+
}
167+
return has_fts_sensor;
168+
}
169+
170+
void ForceTorqueStateController::init_sensor_state_msg()
171+
{
172+
// default initialization for wrenchstamped state message
173+
}
170174

171175
} // namespace ur_controllers
172176

0 commit comments

Comments
 (0)