diff --git a/kuka_rox_hardware_interface/include/kuka_rox_hw_interface/rox_hardware_interface.hpp b/kuka_rox_hardware_interface/include/kuka_rox_hw_interface/rox_hardware_interface.hpp index fac15563..9dab89af 100644 --- a/kuka_rox_hardware_interface/include/kuka_rox_hw_interface/rox_hardware_interface.hpp +++ b/kuka_rox_hardware_interface/include/kuka_rox_hw_interface/rox_hardware_interface.hpp @@ -91,8 +91,6 @@ class KukaRoXHardwareInterface : public hardware_interface::SystemInterface #endif std::thread observe_thread_; - std::atomic terminate_{false}; - std::mutex observe_mutex_; std::unique_ptr udp_replier_; std::chrono::milliseconds receive_timeout_ {100}; diff --git a/kuka_rox_hardware_interface/src/robot_manager_node.cpp b/kuka_rox_hardware_interface/src/robot_manager_node.cpp index e35f44cf..0e37e198 100644 --- a/kuka_rox_hardware_interface/src/robot_manager_node.cpp +++ b/kuka_rox_hardware_interface/src/robot_manager_node.cpp @@ -106,6 +106,11 @@ RobotManagerNode::RobotManagerNode() RobotManagerNode::~RobotManagerNode() { +#ifdef NON_MOCK_SETUP + if (context_ != nullptr) { + context_->TryCancel(); + } +#endif if (observe_thread_.joinable()) { observe_thread_.join(); } @@ -161,33 +166,28 @@ RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &) void RobotManagerNode::ObserveControl() { #ifdef NON_MOCK_SETUP - while (!terminate_) { - context_ = std::make_unique<::grpc::ClientContext>(); - kuka::ecs::v1::ObserveControlStateRequest observe_request; - std::unique_ptr> reader( - stub_->ObserveControlState(context_.get(), observe_request)); - - kuka::ecs::v1::CommandState response; - - if (reader->Read(&response)) { - switch (static_cast(response.event())) { - case kuka::ecs::v1::CommandEvent::STOPPED: - case kuka::ecs::v1::CommandEvent::ERROR: - RCLCPP_INFO(get_logger(), "External control stopped"); - terminate_ = true; - if (this->get_current_state().id() == State::PRIMARY_STATE_ACTIVE) { - this->deactivate(); - } else if (this->get_current_state().id() == State::TRANSITION_STATE_ACTIVATING) { - // TODO(Svastits): this can be removed if rollback is implemented properly - this->on_deactivate(get_current_state()); - } - break; - default: - break; - } - } else { - // WORKAROUND: Ec is starting later so we have some errors before stable work. - std::this_thread::sleep_for(std::chrono::milliseconds(300)); + context_ = std::make_unique<::grpc::ClientContext>(); + kuka::ecs::v1::ObserveControlStateRequest observe_request; + std::unique_ptr> reader( + stub_->ObserveControlState(context_.get(), observe_request)); + + kuka::ecs::v1::CommandState response; + + while (reader->Read(&response)) { + switch (static_cast(response.event())) { + case kuka::ecs::v1::CommandEvent::STOPPED: + case kuka::ecs::v1::CommandEvent::ERROR: + RCLCPP_INFO(get_logger(), "External control stopped"); + terminate_ = true; + if (this->get_current_state().id() == State::PRIMARY_STATE_ACTIVE) { + this->deactivate(); + } else if (this->get_current_state().id() == State::TRANSITION_STATE_ACTIVATING) { + // TODO(Svastits): this can be removed if rollback is implemented properly + this->on_deactivate(get_current_state()); + } + break; + default: + break; } } #endif @@ -197,6 +197,11 @@ void RobotManagerNode::ObserveControl() rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) { +#ifdef NON_MOCK_SETUP + if (context_ != nullptr) { + context_->TryCancel(); + } +#endif // Join observe thread, necessary if previous activation failed if (observe_thread_.joinable()) { observe_thread_.join(); @@ -215,8 +220,6 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) change_hardware_state_client_, hw_request, 0, 2000); if (!hw_response || !hw_response->ok) { RCLCPP_ERROR(get_logger(), "Could not activate hardware interface"); - // 'unset config' does not exist, safe to return - terminate_ = true; return FAILURE; } @@ -231,6 +234,8 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) ); if (!controller_response || !controller_response->ok) { RCLCPP_ERROR(get_logger(), "Could not start joint state broadcaster"); + // TODO(Svastits): this can be removed if rollback is implemented properly + this->on_deactivate(get_current_state()); return FAILURE; } @@ -248,6 +253,8 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) ); if (!controller_response || !controller_response->ok) { RCLCPP_ERROR(get_logger(), "Could not activate controller"); + // TODO(Svastits): this can be removed if rollback is implemented properly + this->on_deactivate(get_current_state()); return FAILURE; } RCLCPP_INFO(get_logger(), "Successfully activated controllers"); @@ -272,7 +279,7 @@ RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &) hw_request->target_state.id = State::PRIMARY_STATE_INACTIVE; auto hw_response = kroshu_ros2_core::sendRequest( - change_hardware_state_client_, hw_request, 0, 2000); + change_hardware_state_client_, hw_request, 0, 3000); // was not stable with 2000 ms timeout if (!hw_response || !hw_response->ok) { RCLCPP_ERROR(get_logger(), "Could not deactivate hardware interface"); return ERROR; diff --git a/kuka_rox_hardware_interface/src/rox_hardware_interface.cpp b/kuka_rox_hardware_interface/src/rox_hardware_interface.cpp index 5b4fd47f..8ba62d13 100644 --- a/kuka_rox_hardware_interface/src/rox_hardware_interface.cpp +++ b/kuka_rox_hardware_interface/src/rox_hardware_interface.cpp @@ -70,12 +70,6 @@ CallbackReturn KukaRoXHardwareInterface::on_init(const hardware_interface::Hardw } #endif - // if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) { - // RCLCPP_ERROR(rclcpp::get_logger("KukaRoXHardwareInterface"), "mlockall error"); - // RCLCPP_ERROR(rclcpp::get_logger("KukaRoXHardwareInterface"), strerror(errno)); - // return CallbackReturn::ERROR; - // } - RCLCPP_INFO(rclcpp::get_logger("KukaRoXHardwareInterface"), "Init successful"); return CallbackReturn::SUCCESS; @@ -133,8 +127,6 @@ export_command_interfaces() CallbackReturn KukaRoXHardwareInterface::on_configure(const rclcpp_lifecycle::State &) { - // TODO(Svastits): Set QoS profile here - // should be using another configuration file read from xacro #ifdef NON_MOCK_SETUP SetQoSProfileRequest request; SetQoSProfileResponse response; @@ -175,9 +167,12 @@ CallbackReturn KukaRoXHardwareInterface::on_activate(const rclcpp_lifecycle::Sta RCLCPP_INFO(rclcpp::get_logger("KukaRoXHardwareInterface"), "Connecting to robot . . ."); // Reset timeout to catch first tick message receive_timeout_ = std::chrono::milliseconds(100); - terminate_ = false; control_signal_ext_.control_signal.stop_ipo = false; - +#ifdef NON_MOCK_SETUP + if (context_ != nullptr) { + context_->TryCancel(); + } +#endif if (observe_thread_.joinable()) { observe_thread_.join(); } @@ -208,7 +203,6 @@ CallbackReturn KukaRoXHardwareInterface::on_activate(const rclcpp_lifecycle::Sta .error_code() != grpc::StatusCode::OK) { RCLCPP_ERROR(rclcpp::get_logger("KukaRoXHardwareInterface"), "OpenControlChannel failed"); - terminate_ = true; return CallbackReturn::FAILURE; } #endif @@ -226,13 +220,11 @@ CallbackReturn KukaRoXHardwareInterface::on_deactivate( std::this_thread::sleep_for(std::chrono::milliseconds(10)); } - terminate_ = true; #ifdef NON_MOCK_SETUP if (context_ != nullptr) { context_->TryCancel(); } #endif - if (observe_thread_.joinable()) { observe_thread_.join(); } @@ -345,53 +337,44 @@ bool KukaRoXHardwareInterface::isActive() const void KukaRoXHardwareInterface::ObserveControl() { #ifdef NON_MOCK_SETUP - RCLCPP_INFO( - rclcpp::get_logger( - "KukaRoXHardwareInterface"), - "Observe control"); - while (!terminate_) { - context_ = std::make_unique<::grpc::ClientContext>(); - ObserveControlStateRequest obs_control; - std::unique_ptr> reader( - stub_->ObserveControlState(context_.get(), obs_control)); - - CommandState response; - - if (reader->Read(&response)) { - std::unique_lock lck(observe_mutex_); // TODO(Svastits): is this necessary? - command_state_ = response; - RCLCPP_INFO( - rclcpp::get_logger( - "KukaRoXHardwareInterface"), - "Event streamed from external control service: %s", CommandEvent_Name( - response.event()).c_str()); - - switch (static_cast(response.event())) { - case CommandEvent::COMMAND_READY: - RCLCPP_INFO(rclcpp::get_logger("KukaRoXHardwareInterface"), "Command accepted"); - break; - case CommandEvent::SAMPLING: - RCLCPP_INFO(rclcpp::get_logger("KukaRoXHardwareInterface"), "External control is active"); - is_active_ = true; - break; - case CommandEvent::STOPPED: - RCLCPP_INFO(rclcpp::get_logger("KukaRoXHardwareInterface"), "External control finished"); - is_active_ = false; - break; - case CommandEvent::ERROR: - RCLCPP_ERROR( - rclcpp::get_logger( - "KukaRoXHardwareInterface"), - "External control stopped by an error"); - RCLCPP_ERROR(rclcpp::get_logger("KukaRoXHardwareInterface"), response.message().c_str()); - is_active_ = false; - break; - default: - break; - } - } else { - // WORKAROUND: Ec is starting later so we have some errors before stable work. - std::this_thread::sleep_for(std::chrono::milliseconds(300)); + RCLCPP_INFO(rclcpp::get_logger("KukaRoXHardwareInterface"), "Observe control"); + context_ = std::make_unique<::grpc::ClientContext>(); + ObserveControlStateRequest obs_control; + std::unique_ptr> reader( + stub_->ObserveControlState(context_.get(), obs_control)); + + CommandState response; + + while (reader->Read(&response)) { + command_state_ = response; + RCLCPP_INFO( + rclcpp::get_logger( + "KukaRoXHardwareInterface"), + "Event streamed from external control service: %s", CommandEvent_Name( + response.event()).c_str()); + + switch (static_cast(response.event())) { + case CommandEvent::COMMAND_READY: + RCLCPP_INFO(rclcpp::get_logger("KukaRoXHardwareInterface"), "Command accepted"); + break; + case CommandEvent::SAMPLING: + RCLCPP_INFO(rclcpp::get_logger("KukaRoXHardwareInterface"), "External control is active"); + is_active_ = true; + break; + case CommandEvent::STOPPED: + RCLCPP_INFO(rclcpp::get_logger("KukaRoXHardwareInterface"), "External control finished"); + is_active_ = false; + break; + case CommandEvent::ERROR: + RCLCPP_ERROR( + rclcpp::get_logger( + "KukaRoXHardwareInterface"), + "External control stopped by an error"); + RCLCPP_ERROR(rclcpp::get_logger("KukaRoXHardwareInterface"), response.message().c_str()); + is_active_ = false; + break; + default: + break; } } #endif