From 859cf2b64e111d06badb40a92bbdd3a64979a5dd Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 27 Jan 2023 17:13:51 +0100 Subject: [PATCH] change namespace for service tools --- .../src/robot_manager_node.cpp | 14 ++++++------- .../kuka_sunrise/configuration_manager.cpp | 4 ++-- .../src/kuka_sunrise/robot_manager.cpp | 10 +++++----- .../src/kuka_sunrise/robot_manager_node.cpp | 20 +++++++++---------- 4 files changed, 24 insertions(+), 24 deletions(-) diff --git a/kuka_rox_hardware_interface/src/robot_manager_node.cpp b/kuka_rox_hardware_interface/src/robot_manager_node.cpp index ebdce871..92bfcb2e 100644 --- a/kuka_rox_hardware_interface/src/robot_manager_node.cpp +++ b/kuka_rox_hardware_interface/src/robot_manager_node.cpp @@ -50,7 +50,7 @@ RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) hw_request->name = "iisy_hardware"; hw_request->target_state.label = "inactive"; auto hw_response = - kuka_sunrise::sendRequest( + kroshu_ros2_core::sendRequest( change_hardware_state_client_, hw_request, 0, 2000 ); if (!hw_response || !hw_response->ok) { @@ -73,7 +73,7 @@ RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &) hw_request->name = "iisy_hardware"; hw_request->target_state.label = "inactive"; auto hw_response = - kuka_sunrise::sendRequest( + kroshu_ros2_core::sendRequest( change_hardware_state_client_, hw_request, 0, 2000 ); if (!hw_response || !hw_response->ok) { @@ -120,7 +120,7 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) hw_request->name = "iisy_hardware"; hw_request->target_state.label = "active"; auto hw_response = - kuka_sunrise::sendRequest( + kroshu_ros2_core::sendRequest( change_hardware_state_client_, hw_request, 0, 2000 ); if (!hw_response || !hw_response->ok) { @@ -137,7 +137,7 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) controller_request->strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; controller_request->activate_controllers = std::vector{"joint_state_broadcaster"}; auto controller_response = - kuka_sunrise::sendRequest( + kroshu_ros2_core::sendRequest( change_controller_state_client_, controller_request, 0, 2000 ); if (!controller_response || !controller_response->ok) { @@ -162,7 +162,7 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) controller_request->strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; controller_request->activate_controllers = controller_names_; controller_response = - kuka_sunrise::sendRequest( + kroshu_ros2_core::sendRequest( change_controller_state_client_, controller_request, 0, 2000 ); if (!controller_response || !controller_response->ok) { @@ -185,7 +185,7 @@ RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &) hw_request->name = "iisy_hardware"; hw_request->target_state.label = "inactive"; auto hw_response = - kuka_sunrise::sendRequest( + kroshu_ros2_core::sendRequest( change_hardware_state_client_, hw_request, 0, 2000 ); if (!hw_response || !hw_response->ok) { @@ -204,7 +204,7 @@ RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &) controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; controller_request->deactivate_controllers = controller_names_; auto controller_response = - kuka_sunrise::sendRequest( + kroshu_ros2_core::sendRequest( change_controller_state_client_, controller_request, 0, 2000 ); if (!controller_response || !controller_response->ok) { diff --git a/kuka_sunrise_driver/src/kuka_sunrise/configuration_manager.cpp b/kuka_sunrise_driver/src/kuka_sunrise/configuration_manager.cpp index 79ceb0c5..7830527b 100644 --- a/kuka_sunrise_driver/src/kuka_sunrise/configuration_manager.cpp +++ b/kuka_sunrise_driver/src/kuka_sunrise/configuration_manager.cpp @@ -207,7 +207,7 @@ bool ConfigurationManager::onControllerNameChangeRequest( { auto request = std::make_shared(); auto response = - kuka_sunrise::sendRequest( + kroshu_ros2_core::sendRequest( get_controllers_client_, request, 0, 1000); if (!response) { @@ -264,7 +264,7 @@ bool ConfigurationManager::setReceiveMultiplier(int receive_multiplier) const // Set receive multiplier of hardware interface through controller manager service auto request = std::make_shared(); request->data = receive_multiplier; - auto response = kuka_sunrise::sendRequest( + auto response = kroshu_ros2_core::sendRequest( receive_multiplier_client_, request, 0, 1000); if (!response || !response->success) { diff --git a/kuka_sunrise_driver/src/kuka_sunrise/robot_manager.cpp b/kuka_sunrise_driver/src/kuka_sunrise/robot_manager.cpp index 4d169645..d911cc75 100644 --- a/kuka_sunrise_driver/src/kuka_sunrise/robot_manager.cpp +++ b/kuka_sunrise_driver/src/kuka_sunrise/robot_manager.cpp @@ -115,11 +115,11 @@ bool RobotManager::setJointImpedanceControlMode( } for (double js : joint_stiffness) { printf("js = %lf\n", js); - msg_size += serializeNext(js, serialized); + msg_size += kroshu_ros2_core::serializeNext(js, serialized); } for (double jd : joint_damping) { printf("jd = %lf\n", jd); - msg_size += serializeNext(jd, serialized); + msg_size += kroshu_ros2_core::serializeNext(jd, serialized); } return sendCommandAndWait(SET_CONTROL_MODE, serialized); } @@ -139,9 +139,9 @@ bool RobotManager::setFRIConfig(int remote_port, int send_period_ms, int receive serialized.emplace_back(byte); msg_size++; } - msg_size += serializeNext(remote_port, serialized); - msg_size += serializeNext(send_period_ms, serialized); - msg_size += serializeNext(receive_multiplier, serialized); + msg_size += kroshu_ros2_core::serializeNext(remote_port, serialized); + msg_size += kroshu_ros2_core::serializeNext(send_period_ms, serialized); + msg_size += kroshu_ros2_core::serializeNext(receive_multiplier, serialized); return sendCommandAndWait(SET_FRI_CONFIG, serialized); } diff --git a/kuka_sunrise_driver/src/kuka_sunrise/robot_manager_node.cpp b/kuka_sunrise_driver/src/kuka_sunrise/robot_manager_node.cpp index 411a67fb..09563208 100644 --- a/kuka_sunrise_driver/src/kuka_sunrise/robot_manager_node.cpp +++ b/kuka_sunrise_driver/src/kuka_sunrise/robot_manager_node.cpp @@ -68,7 +68,7 @@ RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) hw_request->name = "iiwa_hardware"; hw_request->target_state.label = "inactive"; auto hw_response = - kuka_sunrise::sendRequest( + kroshu_ros2_core::sendRequest( change_hardware_state_client_, hw_request, 0, 2000); if (!hw_response || !hw_response->ok) { RCLCPP_ERROR(get_logger(), "Could not configure hardware interface"); @@ -93,7 +93,7 @@ RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) controller_request->activate_controllers = std::vector{"timing_controller", "robot_state_broadcaster"}; auto controller_response = - kuka_sunrise::sendRequest( + kroshu_ros2_core::sendRequest( change_controller_state_client_, controller_request, 0, 3000); if (!controller_response || !controller_response->ok) { RCLCPP_ERROR(get_logger(), "Could not start controllers"); @@ -115,7 +115,7 @@ RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) if (result == SUCCESS) { auto trigger_request = std::make_shared(); - auto response = kuka_sunrise::sendRequest( + auto response = kroshu_ros2_core::sendRequest( set_parameter_client_, trigger_request, 0, 1000); if (!response || !response->success) { @@ -151,7 +151,7 @@ RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &) controller_request->deactivate_controllers = std::vector{"timing_controller", "robot_state_broadcaster"}; auto controller_response = - kuka_sunrise::sendRequest( + kroshu_ros2_core::sendRequest( change_controller_state_client_, controller_request, 0, 2000); if (!controller_response || !controller_response->ok) { RCLCPP_ERROR(get_logger(), "Could not stop controllers"); @@ -165,7 +165,7 @@ RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &) hw_request->name = "iiwa_hardware"; hw_request->target_state.label = "unconfigured"; auto hw_response = - kuka_sunrise::sendRequest( + kroshu_ros2_core::sendRequest( change_hardware_state_client_, hw_request, 0, 2000); if (!hw_response || !hw_response->ok) { RCLCPP_ERROR(get_logger(), "Could not cleanup hardware interface"); @@ -233,7 +233,7 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) hw_request->name = "iiwa_hardware"; hw_request->target_state.label = "active"; auto hw_response = - kuka_sunrise::sendRequest( + kroshu_ros2_core::sendRequest( change_hardware_state_client_, hw_request, 0, 2000); if (!hw_response || !hw_response->ok) { RCLCPP_ERROR(get_logger(), "Could not activate hardware interface"); @@ -258,7 +258,7 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) controller_request->strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; controller_request->activate_controllers = std::vector{"joint_state_broadcaster"}; auto controller_response = - kuka_sunrise::sendRequest( + kroshu_ros2_core::sendRequest( change_controller_state_client_, controller_request, 0, 2000); if (!controller_response || !controller_response->ok) { RCLCPP_ERROR(get_logger(), "Could not start joint state broadcaster"); @@ -274,7 +274,7 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) controller_request->strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; controller_request->activate_controllers = std::vector{controller_name_}; controller_response = - kuka_sunrise::sendRequest( + kroshu_ros2_core::sendRequest( change_controller_state_client_, controller_request, 0, 2000); if (!controller_response || !controller_response->ok) { RCLCPP_ERROR(get_logger(), "Could not activate controller"); @@ -317,7 +317,7 @@ RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &) hw_request->name = "iiwa_hardware"; hw_request->target_state.label = "inactive"; auto hw_response = - kuka_sunrise::sendRequest( + kroshu_ros2_core::sendRequest( change_hardware_state_client_, hw_request, 0, 2000); if (!hw_response || !hw_response->ok) { RCLCPP_ERROR(get_logger(), "Could not deactivate hardware interface"); @@ -334,7 +334,7 @@ RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &) controller_request->deactivate_controllers = std::vector{controller_name_, "joint_state_broadcaster"}; auto controller_response = - kuka_sunrise::sendRequest( + kroshu_ros2_core::sendRequest( change_controller_state_client_, controller_request, 0, 2000); if (!controller_response || !controller_response->ok) { RCLCPP_ERROR(get_logger(), "Could not stop controllers");