Skip to content

Commit

Permalink
Merge pull request #51 from kroshu/fix/ci
Browse files Browse the repository at this point in the history
lint, uncrustify
  • Loading branch information
Svastits authored Jan 27, 2023
2 parents 859cf2b + 9026d79 commit 6e63b8b
Show file tree
Hide file tree
Showing 4 changed files with 53 additions and 53 deletions.
50 changes: 24 additions & 26 deletions kuka_rox_hardware_interface/src/robot_manager_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,8 @@

#include "kuka_rox_hw_interface/robot_manager_node.hpp"

using namespace controller_manager_msgs::srv; // NOLINT

namespace kuka_rox
{

Expand All @@ -25,11 +27,11 @@ RobotManagerNode::RobotManagerNode()
qos.reliable();
cbg_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
change_hardware_state_client_ =
this->create_client<controller_manager_msgs::srv::SetHardwareComponentState>(
this->create_client<SetHardwareComponentState>(
"controller_manager/set_hardware_component_state", qos.get_rmw_qos_profile(), cbg_
);
change_controller_state_client_ =
this->create_client<controller_manager_msgs::srv::SwitchController>(
this->create_client<SwitchController>(
"controller_manager/switch_controller", qos.get_rmw_qos_profile(), cbg_
);

Expand All @@ -46,13 +48,12 @@ RobotManagerNode::on_configure(const rclcpp_lifecycle::State &)
{
// Configure hardware interface
auto hw_request =
std::make_shared<controller_manager_msgs::srv::SetHardwareComponentState::Request>();
std::make_shared<SetHardwareComponentState::Request>();
hw_request->name = "iisy_hardware";
hw_request->target_state.label = "inactive";
auto hw_response =
kroshu_ros2_core::sendRequest<controller_manager_msgs::srv::SetHardwareComponentState::Response>(
change_hardware_state_client_, hw_request, 0, 2000
);
kroshu_ros2_core::sendRequest<SetHardwareComponentState::Response>(
change_hardware_state_client_, hw_request, 0, 2000);
if (!hw_response || !hw_response->ok) {
RCLCPP_ERROR(get_logger(), "Could not configure hardware interface");
return FAILURE;
Expand All @@ -69,13 +70,12 @@ RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &)
{
// Cleanup hardware interface
auto hw_request =
std::make_shared<controller_manager_msgs::srv::SetHardwareComponentState::Request>();
std::make_shared<SetHardwareComponentState::Request>();
hw_request->name = "iisy_hardware";
hw_request->target_state.label = "inactive";
auto hw_response =
kroshu_ros2_core::sendRequest<controller_manager_msgs::srv::SetHardwareComponentState::Response>(
change_hardware_state_client_, hw_request, 0, 2000
);
kroshu_ros2_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");
return FAILURE;
Expand Down Expand Up @@ -116,13 +116,12 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &)
{
// Activate hardware interface
auto hw_request =
std::make_shared<controller_manager_msgs::srv::SetHardwareComponentState::Request>();
std::make_shared<SetHardwareComponentState::Request>();
hw_request->name = "iisy_hardware";
hw_request->target_state.label = "active";
auto hw_response =
kroshu_ros2_core::sendRequest<controller_manager_msgs::srv::SetHardwareComponentState::Response>(
change_hardware_state_client_, hw_request, 0, 2000
);
kroshu_ros2_core::sendRequest<SetHardwareComponentState::Response>(
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
Expand All @@ -133,11 +132,11 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &)

// Activate joint state broadcaster
auto controller_request =
std::make_shared<controller_manager_msgs::srv::SwitchController::Request>();
controller_request->strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT;
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 =
kroshu_ros2_core::sendRequest<controller_manager_msgs::srv::SwitchController::Response>(
kroshu_ros2_core::sendRequest<SwitchController::Response>(
change_controller_state_client_, controller_request, 0, 2000
);
if (!controller_response || !controller_response->ok) {
Expand All @@ -159,10 +158,10 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &)
}

// Activate RT commander
controller_request->strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT;
controller_request->strictness = SwitchController::Request::STRICT;
controller_request->activate_controllers = controller_names_;
controller_response =
kroshu_ros2_core::sendRequest<controller_manager_msgs::srv::SwitchController::Response>(
kroshu_ros2_core::sendRequest<SwitchController::Response>(
change_controller_state_client_, controller_request, 0, 2000
);
if (!controller_response || !controller_response->ok) {
Expand All @@ -181,13 +180,12 @@ RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &)
{
// Deactivate hardware interface
auto hw_request =
std::make_shared<controller_manager_msgs::srv::SetHardwareComponentState::Request>();
std::make_shared<SetHardwareComponentState::Request>();
hw_request->name = "iisy_hardware";
hw_request->target_state.label = "inactive";
auto hw_response =
kroshu_ros2_core::sendRequest<controller_manager_msgs::srv::SetHardwareComponentState::Response>(
change_hardware_state_client_, hw_request, 0, 2000
);
kroshu_ros2_core::sendRequest<SetHardwareComponentState::Response>(
change_hardware_state_client_, hw_request, 0, 2000);
if (!hw_response || !hw_response->ok) {
RCLCPP_ERROR(get_logger(), "Could not deactivate hardware interface");
return ERROR;
Expand All @@ -198,13 +196,13 @@ RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &)
// Stop RT controllers
controller_names_.emplace_back("joint_state_broadcaster");
auto controller_request =
std::make_shared<controller_manager_msgs::srv::SwitchController::Request>();
std::make_shared<SwitchController::Request>();
// With best effort strictness, deactivation succeeds if specific controller is not active
controller_request->strictness =
controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT;
SwitchController::Request::BEST_EFFORT;
controller_request->deactivate_controllers = controller_names_;
auto controller_response =
kroshu_ros2_core::sendRequest<controller_manager_msgs::srv::SwitchController::Response>(
kroshu_ros2_core::sendRequest<SwitchController::Response>(
change_controller_state_client_, controller_request, 0, 2000
);
if (!controller_response || !controller_response->ok) {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,7 @@ static std::unordered_map<std::string,

class KUKAFRIHardwareInterface : public hardware_interface::SystemInterface,
public KUKA::FRI::LBRClient
{
{
public:
KUKAFRIHardwareInterface()
: client_application_(udp_connection_, *this) {}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -125,14 +125,14 @@ CallbackReturn KUKAFRIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta
RCLCPP_ERROR(rclcpp::get_logger("KUKAFRIHardwareInterface"), "Could not connect");
return CallbackReturn::FAILURE;
}
is_active_= true;
is_active_ = true;
return CallbackReturn::SUCCESS;
}

CallbackReturn KUKAFRIHardwareInterface::on_deactivate(const rclcpp_lifecycle::State &)
{
client_application_.disconnect();
is_active_= false;
is_active_ = false;
return CallbackReturn::SUCCESS;
}

Expand Down
50 changes: 26 additions & 24 deletions kuka_sunrise_driver/src/kuka_sunrise/robot_manager_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,8 @@

#include "kuka_sunrise/robot_manager_node.hpp"

using namespace controller_manager_msgs::srv; // NOLINT

namespace kuka_sunrise
{

Expand All @@ -41,10 +43,10 @@ RobotManagerNode::RobotManagerNode()
qos.reliable();
cbg_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
change_hardware_state_client_ =
this->create_client<controller_manager_msgs::srv::SetHardwareComponentState>(
this->create_client<SetHardwareComponentState>(
"controller_manager/set_hardware_component_state", qos.get_rmw_qos_profile(), cbg_);
change_controller_state_client_ =
this->create_client<controller_manager_msgs::srv::SwitchController>(
this->create_client<SwitchController>(
"controller_manager/switch_controller", qos.get_rmw_qos_profile(), cbg_);
command_state_changed_publisher_ = this->create_publisher<std_msgs::msg::Bool>(
"robot_manager/commanding_state_changed", qos);
Expand All @@ -64,11 +66,11 @@ RobotManagerNode::on_configure(const rclcpp_lifecycle::State &)
{
// Configure hardware interface
auto hw_request =
std::make_shared<controller_manager_msgs::srv::SetHardwareComponentState::Request>();
std::make_shared<SetHardwareComponentState::Request>();
hw_request->name = "iiwa_hardware";
hw_request->target_state.label = "inactive";
auto hw_response =
kroshu_ros2_core::sendRequest<controller_manager_msgs::srv::SetHardwareComponentState::Response>(
kroshu_ros2_core::sendRequest<SetHardwareComponentState::Response>(
change_hardware_state_client_, hw_request, 0, 2000);
if (!hw_response || !hw_response->ok) {
RCLCPP_ERROR(get_logger(), "Could not configure hardware interface");
Expand All @@ -88,12 +90,12 @@ RobotManagerNode::on_configure(const rclcpp_lifecycle::State &)

// Start non-RT controllers
auto controller_request =
std::make_shared<controller_manager_msgs::srv::SwitchController::Request>();
controller_request->strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT;
std::make_shared<SwitchController::Request>();
controller_request->strictness = SwitchController::Request::STRICT;
controller_request->activate_controllers =
std::vector<std::string>{"timing_controller", "robot_state_broadcaster"};
auto controller_response =
kroshu_ros2_core::sendRequest<controller_manager_msgs::srv::SwitchController::Response>(
kroshu_ros2_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");
Expand Down Expand Up @@ -144,14 +146,14 @@ RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &)

// Stop non-RT controllers
auto controller_request =
std::make_shared<controller_manager_msgs::srv::SwitchController::Request>();
std::make_shared<SwitchController::Request>();
// With best effort strictness, cleanup succeeds if specific controller is not active
controller_request->strictness =
controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT;
SwitchController::Request::BEST_EFFORT;
controller_request->deactivate_controllers =
std::vector<std::string>{"timing_controller", "robot_state_broadcaster"};
auto controller_response =
kroshu_ros2_core::sendRequest<controller_manager_msgs::srv::SwitchController::Response>(
kroshu_ros2_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");
Expand All @@ -161,11 +163,11 @@ RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &)
// Cleanup hardware interface
// If it is inactive, cleanup will also succeed
auto hw_request =
std::make_shared<controller_manager_msgs::srv::SetHardwareComponentState::Request>();
std::make_shared<SetHardwareComponentState::Request>();
hw_request->name = "iiwa_hardware";
hw_request->target_state.label = "unconfigured";
auto hw_response =
kroshu_ros2_core::sendRequest<controller_manager_msgs::srv::SetHardwareComponentState::Response>(
kroshu_ros2_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");
Expand Down Expand Up @@ -229,11 +231,11 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &)

// Activate hardware interface
auto hw_request =
std::make_shared<controller_manager_msgs::srv::SetHardwareComponentState::Request>();
std::make_shared<SetHardwareComponentState::Request>();
hw_request->name = "iiwa_hardware";
hw_request->target_state.label = "active";
auto hw_response =
kroshu_ros2_core::sendRequest<controller_manager_msgs::srv::SetHardwareComponentState::Response>(
kroshu_ros2_core::sendRequest<SetHardwareComponentState::Response>(
change_hardware_state_client_, hw_request, 0, 2000);
if (!hw_response || !hw_response->ok) {
RCLCPP_ERROR(get_logger(), "Could not activate hardware interface");
Expand All @@ -254,11 +256,11 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &)
// The other controller must be started later so that it can initialize internal state
// with broadcaster information
auto controller_request =
std::make_shared<controller_manager_msgs::srv::SwitchController::Request>();
controller_request->strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT;
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 =
kroshu_ros2_core::sendRequest<controller_manager_msgs::srv::SwitchController::Response>(
kroshu_ros2_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");
Expand All @@ -271,10 +273,10 @@ 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 = controller_manager_msgs::srv::SwitchController::Request::STRICT;
controller_request->strictness = SwitchController::Request::STRICT;
controller_request->activate_controllers = std::vector<std::string>{controller_name_};
controller_response =
kroshu_ros2_core::sendRequest<controller_manager_msgs::srv::SwitchController::Response>(
kroshu_ros2_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");
Expand Down Expand Up @@ -313,11 +315,11 @@ RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &)
// Deactivate hardware interface
// If it is inactive, deactivation will also succeed
auto hw_request =
std::make_shared<controller_manager_msgs::srv::SetHardwareComponentState::Request>();
std::make_shared<SetHardwareComponentState::Request>();
hw_request->name = "iiwa_hardware";
hw_request->target_state.label = "inactive";
auto hw_response =
kroshu_ros2_core::sendRequest<controller_manager_msgs::srv::SetHardwareComponentState::Response>(
kroshu_ros2_core::sendRequest<SetHardwareComponentState::Response>(
change_hardware_state_client_, hw_request, 0, 2000);
if (!hw_response || !hw_response->ok) {
RCLCPP_ERROR(get_logger(), "Could not deactivate hardware interface");
Expand All @@ -327,14 +329,14 @@ RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &)

// Stop RT controllers
auto controller_request =
std::make_shared<controller_manager_msgs::srv::SwitchController::Request>();
std::make_shared<SwitchController::Request>();
// With best effort strictness, deactivation succeeds if specific controller is not active
controller_request->strictness =
controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT;
SwitchController::Request::BEST_EFFORT;
controller_request->deactivate_controllers =
std::vector<std::string>{controller_name_, "joint_state_broadcaster"};
auto controller_response =
kroshu_ros2_core::sendRequest<controller_manager_msgs::srv::SwitchController::Response>(
kroshu_ros2_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");
Expand Down

0 comments on commit 6e63b8b

Please sign in to comment.