Skip to content

Commit

Permalink
remove sendrequests
Browse files Browse the repository at this point in the history
  • Loading branch information
Aron Svastits committed Dec 14, 2023
1 parent ffee3d3 commit c8c3597
Show file tree
Hide file tree
Showing 3 changed files with 71 additions and 141 deletions.
4 changes: 0 additions & 4 deletions kuka_iiqka_eac_driver/src/robot_manager_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -376,8 +376,6 @@ bool RobotManagerNode::onControlModeChangeRequest(int control_mode)
if (is_active_state) {
// The driver is in active state
// Activate controllers needed for the new control mode
auto controller_request =
std::make_shared<SwitchController::Request>();
if (!switch_controllers.first.empty()) {
if (!kuka_drivers_core::changeControllerState(
change_controller_state_client_, switch_controllers.first, {}))
Expand Down Expand Up @@ -421,8 +419,6 @@ bool RobotManagerNode::onControlModeChangeRequest(int control_mode)
RCLCPP_INFO(get_logger(), "Robot Controller finished control mode change");
#endif

auto controller_request =
std::make_shared<SwitchController::Request>();
// Deactivate unnecessary controllers
if (!switch_controllers.second.empty()) {
if (!kuka_drivers_core::changeControllerState(
Expand Down
84 changes: 28 additions & 56 deletions kuka_kss_rsi_driver/src/robot_manager_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,8 @@
#include "kuka_kss_rsi_driver/robot_manager_node.hpp"
#include "kuka_drivers_core/control_mode.hpp"

#include "communication_helpers/ros2_control_tools.hpp"


using namespace controller_manager_msgs::srv; // NOLINT
using namespace lifecycle_msgs::msg; // NOLINT
Expand Down Expand Up @@ -55,14 +57,10 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
RobotManagerNode::on_configure(const rclcpp_lifecycle::State &)
{
// Configure hardware interface
auto hw_request =
std::make_shared<SetHardwareComponentState::Request>();
hw_request->name = robot_model_;
hw_request->target_state.id = State::PRIMARY_STATE_INACTIVE;
auto hw_response =
kuka_drivers_core::sendRequest<SetHardwareComponentState::Response>(
change_hardware_state_client_, hw_request, 0, 2000);
if (!hw_response || !hw_response->ok) {
if (!kuka_drivers_core::changeHardwareState(
change_hardware_state_client_, robot_model_,
State::PRIMARY_STATE_INACTIVE))
{
RCLCPP_ERROR(get_logger(), "Could not configure hardware interface");
return FAILURE;
}
Expand All @@ -78,14 +76,10 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &)
{
// Clean up hardware interface
auto hw_request =
std::make_shared<SetHardwareComponentState::Request>();
hw_request->name = robot_model_;
hw_request->target_state.id = State::PRIMARY_STATE_UNCONFIGURED;
auto hw_response =
kuka_drivers_core::sendRequest<SetHardwareComponentState::Response>(
change_hardware_state_client_, hw_request, 0, 2000);
if (!hw_response || !hw_response->ok) {
if (!kuka_drivers_core::changeHardwareState(
change_hardware_state_client_, robot_model_,
State::PRIMARY_STATE_UNCONFIGURED))
{
RCLCPP_ERROR(get_logger(), "Could not clean up hardware interface");
return FAILURE;
}
Expand All @@ -104,32 +98,21 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
RobotManagerNode::on_activate(const rclcpp_lifecycle::State &)
{
// Activate hardware interface
auto hw_request =
std::make_shared<SetHardwareComponentState::Request>();
hw_request->name = robot_model_;
hw_request->target_state.id = State::PRIMARY_STATE_ACTIVE;
auto hw_response =
kuka_drivers_core::sendRequest<SetHardwareComponentState::Response>(
change_hardware_state_client_, hw_request, 0, 10000);
if (!hw_response || !hw_response->ok) {
if (!kuka_drivers_core::changeHardwareState(
change_hardware_state_client_, robot_model_,
State::PRIMARY_STATE_ACTIVE, 10000))
{
RCLCPP_ERROR(get_logger(), "Could not activate hardware interface");
return FAILURE;
}


// Activate RT controller(s)
auto controller_request =
std::make_shared<SwitchController::Request>();
controller_request->strictness = SwitchController::Request::STRICT;
controller_request->activate_controllers =
{"joint_state_broadcaster", "joint_trajectory_controller"};

auto controller_response =
kuka_drivers_core::sendRequest<SwitchController::Response>(
change_controller_state_client_, controller_request, 0, 2000
);
if (!controller_response || !controller_response->ok) {
RCLCPP_ERROR(get_logger(), "Could not activate controller");
if (!kuka_drivers_core::changeControllerState(
change_controller_state_client_, {"joint_state_broadcaster", "joint_trajectory_controller"},
{}))
{
RCLCPP_ERROR(get_logger(), "Could not activate RT controllers");
// TODO(Svastits): this can be removed if rollback is implemented properly
this->on_deactivate(get_current_state());
return FAILURE;
Expand All @@ -143,34 +126,23 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &)
{
// Deactivate hardware interface
auto hw_request =
std::make_shared<SetHardwareComponentState::Request>();
hw_request->name = robot_model_;
hw_request->target_state.id = State::PRIMARY_STATE_INACTIVE;
auto hw_response =
kuka_drivers_core::sendRequest<SetHardwareComponentState::Response>(
change_hardware_state_client_, hw_request, 0, 3000); // was not stable with 2000 ms timeout
if (!hw_response || !hw_response->ok) {
if (!kuka_drivers_core::changeHardwareState(
change_hardware_state_client_, robot_model_,
State::PRIMARY_STATE_INACTIVE))
{
RCLCPP_ERROR(get_logger(), "Could not deactivate hardware interface");
return ERROR;
}

RCLCPP_INFO(get_logger(), "Deactivated hardware interface");


// Stop RT controllers
auto controller_request =
std::make_shared<SwitchController::Request>();
// With best effort strictness, deactivation succeeds if specific controller is not active
controller_request->strictness =
SwitchController::Request::BEST_EFFORT;
controller_request->deactivate_controllers =
{"joint_state_broadcaster", "joint_trajectory_controller"};
auto controller_response =
kuka_drivers_core::sendRequest<SwitchController::Response>(
change_controller_state_client_, controller_request, 0, 2000
);
if (!controller_response || !controller_response->ok) {
if (!kuka_drivers_core::changeControllerState(
change_controller_state_client_, {},
{"joint_state_broadcaster", "joint_trajectory_controller"},
SwitchController::Request::BEST_EFFORT))
{
RCLCPP_ERROR(get_logger(), "Could not stop controllers");
return ERROR;
}
Expand Down
124 changes: 43 additions & 81 deletions kuka_sunrise_fri_driver/src/robot_manager_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -71,14 +71,10 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
RobotManagerNode::on_configure(const rclcpp_lifecycle::State &)
{
// Configure hardware interface
auto hw_request =
std::make_shared<SetHardwareComponentState::Request>();
hw_request->name = robot_model_;
hw_request->target_state.label = "inactive";
auto hw_response =
kuka_drivers_core::sendRequest<SetHardwareComponentState::Response>(
change_hardware_state_client_, hw_request, 0, 2000);
if (!hw_response || !hw_response->ok) {
if (!kuka_drivers_core::changeHardwareState(
change_hardware_state_client_, robot_model_,
State::PRIMARY_STATE_INACTIVE))
{
RCLCPP_ERROR(get_logger(), "Could not configure hardware interface");
return FAILURE;
}
Expand All @@ -95,16 +91,11 @@ RobotManagerNode::on_configure(const rclcpp_lifecycle::State &)
RCLCPP_INFO(get_logger(), "Successfully set 'controller_ip' parameter");

// Start non-RT controllers
auto controller_request =
std::make_shared<SwitchController::Request>();
controller_request->strictness = SwitchController::Request::STRICT;
controller_request->activate_controllers =
std::vector<std::string>{"timing_controller", "fri_state_broadcaster"};
auto controller_response =
kuka_drivers_core::sendRequest<SwitchController::Response>(
change_controller_state_client_, controller_request, 0, 3000);
if (!controller_response || !controller_response->ok) {
RCLCPP_ERROR(get_logger(), "Could not start controllers");
if (!kuka_drivers_core::changeControllerState(
change_controller_state_client_, {"fri_configuration_controller", "fri_state_broadcaster"},
{}))
{
RCLCPP_ERROR(get_logger(), "Could not activate configuration controllers");
result = FAILURE;
}

Expand Down Expand Up @@ -151,32 +142,23 @@ RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &)
}

// Stop non-RT controllers
auto controller_request =
std::make_shared<SwitchController::Request>();
// With best effort strictness, cleanup succeeds if specific controller is not active
controller_request->strictness =
SwitchController::Request::BEST_EFFORT;
controller_request->deactivate_controllers =
std::vector<std::string>{"fri_configuration_controller", "fri_state_broadcaster"};
auto controller_response =
kuka_drivers_core::sendRequest<SwitchController::Response>(
change_controller_state_client_, controller_request, 0, 2000);
if (!controller_response || !controller_response->ok) {
if (!kuka_drivers_core::changeControllerState(
change_controller_state_client_, {},
{"fri_configuration_controller", "fri_state_broadcaster"},
SwitchController::Request::BEST_EFFORT))
{
RCLCPP_ERROR(get_logger(), "Could not stop controllers");
return ERROR;
}

// Cleanup hardware interface
// If it is inactive, cleanup will also succeed
auto hw_request =
std::make_shared<SetHardwareComponentState::Request>();
hw_request->name = robot_model_;
hw_request->target_state.label = "unconfigured";
auto hw_response =
kuka_drivers_core::sendRequest<SetHardwareComponentState::Response>(
change_hardware_state_client_, hw_request, 0, 2000);
if (!hw_response || !hw_response->ok) {
RCLCPP_ERROR(get_logger(), "Could not cleanup hardware interface");
if (!kuka_drivers_core::changeHardwareState(
change_hardware_state_client_, robot_model_,
State::PRIMARY_STATE_UNCONFIGURED))
{
RCLCPP_ERROR(get_logger(), "Could not clean up hardware interface");
return FAILURE;
}

Expand Down Expand Up @@ -209,18 +191,15 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &)
RCLCPP_INFO(get_logger(), "Successfully set FRI config");

// Activate hardware interface
auto hw_request =
std::make_shared<SetHardwareComponentState::Request>();
hw_request->name = robot_model_;
hw_request->target_state.label = "active";
auto hw_response =
kuka_drivers_core::sendRequest<SetHardwareComponentState::Response>(
change_hardware_state_client_, hw_request, 0, 2000);
if (!hw_response || !hw_response->ok) {
if (!kuka_drivers_core::changeHardwareState(
change_hardware_state_client_, robot_model_,
State::PRIMARY_STATE_ACTIVE))
{
RCLCPP_ERROR(get_logger(), "Could not activate hardware interface");
// 'unset config' does not exist, safe to return
return FAILURE;
}

RCLCPP_INFO(get_logger(), "Activated LBR iiwa hardware interface");

// Start FRI (in monitoring mode)
Expand All @@ -233,16 +212,12 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &)

// Activate joint state broadcaster
// The other controller must be started later so that it can initialize internal state
// with broadcaster information
auto controller_request =
std::make_shared<SwitchController::Request>();
controller_request->strictness = SwitchController::Request::STRICT;
controller_request->activate_controllers = std::vector<std::string>{"joint_state_broadcaster"};
auto controller_response =
kuka_drivers_core::sendRequest<SwitchController::Response>(
change_controller_state_client_, controller_request, 0, 2000);
if (!controller_response || !controller_response->ok) {
RCLCPP_ERROR(get_logger(), "Could not start joint state broadcaster");
// with broadcaster information -> TODO(Svastits): validate whether this is true
if (!kuka_drivers_core::changeControllerState(
change_controller_state_client_, {"joint_state_broadcaster"},
{}))
{
RCLCPP_ERROR(get_logger(), "Could not activate joint state broadcaster");
this->on_deactivate(get_current_state());
return FAILURE;
}
Expand All @@ -252,13 +227,11 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &)
controller_name_ = (this->get_parameter("command_mode").as_string() ==
"position") ? position_controller_name : torque_controller_name;
// Activate RT commander
controller_request->strictness = SwitchController::Request::STRICT;
controller_request->activate_controllers = std::vector<std::string>{controller_name_};
controller_response =
kuka_drivers_core::sendRequest<SwitchController::Response>(
change_controller_state_client_, controller_request, 0, 2000);
if (!controller_response || !controller_response->ok) {
RCLCPP_ERROR(get_logger(), "Could not activate controller");
if (!kuka_drivers_core::changeControllerState(
change_controller_state_client_, {controller_name_},
{}))
{
RCLCPP_ERROR(get_logger(), "Could not activate RT controller");
this->on_deactivate(get_current_state());
return FAILURE;
}
Expand Down Expand Up @@ -293,32 +266,21 @@ RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &)

// Deactivate hardware interface
// If it is inactive, deactivation will also succeed
auto hw_request =
std::make_shared<SetHardwareComponentState::Request>();
hw_request->name = robot_model_;
hw_request->target_state.label = "inactive";
auto hw_response =
kuka_drivers_core::sendRequest<SetHardwareComponentState::Response>(
change_hardware_state_client_, hw_request, 0, 2000);
if (!hw_response || !hw_response->ok) {
if (!kuka_drivers_core::changeHardwareState(
change_hardware_state_client_, robot_model_,
State::PRIMARY_STATE_INACTIVE))
{
RCLCPP_ERROR(get_logger(), "Could not deactivate hardware interface");
return ERROR;
}

RCLCPP_INFO(get_logger(), "Deactivated LBR iiwa hardware interface");

// Stop RT controllers
auto controller_request =
std::make_shared<SwitchController::Request>();
// With best effort strictness, deactivation succeeds if specific controller is not active
controller_request->strictness =
SwitchController::Request::BEST_EFFORT;
controller_request->deactivate_controllers =
std::vector<std::string>{controller_name_, "joint_state_broadcaster"};
auto controller_response =
kuka_drivers_core::sendRequest<SwitchController::Response>(
change_controller_state_client_, controller_request, 0, 2000);
if (!controller_response || !controller_response->ok) {
RCLCPP_ERROR(get_logger(), "Could not stop controllers");
if (!kuka_drivers_core::changeControllerState(change_controller_state_client_, {},
{controller_name_, "joint_state_broadcaster"}, SwitchController::Request::BEST_EFFORT)) {
RCLCPP_ERROR(get_logger(), "Could not deactivate controllers");
return ERROR;
}

Expand Down

0 comments on commit c8c3597

Please sign in to comment.