3939// ----------------------------------------------------------------------
4040
4141#include < ur_controllers/tool_contact_controller.hpp>
42+ #include " controller_interface/helpers.hpp"
43+ #include < rclcpp/rclcpp.hpp>
44+ #include " std_msgs/msg/bool.hpp"
45+ #include < iostream>
4246
4347namespace ur_controllers
4448{
4549
50+ controller_interface::CallbackReturn ToolContactController::on_init ()
51+ {
52+ tool_contact_param_listener_ = std::make_shared<tool_contact_controller::ParamListener>(get_node ());
53+ tool_contact_params_ = tool_contact_param_listener_->get_params ();
54+
55+ // Resize this value depending on reference interfaces to be sent
56+ reference_interfaces_.resize (1 , std::numeric_limits<double >::quiet_NaN ());
57+ return controller_interface::CallbackReturn::SUCCESS;
58+ }
59+
60+ controller_interface::CallbackReturn ToolContactController::on_configure (const rclcpp_lifecycle::State& previous_state)
61+ {
62+ return controller_interface::CallbackReturn::SUCCESS;
63+ }
64+
65+ controller_interface::InterfaceConfiguration ToolContactController::command_interface_configuration () const
66+ {
67+ controller_interface::InterfaceConfiguration config;
68+ config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
69+ const std::string tf_prefix = tool_contact_params_.tf_prefix ;
70+
71+ // Start or stop tool contact functionality
72+ config.names .emplace_back (tf_prefix + " tool_contact/enable_cmd"
73+ config.names .emplace_back (tf_prefix + " tool_contact/async_success"
74+ config.names .emplace_back (tf_prefix + " end_tool_contact/end_tool_contact_cmd"
75+ config.names .emplace_back (tf_prefix + " end_tool_contact/end_tool_contact_async_success"
76+ return config;
77+ }
78+
79+ controller_interface::InterfaceConfiguration ToolContactController::state_interface_configuration () const
80+ {
81+ controller_interface::InterfaceConfiguration config;
82+ config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
83+ return config;
84+ }
85+
86+ controller_interface::CallbackReturn ToolContactController::on_activate (const rclcpp_lifecycle::State& previous_state)
87+ {
88+ tool_contact_enable_sub_ = get_node ()->create_subscription <std_msgs::msg::Bool>(
89+ " ~/enable_tool_contact" 10 ,
90+ std::bind (&ToolContactController::tool_contact_sub_callback, this , std::placeholders::_1));
91+
92+ /* Figure out how to get reference interface by name*/
93+ // const std::string interface_name = tool_contact_params_.tf_prefix + get_node()->get_name() + "tool_contact/enable";
94+ // auto it = std::find_if(reference_interfaces_.begin(), reference_interfaces_.end(),
95+ // [&](auto& interface) { return (interface.get_name() == interface_name); });
96+ // if (it != reference_interfaces_.end()) {
97+ // reference_interface_ = *it;
98+ // reference_interface_->get().set_value(0.0);
99+ // } else {
100+ // RCLCPP_ERROR(get_node()->get_logger(), "Did not find '%s' in command interfaces.", interface_name.c_str());
101+ // return controller_interface::CallbackReturn::ERROR;
102+ // }
103+ reference_interfaces_[0 ] = 0.0 ;
104+ {
105+ const std::string interface_name = tool_contact_params_.tf_prefix + " tool_contact/enable_cmd"
106+ auto it = std::find_if (command_interfaces_.begin (), command_interfaces_.end (),
107+ [&](auto & interface) { return (interface.get_name () == interface_name); });
108+ if (it != command_interfaces_.end ()) {
109+ enable_command_interface_ = *it;
110+ enable_command_interface_->get ().set_value (ASYNC_STANDBY_);
111+ } else {
112+ RCLCPP_ERROR (get_node ()->get_logger (), " Did not find '%s' in command interfaces." c_str ());
113+ return controller_interface::CallbackReturn::ERROR;
114+ }
115+ }
116+ {
117+ const std::string interface_name = tool_contact_params_.tf_prefix + " tool_contact/async_success"
118+ auto it = std::find_if (command_interfaces_.begin (), command_interfaces_.end (),
119+ [&](auto & interface) { return (interface.get_name () == interface_name); });
120+ if (it != command_interfaces_.end ()) {
121+ async_success_interface_ = *it;
122+ async_success_interface_->get ().set_value (ASYNC_STANDBY_);
123+ } else {
124+ RCLCPP_ERROR (get_node ()->get_logger (), " Did not find '%s' in command interfaces." c_str ());
125+ return controller_interface::CallbackReturn::ERROR;
126+ }
127+ }
128+
129+ return controller_interface::CallbackReturn::SUCCESS;
130+ }
131+
132+ controller_interface::CallbackReturn ToolContactController::on_deactivate (const rclcpp_lifecycle::State& previous_state)
133+ {
134+ tool_contact_enable_sub_.reset ();
135+
136+ return controller_interface::CallbackReturn::SUCCESS;
137+ }
138+
139+ void ToolContactController::tool_contact_sub_callback (const std_msgs::msg::Bool::SharedPtr msg)
140+ {
141+ std::cout << " data received: " data << std::endl;
142+ if (msg->data ) {
143+ if (!tool_contact_active_ && !change_requested_) {
144+ tool_contact_active_ = true ;
145+ change_requested_ = true ;
146+ }
147+ } else {
148+ if (tool_contact_active_ && !change_requested_) {
149+ tool_contact_active_ = false ;
150+ change_requested_ = true ;
151+ }
152+ }
153+ }
154+
155+ std::vector<hardware_interface::CommandInterface> ToolContactController::on_export_reference_interfaces ()
156+ {
157+ std::vector<hardware_interface::CommandInterface> reference_interfaces;
158+ reference_interfaces.reserve (1 );
159+ reference_interfaces.push_back (
160+ hardware_interface::CommandInterface (get_node ()->get_name (), " tool_contact/enable"
161+
162+ return reference_interfaces;
163+ }
164+
165+ std::vector<hardware_interface::StateInterface> ToolContactController::on_export_state_interfaces ()
166+ {
167+ std::vector<hardware_interface::StateInterface> state_interfaces;
168+ state_interfaces.push_back (
169+ hardware_interface::StateInterface (get_node ()->get_name (), " tool_contact/active"
170+
171+ return state_interfaces;
172+ }
173+
174+ bool ToolContactController::on_set_chained_mode (bool chained_mode)
175+ {
176+ if (chained_mode) {
177+ tool_contact_enable_sub_.reset ();
178+ } else {
179+ tool_contact_enable_sub_ = get_node ()->create_subscription <std_msgs::msg::Bool>(
180+ " ~/enable_tool_contact" 10 ,
181+ std::bind (&ToolContactController::tool_contact_sub_callback, this , std::placeholders::_1));
182+ }
183+ change_requested_ = false ;
184+ tool_contact_active_ = false ;
185+ return true ;
186+ }
187+
188+ controller_interface::return_type
189+ ToolContactController::update_reference_from_subscribers (const rclcpp::Time& time, const rclcpp::Duration& period)
190+ {
191+ if (change_requested_) {
192+ reference_interfaces_[0 ] = static_cast <double >(tool_contact_active_);
193+ change_requested_ = false ;
194+ }
195+ return controller_interface::return_type::OK;
196+ }
197+
198+ controller_interface::return_type ToolContactController::update_and_write_commands (const rclcpp::Time& time,
199+ const rclcpp::Duration& period)
200+ {
201+ if (reference_interfaces_[0 ] != old_reference_val) {
202+ enable_command_interface_->get ().set_value (reference_interfaces_[0 ]);
203+ async_success_interface_->get ().set_value (ASYNC_WAITING);
204+ std::cout << " Tool contact interface set to: " 0 ] << std::endl;
205+ }
206+
207+ if (async_success_interface_->get ().get_value () == 0.0 ) {
208+ RCLCPP_ERROR (get_node ()->get_logger (), " Failed to enable tool contact mode."
209+ async_success_interface_->get ().set_value (ASYNC_STANDBY_);
210+
211+ } else if (async_success_interface_->get ().get_value () == 1.0 ) {
212+ RCLCPP_INFO (get_node ()->get_logger (), " Tool contact enabled successfully "
213+ async_success_interface_->get ().set_value (ASYNC_STANDBY_);
214+ }
215+
216+ old_reference_val = reference_interfaces_[0 ];
217+ return controller_interface::return_type::OK;
218+ }
219+
46220} // namespace ur_controllers
221+
222+ #include " pluginlib/class_list_macros.hpp"
223+
224+ PLUGINLIB_EXPORT_CLASS (ur_controllers::ToolContactController, controller_interface::ChainableControllerInterface)
0 commit comments