From 7f5f14ca6ecf835c8500ab96fd8d13874b5617d5 Mon Sep 17 00:00:00 2001 From: Alexandre Vannobel Date: Tue, 9 Jun 2020 15:49:07 -0400 Subject: [PATCH 01/33] Created interface class and put the RPC handlers in robot folder --- kortex_driver/scripts/ros_kortex_generator.py | 40 ++++++++++++---- ...s.h.jinja2 => services_interface.h.jinja2} | 28 ++++------- ...s.cpp.jinja2 => services_robot.cpp.jinja2} | 24 +++++----- .../templates/services_robot.h.jinja2 | 48 +++++++++++++++++++ 4 files changed, 100 insertions(+), 40 deletions(-) rename kortex_driver/templates/{services.h.jinja2 => services_interface.h.jinja2} (72%) rename kortex_driver/templates/{services.cpp.jinja2 => services_robot.cpp.jinja2} (68%) create mode 100644 kortex_driver/templates/services_robot.h.jinja2 diff --git a/kortex_driver/scripts/ros_kortex_generator.py b/kortex_driver/scripts/ros_kortex_generator.py index 7bc002a7..c162807a 100755 --- a/kortex_driver/scripts/ros_kortex_generator.py +++ b/kortex_driver/scripts/ros_kortex_generator.py @@ -377,8 +377,11 @@ def generate_code(request, response): os.makedirs("../{}/generated/{}".format(s, package.short_name_lowercase_with_underscores)) shutil.rmtree("../src/generated", ignore_errors=True) shutil.rmtree("../include/kortex_driver/generated", ignore_errors=True) - os.makedirs("../src/generated") - os.makedirs("../include/kortex_driver/generated") + os.makedirs("../src/generated/robot") + os.makedirs("../src/generated/simulation") + os.makedirs("../include/kortex_driver/generated/interfaces") + os.makedirs("../include/kortex_driver/generated/robot") + os.makedirs("../include/kortex_driver/generated/simulation") ########################################### # Parse the proto files to add the messages and RPC's to the DetailedPackage's @@ -498,33 +501,50 @@ def generate_code(request, response): if package.messages: # package contains at least one message # Proto converter header file - current_header_filename = "kortex_driver/generated/{}_proto_converter.h".format(package.short_name.lower()) + current_header_filename = "kortex_driver/generated/robot/{}_proto_converter.h".format(package.short_name.lower()) this_package_context.include_file_names = filter(lambda x : "proto_converter" in x and x != current_header_filename, include_file_names) with open(os.path.join("..", "include/" + current_header_filename), 'wt') as converterFile: converterFile.write(render("../templates/proto_converter.h.jinja2", this_package_context.__dict__)) # Proto converter cpp file this_package_context.current_header_filename = current_header_filename - with open(os.path.join("..", "src/generated/{}_proto_converter.cpp".format(package.short_name.lower())), 'wt') as converterFile: + with open(os.path.join("..", "src/generated/robot/{}_proto_converter.cpp".format(package.short_name.lower())), 'wt') as converterFile: converterFile.write(render("../templates/proto_converter.cpp.jinja2", this_package_context.__dict__)) # ROS converter header file - current_header_filename = "kortex_driver/generated/{}_ros_converter.h".format(package.short_name.lower()) + current_header_filename = "kortex_driver/generated/robot/{}_ros_converter.h".format(package.short_name.lower()) this_package_context.include_file_names = filter(lambda x : "ros_converter" in x and x != current_header_filename, include_file_names) with open(os.path.join("..", "include/" + current_header_filename), 'wt') as converterFile: converterFile.write(render("../templates/ros_converter.h.jinja2", this_package_context.__dict__)) # ROS converter cpp file this_package_context.current_header_filename = current_header_filename - with open(os.path.join("..", "src/generated/{}_ros_converter.cpp".format(package.short_name.lower())), 'wt') as converterFile: + with open(os.path.join("..", "src/generated/robot/{}_ros_converter.cpp".format(package.short_name.lower())), 'wt') as converterFile: converterFile.write(render("../templates/ros_converter.cpp.jinja2", this_package_context.__dict__)) # Generate the ServiceProxy's for every Kortex API method if package.methods: # package contains at least one RPC - current_header_filename = "kortex_driver/generated/{}_services.h".format(package.short_name.lower()) + # Generate interface files + current_header_filename = "kortex_driver/generated/interfaces/{}_services_interface.h".format(package.short_name.lower()) + with open(os.path.join("..", "include/" + current_header_filename), 'wt') as services_file: + services_file.write(render("../templates/services_interface.h.jinja2", this_package_context.__dict__)) + + # Generate robot files + current_interface_header_filename = current_header_filename + current_header_filename = "kortex_driver/generated/robot/{}_services.h".format(package.short_name.lower()) + this_package_context.current_interface_header_filename = current_interface_header_filename this_package_context.current_header_filename = current_header_filename this_package_context.include_file_names = include_file_names with open(os.path.join("..", "include/" + current_header_filename), 'wt') as services_file: - services_file.write(render("../templates/services.h.jinja2", this_package_context.__dict__)) - with open(os.path.join("..", "src/generated/{}_services.cpp".format(package.short_name.lower())), 'wt') as services_file: - services_file.write(render("../templates/services.cpp.jinja2", this_package_context.__dict__)) + services_file.write(render("../templates/services_robot.h.jinja2", this_package_context.__dict__)) + with open(os.path.join("..", "src/generated/robot/{}_services.cpp".format(package.short_name.lower())), 'wt') as services_file: + services_file.write(render("../templates/services_robot.cpp.jinja2", this_package_context.__dict__)) + + # Generate simulation files + # current_header_filename = "kortex_driver/generated/simulation/{}_services.h".format(package.short_name.lower()) + # this_package_context.current_header_filename = current_header_filename + # this_package_context.include_file_names = include_file_names + # with open(os.path.join("..", "include/" + current_header_filename), 'wt') as services_file: + # services_file.write(render("../templates/services.h.jinja2", this_package_context.__dict__)) + # with open(os.path.join("..", "src/generated/{}_services.cpp".format(package.short_name.lower())), 'wt') as services_file: + # services_file.write(render("../templates/services.cpp.jinja2", this_package_context.__dict__)) # Delete unused folders we created for None for package in packages_dict.values(): diff --git a/kortex_driver/templates/services.h.jinja2 b/kortex_driver/templates/services_interface.h.jinja2 similarity index 72% rename from kortex_driver/templates/services.h.jinja2 rename to kortex_driver/templates/services_interface.h.jinja2 index 1199f9dd..e0f0c8c6 100644 --- a/kortex_driver/templates/services.h.jinja2 +++ b/kortex_driver/templates/services_interface.h.jinja2 @@ -14,8 +14,8 @@ * This file has been auto-generated and should not be modified. */ -#ifndef _KORTEX_{{package.short_name|upper}}_SERVICES_H_ -#define _KORTEX_{{package.short_name|upper}}_SERVICES_H_ +#ifndef _KORTEX_{{package.short_name|upper}}_SERVICES_INTERFACE_H_ +#define _KORTEX_{{package.short_name|upper}}_SERVICES_INTERFACE_H_ #include "ros/ros.h" @@ -25,9 +25,6 @@ #include #include -#include <{{package.short_name}}.pb.h> -#include <{{package.short_name}}ClientRpc.h> - {%- for method in package.methods %} #include "kortex_driver/{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}.h" {%- if method.is_notification_rpc %} @@ -42,27 +39,22 @@ using namespace std; -class {{package.short_name}}Services +class I{{package.short_name}}Services { public: - {{package.short_name}}Services(ros::NodeHandle& n, {{package.cpp_namespace}}::{{package.short_name}}Client* {{package.short_name|lower}}, uint32_t device_id, uint32_t timeout_ms); + {{package.short_name}}Services(ros::NodeHandle& node_handle); - bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res); - bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res); + bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) = 0; + bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) = 0; {%- for method in package.methods %} - bool {{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}(kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Request &req, kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Response &res); + bool {{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}(kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Request &req, kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Response &res) = 0; {%- if method.is_notification_rpc %} - void cb_{{method.name}}({{method.notification_message_cpp_namespace}}::{{method.prepend_rpc_package_name}}{{method.name|replace("Topic", "")}}Notification notif); + void cb_{{method.name}}({{method.notification_message_cpp_namespace}}::{{method.prepend_rpc_package_name}}{{method.name|replace("Topic", "")}}Notification notif) = 0; {%- endif %} {%- endfor %} -private: - uint32_t m_current_device_id; - Kinova::Api::RouterClientSendOptions m_api_options; - - {{package.cpp_namespace}}::{{package.short_name}}Client* m_{{package.short_name|lower}}; - - ros::NodeHandle m_n; +protected: + ros::NodeHandle m_node_handle; ros::Publisher m_pub_Error; {%- for method in package.methods %} diff --git a/kortex_driver/templates/services.cpp.jinja2 b/kortex_driver/templates/services_robot.cpp.jinja2 similarity index 68% rename from kortex_driver/templates/services.cpp.jinja2 rename to kortex_driver/templates/services_robot.cpp.jinja2 index 36f5af45..20da388c 100644 --- a/kortex_driver/templates/services.cpp.jinja2 +++ b/kortex_driver/templates/services_robot.cpp.jinja2 @@ -19,36 +19,36 @@ {% endfor -%} #include "{{current_header_filename}}" -{{package.short_name}}Services::{{package.short_name}}Services(ros::NodeHandle& n, {{package.cpp_namespace}}::{{package.short_name}}Client* {{package.short_name|lower}}, uint32_t device_id, uint32_t timeout_ms): - m_n(n), +{{package.short_name}}RobotServices::{{package.short_name}}RobotServices(ros::NodeHandle& node_handle, {{package.cpp_namespace}}::{{package.short_name}}Client* {{package.short_name|lower}}, uint32_t device_id, uint32_t timeout_ms): + I{{package.short_name}}Services(node_handle), m_{{package.short_name|lower}}({{package.short_name|lower}}), m_current_device_id(device_id) { m_api_options.timeout_ms = timeout_ms; - m_pub_Error = m_n.advertise("kortex_error", 1000); + m_pub_Error = m_node_handle.advertise("kortex_error", 1000); {%- for method in package.methods -%} {%- if method.is_notification_rpc %} - m_pub_{{method.name}} = m_n.advertise("{{method.name_lowercase_with_underscores}}", 1000); + m_pub_{{method.name}} = m_node_handle.advertise("{{method.name_lowercase_with_underscores}}", 1000); m_is_activated_{{method.name}} = false; {%- endif -%} {%- endfor %} - m_serviceSetDeviceID = n.advertiseService("{{package.short_name_lowercase_with_underscores}}/set_device_id", &{{package.short_name}}Services::SetDeviceID, this); - m_serviceSetApiOptions = n.advertiseService("{{package.short_name_lowercase_with_underscores}}/set_api_options", &{{package.short_name}}Services::SetApiOptions, this); + m_serviceSetDeviceID = n.advertiseService("{{package.short_name_lowercase_with_underscores}}/set_device_id", &{{package.short_name}}RobotServices::SetDeviceID, this); + m_serviceSetApiOptions = n.advertiseService("{{package.short_name_lowercase_with_underscores}}/set_api_options", &{{package.short_name}}RobotServices::SetApiOptions, this); {% for method in package.methods %} - m_service{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}} = m_n.advertiseService("{{package.short_name_lowercase_with_underscores}}/{%- if method.prepend_on_notification -%}activate_publishing_of_{%- endif -%}{{method.name_lowercase_with_underscores}}", &{{package.short_name}}Services::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}, this); + m_service{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}} = m_node_handle.advertiseService("{{package.short_name_lowercase_with_underscores}}/{%- if method.prepend_on_notification -%}activate_publishing_of_{%- endif -%}{{method.name_lowercase_with_underscores}}", &{{package.short_name}}RobotServices::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}, this); {%- endfor %} } -bool {{package.short_name}}Services::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) +bool {{package.short_name}}RobotServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) { m_current_device_id = req.device_id; return true; } -bool {{package.short_name}}Services::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) +bool {{package.short_name}}RobotServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) { m_api_options.timeout_ms = req.input.timeout_ms; @@ -56,7 +56,7 @@ bool {{package.short_name}}Services::SetApiOptions(kortex_driver::SetApiOptions: } {% for method in package.methods %} -bool {{package.short_name}}Services::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}(kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Request &req, kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Response &res) +bool {{package.short_name}}RobotServices::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}(kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Request &req, kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Response &res) { {% if method.is_rpc_deprecated -%} ROS_WARN("The {{package.short_name_lowercase_with_underscores}}/{%- if method.prepend_on_notification -%}activate_publishing_of_{%- endif -%}{{method.name_lowercase_with_underscores}} service is now deprecated and will be removed in a future release."); @@ -82,7 +82,7 @@ bool {{package.short_name}}Services::{{method.prepend_rpc_package_name}}{{method {%- if not method.output_type_short_name == "Empty" %} {%- if not method.input_type_short_name == "Empty" %} {%- if method.is_notification_rpc %} - std::function< void ({{method.notification_message_cpp_namespace}}::{{method.name|replace("Topic", "")}}Notification) > callback = std::bind(&{{package.short_name}}Services::cb_{{method.name}}, this, std::placeholders::_1); + std::function< void ({{method.notification_message_cpp_namespace}}::{{method.name|replace("Topic", "")}}Notification) > callback = std::bind(&{{package.short_name}}RobotServices::cb_{{method.name}}, this, std::placeholders::_1); output = m_{{package.short_name|lower}}->{{method.prepend_on_notification}}{{method.name}}(callback, input, m_current_device_id); m_is_activated_{{method.name}} = true; {%- else %} @@ -123,7 +123,7 @@ bool {{package.short_name}}Services::{{method.prepend_rpc_package_name}}{{method return true; } {%- if method.is_notification_rpc %} -void {{package.short_name}}Services::cb_{{method.name}}({{method.notification_message_cpp_namespace}}::{{method.name|replace("Topic", "")}}Notification notif) +void {{package.short_name}}RobotServices::cb_{{method.name}}({{method.notification_message_cpp_namespace}}::{{method.name|replace("Topic", "")}}Notification notif) { kortex_driver::{{method.name|replace("Topic", "")}}Notification ros_msg; ToRosData(notif, ros_msg); diff --git a/kortex_driver/templates/services_robot.h.jinja2 b/kortex_driver/templates/services_robot.h.jinja2 new file mode 100644 index 00000000..54214385 --- /dev/null +++ b/kortex_driver/templates/services_robot.h.jinja2 @@ -0,0 +1,48 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_{{package.short_name|upper}}_ROBOT_SERVICES_H_ +#define _KORTEX_{{package.short_name|upper}}_ROBOT_SERVICES_H_ + +#include "{{current_interface_header_filename}}" + +#include <{{package.short_name}}.pb.h> +#include <{{package.short_name}}ClientRpc.h> + +using namespace std; + +class {{package.short_name}}RobotServices : public I{{package.short_name}}Services +{ + public: + {{package.short_name}}RobotServices(ros::NodeHandle& node_handle, {{package.cpp_namespace}}::{{package.short_name}}Client* {{package.short_name|lower}}, uint32_t device_id, uint32_t timeout_ms); + + bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; + bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; +{%- for method in package.methods %} + bool {{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}(kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Request &req, kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Response &res) override; +{%- if method.is_notification_rpc %} + void cb_{{method.name}}({{method.notification_message_cpp_namespace}}::{{method.prepend_rpc_package_name}}{{method.name|replace("Topic", "")}}Notification notif) override; +{%- endif %} +{%- endfor %} + +private: + uint32_t m_current_device_id; + Kinova::Api::RouterClientSendOptions m_api_options; + + {{package.cpp_namespace}}::{{package.short_name}}Client* m_{{package.short_name|lower}}; +}; +#endif + From 8fe70f325e70f6aad9bfdddfc94e61ecb036e3c9 Mon Sep 17 00:00:00 2001 From: Alexandre Vannobel Date: Tue, 9 Jun 2020 16:38:30 -0400 Subject: [PATCH 02/33] Now the robot functions interface work just like before --- .../non-generated/kortex_arm_driver.h | 30 +++++++++---------- .../non-generated/kortex_subscribers.h | 2 +- kortex_driver/scripts/ros_kortex_generator.py | 2 +- .../driver/kortex_arm_driver.cpp | 14 ++++----- .../templates/services_interface.h.jinja2 | 10 +++---- .../templates/services_robot.cpp.jinja2 | 4 +-- .../templates/services_robot.h.jinja2 | 8 ++--- 7 files changed, 35 insertions(+), 35 deletions(-) diff --git a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h index 09595b86..40a1cfb4 100644 --- a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h +++ b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h @@ -35,15 +35,15 @@ #include "kortex_driver/non-generated/kortex_math_util.h" #include "kortex_driver/BaseCyclic_Feedback.h" -#include "kortex_driver/generated/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" -#include "kortex_driver/generated/actuatorconfig_services.h" -#include "kortex_driver/generated/base_services.h" -#include "kortex_driver/generated/deviceconfig_services.h" -#include "kortex_driver/generated/devicemanager_services.h" -#include "kortex_driver/generated/interconnectconfig_services.h" -#include "kortex_driver/generated/visionconfig_services.h" -#include "kortex_driver/generated/controlconfig_services.h" +#include "kortex_driver/generated/robot/actuatorconfig_services.h" +#include "kortex_driver/generated/robot/base_services.h" +#include "kortex_driver/generated/robot/deviceconfig_services.h" +#include "kortex_driver/generated/robot/devicemanager_services.h" +#include "kortex_driver/generated/robot/interconnectconfig_services.h" +#include "kortex_driver/generated/robot/visionconfig_services.h" +#include "kortex_driver/generated/robot/controlconfig_services.h" #include "kortex_driver/non-generated/pre_computed_joint_trajectory_action_server.h" #include "kortex_driver/non-generated/robotiq_gripper_command_action_server.h" @@ -122,13 +122,13 @@ class KortexArmDriver Kinova::Api::SessionManager* m_udp_session_manager; // ROS ServiceProxy's - ActuatorConfigServices* m_actuator_config_ros_services; - BaseServices* m_base_ros_services; - ControlConfigServices* m_control_config_ros_services; - DeviceConfigServices* m_device_config_ros_services; - DeviceManagerServices* m_device_manager_ros_services; - InterconnectConfigServices* m_interconnect_config_ros_services; - VisionConfigServices* m_vision_config_ros_services; + IActuatorConfigServices* m_actuator_config_ros_services; + IBaseServices* m_base_ros_services; + IControlConfigServices* m_control_config_ros_services; + IDeviceConfigServices* m_device_config_ros_services; + IDeviceManagerServices* m_device_manager_ros_services; + IInterconnectConfigServices* m_interconnect_config_ros_services; + IVisionConfigServices* m_vision_config_ros_services; // Action servers PreComputedJointTrajectoryActionServer* m_action_server_follow_joint_trajectory; diff --git a/kortex_driver/include/kortex_driver/non-generated/kortex_subscribers.h b/kortex_driver/include/kortex_driver/non-generated/kortex_subscribers.h index 26e14216..45ed76bd 100644 --- a/kortex_driver/include/kortex_driver/non-generated/kortex_subscribers.h +++ b/kortex_driver/include/kortex_driver/non-generated/kortex_subscribers.h @@ -21,7 +21,7 @@ #include "kortex_driver/TwistCommand.h" #include "kortex_driver/Base_JointSpeeds.h" -#include "kortex_driver/generated/base_proto_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" #include "kortex_driver/non-generated/kortex_math_util.h" class KortexSubscribers diff --git a/kortex_driver/scripts/ros_kortex_generator.py b/kortex_driver/scripts/ros_kortex_generator.py index c162807a..8d26368b 100755 --- a/kortex_driver/scripts/ros_kortex_generator.py +++ b/kortex_driver/scripts/ros_kortex_generator.py @@ -456,7 +456,7 @@ def generate_code(request, response): include_file_names = [] for p in packages_with_messages: for s in ["proto", "ros"]: - include_file_names.append("kortex_driver/generated/{}_{}_converter.h".format(p.short_name.lower(), s)) + include_file_names.append("kortex_driver/generated/robot/{}_{}_converter.h".format(p.short_name.lower(), s)) # Generate the ROS files for each package for package in packages_dict.values(): diff --git a/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp b/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp index a93a4d20..5f57e83e 100644 --- a/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp +++ b/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp @@ -415,14 +415,14 @@ void KortexArmDriver::initRosServices() { ROS_INFO("-------------------------------------------------"); ROS_INFO("Initializing Kortex Driver's services..."); - m_actuator_config_ros_services = new ActuatorConfigServices(m_node_handle, m_actuator_config, 0, m_api_rpc_timeout_ms); - m_base_ros_services = new BaseServices(m_node_handle, m_base, 0, m_api_rpc_timeout_ms); - m_control_config_ros_services = new ControlConfigServices(m_node_handle, m_control_config, 0, m_api_rpc_timeout_ms); - m_device_config_ros_services = new DeviceConfigServices(m_node_handle, m_device_config, 0, m_api_rpc_timeout_ms); - m_device_manager_ros_services = new DeviceManagerServices(m_node_handle, m_device_manager, 0, m_api_rpc_timeout_ms); + m_actuator_config_ros_services = new ActuatorConfigRobotServices(m_node_handle, m_actuator_config, 0, m_api_rpc_timeout_ms); + m_base_ros_services = new BaseRobotServices(m_node_handle, m_base, 0, m_api_rpc_timeout_ms); + m_control_config_ros_services = new ControlConfigRobotServices(m_node_handle, m_control_config, 0, m_api_rpc_timeout_ms); + m_device_config_ros_services = new DeviceConfigRobotServices(m_node_handle, m_device_config, 0, m_api_rpc_timeout_ms); + m_device_manager_ros_services = new DeviceManagerRobotServices(m_node_handle, m_device_manager, 0, m_api_rpc_timeout_ms); if (m_is_interconnect_module_present) { - m_interconnect_config_ros_services = new InterconnectConfigServices(m_node_handle, m_interconnect_config, m_interconnect_device_id, m_api_rpc_timeout_ms); + m_interconnect_config_ros_services = new InterconnectConfigRobotServices(m_node_handle, m_interconnect_config, m_interconnect_device_id, m_api_rpc_timeout_ms); } else { @@ -430,7 +430,7 @@ void KortexArmDriver::initRosServices() } if (m_is_vision_module_present) { - m_vision_config_ros_services = new VisionConfigServices(m_node_handle, m_vision_config, m_vision_device_id, m_api_rpc_timeout_ms); + m_vision_config_ros_services = new VisionConfigRobotServices(m_node_handle, m_vision_config, m_vision_device_id, m_api_rpc_timeout_ms); } else { diff --git a/kortex_driver/templates/services_interface.h.jinja2 b/kortex_driver/templates/services_interface.h.jinja2 index e0f0c8c6..9350d79f 100644 --- a/kortex_driver/templates/services_interface.h.jinja2 +++ b/kortex_driver/templates/services_interface.h.jinja2 @@ -42,14 +42,14 @@ using namespace std; class I{{package.short_name}}Services { public: - {{package.short_name}}Services(ros::NodeHandle& node_handle); + I{{package.short_name}}Services(ros::NodeHandle& node_handle) : m_node_handle(node_handle) {} - bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) = 0; - bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) = 0; + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) = 0; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) = 0; {%- for method in package.methods %} - bool {{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}(kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Request &req, kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Response &res) = 0; + virtual bool {{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}(kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Request &req, kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Response &res) = 0; {%- if method.is_notification_rpc %} - void cb_{{method.name}}({{method.notification_message_cpp_namespace}}::{{method.prepend_rpc_package_name}}{{method.name|replace("Topic", "")}}Notification notif) = 0; + virtual void cb_{{method.name}}({{method.notification_message_cpp_namespace}}::{{method.prepend_rpc_package_name}}{{method.name|replace("Topic", "")}}Notification notif) = 0; {%- endif %} {%- endfor %} diff --git a/kortex_driver/templates/services_robot.cpp.jinja2 b/kortex_driver/templates/services_robot.cpp.jinja2 index 20da388c..d559de06 100644 --- a/kortex_driver/templates/services_robot.cpp.jinja2 +++ b/kortex_driver/templates/services_robot.cpp.jinja2 @@ -34,8 +34,8 @@ {%- endif -%} {%- endfor %} - m_serviceSetDeviceID = n.advertiseService("{{package.short_name_lowercase_with_underscores}}/set_device_id", &{{package.short_name}}RobotServices::SetDeviceID, this); - m_serviceSetApiOptions = n.advertiseService("{{package.short_name_lowercase_with_underscores}}/set_api_options", &{{package.short_name}}RobotServices::SetApiOptions, this); + m_serviceSetDeviceID = m_node_handle.advertiseService("{{package.short_name_lowercase_with_underscores}}/set_device_id", &{{package.short_name}}RobotServices::SetDeviceID, this); + m_serviceSetApiOptions = m_node_handle.advertiseService("{{package.short_name_lowercase_with_underscores}}/set_api_options", &{{package.short_name}}RobotServices::SetApiOptions, this); {% for method in package.methods %} m_service{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}} = m_node_handle.advertiseService("{{package.short_name_lowercase_with_underscores}}/{%- if method.prepend_on_notification -%}activate_publishing_of_{%- endif -%}{{method.name_lowercase_with_underscores}}", &{{package.short_name}}RobotServices::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}, this); {%- endfor %} diff --git a/kortex_driver/templates/services_robot.h.jinja2 b/kortex_driver/templates/services_robot.h.jinja2 index 54214385..7a19498d 100644 --- a/kortex_driver/templates/services_robot.h.jinja2 +++ b/kortex_driver/templates/services_robot.h.jinja2 @@ -29,12 +29,12 @@ class {{package.short_name}}RobotServices : public I{{package.short_name}}Servic public: {{package.short_name}}RobotServices(ros::NodeHandle& node_handle, {{package.cpp_namespace}}::{{package.short_name}}Client* {{package.short_name|lower}}, uint32_t device_id, uint32_t timeout_ms); - bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; - bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; {%- for method in package.methods %} - bool {{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}(kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Request &req, kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Response &res) override; + virtual bool {{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}(kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Request &req, kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Response &res) override; {%- if method.is_notification_rpc %} - void cb_{{method.name}}({{method.notification_message_cpp_namespace}}::{{method.prepend_rpc_package_name}}{{method.name|replace("Topic", "")}}Notification notif) override; + virtual void cb_{{method.name}}({{method.notification_message_cpp_namespace}}::{{method.prepend_rpc_package_name}}{{method.name|replace("Topic", "")}}Notification notif) override; {%- endif %} {%- endfor %} From e0d02d3e075029d986a8814c2740a96fc24c7ed6 Mon Sep 17 00:00:00 2001 From: Alexandre Vannobel Date: Tue, 9 Jun 2020 17:37:53 -0400 Subject: [PATCH 03/33] Added templates and generation for simulated services --- kortex_driver/scripts/ros_kortex_generator.py | 18 ++--- .../templates/services_simulation.cpp.jinja2 | 81 +++++++++++++++++++ .../templates/services_simulation.h.jinja2 | 41 ++++++++++ 3 files changed, 131 insertions(+), 9 deletions(-) create mode 100644 kortex_driver/templates/services_simulation.cpp.jinja2 create mode 100644 kortex_driver/templates/services_simulation.h.jinja2 diff --git a/kortex_driver/scripts/ros_kortex_generator.py b/kortex_driver/scripts/ros_kortex_generator.py index 8d26368b..8d46ad33 100755 --- a/kortex_driver/scripts/ros_kortex_generator.py +++ b/kortex_driver/scripts/ros_kortex_generator.py @@ -523,13 +523,13 @@ def generate_code(request, response): if package.methods: # package contains at least one RPC # Generate interface files current_header_filename = "kortex_driver/generated/interfaces/{}_services_interface.h".format(package.short_name.lower()) + current_interface_header_filename = current_header_filename + this_package_context.current_interface_header_filename = current_interface_header_filename with open(os.path.join("..", "include/" + current_header_filename), 'wt') as services_file: services_file.write(render("../templates/services_interface.h.jinja2", this_package_context.__dict__)) # Generate robot files - current_interface_header_filename = current_header_filename current_header_filename = "kortex_driver/generated/robot/{}_services.h".format(package.short_name.lower()) - this_package_context.current_interface_header_filename = current_interface_header_filename this_package_context.current_header_filename = current_header_filename this_package_context.include_file_names = include_file_names with open(os.path.join("..", "include/" + current_header_filename), 'wt') as services_file: @@ -538,13 +538,13 @@ def generate_code(request, response): services_file.write(render("../templates/services_robot.cpp.jinja2", this_package_context.__dict__)) # Generate simulation files - # current_header_filename = "kortex_driver/generated/simulation/{}_services.h".format(package.short_name.lower()) - # this_package_context.current_header_filename = current_header_filename - # this_package_context.include_file_names = include_file_names - # with open(os.path.join("..", "include/" + current_header_filename), 'wt') as services_file: - # services_file.write(render("../templates/services.h.jinja2", this_package_context.__dict__)) - # with open(os.path.join("..", "src/generated/{}_services.cpp".format(package.short_name.lower())), 'wt') as services_file: - # services_file.write(render("../templates/services.cpp.jinja2", this_package_context.__dict__)) + current_header_filename = "kortex_driver/generated/simulation/{}_services.h".format(package.short_name.lower()) + this_package_context.current_header_filename = current_header_filename + this_package_context.include_file_names = include_file_names + with open(os.path.join("..", "include/" + current_header_filename), 'wt') as services_file: + services_file.write(render("../templates/services_simulation.h.jinja2", this_package_context.__dict__)) + with open(os.path.join("..", "src/generated/simulation/{}_services.cpp".format(package.short_name.lower())), 'wt') as services_file: + services_file.write(render("../templates/services_simulation.cpp.jinja2", this_package_context.__dict__)) # Delete unused folders we created for None for package in packages_dict.values(): diff --git a/kortex_driver/templates/services_simulation.cpp.jinja2 b/kortex_driver/templates/services_simulation.cpp.jinja2 new file mode 100644 index 00000000..6c478764 --- /dev/null +++ b/kortex_driver/templates/services_simulation.cpp.jinja2 @@ -0,0 +1,81 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +{% for include_file_name in include_file_names -%} +#include "{{include_file_name}}" +{% endfor -%} +#include "{{current_header_filename}}" + +{{package.short_name}}SimulationServices::{{package.short_name}}SimulationServices(ros::NodeHandle& node_handle): + I{{package.short_name}}Services(node_handle) +{ + m_pub_Error = m_node_handle.advertise("kortex_error", 1000); +{%- for method in package.methods -%} +{%- if method.is_notification_rpc %} + m_pub_{{method.name}} = m_node_handle.advertise("{{method.name_lowercase_with_underscores}}", 1000); + m_is_activated_{{method.name}} = false; +{%- endif -%} +{%- endfor %} + + m_serviceSetDeviceID = m_node_handle.advertiseService("{{package.short_name_lowercase_with_underscores}}/set_device_id", &{{package.short_name}}SimulationServices::SetDeviceID, this); + m_serviceSetApiOptions = m_node_handle.advertiseService("{{package.short_name_lowercase_with_underscores}}/set_api_options", &{{package.short_name}}SimulationServices::SetApiOptions, this); +{% for method in package.methods %} + m_service{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}} = m_node_handle.advertiseService("{{package.short_name_lowercase_with_underscores}}/{%- if method.prepend_on_notification -%}activate_publishing_of_{%- endif -%}{{method.name_lowercase_with_underscores}}", &{{package.short_name}}SimulationServices::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}, this); +{%- endfor %} +} + +bool {{package.short_name}}SimulationServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) +{ + ROS_WARN_ONCE("The SetDeviceID service is not implemented in simulation, and has no effect."); + return true; +} + +bool {{package.short_name}}SimulationServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) +{ + ROS_WARN_ONCE("The SetDeviceID service is not implemented in simulation, and has no effect."); + return true; +} + +{% for method in package.methods %} +bool {{package.short_name}}SimulationServices::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}(kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Request &req, kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Response &res) +{ + {% if method.is_rpc_deprecated -%} + ROS_WARN("The {{package.short_name_lowercase_with_underscores}}/{%- if method.prepend_on_notification -%}activate_publishing_of_{%- endif -%}{{method.name_lowercase_with_underscores}} service is now deprecated and will be removed in a future release."); + {% endif -%} + + {%- if method.is_notification_rpc %} + m_is_activated_{{method.name}} = true; + {%- endif %} + + if ({{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}Handler) + { + res = {{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}Handler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for {{package.short_name_lowercase_with_underscores}}/{%- if method.prepend_on_notification -%}activate_publishing_of_{%- endif -%}{{method.name_lowercase_with_underscores}} is not implemented, so the service calls will return the default response."); + } + return true; +} +{%- if method.is_notification_rpc %} +void {{package.short_name}}SimulationServices::cb_{{method.name}}({{method.notification_message_cpp_namespace}}::{{method.name|replace("Topic", "")}}Notification notif) +{ + kortex_driver::{{method.name|replace("Topic", "")}}Notification ros_msg; + ToRosData(notif, ros_msg); + m_pub_{{method.name}}.publish(ros_msg); +} +{%- endif %} +{% endfor -%} diff --git a/kortex_driver/templates/services_simulation.h.jinja2 b/kortex_driver/templates/services_simulation.h.jinja2 new file mode 100644 index 00000000..9a9e49a5 --- /dev/null +++ b/kortex_driver/templates/services_simulation.h.jinja2 @@ -0,0 +1,41 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_{{package.short_name|upper}}_SIMULATION_SERVICES_H_ +#define _KORTEX_{{package.short_name|upper}}_SIMULATION_SERVICES_H_ + +#include "{{current_interface_header_filename}}" + +using namespace std; + +class {{package.short_name}}SimulationServices : public I{{package.short_name}}Services +{ + public: + {{package.short_name}}SimulationServices(ros::NodeHandle& node_handle); + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; +{%- for method in package.methods %} + std::function {{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}Handler = nullptr; + virtual bool {{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}(kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Request &req, kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Response &res) override; +{%- if method.is_notification_rpc %} + virtual void cb_{{method.name}}({{method.notification_message_cpp_namespace}}::{{method.prepend_rpc_package_name}}{{method.name|replace("Topic", "")}}Notification notif) override; +{%- endif %} +{%- endfor %} + +}; +#endif + From 6637d45eaa341cf3a1def10d4b9b0cb52f12621f Mon Sep 17 00:00:00 2001 From: Alexandre Vannobel Date: Tue, 9 Jun 2020 17:38:30 -0400 Subject: [PATCH 04/33] Added the simulation services to the driver headers --- .../kortex_driver/non-generated/kortex_arm_driver.h | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h index 40a1cfb4..335a8cb0 100644 --- a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h +++ b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h @@ -45,6 +45,14 @@ #include "kortex_driver/generated/robot/visionconfig_services.h" #include "kortex_driver/generated/robot/controlconfig_services.h" +#include "kortex_driver/generated/simulation/actuatorconfig_services.h" +#include "kortex_driver/generated/simulation/base_services.h" +#include "kortex_driver/generated/simulation/deviceconfig_services.h" +#include "kortex_driver/generated/simulation/devicemanager_services.h" +#include "kortex_driver/generated/simulation/interconnectconfig_services.h" +#include "kortex_driver/generated/simulation/visionconfig_services.h" +#include "kortex_driver/generated/simulation/controlconfig_services.h" + #include "kortex_driver/non-generated/pre_computed_joint_trajectory_action_server.h" #include "kortex_driver/non-generated/robotiq_gripper_command_action_server.h" #include "kortex_driver/non-generated/kortex_subscribers.h" From 892513bb30bdb18207705e27e73f59e2572ed95c Mon Sep 17 00:00:00 2001 From: Alexandre Vannobel Date: Tue, 9 Jun 2020 17:39:50 -0400 Subject: [PATCH 05/33] Added newly generated files to repo --- CMakeLists.txt | 2 +- kortex_driver/CMakeLists.txt | 2 +- .../generated/actuatorconfig_services.h | 119 - .../kortex_driver/generated/base_services.h | 547 ---- .../generated/controlconfig_services.h | 150 - .../generated/deviceconfig_services.h | 159 -- .../generated/devicemanager_proto_converter.h | 49 - .../generated/devicemanager_ros_converter.h | 49 - .../generated/devicemanager_services.h | 62 - .../generated/interconnectconfig_services.h | 101 - .../actuatorconfig_services_interface.h | 111 + .../interfaces/base_services_interface.h | 539 ++++ .../controlconfig_services_interface.h | 142 + .../deviceconfig_services_interface.h | 151 + .../devicemanager_services_interface.h | 54 + .../interconnectconfig_services_interface.h | 93 + .../visionconfig_services_interface.h | 91 + .../actuatorconfig_proto_converter.h | 24 +- .../actuatorconfig_ros_converter.h | 24 +- .../generated/robot/actuatorconfig_services.h | 61 + .../actuatorcyclic_proto_converter.h | 24 +- .../actuatorcyclic_ros_converter.h | 24 +- .../{ => robot}/base_proto_converter.h | 24 +- .../{ => robot}/base_ros_converter.h | 24 +- .../generated/robot/base_services.h | 199 ++ .../{ => robot}/basecyclic_proto_converter.h | 24 +- .../{ => robot}/basecyclic_ros_converter.h | 24 +- .../{ => robot}/common_proto_converter.h | 24 +- .../{ => robot}/common_ros_converter.h | 24 +- .../controlconfig_proto_converter.h | 24 +- .../{ => robot}/controlconfig_ros_converter.h | 24 +- .../generated/robot/controlconfig_services.h | 71 + .../deviceconfig_proto_converter.h | 24 +- .../{ => robot}/deviceconfig_ros_converter.h | 24 +- .../generated/robot/deviceconfig_services.h | 74 + .../robot/devicemanager_proto_converter.h | 49 + .../robot/devicemanager_ros_converter.h | 49 + .../generated/robot/devicemanager_services.h | 42 + .../grippercyclic_proto_converter.h | 24 +- .../{ => robot}/grippercyclic_ros_converter.h | 24 +- .../interconnectconfig_proto_converter.h | 24 +- .../interconnectconfig_ros_converter.h | 24 +- .../robot/interconnectconfig_services.h | 55 + .../interconnectcyclic_proto_converter.h | 24 +- .../interconnectcyclic_ros_converter.h | 24 +- .../productconfiguration_proto_converter.h | 24 +- .../productconfiguration_ros_converter.h | 24 +- .../visionconfig_proto_converter.h | 24 +- .../{ => robot}/visionconfig_ros_converter.h | 24 +- .../generated/robot/visionconfig_services.h | 54 + .../simulation/actuatorconfig_services.h | 73 + .../generated/simulation/base_services.h | 335 +++ .../simulation/controlconfig_services.h | 92 + .../simulation/deviceconfig_services.h | 98 + .../simulation/devicemanager_services.h | 35 + .../simulation/interconnectconfig_services.h | 61 + .../simulation/visionconfig_services.h | 58 + .../generated/visionconfig_services.h | 99 - .../src/generated/devicemanager_services.cpp | 106 - .../actuatorconfig_proto_converter.cpp | 2 +- .../actuatorconfig_ros_converter.cpp | 2 +- .../{ => robot}/actuatorconfig_services.cpp | 154 +- .../actuatorcyclic_proto_converter.cpp | 2 +- .../actuatorcyclic_ros_converter.cpp | 2 +- .../{ => robot}/base_proto_converter.cpp | 2 +- .../{ => robot}/base_ros_converter.cpp | 2 +- .../generated/{ => robot}/base_services.cpp | 752 ++--- .../basecyclic_proto_converter.cpp | 2 +- .../{ => robot}/basecyclic_ros_converter.cpp | 2 +- .../{ => robot}/common_proto_converter.cpp | 2 +- .../{ => robot}/common_ros_converter.cpp | 2 +- .../controlconfig_proto_converter.cpp | 2 +- .../controlconfig_ros_converter.cpp | 2 +- .../{ => robot}/controlconfig_services.cpp | 194 +- .../deviceconfig_proto_converter.cpp | 2 +- .../deviceconfig_ros_converter.cpp | 2 +- .../{ => robot}/deviceconfig_services.cpp | 206 +- .../devicemanager_proto_converter.cpp | 2 +- .../devicemanager_ros_converter.cpp | 2 +- .../robot/devicemanager_services.cpp | 106 + .../grippercyclic_proto_converter.cpp | 2 +- .../grippercyclic_ros_converter.cpp | 2 +- .../interconnectconfig_proto_converter.cpp | 2 +- .../interconnectconfig_ros_converter.cpp | 2 +- .../interconnectconfig_services.cpp | 130 +- .../interconnectcyclic_proto_converter.cpp | 2 +- .../interconnectcyclic_ros_converter.cpp | 2 +- .../productconfiguration_proto_converter.cpp | 2 +- .../productconfiguration_ros_converter.cpp | 2 +- .../visionconfig_proto_converter.cpp | 2 +- .../visionconfig_ros_converter.cpp | 2 +- .../{ => robot}/visionconfig_services.cpp | 126 +- .../simulation/actuatorconfig_services.cpp | 386 +++ .../generated/simulation/base_services.cpp | 2504 +++++++++++++++++ .../simulation/controlconfig_services.cpp | 539 ++++ .../simulation/deviceconfig_services.cpp | 587 ++++ .../simulation/devicemanager_services.cpp | 82 + .../interconnectconfig_services.cpp | 290 ++ .../simulation/visionconfig_services.cpp | 267 ++ 99 files changed, 8445 insertions(+), 2538 deletions(-) delete mode 100644 kortex_driver/include/kortex_driver/generated/actuatorconfig_services.h delete mode 100644 kortex_driver/include/kortex_driver/generated/base_services.h delete mode 100644 kortex_driver/include/kortex_driver/generated/controlconfig_services.h delete mode 100644 kortex_driver/include/kortex_driver/generated/deviceconfig_services.h delete mode 100644 kortex_driver/include/kortex_driver/generated/devicemanager_proto_converter.h delete mode 100644 kortex_driver/include/kortex_driver/generated/devicemanager_ros_converter.h delete mode 100644 kortex_driver/include/kortex_driver/generated/devicemanager_services.h delete mode 100644 kortex_driver/include/kortex_driver/generated/interconnectconfig_services.h create mode 100644 kortex_driver/include/kortex_driver/generated/interfaces/actuatorconfig_services_interface.h create mode 100644 kortex_driver/include/kortex_driver/generated/interfaces/base_services_interface.h create mode 100644 kortex_driver/include/kortex_driver/generated/interfaces/controlconfig_services_interface.h create mode 100644 kortex_driver/include/kortex_driver/generated/interfaces/deviceconfig_services_interface.h create mode 100644 kortex_driver/include/kortex_driver/generated/interfaces/devicemanager_services_interface.h create mode 100644 kortex_driver/include/kortex_driver/generated/interfaces/interconnectconfig_services_interface.h create mode 100644 kortex_driver/include/kortex_driver/generated/interfaces/visionconfig_services_interface.h rename kortex_driver/include/kortex_driver/generated/{ => robot}/actuatorconfig_proto_converter.h (80%) rename kortex_driver/include/kortex_driver/generated/{ => robot}/actuatorconfig_ros_converter.h (80%) create mode 100644 kortex_driver/include/kortex_driver/generated/robot/actuatorconfig_services.h rename kortex_driver/include/kortex_driver/generated/{ => robot}/actuatorcyclic_proto_converter.h (59%) rename kortex_driver/include/kortex_driver/generated/{ => robot}/actuatorcyclic_ros_converter.h (59%) rename kortex_driver/include/kortex_driver/generated/{ => robot}/base_proto_converter.h (96%) rename kortex_driver/include/kortex_driver/generated/{ => robot}/base_ros_converter.h (96%) create mode 100644 kortex_driver/include/kortex_driver/generated/robot/base_services.h rename kortex_driver/include/kortex_driver/generated/{ => robot}/basecyclic_proto_converter.h (65%) rename kortex_driver/include/kortex_driver/generated/{ => robot}/basecyclic_ros_converter.h (65%) rename kortex_driver/include/kortex_driver/generated/{ => robot}/common_proto_converter.h (72%) rename kortex_driver/include/kortex_driver/generated/{ => robot}/common_ros_converter.h (72%) rename kortex_driver/include/kortex_driver/generated/{ => robot}/controlconfig_proto_converter.h (80%) rename kortex_driver/include/kortex_driver/generated/{ => robot}/controlconfig_ros_converter.h (80%) create mode 100644 kortex_driver/include/kortex_driver/generated/robot/controlconfig_services.h rename kortex_driver/include/kortex_driver/generated/{ => robot}/deviceconfig_proto_converter.h (83%) rename kortex_driver/include/kortex_driver/generated/{ => robot}/deviceconfig_ros_converter.h (83%) create mode 100644 kortex_driver/include/kortex_driver/generated/robot/deviceconfig_services.h create mode 100644 kortex_driver/include/kortex_driver/generated/robot/devicemanager_proto_converter.h create mode 100644 kortex_driver/include/kortex_driver/generated/robot/devicemanager_ros_converter.h create mode 100644 kortex_driver/include/kortex_driver/generated/robot/devicemanager_services.h rename kortex_driver/include/kortex_driver/generated/{ => robot}/grippercyclic_proto_converter.h (66%) rename kortex_driver/include/kortex_driver/generated/{ => robot}/grippercyclic_ros_converter.h (66%) rename kortex_driver/include/kortex_driver/generated/{ => robot}/interconnectconfig_proto_converter.h (75%) rename kortex_driver/include/kortex_driver/generated/{ => robot}/interconnectconfig_ros_converter.h (75%) create mode 100644 kortex_driver/include/kortex_driver/generated/robot/interconnectconfig_services.h rename kortex_driver/include/kortex_driver/generated/{ => robot}/interconnectcyclic_proto_converter.h (60%) rename kortex_driver/include/kortex_driver/generated/{ => robot}/interconnectcyclic_ros_converter.h (60%) rename kortex_driver/include/kortex_driver/generated/{ => robot}/productconfiguration_proto_converter.h (54%) rename kortex_driver/include/kortex_driver/generated/{ => robot}/productconfiguration_ros_converter.h (54%) rename kortex_driver/include/kortex_driver/generated/{ => robot}/visionconfig_proto_converter.h (78%) rename kortex_driver/include/kortex_driver/generated/{ => robot}/visionconfig_ros_converter.h (78%) create mode 100644 kortex_driver/include/kortex_driver/generated/robot/visionconfig_services.h create mode 100644 kortex_driver/include/kortex_driver/generated/simulation/actuatorconfig_services.h create mode 100644 kortex_driver/include/kortex_driver/generated/simulation/base_services.h create mode 100644 kortex_driver/include/kortex_driver/generated/simulation/controlconfig_services.h create mode 100644 kortex_driver/include/kortex_driver/generated/simulation/deviceconfig_services.h create mode 100644 kortex_driver/include/kortex_driver/generated/simulation/devicemanager_services.h create mode 100644 kortex_driver/include/kortex_driver/generated/simulation/interconnectconfig_services.h create mode 100644 kortex_driver/include/kortex_driver/generated/simulation/visionconfig_services.h delete mode 100644 kortex_driver/include/kortex_driver/generated/visionconfig_services.h delete mode 100644 kortex_driver/src/generated/devicemanager_services.cpp rename kortex_driver/src/generated/{ => robot}/actuatorconfig_proto_converter.cpp (98%) rename kortex_driver/src/generated/{ => robot}/actuatorconfig_ros_converter.cpp (98%) rename kortex_driver/src/generated/{ => robot}/actuatorconfig_services.cpp (67%) rename kortex_driver/src/generated/{ => robot}/actuatorcyclic_proto_converter.cpp (97%) rename kortex_driver/src/generated/{ => robot}/actuatorcyclic_ros_converter.cpp (97%) rename kortex_driver/src/generated/{ => robot}/base_proto_converter.cpp (99%) rename kortex_driver/src/generated/{ => robot}/base_ros_converter.cpp (99%) rename kortex_driver/src/generated/{ => robot}/base_services.cpp (73%) rename kortex_driver/src/generated/{ => robot}/basecyclic_proto_converter.cpp (98%) rename kortex_driver/src/generated/{ => robot}/basecyclic_ros_converter.cpp (98%) rename kortex_driver/src/generated/{ => robot}/common_proto_converter.cpp (98%) rename kortex_driver/src/generated/{ => robot}/common_ros_converter.cpp (97%) rename kortex_driver/src/generated/{ => robot}/controlconfig_proto_converter.cpp (98%) rename kortex_driver/src/generated/{ => robot}/controlconfig_ros_converter.cpp (98%) rename kortex_driver/src/generated/{ => robot}/controlconfig_services.cpp (68%) rename kortex_driver/src/generated/{ => robot}/deviceconfig_proto_converter.cpp (98%) rename kortex_driver/src/generated/{ => robot}/deviceconfig_ros_converter.cpp (98%) rename kortex_driver/src/generated/{ => robot}/deviceconfig_services.cpp (70%) rename kortex_driver/src/generated/{ => robot}/devicemanager_proto_converter.cpp (88%) rename kortex_driver/src/generated/{ => robot}/devicemanager_ros_converter.cpp (89%) create mode 100644 kortex_driver/src/generated/robot/devicemanager_services.cpp rename kortex_driver/src/generated/{ => robot}/grippercyclic_proto_converter.cpp (97%) rename kortex_driver/src/generated/{ => robot}/grippercyclic_ros_converter.cpp (97%) rename kortex_driver/src/generated/{ => robot}/interconnectconfig_proto_converter.cpp (98%) rename kortex_driver/src/generated/{ => robot}/interconnectconfig_ros_converter.cpp (97%) rename kortex_driver/src/generated/{ => robot}/interconnectconfig_services.cpp (65%) rename kortex_driver/src/generated/{ => robot}/interconnectcyclic_proto_converter.cpp (97%) rename kortex_driver/src/generated/{ => robot}/interconnectcyclic_ros_converter.cpp (97%) rename kortex_driver/src/generated/{ => robot}/productconfiguration_proto_converter.cpp (95%) rename kortex_driver/src/generated/{ => robot}/productconfiguration_ros_converter.cpp (94%) rename kortex_driver/src/generated/{ => robot}/visionconfig_proto_converter.cpp (98%) rename kortex_driver/src/generated/{ => robot}/visionconfig_ros_converter.cpp (98%) rename kortex_driver/src/generated/{ => robot}/visionconfig_services.cpp (64%) create mode 100644 kortex_driver/src/generated/simulation/actuatorconfig_services.cpp create mode 100644 kortex_driver/src/generated/simulation/base_services.cpp create mode 100644 kortex_driver/src/generated/simulation/controlconfig_services.cpp create mode 100644 kortex_driver/src/generated/simulation/deviceconfig_services.cpp create mode 100644 kortex_driver/src/generated/simulation/devicemanager_services.cpp create mode 100644 kortex_driver/src/generated/simulation/interconnectconfig_services.cpp create mode 100644 kortex_driver/src/generated/simulation/visionconfig_services.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 581e61db..66dd650a 120000 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1 +1 @@ -/opt/ros/kinetic/share/catkin/cmake/toplevel.cmake \ No newline at end of file +/opt/ros/melodic/share/catkin/cmake/toplevel.cmake \ No newline at end of file diff --git a/kortex_driver/CMakeLists.txt b/kortex_driver/CMakeLists.txt index c092cfdd..58da43ad 100644 --- a/kortex_driver/CMakeLists.txt +++ b/kortex_driver/CMakeLists.txt @@ -20,7 +20,7 @@ endif() find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs message_generation actionlib control_msgs urdf) find_package(Boost REQUIRED COMPONENTS system) -file(GLOB_RECURSE generated_files RELATIVE ${PROJECT_SOURCE_DIR} "src/generated/*.cpp") +file(GLOB_RECURSE generated_files RELATIVE ${PROJECT_SOURCE_DIR} "src/generated/robot/*.cpp" "src/generated/simulation/*.cpp") file(GLOB_RECURSE non_generated_files RELATIVE ${PROJECT_SOURCE_DIR} "src/non-generated/driver/*.cpp") file(GLOB_RECURSE test_files RELATIVE ${PROJECT_SOURCE_DIR} "src/non-generated/tests/*.cc") diff --git a/kortex_driver/include/kortex_driver/generated/actuatorconfig_services.h b/kortex_driver/include/kortex_driver/generated/actuatorconfig_services.h deleted file mode 100644 index 7ec46094..00000000 --- a/kortex_driver/include/kortex_driver/generated/actuatorconfig_services.h +++ /dev/null @@ -1,119 +0,0 @@ -/* -* KINOVA (R) KORTEX (TM) -* -* Copyright (c) 2019 Kinova inc. All rights reserved. -* -* This software may be modified and distributed under the -* terms of the BSD 3-Clause license. -* -* Refer to the LICENSE file for details. -* -*/ - -/* - * This file has been auto-generated and should not be modified. - */ - -#ifndef _KORTEX_ACTUATORCONFIG_SERVICES_H_ -#define _KORTEX_ACTUATORCONFIG_SERVICES_H_ - -#include "ros/ros.h" - -#include -#include -#include -#include -#include - -#include -#include -#include "kortex_driver/GetAxisOffsets.h" -#include "kortex_driver/SetAxisOffsets.h" -#include "kortex_driver/SetTorqueOffset.h" -#include "kortex_driver/ActuatorConfig_GetControlMode.h" -#include "kortex_driver/SetControlMode.h" -#include "kortex_driver/GetActivatedControlLoop.h" -#include "kortex_driver/SetActivatedControlLoop.h" -#include "kortex_driver/GetControlLoopParameters.h" -#include "kortex_driver/SetControlLoopParameters.h" -#include "kortex_driver/SelectCustomData.h" -#include "kortex_driver/GetSelectedCustomData.h" -#include "kortex_driver/SetCommandMode.h" -#include "kortex_driver/ActuatorConfig_ClearFaults.h" -#include "kortex_driver/SetServoing.h" -#include "kortex_driver/MoveToPosition.h" -#include "kortex_driver/GetCommandMode.h" -#include "kortex_driver/GetServoing.h" -#include "kortex_driver/GetTorqueOffset.h" -#include "kortex_driver/SetCoggingFeedforwardMode.h" -#include "kortex_driver/GetCoggingFeedforwardMode.h" - -#include "kortex_driver/KortexError.h" -#include "kortex_driver/SetDeviceID.h" -#include "kortex_driver/SetApiOptions.h" -#include "kortex_driver/ApiOptions.h" - -using namespace std; - -class ActuatorConfigServices -{ - public: - ActuatorConfigServices(ros::NodeHandle& n, Kinova::Api::ActuatorConfig::ActuatorConfigClient* actuatorconfig, uint32_t device_id, uint32_t timeout_ms); - - bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res); - bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res); - bool GetAxisOffsets(kortex_driver::GetAxisOffsets::Request &req, kortex_driver::GetAxisOffsets::Response &res); - bool SetAxisOffsets(kortex_driver::SetAxisOffsets::Request &req, kortex_driver::SetAxisOffsets::Response &res); - bool SetTorqueOffset(kortex_driver::SetTorqueOffset::Request &req, kortex_driver::SetTorqueOffset::Response &res); - bool ActuatorConfig_GetControlMode(kortex_driver::ActuatorConfig_GetControlMode::Request &req, kortex_driver::ActuatorConfig_GetControlMode::Response &res); - bool SetControlMode(kortex_driver::SetControlMode::Request &req, kortex_driver::SetControlMode::Response &res); - bool GetActivatedControlLoop(kortex_driver::GetActivatedControlLoop::Request &req, kortex_driver::GetActivatedControlLoop::Response &res); - bool SetActivatedControlLoop(kortex_driver::SetActivatedControlLoop::Request &req, kortex_driver::SetActivatedControlLoop::Response &res); - bool GetControlLoopParameters(kortex_driver::GetControlLoopParameters::Request &req, kortex_driver::GetControlLoopParameters::Response &res); - bool SetControlLoopParameters(kortex_driver::SetControlLoopParameters::Request &req, kortex_driver::SetControlLoopParameters::Response &res); - bool SelectCustomData(kortex_driver::SelectCustomData::Request &req, kortex_driver::SelectCustomData::Response &res); - bool GetSelectedCustomData(kortex_driver::GetSelectedCustomData::Request &req, kortex_driver::GetSelectedCustomData::Response &res); - bool SetCommandMode(kortex_driver::SetCommandMode::Request &req, kortex_driver::SetCommandMode::Response &res); - bool ActuatorConfig_ClearFaults(kortex_driver::ActuatorConfig_ClearFaults::Request &req, kortex_driver::ActuatorConfig_ClearFaults::Response &res); - bool SetServoing(kortex_driver::SetServoing::Request &req, kortex_driver::SetServoing::Response &res); - bool MoveToPosition(kortex_driver::MoveToPosition::Request &req, kortex_driver::MoveToPosition::Response &res); - bool GetCommandMode(kortex_driver::GetCommandMode::Request &req, kortex_driver::GetCommandMode::Response &res); - bool GetServoing(kortex_driver::GetServoing::Request &req, kortex_driver::GetServoing::Response &res); - bool GetTorqueOffset(kortex_driver::GetTorqueOffset::Request &req, kortex_driver::GetTorqueOffset::Response &res); - bool SetCoggingFeedforwardMode(kortex_driver::SetCoggingFeedforwardMode::Request &req, kortex_driver::SetCoggingFeedforwardMode::Response &res); - bool GetCoggingFeedforwardMode(kortex_driver::GetCoggingFeedforwardMode::Request &req, kortex_driver::GetCoggingFeedforwardMode::Response &res); - -private: - uint32_t m_current_device_id; - Kinova::Api::RouterClientSendOptions m_api_options; - - Kinova::Api::ActuatorConfig::ActuatorConfigClient* m_actuatorconfig; - - ros::NodeHandle m_n; - ros::Publisher m_pub_Error; - - ros::ServiceServer m_serviceSetDeviceID; - ros::ServiceServer m_serviceSetApiOptions; - - ros::ServiceServer m_serviceGetAxisOffsets; - ros::ServiceServer m_serviceSetAxisOffsets; - ros::ServiceServer m_serviceSetTorqueOffset; - ros::ServiceServer m_serviceActuatorConfig_GetControlMode; - ros::ServiceServer m_serviceSetControlMode; - ros::ServiceServer m_serviceGetActivatedControlLoop; - ros::ServiceServer m_serviceSetActivatedControlLoop; - ros::ServiceServer m_serviceGetControlLoopParameters; - ros::ServiceServer m_serviceSetControlLoopParameters; - ros::ServiceServer m_serviceSelectCustomData; - ros::ServiceServer m_serviceGetSelectedCustomData; - ros::ServiceServer m_serviceSetCommandMode; - ros::ServiceServer m_serviceActuatorConfig_ClearFaults; - ros::ServiceServer m_serviceSetServoing; - ros::ServiceServer m_serviceMoveToPosition; - ros::ServiceServer m_serviceGetCommandMode; - ros::ServiceServer m_serviceGetServoing; - ros::ServiceServer m_serviceGetTorqueOffset; - ros::ServiceServer m_serviceSetCoggingFeedforwardMode; - ros::ServiceServer m_serviceGetCoggingFeedforwardMode; -}; -#endif diff --git a/kortex_driver/include/kortex_driver/generated/base_services.h b/kortex_driver/include/kortex_driver/generated/base_services.h deleted file mode 100644 index ecfa5253..00000000 --- a/kortex_driver/include/kortex_driver/generated/base_services.h +++ /dev/null @@ -1,547 +0,0 @@ -/* -* KINOVA (R) KORTEX (TM) -* -* Copyright (c) 2019 Kinova inc. All rights reserved. -* -* This software may be modified and distributed under the -* terms of the BSD 3-Clause license. -* -* Refer to the LICENSE file for details. -* -*/ - -/* - * This file has been auto-generated and should not be modified. - */ - -#ifndef _KORTEX_BASE_SERVICES_H_ -#define _KORTEX_BASE_SERVICES_H_ - -#include "ros/ros.h" - -#include -#include -#include -#include -#include - -#include -#include -#include "kortex_driver/CreateUserProfile.h" -#include "kortex_driver/UpdateUserProfile.h" -#include "kortex_driver/ReadUserProfile.h" -#include "kortex_driver/DeleteUserProfile.h" -#include "kortex_driver/ReadAllUserProfiles.h" -#include "kortex_driver/ReadAllUsers.h" -#include "kortex_driver/ChangePassword.h" -#include "kortex_driver/CreateSequence.h" -#include "kortex_driver/UpdateSequence.h" -#include "kortex_driver/ReadSequence.h" -#include "kortex_driver/DeleteSequence.h" -#include "kortex_driver/ReadAllSequences.h" -#include "kortex_driver/PlaySequence.h" -#include "kortex_driver/PlayAdvancedSequence.h" -#include "kortex_driver/StopSequence.h" -#include "kortex_driver/PauseSequence.h" -#include "kortex_driver/ResumeSequence.h" -#include "kortex_driver/CreateProtectionZone.h" -#include "kortex_driver/UpdateProtectionZone.h" -#include "kortex_driver/ReadProtectionZone.h" -#include "kortex_driver/DeleteProtectionZone.h" -#include "kortex_driver/ReadAllProtectionZones.h" -#include "kortex_driver/CreateMapping.h" -#include "kortex_driver/ReadMapping.h" -#include "kortex_driver/UpdateMapping.h" -#include "kortex_driver/DeleteMapping.h" -#include "kortex_driver/ReadAllMappings.h" -#include "kortex_driver/CreateMap.h" -#include "kortex_driver/ReadMap.h" -#include "kortex_driver/UpdateMap.h" -#include "kortex_driver/DeleteMap.h" -#include "kortex_driver/ReadAllMaps.h" -#include "kortex_driver/ActivateMap.h" -#include "kortex_driver/CreateAction.h" -#include "kortex_driver/ReadAction.h" -#include "kortex_driver/ReadAllActions.h" -#include "kortex_driver/DeleteAction.h" -#include "kortex_driver/UpdateAction.h" -#include "kortex_driver/ExecuteActionFromReference.h" -#include "kortex_driver/ExecuteAction.h" -#include "kortex_driver/PauseAction.h" -#include "kortex_driver/StopAction.h" -#include "kortex_driver/ResumeAction.h" -#include "kortex_driver/GetIPv4Configuration.h" -#include "kortex_driver/SetIPv4Configuration.h" -#include "kortex_driver/SetCommunicationInterfaceEnable.h" -#include "kortex_driver/IsCommunicationInterfaceEnable.h" -#include "kortex_driver/GetAvailableWifi.h" -#include "kortex_driver/GetWifiInformation.h" -#include "kortex_driver/AddWifiConfiguration.h" -#include "kortex_driver/DeleteWifiConfiguration.h" -#include "kortex_driver/GetAllConfiguredWifis.h" -#include "kortex_driver/ConnectWifi.h" -#include "kortex_driver/DisconnectWifi.h" -#include "kortex_driver/GetConnectedWifiInformation.h" -#include "kortex_driver/Base_Unsubscribe.h" -#include "kortex_driver/OnNotificationConfigurationChangeTopic.h" -#include "kortex_driver/ConfigurationChangeNotification.h" -#include "kortex_driver/OnNotificationMappingInfoTopic.h" -#include "kortex_driver/MappingInfoNotification.h" -#include "kortex_driver/OnNotificationControlModeTopic.h" -#include "kortex_driver/ControlModeNotification.h" -#include "kortex_driver/OnNotificationOperatingModeTopic.h" -#include "kortex_driver/OperatingModeNotification.h" -#include "kortex_driver/OnNotificationSequenceInfoTopic.h" -#include "kortex_driver/SequenceInfoNotification.h" -#include "kortex_driver/OnNotificationProtectionZoneTopic.h" -#include "kortex_driver/ProtectionZoneNotification.h" -#include "kortex_driver/OnNotificationUserTopic.h" -#include "kortex_driver/UserNotification.h" -#include "kortex_driver/OnNotificationControllerTopic.h" -#include "kortex_driver/ControllerNotification.h" -#include "kortex_driver/OnNotificationActionTopic.h" -#include "kortex_driver/ActionNotification.h" -#include "kortex_driver/OnNotificationRobotEventTopic.h" -#include "kortex_driver/RobotEventNotification.h" -#include "kortex_driver/PlayCartesianTrajectory.h" -#include "kortex_driver/PlayCartesianTrajectoryPosition.h" -#include "kortex_driver/PlayCartesianTrajectoryOrientation.h" -#include "kortex_driver/Stop.h" -#include "kortex_driver/GetMeasuredCartesianPose.h" -#include "kortex_driver/SendWrenchCommand.h" -#include "kortex_driver/SendWrenchJoystickCommand.h" -#include "kortex_driver/SendTwistJoystickCommand.h" -#include "kortex_driver/SendTwistCommand.h" -#include "kortex_driver/PlayJointTrajectory.h" -#include "kortex_driver/PlaySelectedJointTrajectory.h" -#include "kortex_driver/GetMeasuredJointAngles.h" -#include "kortex_driver/SendJointSpeedsCommand.h" -#include "kortex_driver/SendSelectedJointSpeedCommand.h" -#include "kortex_driver/SendGripperCommand.h" -#include "kortex_driver/GetMeasuredGripperMovement.h" -#include "kortex_driver/SetAdmittance.h" -#include "kortex_driver/SetOperatingMode.h" -#include "kortex_driver/ApplyEmergencyStop.h" -#include "kortex_driver/Base_ClearFaults.h" -#include "kortex_driver/Base_GetControlMode.h" -#include "kortex_driver/GetOperatingMode.h" -#include "kortex_driver/SetServoingMode.h" -#include "kortex_driver/GetServoingMode.h" -#include "kortex_driver/OnNotificationServoingModeTopic.h" -#include "kortex_driver/ServoingModeNotification.h" -#include "kortex_driver/RestoreFactorySettings.h" -#include "kortex_driver/OnNotificationFactoryTopic.h" -#include "kortex_driver/FactoryNotification.h" -#include "kortex_driver/GetAllConnectedControllers.h" -#include "kortex_driver/GetControllerState.h" -#include "kortex_driver/GetActuatorCount.h" -#include "kortex_driver/StartWifiScan.h" -#include "kortex_driver/GetConfiguredWifi.h" -#include "kortex_driver/OnNotificationNetworkTopic.h" -#include "kortex_driver/NetworkNotification.h" -#include "kortex_driver/GetArmState.h" -#include "kortex_driver/OnNotificationArmStateTopic.h" -#include "kortex_driver/ArmStateNotification.h" -#include "kortex_driver/GetIPv4Information.h" -#include "kortex_driver/SetWifiCountryCode.h" -#include "kortex_driver/GetWifiCountryCode.h" -#include "kortex_driver/Base_SetCapSenseConfig.h" -#include "kortex_driver/Base_GetCapSenseConfig.h" -#include "kortex_driver/GetAllJointsSpeedHardLimitation.h" -#include "kortex_driver/GetAllJointsTorqueHardLimitation.h" -#include "kortex_driver/GetTwistHardLimitation.h" -#include "kortex_driver/GetWrenchHardLimitation.h" -#include "kortex_driver/SendJointSpeedsJoystickCommand.h" -#include "kortex_driver/SendSelectedJointSpeedJoystickCommand.h" -#include "kortex_driver/EnableBridge.h" -#include "kortex_driver/DisableBridge.h" -#include "kortex_driver/GetBridgeList.h" -#include "kortex_driver/GetBridgeConfig.h" -#include "kortex_driver/PlayPreComputedJointTrajectory.h" -#include "kortex_driver/GetProductConfiguration.h" -#include "kortex_driver/UpdateEndEffectorTypeConfiguration.h" -#include "kortex_driver/RestoreFactoryProductConfiguration.h" -#include "kortex_driver/GetTrajectoryErrorReport.h" -#include "kortex_driver/GetAllJointsSpeedSoftLimitation.h" -#include "kortex_driver/GetAllJointsTorqueSoftLimitation.h" -#include "kortex_driver/GetTwistSoftLimitation.h" -#include "kortex_driver/GetWrenchSoftLimitation.h" -#include "kortex_driver/SetControllerConfigurationMode.h" -#include "kortex_driver/GetControllerConfigurationMode.h" -#include "kortex_driver/StartTeaching.h" -#include "kortex_driver/StopTeaching.h" -#include "kortex_driver/AddSequenceTasks.h" -#include "kortex_driver/UpdateSequenceTask.h" -#include "kortex_driver/SwapSequenceTasks.h" -#include "kortex_driver/ReadSequenceTask.h" -#include "kortex_driver/ReadAllSequenceTasks.h" -#include "kortex_driver/DeleteSequenceTask.h" -#include "kortex_driver/DeleteAllSequenceTasks.h" -#include "kortex_driver/TakeSnapshot.h" -#include "kortex_driver/GetFirmwareBundleVersions.h" -#include "kortex_driver/MoveSequenceTask.h" -#include "kortex_driver/DuplicateMapping.h" -#include "kortex_driver/DuplicateMap.h" -#include "kortex_driver/SetControllerConfiguration.h" -#include "kortex_driver/GetControllerConfiguration.h" -#include "kortex_driver/GetAllControllerConfigurations.h" - -#include "kortex_driver/KortexError.h" -#include "kortex_driver/SetDeviceID.h" -#include "kortex_driver/SetApiOptions.h" -#include "kortex_driver/ApiOptions.h" - -using namespace std; - -class BaseServices -{ - public: - BaseServices(ros::NodeHandle& n, Kinova::Api::Base::BaseClient* base, uint32_t device_id, uint32_t timeout_ms); - - bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res); - bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res); - bool CreateUserProfile(kortex_driver::CreateUserProfile::Request &req, kortex_driver::CreateUserProfile::Response &res); - bool UpdateUserProfile(kortex_driver::UpdateUserProfile::Request &req, kortex_driver::UpdateUserProfile::Response &res); - bool ReadUserProfile(kortex_driver::ReadUserProfile::Request &req, kortex_driver::ReadUserProfile::Response &res); - bool DeleteUserProfile(kortex_driver::DeleteUserProfile::Request &req, kortex_driver::DeleteUserProfile::Response &res); - bool ReadAllUserProfiles(kortex_driver::ReadAllUserProfiles::Request &req, kortex_driver::ReadAllUserProfiles::Response &res); - bool ReadAllUsers(kortex_driver::ReadAllUsers::Request &req, kortex_driver::ReadAllUsers::Response &res); - bool ChangePassword(kortex_driver::ChangePassword::Request &req, kortex_driver::ChangePassword::Response &res); - bool CreateSequence(kortex_driver::CreateSequence::Request &req, kortex_driver::CreateSequence::Response &res); - bool UpdateSequence(kortex_driver::UpdateSequence::Request &req, kortex_driver::UpdateSequence::Response &res); - bool ReadSequence(kortex_driver::ReadSequence::Request &req, kortex_driver::ReadSequence::Response &res); - bool DeleteSequence(kortex_driver::DeleteSequence::Request &req, kortex_driver::DeleteSequence::Response &res); - bool ReadAllSequences(kortex_driver::ReadAllSequences::Request &req, kortex_driver::ReadAllSequences::Response &res); - bool PlaySequence(kortex_driver::PlaySequence::Request &req, kortex_driver::PlaySequence::Response &res); - bool PlayAdvancedSequence(kortex_driver::PlayAdvancedSequence::Request &req, kortex_driver::PlayAdvancedSequence::Response &res); - bool StopSequence(kortex_driver::StopSequence::Request &req, kortex_driver::StopSequence::Response &res); - bool PauseSequence(kortex_driver::PauseSequence::Request &req, kortex_driver::PauseSequence::Response &res); - bool ResumeSequence(kortex_driver::ResumeSequence::Request &req, kortex_driver::ResumeSequence::Response &res); - bool CreateProtectionZone(kortex_driver::CreateProtectionZone::Request &req, kortex_driver::CreateProtectionZone::Response &res); - bool UpdateProtectionZone(kortex_driver::UpdateProtectionZone::Request &req, kortex_driver::UpdateProtectionZone::Response &res); - bool ReadProtectionZone(kortex_driver::ReadProtectionZone::Request &req, kortex_driver::ReadProtectionZone::Response &res); - bool DeleteProtectionZone(kortex_driver::DeleteProtectionZone::Request &req, kortex_driver::DeleteProtectionZone::Response &res); - bool ReadAllProtectionZones(kortex_driver::ReadAllProtectionZones::Request &req, kortex_driver::ReadAllProtectionZones::Response &res); - bool CreateMapping(kortex_driver::CreateMapping::Request &req, kortex_driver::CreateMapping::Response &res); - bool ReadMapping(kortex_driver::ReadMapping::Request &req, kortex_driver::ReadMapping::Response &res); - bool UpdateMapping(kortex_driver::UpdateMapping::Request &req, kortex_driver::UpdateMapping::Response &res); - bool DeleteMapping(kortex_driver::DeleteMapping::Request &req, kortex_driver::DeleteMapping::Response &res); - bool ReadAllMappings(kortex_driver::ReadAllMappings::Request &req, kortex_driver::ReadAllMappings::Response &res); - bool CreateMap(kortex_driver::CreateMap::Request &req, kortex_driver::CreateMap::Response &res); - bool ReadMap(kortex_driver::ReadMap::Request &req, kortex_driver::ReadMap::Response &res); - bool UpdateMap(kortex_driver::UpdateMap::Request &req, kortex_driver::UpdateMap::Response &res); - bool DeleteMap(kortex_driver::DeleteMap::Request &req, kortex_driver::DeleteMap::Response &res); - bool ReadAllMaps(kortex_driver::ReadAllMaps::Request &req, kortex_driver::ReadAllMaps::Response &res); - bool ActivateMap(kortex_driver::ActivateMap::Request &req, kortex_driver::ActivateMap::Response &res); - bool CreateAction(kortex_driver::CreateAction::Request &req, kortex_driver::CreateAction::Response &res); - bool ReadAction(kortex_driver::ReadAction::Request &req, kortex_driver::ReadAction::Response &res); - bool ReadAllActions(kortex_driver::ReadAllActions::Request &req, kortex_driver::ReadAllActions::Response &res); - bool DeleteAction(kortex_driver::DeleteAction::Request &req, kortex_driver::DeleteAction::Response &res); - bool UpdateAction(kortex_driver::UpdateAction::Request &req, kortex_driver::UpdateAction::Response &res); - bool ExecuteActionFromReference(kortex_driver::ExecuteActionFromReference::Request &req, kortex_driver::ExecuteActionFromReference::Response &res); - bool ExecuteAction(kortex_driver::ExecuteAction::Request &req, kortex_driver::ExecuteAction::Response &res); - bool PauseAction(kortex_driver::PauseAction::Request &req, kortex_driver::PauseAction::Response &res); - bool StopAction(kortex_driver::StopAction::Request &req, kortex_driver::StopAction::Response &res); - bool ResumeAction(kortex_driver::ResumeAction::Request &req, kortex_driver::ResumeAction::Response &res); - bool GetIPv4Configuration(kortex_driver::GetIPv4Configuration::Request &req, kortex_driver::GetIPv4Configuration::Response &res); - bool SetIPv4Configuration(kortex_driver::SetIPv4Configuration::Request &req, kortex_driver::SetIPv4Configuration::Response &res); - bool SetCommunicationInterfaceEnable(kortex_driver::SetCommunicationInterfaceEnable::Request &req, kortex_driver::SetCommunicationInterfaceEnable::Response &res); - bool IsCommunicationInterfaceEnable(kortex_driver::IsCommunicationInterfaceEnable::Request &req, kortex_driver::IsCommunicationInterfaceEnable::Response &res); - bool GetAvailableWifi(kortex_driver::GetAvailableWifi::Request &req, kortex_driver::GetAvailableWifi::Response &res); - bool GetWifiInformation(kortex_driver::GetWifiInformation::Request &req, kortex_driver::GetWifiInformation::Response &res); - bool AddWifiConfiguration(kortex_driver::AddWifiConfiguration::Request &req, kortex_driver::AddWifiConfiguration::Response &res); - bool DeleteWifiConfiguration(kortex_driver::DeleteWifiConfiguration::Request &req, kortex_driver::DeleteWifiConfiguration::Response &res); - bool GetAllConfiguredWifis(kortex_driver::GetAllConfiguredWifis::Request &req, kortex_driver::GetAllConfiguredWifis::Response &res); - bool ConnectWifi(kortex_driver::ConnectWifi::Request &req, kortex_driver::ConnectWifi::Response &res); - bool DisconnectWifi(kortex_driver::DisconnectWifi::Request &req, kortex_driver::DisconnectWifi::Response &res); - bool GetConnectedWifiInformation(kortex_driver::GetConnectedWifiInformation::Request &req, kortex_driver::GetConnectedWifiInformation::Response &res); - bool Base_Unsubscribe(kortex_driver::Base_Unsubscribe::Request &req, kortex_driver::Base_Unsubscribe::Response &res); - bool OnNotificationConfigurationChangeTopic(kortex_driver::OnNotificationConfigurationChangeTopic::Request &req, kortex_driver::OnNotificationConfigurationChangeTopic::Response &res); - void cb_ConfigurationChangeTopic(Kinova::Api::Base::ConfigurationChangeNotification notif); - bool OnNotificationMappingInfoTopic(kortex_driver::OnNotificationMappingInfoTopic::Request &req, kortex_driver::OnNotificationMappingInfoTopic::Response &res); - void cb_MappingInfoTopic(Kinova::Api::Base::MappingInfoNotification notif); - bool OnNotificationControlModeTopic(kortex_driver::OnNotificationControlModeTopic::Request &req, kortex_driver::OnNotificationControlModeTopic::Response &res); - void cb_ControlModeTopic(Kinova::Api::Base::ControlModeNotification notif); - bool OnNotificationOperatingModeTopic(kortex_driver::OnNotificationOperatingModeTopic::Request &req, kortex_driver::OnNotificationOperatingModeTopic::Response &res); - void cb_OperatingModeTopic(Kinova::Api::Base::OperatingModeNotification notif); - bool OnNotificationSequenceInfoTopic(kortex_driver::OnNotificationSequenceInfoTopic::Request &req, kortex_driver::OnNotificationSequenceInfoTopic::Response &res); - void cb_SequenceInfoTopic(Kinova::Api::Base::SequenceInfoNotification notif); - bool OnNotificationProtectionZoneTopic(kortex_driver::OnNotificationProtectionZoneTopic::Request &req, kortex_driver::OnNotificationProtectionZoneTopic::Response &res); - void cb_ProtectionZoneTopic(Kinova::Api::Base::ProtectionZoneNotification notif); - bool OnNotificationUserTopic(kortex_driver::OnNotificationUserTopic::Request &req, kortex_driver::OnNotificationUserTopic::Response &res); - void cb_UserTopic(Kinova::Api::Base::UserNotification notif); - bool OnNotificationControllerTopic(kortex_driver::OnNotificationControllerTopic::Request &req, kortex_driver::OnNotificationControllerTopic::Response &res); - void cb_ControllerTopic(Kinova::Api::Base::ControllerNotification notif); - bool OnNotificationActionTopic(kortex_driver::OnNotificationActionTopic::Request &req, kortex_driver::OnNotificationActionTopic::Response &res); - void cb_ActionTopic(Kinova::Api::Base::ActionNotification notif); - bool OnNotificationRobotEventTopic(kortex_driver::OnNotificationRobotEventTopic::Request &req, kortex_driver::OnNotificationRobotEventTopic::Response &res); - void cb_RobotEventTopic(Kinova::Api::Base::RobotEventNotification notif); - bool PlayCartesianTrajectory(kortex_driver::PlayCartesianTrajectory::Request &req, kortex_driver::PlayCartesianTrajectory::Response &res); - bool PlayCartesianTrajectoryPosition(kortex_driver::PlayCartesianTrajectoryPosition::Request &req, kortex_driver::PlayCartesianTrajectoryPosition::Response &res); - bool PlayCartesianTrajectoryOrientation(kortex_driver::PlayCartesianTrajectoryOrientation::Request &req, kortex_driver::PlayCartesianTrajectoryOrientation::Response &res); - bool Stop(kortex_driver::Stop::Request &req, kortex_driver::Stop::Response &res); - bool GetMeasuredCartesianPose(kortex_driver::GetMeasuredCartesianPose::Request &req, kortex_driver::GetMeasuredCartesianPose::Response &res); - bool SendWrenchCommand(kortex_driver::SendWrenchCommand::Request &req, kortex_driver::SendWrenchCommand::Response &res); - bool SendWrenchJoystickCommand(kortex_driver::SendWrenchJoystickCommand::Request &req, kortex_driver::SendWrenchJoystickCommand::Response &res); - bool SendTwistJoystickCommand(kortex_driver::SendTwistJoystickCommand::Request &req, kortex_driver::SendTwistJoystickCommand::Response &res); - bool SendTwistCommand(kortex_driver::SendTwistCommand::Request &req, kortex_driver::SendTwistCommand::Response &res); - bool PlayJointTrajectory(kortex_driver::PlayJointTrajectory::Request &req, kortex_driver::PlayJointTrajectory::Response &res); - bool PlaySelectedJointTrajectory(kortex_driver::PlaySelectedJointTrajectory::Request &req, kortex_driver::PlaySelectedJointTrajectory::Response &res); - bool GetMeasuredJointAngles(kortex_driver::GetMeasuredJointAngles::Request &req, kortex_driver::GetMeasuredJointAngles::Response &res); - bool SendJointSpeedsCommand(kortex_driver::SendJointSpeedsCommand::Request &req, kortex_driver::SendJointSpeedsCommand::Response &res); - bool SendSelectedJointSpeedCommand(kortex_driver::SendSelectedJointSpeedCommand::Request &req, kortex_driver::SendSelectedJointSpeedCommand::Response &res); - bool SendGripperCommand(kortex_driver::SendGripperCommand::Request &req, kortex_driver::SendGripperCommand::Response &res); - bool GetMeasuredGripperMovement(kortex_driver::GetMeasuredGripperMovement::Request &req, kortex_driver::GetMeasuredGripperMovement::Response &res); - bool SetAdmittance(kortex_driver::SetAdmittance::Request &req, kortex_driver::SetAdmittance::Response &res); - bool SetOperatingMode(kortex_driver::SetOperatingMode::Request &req, kortex_driver::SetOperatingMode::Response &res); - bool ApplyEmergencyStop(kortex_driver::ApplyEmergencyStop::Request &req, kortex_driver::ApplyEmergencyStop::Response &res); - bool Base_ClearFaults(kortex_driver::Base_ClearFaults::Request &req, kortex_driver::Base_ClearFaults::Response &res); - bool Base_GetControlMode(kortex_driver::Base_GetControlMode::Request &req, kortex_driver::Base_GetControlMode::Response &res); - bool GetOperatingMode(kortex_driver::GetOperatingMode::Request &req, kortex_driver::GetOperatingMode::Response &res); - bool SetServoingMode(kortex_driver::SetServoingMode::Request &req, kortex_driver::SetServoingMode::Response &res); - bool GetServoingMode(kortex_driver::GetServoingMode::Request &req, kortex_driver::GetServoingMode::Response &res); - bool OnNotificationServoingModeTopic(kortex_driver::OnNotificationServoingModeTopic::Request &req, kortex_driver::OnNotificationServoingModeTopic::Response &res); - void cb_ServoingModeTopic(Kinova::Api::Base::ServoingModeNotification notif); - bool RestoreFactorySettings(kortex_driver::RestoreFactorySettings::Request &req, kortex_driver::RestoreFactorySettings::Response &res); - bool OnNotificationFactoryTopic(kortex_driver::OnNotificationFactoryTopic::Request &req, kortex_driver::OnNotificationFactoryTopic::Response &res); - void cb_FactoryTopic(Kinova::Api::Base::FactoryNotification notif); - bool GetAllConnectedControllers(kortex_driver::GetAllConnectedControllers::Request &req, kortex_driver::GetAllConnectedControllers::Response &res); - bool GetControllerState(kortex_driver::GetControllerState::Request &req, kortex_driver::GetControllerState::Response &res); - bool GetActuatorCount(kortex_driver::GetActuatorCount::Request &req, kortex_driver::GetActuatorCount::Response &res); - bool StartWifiScan(kortex_driver::StartWifiScan::Request &req, kortex_driver::StartWifiScan::Response &res); - bool GetConfiguredWifi(kortex_driver::GetConfiguredWifi::Request &req, kortex_driver::GetConfiguredWifi::Response &res); - bool OnNotificationNetworkTopic(kortex_driver::OnNotificationNetworkTopic::Request &req, kortex_driver::OnNotificationNetworkTopic::Response &res); - void cb_NetworkTopic(Kinova::Api::Base::NetworkNotification notif); - bool GetArmState(kortex_driver::GetArmState::Request &req, kortex_driver::GetArmState::Response &res); - bool OnNotificationArmStateTopic(kortex_driver::OnNotificationArmStateTopic::Request &req, kortex_driver::OnNotificationArmStateTopic::Response &res); - void cb_ArmStateTopic(Kinova::Api::Base::ArmStateNotification notif); - bool GetIPv4Information(kortex_driver::GetIPv4Information::Request &req, kortex_driver::GetIPv4Information::Response &res); - bool SetWifiCountryCode(kortex_driver::SetWifiCountryCode::Request &req, kortex_driver::SetWifiCountryCode::Response &res); - bool GetWifiCountryCode(kortex_driver::GetWifiCountryCode::Request &req, kortex_driver::GetWifiCountryCode::Response &res); - bool Base_SetCapSenseConfig(kortex_driver::Base_SetCapSenseConfig::Request &req, kortex_driver::Base_SetCapSenseConfig::Response &res); - bool Base_GetCapSenseConfig(kortex_driver::Base_GetCapSenseConfig::Request &req, kortex_driver::Base_GetCapSenseConfig::Response &res); - bool GetAllJointsSpeedHardLimitation(kortex_driver::GetAllJointsSpeedHardLimitation::Request &req, kortex_driver::GetAllJointsSpeedHardLimitation::Response &res); - bool GetAllJointsTorqueHardLimitation(kortex_driver::GetAllJointsTorqueHardLimitation::Request &req, kortex_driver::GetAllJointsTorqueHardLimitation::Response &res); - bool GetTwistHardLimitation(kortex_driver::GetTwistHardLimitation::Request &req, kortex_driver::GetTwistHardLimitation::Response &res); - bool GetWrenchHardLimitation(kortex_driver::GetWrenchHardLimitation::Request &req, kortex_driver::GetWrenchHardLimitation::Response &res); - bool SendJointSpeedsJoystickCommand(kortex_driver::SendJointSpeedsJoystickCommand::Request &req, kortex_driver::SendJointSpeedsJoystickCommand::Response &res); - bool SendSelectedJointSpeedJoystickCommand(kortex_driver::SendSelectedJointSpeedJoystickCommand::Request &req, kortex_driver::SendSelectedJointSpeedJoystickCommand::Response &res); - bool EnableBridge(kortex_driver::EnableBridge::Request &req, kortex_driver::EnableBridge::Response &res); - bool DisableBridge(kortex_driver::DisableBridge::Request &req, kortex_driver::DisableBridge::Response &res); - bool GetBridgeList(kortex_driver::GetBridgeList::Request &req, kortex_driver::GetBridgeList::Response &res); - bool GetBridgeConfig(kortex_driver::GetBridgeConfig::Request &req, kortex_driver::GetBridgeConfig::Response &res); - bool PlayPreComputedJointTrajectory(kortex_driver::PlayPreComputedJointTrajectory::Request &req, kortex_driver::PlayPreComputedJointTrajectory::Response &res); - bool GetProductConfiguration(kortex_driver::GetProductConfiguration::Request &req, kortex_driver::GetProductConfiguration::Response &res); - bool UpdateEndEffectorTypeConfiguration(kortex_driver::UpdateEndEffectorTypeConfiguration::Request &req, kortex_driver::UpdateEndEffectorTypeConfiguration::Response &res); - bool RestoreFactoryProductConfiguration(kortex_driver::RestoreFactoryProductConfiguration::Request &req, kortex_driver::RestoreFactoryProductConfiguration::Response &res); - bool GetTrajectoryErrorReport(kortex_driver::GetTrajectoryErrorReport::Request &req, kortex_driver::GetTrajectoryErrorReport::Response &res); - bool GetAllJointsSpeedSoftLimitation(kortex_driver::GetAllJointsSpeedSoftLimitation::Request &req, kortex_driver::GetAllJointsSpeedSoftLimitation::Response &res); - bool GetAllJointsTorqueSoftLimitation(kortex_driver::GetAllJointsTorqueSoftLimitation::Request &req, kortex_driver::GetAllJointsTorqueSoftLimitation::Response &res); - bool GetTwistSoftLimitation(kortex_driver::GetTwistSoftLimitation::Request &req, kortex_driver::GetTwistSoftLimitation::Response &res); - bool GetWrenchSoftLimitation(kortex_driver::GetWrenchSoftLimitation::Request &req, kortex_driver::GetWrenchSoftLimitation::Response &res); - bool SetControllerConfigurationMode(kortex_driver::SetControllerConfigurationMode::Request &req, kortex_driver::SetControllerConfigurationMode::Response &res); - bool GetControllerConfigurationMode(kortex_driver::GetControllerConfigurationMode::Request &req, kortex_driver::GetControllerConfigurationMode::Response &res); - bool StartTeaching(kortex_driver::StartTeaching::Request &req, kortex_driver::StartTeaching::Response &res); - bool StopTeaching(kortex_driver::StopTeaching::Request &req, kortex_driver::StopTeaching::Response &res); - bool AddSequenceTasks(kortex_driver::AddSequenceTasks::Request &req, kortex_driver::AddSequenceTasks::Response &res); - bool UpdateSequenceTask(kortex_driver::UpdateSequenceTask::Request &req, kortex_driver::UpdateSequenceTask::Response &res); - bool SwapSequenceTasks(kortex_driver::SwapSequenceTasks::Request &req, kortex_driver::SwapSequenceTasks::Response &res); - bool ReadSequenceTask(kortex_driver::ReadSequenceTask::Request &req, kortex_driver::ReadSequenceTask::Response &res); - bool ReadAllSequenceTasks(kortex_driver::ReadAllSequenceTasks::Request &req, kortex_driver::ReadAllSequenceTasks::Response &res); - bool DeleteSequenceTask(kortex_driver::DeleteSequenceTask::Request &req, kortex_driver::DeleteSequenceTask::Response &res); - bool DeleteAllSequenceTasks(kortex_driver::DeleteAllSequenceTasks::Request &req, kortex_driver::DeleteAllSequenceTasks::Response &res); - bool TakeSnapshot(kortex_driver::TakeSnapshot::Request &req, kortex_driver::TakeSnapshot::Response &res); - bool GetFirmwareBundleVersions(kortex_driver::GetFirmwareBundleVersions::Request &req, kortex_driver::GetFirmwareBundleVersions::Response &res); - bool MoveSequenceTask(kortex_driver::MoveSequenceTask::Request &req, kortex_driver::MoveSequenceTask::Response &res); - bool DuplicateMapping(kortex_driver::DuplicateMapping::Request &req, kortex_driver::DuplicateMapping::Response &res); - bool DuplicateMap(kortex_driver::DuplicateMap::Request &req, kortex_driver::DuplicateMap::Response &res); - bool SetControllerConfiguration(kortex_driver::SetControllerConfiguration::Request &req, kortex_driver::SetControllerConfiguration::Response &res); - bool GetControllerConfiguration(kortex_driver::GetControllerConfiguration::Request &req, kortex_driver::GetControllerConfiguration::Response &res); - bool GetAllControllerConfigurations(kortex_driver::GetAllControllerConfigurations::Request &req, kortex_driver::GetAllControllerConfigurations::Response &res); - -private: - uint32_t m_current_device_id; - Kinova::Api::RouterClientSendOptions m_api_options; - - Kinova::Api::Base::BaseClient* m_base; - - ros::NodeHandle m_n; - ros::Publisher m_pub_Error; - ros::Publisher m_pub_ConfigurationChangeTopic; - bool m_is_activated_ConfigurationChangeTopic; - ros::Publisher m_pub_MappingInfoTopic; - bool m_is_activated_MappingInfoTopic; - ros::Publisher m_pub_ControlModeTopic; - bool m_is_activated_ControlModeTopic; - ros::Publisher m_pub_OperatingModeTopic; - bool m_is_activated_OperatingModeTopic; - ros::Publisher m_pub_SequenceInfoTopic; - bool m_is_activated_SequenceInfoTopic; - ros::Publisher m_pub_ProtectionZoneTopic; - bool m_is_activated_ProtectionZoneTopic; - ros::Publisher m_pub_UserTopic; - bool m_is_activated_UserTopic; - ros::Publisher m_pub_ControllerTopic; - bool m_is_activated_ControllerTopic; - ros::Publisher m_pub_ActionTopic; - bool m_is_activated_ActionTopic; - ros::Publisher m_pub_RobotEventTopic; - bool m_is_activated_RobotEventTopic; - ros::Publisher m_pub_ServoingModeTopic; - bool m_is_activated_ServoingModeTopic; - ros::Publisher m_pub_FactoryTopic; - bool m_is_activated_FactoryTopic; - ros::Publisher m_pub_NetworkTopic; - bool m_is_activated_NetworkTopic; - ros::Publisher m_pub_ArmStateTopic; - bool m_is_activated_ArmStateTopic; - - ros::ServiceServer m_serviceSetDeviceID; - ros::ServiceServer m_serviceSetApiOptions; - - ros::ServiceServer m_serviceCreateUserProfile; - ros::ServiceServer m_serviceUpdateUserProfile; - ros::ServiceServer m_serviceReadUserProfile; - ros::ServiceServer m_serviceDeleteUserProfile; - ros::ServiceServer m_serviceReadAllUserProfiles; - ros::ServiceServer m_serviceReadAllUsers; - ros::ServiceServer m_serviceChangePassword; - ros::ServiceServer m_serviceCreateSequence; - ros::ServiceServer m_serviceUpdateSequence; - ros::ServiceServer m_serviceReadSequence; - ros::ServiceServer m_serviceDeleteSequence; - ros::ServiceServer m_serviceReadAllSequences; - ros::ServiceServer m_servicePlaySequence; - ros::ServiceServer m_servicePlayAdvancedSequence; - ros::ServiceServer m_serviceStopSequence; - ros::ServiceServer m_servicePauseSequence; - ros::ServiceServer m_serviceResumeSequence; - ros::ServiceServer m_serviceCreateProtectionZone; - ros::ServiceServer m_serviceUpdateProtectionZone; - ros::ServiceServer m_serviceReadProtectionZone; - ros::ServiceServer m_serviceDeleteProtectionZone; - ros::ServiceServer m_serviceReadAllProtectionZones; - ros::ServiceServer m_serviceCreateMapping; - ros::ServiceServer m_serviceReadMapping; - ros::ServiceServer m_serviceUpdateMapping; - ros::ServiceServer m_serviceDeleteMapping; - ros::ServiceServer m_serviceReadAllMappings; - ros::ServiceServer m_serviceCreateMap; - ros::ServiceServer m_serviceReadMap; - ros::ServiceServer m_serviceUpdateMap; - ros::ServiceServer m_serviceDeleteMap; - ros::ServiceServer m_serviceReadAllMaps; - ros::ServiceServer m_serviceActivateMap; - ros::ServiceServer m_serviceCreateAction; - ros::ServiceServer m_serviceReadAction; - ros::ServiceServer m_serviceReadAllActions; - ros::ServiceServer m_serviceDeleteAction; - ros::ServiceServer m_serviceUpdateAction; - ros::ServiceServer m_serviceExecuteActionFromReference; - ros::ServiceServer m_serviceExecuteAction; - ros::ServiceServer m_servicePauseAction; - ros::ServiceServer m_serviceStopAction; - ros::ServiceServer m_serviceResumeAction; - ros::ServiceServer m_serviceGetIPv4Configuration; - ros::ServiceServer m_serviceSetIPv4Configuration; - ros::ServiceServer m_serviceSetCommunicationInterfaceEnable; - ros::ServiceServer m_serviceIsCommunicationInterfaceEnable; - ros::ServiceServer m_serviceGetAvailableWifi; - ros::ServiceServer m_serviceGetWifiInformation; - ros::ServiceServer m_serviceAddWifiConfiguration; - ros::ServiceServer m_serviceDeleteWifiConfiguration; - ros::ServiceServer m_serviceGetAllConfiguredWifis; - ros::ServiceServer m_serviceConnectWifi; - ros::ServiceServer m_serviceDisconnectWifi; - ros::ServiceServer m_serviceGetConnectedWifiInformation; - ros::ServiceServer m_serviceBase_Unsubscribe; - ros::ServiceServer m_serviceOnNotificationConfigurationChangeTopic; - ros::ServiceServer m_serviceOnNotificationMappingInfoTopic; - ros::ServiceServer m_serviceOnNotificationControlModeTopic; - ros::ServiceServer m_serviceOnNotificationOperatingModeTopic; - ros::ServiceServer m_serviceOnNotificationSequenceInfoTopic; - ros::ServiceServer m_serviceOnNotificationProtectionZoneTopic; - ros::ServiceServer m_serviceOnNotificationUserTopic; - ros::ServiceServer m_serviceOnNotificationControllerTopic; - ros::ServiceServer m_serviceOnNotificationActionTopic; - ros::ServiceServer m_serviceOnNotificationRobotEventTopic; - ros::ServiceServer m_servicePlayCartesianTrajectory; - ros::ServiceServer m_servicePlayCartesianTrajectoryPosition; - ros::ServiceServer m_servicePlayCartesianTrajectoryOrientation; - ros::ServiceServer m_serviceStop; - ros::ServiceServer m_serviceGetMeasuredCartesianPose; - ros::ServiceServer m_serviceSendWrenchCommand; - ros::ServiceServer m_serviceSendWrenchJoystickCommand; - ros::ServiceServer m_serviceSendTwistJoystickCommand; - ros::ServiceServer m_serviceSendTwistCommand; - ros::ServiceServer m_servicePlayJointTrajectory; - ros::ServiceServer m_servicePlaySelectedJointTrajectory; - ros::ServiceServer m_serviceGetMeasuredJointAngles; - ros::ServiceServer m_serviceSendJointSpeedsCommand; - ros::ServiceServer m_serviceSendSelectedJointSpeedCommand; - ros::ServiceServer m_serviceSendGripperCommand; - ros::ServiceServer m_serviceGetMeasuredGripperMovement; - ros::ServiceServer m_serviceSetAdmittance; - ros::ServiceServer m_serviceSetOperatingMode; - ros::ServiceServer m_serviceApplyEmergencyStop; - ros::ServiceServer m_serviceBase_ClearFaults; - ros::ServiceServer m_serviceBase_GetControlMode; - ros::ServiceServer m_serviceGetOperatingMode; - ros::ServiceServer m_serviceSetServoingMode; - ros::ServiceServer m_serviceGetServoingMode; - ros::ServiceServer m_serviceOnNotificationServoingModeTopic; - ros::ServiceServer m_serviceRestoreFactorySettings; - ros::ServiceServer m_serviceOnNotificationFactoryTopic; - ros::ServiceServer m_serviceGetAllConnectedControllers; - ros::ServiceServer m_serviceGetControllerState; - ros::ServiceServer m_serviceGetActuatorCount; - ros::ServiceServer m_serviceStartWifiScan; - ros::ServiceServer m_serviceGetConfiguredWifi; - ros::ServiceServer m_serviceOnNotificationNetworkTopic; - ros::ServiceServer m_serviceGetArmState; - ros::ServiceServer m_serviceOnNotificationArmStateTopic; - ros::ServiceServer m_serviceGetIPv4Information; - ros::ServiceServer m_serviceSetWifiCountryCode; - ros::ServiceServer m_serviceGetWifiCountryCode; - ros::ServiceServer m_serviceBase_SetCapSenseConfig; - ros::ServiceServer m_serviceBase_GetCapSenseConfig; - ros::ServiceServer m_serviceGetAllJointsSpeedHardLimitation; - ros::ServiceServer m_serviceGetAllJointsTorqueHardLimitation; - ros::ServiceServer m_serviceGetTwistHardLimitation; - ros::ServiceServer m_serviceGetWrenchHardLimitation; - ros::ServiceServer m_serviceSendJointSpeedsJoystickCommand; - ros::ServiceServer m_serviceSendSelectedJointSpeedJoystickCommand; - ros::ServiceServer m_serviceEnableBridge; - ros::ServiceServer m_serviceDisableBridge; - ros::ServiceServer m_serviceGetBridgeList; - ros::ServiceServer m_serviceGetBridgeConfig; - ros::ServiceServer m_servicePlayPreComputedJointTrajectory; - ros::ServiceServer m_serviceGetProductConfiguration; - ros::ServiceServer m_serviceUpdateEndEffectorTypeConfiguration; - ros::ServiceServer m_serviceRestoreFactoryProductConfiguration; - ros::ServiceServer m_serviceGetTrajectoryErrorReport; - ros::ServiceServer m_serviceGetAllJointsSpeedSoftLimitation; - ros::ServiceServer m_serviceGetAllJointsTorqueSoftLimitation; - ros::ServiceServer m_serviceGetTwistSoftLimitation; - ros::ServiceServer m_serviceGetWrenchSoftLimitation; - ros::ServiceServer m_serviceSetControllerConfigurationMode; - ros::ServiceServer m_serviceGetControllerConfigurationMode; - ros::ServiceServer m_serviceStartTeaching; - ros::ServiceServer m_serviceStopTeaching; - ros::ServiceServer m_serviceAddSequenceTasks; - ros::ServiceServer m_serviceUpdateSequenceTask; - ros::ServiceServer m_serviceSwapSequenceTasks; - ros::ServiceServer m_serviceReadSequenceTask; - ros::ServiceServer m_serviceReadAllSequenceTasks; - ros::ServiceServer m_serviceDeleteSequenceTask; - ros::ServiceServer m_serviceDeleteAllSequenceTasks; - ros::ServiceServer m_serviceTakeSnapshot; - ros::ServiceServer m_serviceGetFirmwareBundleVersions; - ros::ServiceServer m_serviceMoveSequenceTask; - ros::ServiceServer m_serviceDuplicateMapping; - ros::ServiceServer m_serviceDuplicateMap; - ros::ServiceServer m_serviceSetControllerConfiguration; - ros::ServiceServer m_serviceGetControllerConfiguration; - ros::ServiceServer m_serviceGetAllControllerConfigurations; -}; -#endif diff --git a/kortex_driver/include/kortex_driver/generated/controlconfig_services.h b/kortex_driver/include/kortex_driver/generated/controlconfig_services.h deleted file mode 100644 index 5483d206..00000000 --- a/kortex_driver/include/kortex_driver/generated/controlconfig_services.h +++ /dev/null @@ -1,150 +0,0 @@ -/* -* KINOVA (R) KORTEX (TM) -* -* Copyright (c) 2019 Kinova inc. All rights reserved. -* -* This software may be modified and distributed under the -* terms of the BSD 3-Clause license. -* -* Refer to the LICENSE file for details. -* -*/ - -/* - * This file has been auto-generated and should not be modified. - */ - -#ifndef _KORTEX_CONTROLCONFIG_SERVICES_H_ -#define _KORTEX_CONTROLCONFIG_SERVICES_H_ - -#include "ros/ros.h" - -#include -#include -#include -#include -#include - -#include -#include -#include "kortex_driver/SetGravityVector.h" -#include "kortex_driver/GetGravityVector.h" -#include "kortex_driver/SetPayloadInformation.h" -#include "kortex_driver/GetPayloadInformation.h" -#include "kortex_driver/SetToolConfiguration.h" -#include "kortex_driver/GetToolConfiguration.h" -#include "kortex_driver/OnNotificationControlConfigurationTopic.h" -#include "kortex_driver/ControlConfigurationNotification.h" -#include "kortex_driver/ControlConfig_Unsubscribe.h" -#include "kortex_driver/SetCartesianReferenceFrame.h" -#include "kortex_driver/GetCartesianReferenceFrame.h" -#include "kortex_driver/ControlConfig_GetControlMode.h" -#include "kortex_driver/SetJointSpeedSoftLimits.h" -#include "kortex_driver/SetTwistLinearSoftLimit.h" -#include "kortex_driver/SetTwistAngularSoftLimit.h" -#include "kortex_driver/SetJointAccelerationSoftLimits.h" -#include "kortex_driver/GetKinematicHardLimits.h" -#include "kortex_driver/GetKinematicSoftLimits.h" -#include "kortex_driver/GetAllKinematicSoftLimits.h" -#include "kortex_driver/SetDesiredLinearTwist.h" -#include "kortex_driver/SetDesiredAngularTwist.h" -#include "kortex_driver/SetDesiredJointSpeeds.h" -#include "kortex_driver/GetDesiredSpeeds.h" -#include "kortex_driver/ResetGravityVector.h" -#include "kortex_driver/ResetPayloadInformation.h" -#include "kortex_driver/ResetToolConfiguration.h" -#include "kortex_driver/ResetJointSpeedSoftLimits.h" -#include "kortex_driver/ResetTwistLinearSoftLimit.h" -#include "kortex_driver/ResetTwistAngularSoftLimit.h" -#include "kortex_driver/ResetJointAccelerationSoftLimits.h" - -#include "kortex_driver/KortexError.h" -#include "kortex_driver/SetDeviceID.h" -#include "kortex_driver/SetApiOptions.h" -#include "kortex_driver/ApiOptions.h" - -using namespace std; - -class ControlConfigServices -{ - public: - ControlConfigServices(ros::NodeHandle& n, Kinova::Api::ControlConfig::ControlConfigClient* controlconfig, uint32_t device_id, uint32_t timeout_ms); - - bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res); - bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res); - bool SetGravityVector(kortex_driver::SetGravityVector::Request &req, kortex_driver::SetGravityVector::Response &res); - bool GetGravityVector(kortex_driver::GetGravityVector::Request &req, kortex_driver::GetGravityVector::Response &res); - bool SetPayloadInformation(kortex_driver::SetPayloadInformation::Request &req, kortex_driver::SetPayloadInformation::Response &res); - bool GetPayloadInformation(kortex_driver::GetPayloadInformation::Request &req, kortex_driver::GetPayloadInformation::Response &res); - bool SetToolConfiguration(kortex_driver::SetToolConfiguration::Request &req, kortex_driver::SetToolConfiguration::Response &res); - bool GetToolConfiguration(kortex_driver::GetToolConfiguration::Request &req, kortex_driver::GetToolConfiguration::Response &res); - bool OnNotificationControlConfigurationTopic(kortex_driver::OnNotificationControlConfigurationTopic::Request &req, kortex_driver::OnNotificationControlConfigurationTopic::Response &res); - void cb_ControlConfigurationTopic(Kinova::Api::ControlConfig::ControlConfigurationNotification notif); - bool ControlConfig_Unsubscribe(kortex_driver::ControlConfig_Unsubscribe::Request &req, kortex_driver::ControlConfig_Unsubscribe::Response &res); - bool SetCartesianReferenceFrame(kortex_driver::SetCartesianReferenceFrame::Request &req, kortex_driver::SetCartesianReferenceFrame::Response &res); - bool GetCartesianReferenceFrame(kortex_driver::GetCartesianReferenceFrame::Request &req, kortex_driver::GetCartesianReferenceFrame::Response &res); - bool ControlConfig_GetControlMode(kortex_driver::ControlConfig_GetControlMode::Request &req, kortex_driver::ControlConfig_GetControlMode::Response &res); - bool SetJointSpeedSoftLimits(kortex_driver::SetJointSpeedSoftLimits::Request &req, kortex_driver::SetJointSpeedSoftLimits::Response &res); - bool SetTwistLinearSoftLimit(kortex_driver::SetTwistLinearSoftLimit::Request &req, kortex_driver::SetTwistLinearSoftLimit::Response &res); - bool SetTwistAngularSoftLimit(kortex_driver::SetTwistAngularSoftLimit::Request &req, kortex_driver::SetTwistAngularSoftLimit::Response &res); - bool SetJointAccelerationSoftLimits(kortex_driver::SetJointAccelerationSoftLimits::Request &req, kortex_driver::SetJointAccelerationSoftLimits::Response &res); - bool GetKinematicHardLimits(kortex_driver::GetKinematicHardLimits::Request &req, kortex_driver::GetKinematicHardLimits::Response &res); - bool GetKinematicSoftLimits(kortex_driver::GetKinematicSoftLimits::Request &req, kortex_driver::GetKinematicSoftLimits::Response &res); - bool GetAllKinematicSoftLimits(kortex_driver::GetAllKinematicSoftLimits::Request &req, kortex_driver::GetAllKinematicSoftLimits::Response &res); - bool SetDesiredLinearTwist(kortex_driver::SetDesiredLinearTwist::Request &req, kortex_driver::SetDesiredLinearTwist::Response &res); - bool SetDesiredAngularTwist(kortex_driver::SetDesiredAngularTwist::Request &req, kortex_driver::SetDesiredAngularTwist::Response &res); - bool SetDesiredJointSpeeds(kortex_driver::SetDesiredJointSpeeds::Request &req, kortex_driver::SetDesiredJointSpeeds::Response &res); - bool GetDesiredSpeeds(kortex_driver::GetDesiredSpeeds::Request &req, kortex_driver::GetDesiredSpeeds::Response &res); - bool ResetGravityVector(kortex_driver::ResetGravityVector::Request &req, kortex_driver::ResetGravityVector::Response &res); - bool ResetPayloadInformation(kortex_driver::ResetPayloadInformation::Request &req, kortex_driver::ResetPayloadInformation::Response &res); - bool ResetToolConfiguration(kortex_driver::ResetToolConfiguration::Request &req, kortex_driver::ResetToolConfiguration::Response &res); - bool ResetJointSpeedSoftLimits(kortex_driver::ResetJointSpeedSoftLimits::Request &req, kortex_driver::ResetJointSpeedSoftLimits::Response &res); - bool ResetTwistLinearSoftLimit(kortex_driver::ResetTwistLinearSoftLimit::Request &req, kortex_driver::ResetTwistLinearSoftLimit::Response &res); - bool ResetTwistAngularSoftLimit(kortex_driver::ResetTwistAngularSoftLimit::Request &req, kortex_driver::ResetTwistAngularSoftLimit::Response &res); - bool ResetJointAccelerationSoftLimits(kortex_driver::ResetJointAccelerationSoftLimits::Request &req, kortex_driver::ResetJointAccelerationSoftLimits::Response &res); - -private: - uint32_t m_current_device_id; - Kinova::Api::RouterClientSendOptions m_api_options; - - Kinova::Api::ControlConfig::ControlConfigClient* m_controlconfig; - - ros::NodeHandle m_n; - ros::Publisher m_pub_Error; - ros::Publisher m_pub_ControlConfigurationTopic; - bool m_is_activated_ControlConfigurationTopic; - - ros::ServiceServer m_serviceSetDeviceID; - ros::ServiceServer m_serviceSetApiOptions; - - ros::ServiceServer m_serviceSetGravityVector; - ros::ServiceServer m_serviceGetGravityVector; - ros::ServiceServer m_serviceSetPayloadInformation; - ros::ServiceServer m_serviceGetPayloadInformation; - ros::ServiceServer m_serviceSetToolConfiguration; - ros::ServiceServer m_serviceGetToolConfiguration; - ros::ServiceServer m_serviceOnNotificationControlConfigurationTopic; - ros::ServiceServer m_serviceControlConfig_Unsubscribe; - ros::ServiceServer m_serviceSetCartesianReferenceFrame; - ros::ServiceServer m_serviceGetCartesianReferenceFrame; - ros::ServiceServer m_serviceControlConfig_GetControlMode; - ros::ServiceServer m_serviceSetJointSpeedSoftLimits; - ros::ServiceServer m_serviceSetTwistLinearSoftLimit; - ros::ServiceServer m_serviceSetTwistAngularSoftLimit; - ros::ServiceServer m_serviceSetJointAccelerationSoftLimits; - ros::ServiceServer m_serviceGetKinematicHardLimits; - ros::ServiceServer m_serviceGetKinematicSoftLimits; - ros::ServiceServer m_serviceGetAllKinematicSoftLimits; - ros::ServiceServer m_serviceSetDesiredLinearTwist; - ros::ServiceServer m_serviceSetDesiredAngularTwist; - ros::ServiceServer m_serviceSetDesiredJointSpeeds; - ros::ServiceServer m_serviceGetDesiredSpeeds; - ros::ServiceServer m_serviceResetGravityVector; - ros::ServiceServer m_serviceResetPayloadInformation; - ros::ServiceServer m_serviceResetToolConfiguration; - ros::ServiceServer m_serviceResetJointSpeedSoftLimits; - ros::ServiceServer m_serviceResetTwistLinearSoftLimit; - ros::ServiceServer m_serviceResetTwistAngularSoftLimit; - ros::ServiceServer m_serviceResetJointAccelerationSoftLimits; -}; -#endif diff --git a/kortex_driver/include/kortex_driver/generated/deviceconfig_services.h b/kortex_driver/include/kortex_driver/generated/deviceconfig_services.h deleted file mode 100644 index f80caaf0..00000000 --- a/kortex_driver/include/kortex_driver/generated/deviceconfig_services.h +++ /dev/null @@ -1,159 +0,0 @@ -/* -* KINOVA (R) KORTEX (TM) -* -* Copyright (c) 2019 Kinova inc. All rights reserved. -* -* This software may be modified and distributed under the -* terms of the BSD 3-Clause license. -* -* Refer to the LICENSE file for details. -* -*/ - -/* - * This file has been auto-generated and should not be modified. - */ - -#ifndef _KORTEX_DEVICECONFIG_SERVICES_H_ -#define _KORTEX_DEVICECONFIG_SERVICES_H_ - -#include "ros/ros.h" - -#include -#include -#include -#include -#include - -#include -#include -#include "kortex_driver/GetRunMode.h" -#include "kortex_driver/SetRunMode.h" -#include "kortex_driver/GetDeviceType.h" -#include "kortex_driver/GetFirmwareVersion.h" -#include "kortex_driver/GetBootloaderVersion.h" -#include "kortex_driver/GetModelNumber.h" -#include "kortex_driver/GetPartNumber.h" -#include "kortex_driver/GetSerialNumber.h" -#include "kortex_driver/GetMACAddress.h" -#include "kortex_driver/GetIPv4Settings.h" -#include "kortex_driver/SetIPv4Settings.h" -#include "kortex_driver/GetPartNumberRevision.h" -#include "kortex_driver/RebootRequest.h" -#include "kortex_driver/SetSafetyEnable.h" -#include "kortex_driver/SetSafetyErrorThreshold.h" -#include "kortex_driver/SetSafetyWarningThreshold.h" -#include "kortex_driver/SetSafetyConfiguration.h" -#include "kortex_driver/GetSafetyConfiguration.h" -#include "kortex_driver/GetSafetyInformation.h" -#include "kortex_driver/GetSafetyEnable.h" -#include "kortex_driver/GetSafetyStatus.h" -#include "kortex_driver/ClearAllSafetyStatus.h" -#include "kortex_driver/ClearSafetyStatus.h" -#include "kortex_driver/GetAllSafetyConfiguration.h" -#include "kortex_driver/GetAllSafetyInformation.h" -#include "kortex_driver/ResetSafetyDefaults.h" -#include "kortex_driver/OnNotificationSafetyTopic.h" -#include "kortex_driver/SafetyNotification.h" -#include "kortex_driver/ExecuteCalibration.h" -#include "kortex_driver/GetCalibrationResult.h" -#include "kortex_driver/StopCalibration.h" -#include "kortex_driver/DeviceConfig_SetCapSenseConfig.h" -#include "kortex_driver/DeviceConfig_GetCapSenseConfig.h" - -#include "kortex_driver/KortexError.h" -#include "kortex_driver/SetDeviceID.h" -#include "kortex_driver/SetApiOptions.h" -#include "kortex_driver/ApiOptions.h" - -using namespace std; - -class DeviceConfigServices -{ - public: - DeviceConfigServices(ros::NodeHandle& n, Kinova::Api::DeviceConfig::DeviceConfigClient* deviceconfig, uint32_t device_id, uint32_t timeout_ms); - - bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res); - bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res); - bool GetRunMode(kortex_driver::GetRunMode::Request &req, kortex_driver::GetRunMode::Response &res); - bool SetRunMode(kortex_driver::SetRunMode::Request &req, kortex_driver::SetRunMode::Response &res); - bool GetDeviceType(kortex_driver::GetDeviceType::Request &req, kortex_driver::GetDeviceType::Response &res); - bool GetFirmwareVersion(kortex_driver::GetFirmwareVersion::Request &req, kortex_driver::GetFirmwareVersion::Response &res); - bool GetBootloaderVersion(kortex_driver::GetBootloaderVersion::Request &req, kortex_driver::GetBootloaderVersion::Response &res); - bool GetModelNumber(kortex_driver::GetModelNumber::Request &req, kortex_driver::GetModelNumber::Response &res); - bool GetPartNumber(kortex_driver::GetPartNumber::Request &req, kortex_driver::GetPartNumber::Response &res); - bool GetSerialNumber(kortex_driver::GetSerialNumber::Request &req, kortex_driver::GetSerialNumber::Response &res); - bool GetMACAddress(kortex_driver::GetMACAddress::Request &req, kortex_driver::GetMACAddress::Response &res); - bool GetIPv4Settings(kortex_driver::GetIPv4Settings::Request &req, kortex_driver::GetIPv4Settings::Response &res); - bool SetIPv4Settings(kortex_driver::SetIPv4Settings::Request &req, kortex_driver::SetIPv4Settings::Response &res); - bool GetPartNumberRevision(kortex_driver::GetPartNumberRevision::Request &req, kortex_driver::GetPartNumberRevision::Response &res); - bool RebootRequest(kortex_driver::RebootRequest::Request &req, kortex_driver::RebootRequest::Response &res); - bool SetSafetyEnable(kortex_driver::SetSafetyEnable::Request &req, kortex_driver::SetSafetyEnable::Response &res); - bool SetSafetyErrorThreshold(kortex_driver::SetSafetyErrorThreshold::Request &req, kortex_driver::SetSafetyErrorThreshold::Response &res); - bool SetSafetyWarningThreshold(kortex_driver::SetSafetyWarningThreshold::Request &req, kortex_driver::SetSafetyWarningThreshold::Response &res); - bool SetSafetyConfiguration(kortex_driver::SetSafetyConfiguration::Request &req, kortex_driver::SetSafetyConfiguration::Response &res); - bool GetSafetyConfiguration(kortex_driver::GetSafetyConfiguration::Request &req, kortex_driver::GetSafetyConfiguration::Response &res); - bool GetSafetyInformation(kortex_driver::GetSafetyInformation::Request &req, kortex_driver::GetSafetyInformation::Response &res); - bool GetSafetyEnable(kortex_driver::GetSafetyEnable::Request &req, kortex_driver::GetSafetyEnable::Response &res); - bool GetSafetyStatus(kortex_driver::GetSafetyStatus::Request &req, kortex_driver::GetSafetyStatus::Response &res); - bool ClearAllSafetyStatus(kortex_driver::ClearAllSafetyStatus::Request &req, kortex_driver::ClearAllSafetyStatus::Response &res); - bool ClearSafetyStatus(kortex_driver::ClearSafetyStatus::Request &req, kortex_driver::ClearSafetyStatus::Response &res); - bool GetAllSafetyConfiguration(kortex_driver::GetAllSafetyConfiguration::Request &req, kortex_driver::GetAllSafetyConfiguration::Response &res); - bool GetAllSafetyInformation(kortex_driver::GetAllSafetyInformation::Request &req, kortex_driver::GetAllSafetyInformation::Response &res); - bool ResetSafetyDefaults(kortex_driver::ResetSafetyDefaults::Request &req, kortex_driver::ResetSafetyDefaults::Response &res); - bool OnNotificationSafetyTopic(kortex_driver::OnNotificationSafetyTopic::Request &req, kortex_driver::OnNotificationSafetyTopic::Response &res); - void cb_SafetyTopic(Kinova::Api::Common::SafetyNotification notif); - bool ExecuteCalibration(kortex_driver::ExecuteCalibration::Request &req, kortex_driver::ExecuteCalibration::Response &res); - bool GetCalibrationResult(kortex_driver::GetCalibrationResult::Request &req, kortex_driver::GetCalibrationResult::Response &res); - bool StopCalibration(kortex_driver::StopCalibration::Request &req, kortex_driver::StopCalibration::Response &res); - bool DeviceConfig_SetCapSenseConfig(kortex_driver::DeviceConfig_SetCapSenseConfig::Request &req, kortex_driver::DeviceConfig_SetCapSenseConfig::Response &res); - bool DeviceConfig_GetCapSenseConfig(kortex_driver::DeviceConfig_GetCapSenseConfig::Request &req, kortex_driver::DeviceConfig_GetCapSenseConfig::Response &res); - -private: - uint32_t m_current_device_id; - Kinova::Api::RouterClientSendOptions m_api_options; - - Kinova::Api::DeviceConfig::DeviceConfigClient* m_deviceconfig; - - ros::NodeHandle m_n; - ros::Publisher m_pub_Error; - ros::Publisher m_pub_SafetyTopic; - bool m_is_activated_SafetyTopic; - - ros::ServiceServer m_serviceSetDeviceID; - ros::ServiceServer m_serviceSetApiOptions; - - ros::ServiceServer m_serviceGetRunMode; - ros::ServiceServer m_serviceSetRunMode; - ros::ServiceServer m_serviceGetDeviceType; - ros::ServiceServer m_serviceGetFirmwareVersion; - ros::ServiceServer m_serviceGetBootloaderVersion; - ros::ServiceServer m_serviceGetModelNumber; - ros::ServiceServer m_serviceGetPartNumber; - ros::ServiceServer m_serviceGetSerialNumber; - ros::ServiceServer m_serviceGetMACAddress; - ros::ServiceServer m_serviceGetIPv4Settings; - ros::ServiceServer m_serviceSetIPv4Settings; - ros::ServiceServer m_serviceGetPartNumberRevision; - ros::ServiceServer m_serviceRebootRequest; - ros::ServiceServer m_serviceSetSafetyEnable; - ros::ServiceServer m_serviceSetSafetyErrorThreshold; - ros::ServiceServer m_serviceSetSafetyWarningThreshold; - ros::ServiceServer m_serviceSetSafetyConfiguration; - ros::ServiceServer m_serviceGetSafetyConfiguration; - ros::ServiceServer m_serviceGetSafetyInformation; - ros::ServiceServer m_serviceGetSafetyEnable; - ros::ServiceServer m_serviceGetSafetyStatus; - ros::ServiceServer m_serviceClearAllSafetyStatus; - ros::ServiceServer m_serviceClearSafetyStatus; - ros::ServiceServer m_serviceGetAllSafetyConfiguration; - ros::ServiceServer m_serviceGetAllSafetyInformation; - ros::ServiceServer m_serviceResetSafetyDefaults; - ros::ServiceServer m_serviceOnNotificationSafetyTopic; - ros::ServiceServer m_serviceExecuteCalibration; - ros::ServiceServer m_serviceGetCalibrationResult; - ros::ServiceServer m_serviceStopCalibration; - ros::ServiceServer m_serviceDeviceConfig_SetCapSenseConfig; - ros::ServiceServer m_serviceDeviceConfig_GetCapSenseConfig; -}; -#endif diff --git a/kortex_driver/include/kortex_driver/generated/devicemanager_proto_converter.h b/kortex_driver/include/kortex_driver/generated/devicemanager_proto_converter.h deleted file mode 100644 index 352de2e7..00000000 --- a/kortex_driver/include/kortex_driver/generated/devicemanager_proto_converter.h +++ /dev/null @@ -1,49 +0,0 @@ -/* -* KINOVA (R) KORTEX (TM) -* -* Copyright (c) 2019 Kinova inc. All rights reserved. -* -* This software may be modified and distributed under the -* terms of the BSD 3-Clause license. -* -* Refer to the LICENSE file for details. -* -*/ - -/* - * This file has been auto-generated and should not be modified. - */ - -#ifndef _KORTEX_DEVICEMANAGER_PROTO_CONVERTER_H_ -#define _KORTEX_DEVICEMANAGER_PROTO_CONVERTER_H_ - -#include "ros/ros.h" - -#include -#include -#include -#include -#include - -#include - -#include "kortex_driver/generated/common_proto_converter.h" -#include "kortex_driver/generated/actuatorconfig_proto_converter.h" -#include "kortex_driver/generated/actuatorcyclic_proto_converter.h" -#include "kortex_driver/generated/productconfiguration_proto_converter.h" -#include "kortex_driver/generated/base_proto_converter.h" -#include "kortex_driver/generated/grippercyclic_proto_converter.h" -#include "kortex_driver/generated/interconnectcyclic_proto_converter.h" -#include "kortex_driver/generated/basecyclic_proto_converter.h" -#include "kortex_driver/generated/controlconfig_proto_converter.h" -#include "kortex_driver/generated/deviceconfig_proto_converter.h" -#include "kortex_driver/generated/interconnectconfig_proto_converter.h" -#include "kortex_driver/generated/visionconfig_proto_converter.h" - - -#include "kortex_driver/DeviceHandles.h" - - -int ToProtoData(kortex_driver::DeviceHandles input, Kinova::Api::DeviceManager::DeviceHandles *output); - -#endif \ No newline at end of file diff --git a/kortex_driver/include/kortex_driver/generated/devicemanager_ros_converter.h b/kortex_driver/include/kortex_driver/generated/devicemanager_ros_converter.h deleted file mode 100644 index 4e5359b7..00000000 --- a/kortex_driver/include/kortex_driver/generated/devicemanager_ros_converter.h +++ /dev/null @@ -1,49 +0,0 @@ -/* -* KINOVA (R) KORTEX (TM) -* -* Copyright (c) 2019 Kinova inc. All rights reserved. -* -* This software may be modified and distributed under the -* terms of the BSD 3-Clause license. -* -* Refer to the LICENSE file for details. -* -*/ - -/* - * This file has been auto-generated and should not be modified. - */ - -#ifndef _KORTEX_DEVICEMANAGER_ROS_CONVERTER_H_ -#define _KORTEX_DEVICEMANAGER_ROS_CONVERTER_H_ - -#include "ros/ros.h" - -#include -#include -#include -#include -#include - -#include - -#include "kortex_driver/generated/common_ros_converter.h" -#include "kortex_driver/generated/actuatorconfig_ros_converter.h" -#include "kortex_driver/generated/actuatorcyclic_ros_converter.h" -#include "kortex_driver/generated/productconfiguration_ros_converter.h" -#include "kortex_driver/generated/base_ros_converter.h" -#include "kortex_driver/generated/grippercyclic_ros_converter.h" -#include "kortex_driver/generated/interconnectcyclic_ros_converter.h" -#include "kortex_driver/generated/basecyclic_ros_converter.h" -#include "kortex_driver/generated/controlconfig_ros_converter.h" -#include "kortex_driver/generated/deviceconfig_ros_converter.h" -#include "kortex_driver/generated/interconnectconfig_ros_converter.h" -#include "kortex_driver/generated/visionconfig_ros_converter.h" - - -#include "kortex_driver/DeviceHandles.h" - - -int ToRosData(Kinova::Api::DeviceManager::DeviceHandles input, kortex_driver::DeviceHandles &output); - -#endif \ No newline at end of file diff --git a/kortex_driver/include/kortex_driver/generated/devicemanager_services.h b/kortex_driver/include/kortex_driver/generated/devicemanager_services.h deleted file mode 100644 index b5d016a3..00000000 --- a/kortex_driver/include/kortex_driver/generated/devicemanager_services.h +++ /dev/null @@ -1,62 +0,0 @@ -/* -* KINOVA (R) KORTEX (TM) -* -* Copyright (c) 2019 Kinova inc. All rights reserved. -* -* This software may be modified and distributed under the -* terms of the BSD 3-Clause license. -* -* Refer to the LICENSE file for details. -* -*/ - -/* - * This file has been auto-generated and should not be modified. - */ - -#ifndef _KORTEX_DEVICEMANAGER_SERVICES_H_ -#define _KORTEX_DEVICEMANAGER_SERVICES_H_ - -#include "ros/ros.h" - -#include -#include -#include -#include -#include - -#include -#include -#include "kortex_driver/ReadAllDevices.h" - -#include "kortex_driver/KortexError.h" -#include "kortex_driver/SetDeviceID.h" -#include "kortex_driver/SetApiOptions.h" -#include "kortex_driver/ApiOptions.h" - -using namespace std; - -class DeviceManagerServices -{ - public: - DeviceManagerServices(ros::NodeHandle& n, Kinova::Api::DeviceManager::DeviceManagerClient* devicemanager, uint32_t device_id, uint32_t timeout_ms); - - bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res); - bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res); - bool ReadAllDevices(kortex_driver::ReadAllDevices::Request &req, kortex_driver::ReadAllDevices::Response &res); - -private: - uint32_t m_current_device_id; - Kinova::Api::RouterClientSendOptions m_api_options; - - Kinova::Api::DeviceManager::DeviceManagerClient* m_devicemanager; - - ros::NodeHandle m_n; - ros::Publisher m_pub_Error; - - ros::ServiceServer m_serviceSetDeviceID; - ros::ServiceServer m_serviceSetApiOptions; - - ros::ServiceServer m_serviceReadAllDevices; -}; -#endif diff --git a/kortex_driver/include/kortex_driver/generated/interconnectconfig_services.h b/kortex_driver/include/kortex_driver/generated/interconnectconfig_services.h deleted file mode 100644 index 34e135e6..00000000 --- a/kortex_driver/include/kortex_driver/generated/interconnectconfig_services.h +++ /dev/null @@ -1,101 +0,0 @@ -/* -* KINOVA (R) KORTEX (TM) -* -* Copyright (c) 2019 Kinova inc. All rights reserved. -* -* This software may be modified and distributed under the -* terms of the BSD 3-Clause license. -* -* Refer to the LICENSE file for details. -* -*/ - -/* - * This file has been auto-generated and should not be modified. - */ - -#ifndef _KORTEX_INTERCONNECTCONFIG_SERVICES_H_ -#define _KORTEX_INTERCONNECTCONFIG_SERVICES_H_ - -#include "ros/ros.h" - -#include -#include -#include -#include -#include - -#include -#include -#include "kortex_driver/GetUARTConfiguration.h" -#include "kortex_driver/SetUARTConfiguration.h" -#include "kortex_driver/GetEthernetConfiguration.h" -#include "kortex_driver/SetEthernetConfiguration.h" -#include "kortex_driver/GetGPIOConfiguration.h" -#include "kortex_driver/SetGPIOConfiguration.h" -#include "kortex_driver/GetGPIOState.h" -#include "kortex_driver/SetGPIOState.h" -#include "kortex_driver/GetI2CConfiguration.h" -#include "kortex_driver/SetI2CConfiguration.h" -#include "kortex_driver/I2CRead.h" -#include "kortex_driver/I2CReadRegister.h" -#include "kortex_driver/I2CWrite.h" -#include "kortex_driver/I2CWriteRegister.h" - -#include "kortex_driver/KortexError.h" -#include "kortex_driver/SetDeviceID.h" -#include "kortex_driver/SetApiOptions.h" -#include "kortex_driver/ApiOptions.h" - -using namespace std; - -class InterconnectConfigServices -{ - public: - InterconnectConfigServices(ros::NodeHandle& n, Kinova::Api::InterconnectConfig::InterconnectConfigClient* interconnectconfig, uint32_t device_id, uint32_t timeout_ms); - - bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res); - bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res); - bool GetUARTConfiguration(kortex_driver::GetUARTConfiguration::Request &req, kortex_driver::GetUARTConfiguration::Response &res); - bool SetUARTConfiguration(kortex_driver::SetUARTConfiguration::Request &req, kortex_driver::SetUARTConfiguration::Response &res); - bool GetEthernetConfiguration(kortex_driver::GetEthernetConfiguration::Request &req, kortex_driver::GetEthernetConfiguration::Response &res); - bool SetEthernetConfiguration(kortex_driver::SetEthernetConfiguration::Request &req, kortex_driver::SetEthernetConfiguration::Response &res); - bool GetGPIOConfiguration(kortex_driver::GetGPIOConfiguration::Request &req, kortex_driver::GetGPIOConfiguration::Response &res); - bool SetGPIOConfiguration(kortex_driver::SetGPIOConfiguration::Request &req, kortex_driver::SetGPIOConfiguration::Response &res); - bool GetGPIOState(kortex_driver::GetGPIOState::Request &req, kortex_driver::GetGPIOState::Response &res); - bool SetGPIOState(kortex_driver::SetGPIOState::Request &req, kortex_driver::SetGPIOState::Response &res); - bool GetI2CConfiguration(kortex_driver::GetI2CConfiguration::Request &req, kortex_driver::GetI2CConfiguration::Response &res); - bool SetI2CConfiguration(kortex_driver::SetI2CConfiguration::Request &req, kortex_driver::SetI2CConfiguration::Response &res); - bool I2CRead(kortex_driver::I2CRead::Request &req, kortex_driver::I2CRead::Response &res); - bool I2CReadRegister(kortex_driver::I2CReadRegister::Request &req, kortex_driver::I2CReadRegister::Response &res); - bool I2CWrite(kortex_driver::I2CWrite::Request &req, kortex_driver::I2CWrite::Response &res); - bool I2CWriteRegister(kortex_driver::I2CWriteRegister::Request &req, kortex_driver::I2CWriteRegister::Response &res); - -private: - uint32_t m_current_device_id; - Kinova::Api::RouterClientSendOptions m_api_options; - - Kinova::Api::InterconnectConfig::InterconnectConfigClient* m_interconnectconfig; - - ros::NodeHandle m_n; - ros::Publisher m_pub_Error; - - ros::ServiceServer m_serviceSetDeviceID; - ros::ServiceServer m_serviceSetApiOptions; - - ros::ServiceServer m_serviceGetUARTConfiguration; - ros::ServiceServer m_serviceSetUARTConfiguration; - ros::ServiceServer m_serviceGetEthernetConfiguration; - ros::ServiceServer m_serviceSetEthernetConfiguration; - ros::ServiceServer m_serviceGetGPIOConfiguration; - ros::ServiceServer m_serviceSetGPIOConfiguration; - ros::ServiceServer m_serviceGetGPIOState; - ros::ServiceServer m_serviceSetGPIOState; - ros::ServiceServer m_serviceGetI2CConfiguration; - ros::ServiceServer m_serviceSetI2CConfiguration; - ros::ServiceServer m_serviceI2CRead; - ros::ServiceServer m_serviceI2CReadRegister; - ros::ServiceServer m_serviceI2CWrite; - ros::ServiceServer m_serviceI2CWriteRegister; -}; -#endif diff --git a/kortex_driver/include/kortex_driver/generated/interfaces/actuatorconfig_services_interface.h b/kortex_driver/include/kortex_driver/generated/interfaces/actuatorconfig_services_interface.h new file mode 100644 index 00000000..0aedc895 --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/interfaces/actuatorconfig_services_interface.h @@ -0,0 +1,111 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_ACTUATORCONFIG_SERVICES_INTERFACE_H_ +#define _KORTEX_ACTUATORCONFIG_SERVICES_INTERFACE_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include +#include "kortex_driver/GetAxisOffsets.h" +#include "kortex_driver/SetAxisOffsets.h" +#include "kortex_driver/SetTorqueOffset.h" +#include "kortex_driver/ActuatorConfig_GetControlMode.h" +#include "kortex_driver/SetControlMode.h" +#include "kortex_driver/GetActivatedControlLoop.h" +#include "kortex_driver/SetActivatedControlLoop.h" +#include "kortex_driver/GetControlLoopParameters.h" +#include "kortex_driver/SetControlLoopParameters.h" +#include "kortex_driver/SelectCustomData.h" +#include "kortex_driver/GetSelectedCustomData.h" +#include "kortex_driver/SetCommandMode.h" +#include "kortex_driver/ActuatorConfig_ClearFaults.h" +#include "kortex_driver/SetServoing.h" +#include "kortex_driver/MoveToPosition.h" +#include "kortex_driver/GetCommandMode.h" +#include "kortex_driver/GetServoing.h" +#include "kortex_driver/GetTorqueOffset.h" +#include "kortex_driver/SetCoggingFeedforwardMode.h" +#include "kortex_driver/GetCoggingFeedforwardMode.h" + +#include "kortex_driver/KortexError.h" +#include "kortex_driver/SetDeviceID.h" +#include "kortex_driver/SetApiOptions.h" +#include "kortex_driver/ApiOptions.h" + +using namespace std; + +class IActuatorConfigServices +{ + public: + IActuatorConfigServices(ros::NodeHandle& node_handle) : m_node_handle(node_handle) {} + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) = 0; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) = 0; + virtual bool GetAxisOffsets(kortex_driver::GetAxisOffsets::Request &req, kortex_driver::GetAxisOffsets::Response &res) = 0; + virtual bool SetAxisOffsets(kortex_driver::SetAxisOffsets::Request &req, kortex_driver::SetAxisOffsets::Response &res) = 0; + virtual bool SetTorqueOffset(kortex_driver::SetTorqueOffset::Request &req, kortex_driver::SetTorqueOffset::Response &res) = 0; + virtual bool ActuatorConfig_GetControlMode(kortex_driver::ActuatorConfig_GetControlMode::Request &req, kortex_driver::ActuatorConfig_GetControlMode::Response &res) = 0; + virtual bool SetControlMode(kortex_driver::SetControlMode::Request &req, kortex_driver::SetControlMode::Response &res) = 0; + virtual bool GetActivatedControlLoop(kortex_driver::GetActivatedControlLoop::Request &req, kortex_driver::GetActivatedControlLoop::Response &res) = 0; + virtual bool SetActivatedControlLoop(kortex_driver::SetActivatedControlLoop::Request &req, kortex_driver::SetActivatedControlLoop::Response &res) = 0; + virtual bool GetControlLoopParameters(kortex_driver::GetControlLoopParameters::Request &req, kortex_driver::GetControlLoopParameters::Response &res) = 0; + virtual bool SetControlLoopParameters(kortex_driver::SetControlLoopParameters::Request &req, kortex_driver::SetControlLoopParameters::Response &res) = 0; + virtual bool SelectCustomData(kortex_driver::SelectCustomData::Request &req, kortex_driver::SelectCustomData::Response &res) = 0; + virtual bool GetSelectedCustomData(kortex_driver::GetSelectedCustomData::Request &req, kortex_driver::GetSelectedCustomData::Response &res) = 0; + virtual bool SetCommandMode(kortex_driver::SetCommandMode::Request &req, kortex_driver::SetCommandMode::Response &res) = 0; + virtual bool ActuatorConfig_ClearFaults(kortex_driver::ActuatorConfig_ClearFaults::Request &req, kortex_driver::ActuatorConfig_ClearFaults::Response &res) = 0; + virtual bool SetServoing(kortex_driver::SetServoing::Request &req, kortex_driver::SetServoing::Response &res) = 0; + virtual bool MoveToPosition(kortex_driver::MoveToPosition::Request &req, kortex_driver::MoveToPosition::Response &res) = 0; + virtual bool GetCommandMode(kortex_driver::GetCommandMode::Request &req, kortex_driver::GetCommandMode::Response &res) = 0; + virtual bool GetServoing(kortex_driver::GetServoing::Request &req, kortex_driver::GetServoing::Response &res) = 0; + virtual bool GetTorqueOffset(kortex_driver::GetTorqueOffset::Request &req, kortex_driver::GetTorqueOffset::Response &res) = 0; + virtual bool SetCoggingFeedforwardMode(kortex_driver::SetCoggingFeedforwardMode::Request &req, kortex_driver::SetCoggingFeedforwardMode::Response &res) = 0; + virtual bool GetCoggingFeedforwardMode(kortex_driver::GetCoggingFeedforwardMode::Request &req, kortex_driver::GetCoggingFeedforwardMode::Response &res) = 0; + +protected: + ros::NodeHandle m_node_handle; + ros::Publisher m_pub_Error; + + ros::ServiceServer m_serviceSetDeviceID; + ros::ServiceServer m_serviceSetApiOptions; + + ros::ServiceServer m_serviceGetAxisOffsets; + ros::ServiceServer m_serviceSetAxisOffsets; + ros::ServiceServer m_serviceSetTorqueOffset; + ros::ServiceServer m_serviceActuatorConfig_GetControlMode; + ros::ServiceServer m_serviceSetControlMode; + ros::ServiceServer m_serviceGetActivatedControlLoop; + ros::ServiceServer m_serviceSetActivatedControlLoop; + ros::ServiceServer m_serviceGetControlLoopParameters; + ros::ServiceServer m_serviceSetControlLoopParameters; + ros::ServiceServer m_serviceSelectCustomData; + ros::ServiceServer m_serviceGetSelectedCustomData; + ros::ServiceServer m_serviceSetCommandMode; + ros::ServiceServer m_serviceActuatorConfig_ClearFaults; + ros::ServiceServer m_serviceSetServoing; + ros::ServiceServer m_serviceMoveToPosition; + ros::ServiceServer m_serviceGetCommandMode; + ros::ServiceServer m_serviceGetServoing; + ros::ServiceServer m_serviceGetTorqueOffset; + ros::ServiceServer m_serviceSetCoggingFeedforwardMode; + ros::ServiceServer m_serviceGetCoggingFeedforwardMode; +}; +#endif diff --git a/kortex_driver/include/kortex_driver/generated/interfaces/base_services_interface.h b/kortex_driver/include/kortex_driver/generated/interfaces/base_services_interface.h new file mode 100644 index 00000000..1df8686a --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/interfaces/base_services_interface.h @@ -0,0 +1,539 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_BASE_SERVICES_INTERFACE_H_ +#define _KORTEX_BASE_SERVICES_INTERFACE_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include +#include "kortex_driver/CreateUserProfile.h" +#include "kortex_driver/UpdateUserProfile.h" +#include "kortex_driver/ReadUserProfile.h" +#include "kortex_driver/DeleteUserProfile.h" +#include "kortex_driver/ReadAllUserProfiles.h" +#include "kortex_driver/ReadAllUsers.h" +#include "kortex_driver/ChangePassword.h" +#include "kortex_driver/CreateSequence.h" +#include "kortex_driver/UpdateSequence.h" +#include "kortex_driver/ReadSequence.h" +#include "kortex_driver/DeleteSequence.h" +#include "kortex_driver/ReadAllSequences.h" +#include "kortex_driver/PlaySequence.h" +#include "kortex_driver/PlayAdvancedSequence.h" +#include "kortex_driver/StopSequence.h" +#include "kortex_driver/PauseSequence.h" +#include "kortex_driver/ResumeSequence.h" +#include "kortex_driver/CreateProtectionZone.h" +#include "kortex_driver/UpdateProtectionZone.h" +#include "kortex_driver/ReadProtectionZone.h" +#include "kortex_driver/DeleteProtectionZone.h" +#include "kortex_driver/ReadAllProtectionZones.h" +#include "kortex_driver/CreateMapping.h" +#include "kortex_driver/ReadMapping.h" +#include "kortex_driver/UpdateMapping.h" +#include "kortex_driver/DeleteMapping.h" +#include "kortex_driver/ReadAllMappings.h" +#include "kortex_driver/CreateMap.h" +#include "kortex_driver/ReadMap.h" +#include "kortex_driver/UpdateMap.h" +#include "kortex_driver/DeleteMap.h" +#include "kortex_driver/ReadAllMaps.h" +#include "kortex_driver/ActivateMap.h" +#include "kortex_driver/CreateAction.h" +#include "kortex_driver/ReadAction.h" +#include "kortex_driver/ReadAllActions.h" +#include "kortex_driver/DeleteAction.h" +#include "kortex_driver/UpdateAction.h" +#include "kortex_driver/ExecuteActionFromReference.h" +#include "kortex_driver/ExecuteAction.h" +#include "kortex_driver/PauseAction.h" +#include "kortex_driver/StopAction.h" +#include "kortex_driver/ResumeAction.h" +#include "kortex_driver/GetIPv4Configuration.h" +#include "kortex_driver/SetIPv4Configuration.h" +#include "kortex_driver/SetCommunicationInterfaceEnable.h" +#include "kortex_driver/IsCommunicationInterfaceEnable.h" +#include "kortex_driver/GetAvailableWifi.h" +#include "kortex_driver/GetWifiInformation.h" +#include "kortex_driver/AddWifiConfiguration.h" +#include "kortex_driver/DeleteWifiConfiguration.h" +#include "kortex_driver/GetAllConfiguredWifis.h" +#include "kortex_driver/ConnectWifi.h" +#include "kortex_driver/DisconnectWifi.h" +#include "kortex_driver/GetConnectedWifiInformation.h" +#include "kortex_driver/Base_Unsubscribe.h" +#include "kortex_driver/OnNotificationConfigurationChangeTopic.h" +#include "kortex_driver/ConfigurationChangeNotification.h" +#include "kortex_driver/OnNotificationMappingInfoTopic.h" +#include "kortex_driver/MappingInfoNotification.h" +#include "kortex_driver/OnNotificationControlModeTopic.h" +#include "kortex_driver/ControlModeNotification.h" +#include "kortex_driver/OnNotificationOperatingModeTopic.h" +#include "kortex_driver/OperatingModeNotification.h" +#include "kortex_driver/OnNotificationSequenceInfoTopic.h" +#include "kortex_driver/SequenceInfoNotification.h" +#include "kortex_driver/OnNotificationProtectionZoneTopic.h" +#include "kortex_driver/ProtectionZoneNotification.h" +#include "kortex_driver/OnNotificationUserTopic.h" +#include "kortex_driver/UserNotification.h" +#include "kortex_driver/OnNotificationControllerTopic.h" +#include "kortex_driver/ControllerNotification.h" +#include "kortex_driver/OnNotificationActionTopic.h" +#include "kortex_driver/ActionNotification.h" +#include "kortex_driver/OnNotificationRobotEventTopic.h" +#include "kortex_driver/RobotEventNotification.h" +#include "kortex_driver/PlayCartesianTrajectory.h" +#include "kortex_driver/PlayCartesianTrajectoryPosition.h" +#include "kortex_driver/PlayCartesianTrajectoryOrientation.h" +#include "kortex_driver/Stop.h" +#include "kortex_driver/GetMeasuredCartesianPose.h" +#include "kortex_driver/SendWrenchCommand.h" +#include "kortex_driver/SendWrenchJoystickCommand.h" +#include "kortex_driver/SendTwistJoystickCommand.h" +#include "kortex_driver/SendTwistCommand.h" +#include "kortex_driver/PlayJointTrajectory.h" +#include "kortex_driver/PlaySelectedJointTrajectory.h" +#include "kortex_driver/GetMeasuredJointAngles.h" +#include "kortex_driver/SendJointSpeedsCommand.h" +#include "kortex_driver/SendSelectedJointSpeedCommand.h" +#include "kortex_driver/SendGripperCommand.h" +#include "kortex_driver/GetMeasuredGripperMovement.h" +#include "kortex_driver/SetAdmittance.h" +#include "kortex_driver/SetOperatingMode.h" +#include "kortex_driver/ApplyEmergencyStop.h" +#include "kortex_driver/Base_ClearFaults.h" +#include "kortex_driver/Base_GetControlMode.h" +#include "kortex_driver/GetOperatingMode.h" +#include "kortex_driver/SetServoingMode.h" +#include "kortex_driver/GetServoingMode.h" +#include "kortex_driver/OnNotificationServoingModeTopic.h" +#include "kortex_driver/ServoingModeNotification.h" +#include "kortex_driver/RestoreFactorySettings.h" +#include "kortex_driver/OnNotificationFactoryTopic.h" +#include "kortex_driver/FactoryNotification.h" +#include "kortex_driver/GetAllConnectedControllers.h" +#include "kortex_driver/GetControllerState.h" +#include "kortex_driver/GetActuatorCount.h" +#include "kortex_driver/StartWifiScan.h" +#include "kortex_driver/GetConfiguredWifi.h" +#include "kortex_driver/OnNotificationNetworkTopic.h" +#include "kortex_driver/NetworkNotification.h" +#include "kortex_driver/GetArmState.h" +#include "kortex_driver/OnNotificationArmStateTopic.h" +#include "kortex_driver/ArmStateNotification.h" +#include "kortex_driver/GetIPv4Information.h" +#include "kortex_driver/SetWifiCountryCode.h" +#include "kortex_driver/GetWifiCountryCode.h" +#include "kortex_driver/Base_SetCapSenseConfig.h" +#include "kortex_driver/Base_GetCapSenseConfig.h" +#include "kortex_driver/GetAllJointsSpeedHardLimitation.h" +#include "kortex_driver/GetAllJointsTorqueHardLimitation.h" +#include "kortex_driver/GetTwistHardLimitation.h" +#include "kortex_driver/GetWrenchHardLimitation.h" +#include "kortex_driver/SendJointSpeedsJoystickCommand.h" +#include "kortex_driver/SendSelectedJointSpeedJoystickCommand.h" +#include "kortex_driver/EnableBridge.h" +#include "kortex_driver/DisableBridge.h" +#include "kortex_driver/GetBridgeList.h" +#include "kortex_driver/GetBridgeConfig.h" +#include "kortex_driver/PlayPreComputedJointTrajectory.h" +#include "kortex_driver/GetProductConfiguration.h" +#include "kortex_driver/UpdateEndEffectorTypeConfiguration.h" +#include "kortex_driver/RestoreFactoryProductConfiguration.h" +#include "kortex_driver/GetTrajectoryErrorReport.h" +#include "kortex_driver/GetAllJointsSpeedSoftLimitation.h" +#include "kortex_driver/GetAllJointsTorqueSoftLimitation.h" +#include "kortex_driver/GetTwistSoftLimitation.h" +#include "kortex_driver/GetWrenchSoftLimitation.h" +#include "kortex_driver/SetControllerConfigurationMode.h" +#include "kortex_driver/GetControllerConfigurationMode.h" +#include "kortex_driver/StartTeaching.h" +#include "kortex_driver/StopTeaching.h" +#include "kortex_driver/AddSequenceTasks.h" +#include "kortex_driver/UpdateSequenceTask.h" +#include "kortex_driver/SwapSequenceTasks.h" +#include "kortex_driver/ReadSequenceTask.h" +#include "kortex_driver/ReadAllSequenceTasks.h" +#include "kortex_driver/DeleteSequenceTask.h" +#include "kortex_driver/DeleteAllSequenceTasks.h" +#include "kortex_driver/TakeSnapshot.h" +#include "kortex_driver/GetFirmwareBundleVersions.h" +#include "kortex_driver/MoveSequenceTask.h" +#include "kortex_driver/DuplicateMapping.h" +#include "kortex_driver/DuplicateMap.h" +#include "kortex_driver/SetControllerConfiguration.h" +#include "kortex_driver/GetControllerConfiguration.h" +#include "kortex_driver/GetAllControllerConfigurations.h" + +#include "kortex_driver/KortexError.h" +#include "kortex_driver/SetDeviceID.h" +#include "kortex_driver/SetApiOptions.h" +#include "kortex_driver/ApiOptions.h" + +using namespace std; + +class IBaseServices +{ + public: + IBaseServices(ros::NodeHandle& node_handle) : m_node_handle(node_handle) {} + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) = 0; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) = 0; + virtual bool CreateUserProfile(kortex_driver::CreateUserProfile::Request &req, kortex_driver::CreateUserProfile::Response &res) = 0; + virtual bool UpdateUserProfile(kortex_driver::UpdateUserProfile::Request &req, kortex_driver::UpdateUserProfile::Response &res) = 0; + virtual bool ReadUserProfile(kortex_driver::ReadUserProfile::Request &req, kortex_driver::ReadUserProfile::Response &res) = 0; + virtual bool DeleteUserProfile(kortex_driver::DeleteUserProfile::Request &req, kortex_driver::DeleteUserProfile::Response &res) = 0; + virtual bool ReadAllUserProfiles(kortex_driver::ReadAllUserProfiles::Request &req, kortex_driver::ReadAllUserProfiles::Response &res) = 0; + virtual bool ReadAllUsers(kortex_driver::ReadAllUsers::Request &req, kortex_driver::ReadAllUsers::Response &res) = 0; + virtual bool ChangePassword(kortex_driver::ChangePassword::Request &req, kortex_driver::ChangePassword::Response &res) = 0; + virtual bool CreateSequence(kortex_driver::CreateSequence::Request &req, kortex_driver::CreateSequence::Response &res) = 0; + virtual bool UpdateSequence(kortex_driver::UpdateSequence::Request &req, kortex_driver::UpdateSequence::Response &res) = 0; + virtual bool ReadSequence(kortex_driver::ReadSequence::Request &req, kortex_driver::ReadSequence::Response &res) = 0; + virtual bool DeleteSequence(kortex_driver::DeleteSequence::Request &req, kortex_driver::DeleteSequence::Response &res) = 0; + virtual bool ReadAllSequences(kortex_driver::ReadAllSequences::Request &req, kortex_driver::ReadAllSequences::Response &res) = 0; + virtual bool PlaySequence(kortex_driver::PlaySequence::Request &req, kortex_driver::PlaySequence::Response &res) = 0; + virtual bool PlayAdvancedSequence(kortex_driver::PlayAdvancedSequence::Request &req, kortex_driver::PlayAdvancedSequence::Response &res) = 0; + virtual bool StopSequence(kortex_driver::StopSequence::Request &req, kortex_driver::StopSequence::Response &res) = 0; + virtual bool PauseSequence(kortex_driver::PauseSequence::Request &req, kortex_driver::PauseSequence::Response &res) = 0; + virtual bool ResumeSequence(kortex_driver::ResumeSequence::Request &req, kortex_driver::ResumeSequence::Response &res) = 0; + virtual bool CreateProtectionZone(kortex_driver::CreateProtectionZone::Request &req, kortex_driver::CreateProtectionZone::Response &res) = 0; + virtual bool UpdateProtectionZone(kortex_driver::UpdateProtectionZone::Request &req, kortex_driver::UpdateProtectionZone::Response &res) = 0; + virtual bool ReadProtectionZone(kortex_driver::ReadProtectionZone::Request &req, kortex_driver::ReadProtectionZone::Response &res) = 0; + virtual bool DeleteProtectionZone(kortex_driver::DeleteProtectionZone::Request &req, kortex_driver::DeleteProtectionZone::Response &res) = 0; + virtual bool ReadAllProtectionZones(kortex_driver::ReadAllProtectionZones::Request &req, kortex_driver::ReadAllProtectionZones::Response &res) = 0; + virtual bool CreateMapping(kortex_driver::CreateMapping::Request &req, kortex_driver::CreateMapping::Response &res) = 0; + virtual bool ReadMapping(kortex_driver::ReadMapping::Request &req, kortex_driver::ReadMapping::Response &res) = 0; + virtual bool UpdateMapping(kortex_driver::UpdateMapping::Request &req, kortex_driver::UpdateMapping::Response &res) = 0; + virtual bool DeleteMapping(kortex_driver::DeleteMapping::Request &req, kortex_driver::DeleteMapping::Response &res) = 0; + virtual bool ReadAllMappings(kortex_driver::ReadAllMappings::Request &req, kortex_driver::ReadAllMappings::Response &res) = 0; + virtual bool CreateMap(kortex_driver::CreateMap::Request &req, kortex_driver::CreateMap::Response &res) = 0; + virtual bool ReadMap(kortex_driver::ReadMap::Request &req, kortex_driver::ReadMap::Response &res) = 0; + virtual bool UpdateMap(kortex_driver::UpdateMap::Request &req, kortex_driver::UpdateMap::Response &res) = 0; + virtual bool DeleteMap(kortex_driver::DeleteMap::Request &req, kortex_driver::DeleteMap::Response &res) = 0; + virtual bool ReadAllMaps(kortex_driver::ReadAllMaps::Request &req, kortex_driver::ReadAllMaps::Response &res) = 0; + virtual bool ActivateMap(kortex_driver::ActivateMap::Request &req, kortex_driver::ActivateMap::Response &res) = 0; + virtual bool CreateAction(kortex_driver::CreateAction::Request &req, kortex_driver::CreateAction::Response &res) = 0; + virtual bool ReadAction(kortex_driver::ReadAction::Request &req, kortex_driver::ReadAction::Response &res) = 0; + virtual bool ReadAllActions(kortex_driver::ReadAllActions::Request &req, kortex_driver::ReadAllActions::Response &res) = 0; + virtual bool DeleteAction(kortex_driver::DeleteAction::Request &req, kortex_driver::DeleteAction::Response &res) = 0; + virtual bool UpdateAction(kortex_driver::UpdateAction::Request &req, kortex_driver::UpdateAction::Response &res) = 0; + virtual bool ExecuteActionFromReference(kortex_driver::ExecuteActionFromReference::Request &req, kortex_driver::ExecuteActionFromReference::Response &res) = 0; + virtual bool ExecuteAction(kortex_driver::ExecuteAction::Request &req, kortex_driver::ExecuteAction::Response &res) = 0; + virtual bool PauseAction(kortex_driver::PauseAction::Request &req, kortex_driver::PauseAction::Response &res) = 0; + virtual bool StopAction(kortex_driver::StopAction::Request &req, kortex_driver::StopAction::Response &res) = 0; + virtual bool ResumeAction(kortex_driver::ResumeAction::Request &req, kortex_driver::ResumeAction::Response &res) = 0; + virtual bool GetIPv4Configuration(kortex_driver::GetIPv4Configuration::Request &req, kortex_driver::GetIPv4Configuration::Response &res) = 0; + virtual bool SetIPv4Configuration(kortex_driver::SetIPv4Configuration::Request &req, kortex_driver::SetIPv4Configuration::Response &res) = 0; + virtual bool SetCommunicationInterfaceEnable(kortex_driver::SetCommunicationInterfaceEnable::Request &req, kortex_driver::SetCommunicationInterfaceEnable::Response &res) = 0; + virtual bool IsCommunicationInterfaceEnable(kortex_driver::IsCommunicationInterfaceEnable::Request &req, kortex_driver::IsCommunicationInterfaceEnable::Response &res) = 0; + virtual bool GetAvailableWifi(kortex_driver::GetAvailableWifi::Request &req, kortex_driver::GetAvailableWifi::Response &res) = 0; + virtual bool GetWifiInformation(kortex_driver::GetWifiInformation::Request &req, kortex_driver::GetWifiInformation::Response &res) = 0; + virtual bool AddWifiConfiguration(kortex_driver::AddWifiConfiguration::Request &req, kortex_driver::AddWifiConfiguration::Response &res) = 0; + virtual bool DeleteWifiConfiguration(kortex_driver::DeleteWifiConfiguration::Request &req, kortex_driver::DeleteWifiConfiguration::Response &res) = 0; + virtual bool GetAllConfiguredWifis(kortex_driver::GetAllConfiguredWifis::Request &req, kortex_driver::GetAllConfiguredWifis::Response &res) = 0; + virtual bool ConnectWifi(kortex_driver::ConnectWifi::Request &req, kortex_driver::ConnectWifi::Response &res) = 0; + virtual bool DisconnectWifi(kortex_driver::DisconnectWifi::Request &req, kortex_driver::DisconnectWifi::Response &res) = 0; + virtual bool GetConnectedWifiInformation(kortex_driver::GetConnectedWifiInformation::Request &req, kortex_driver::GetConnectedWifiInformation::Response &res) = 0; + virtual bool Base_Unsubscribe(kortex_driver::Base_Unsubscribe::Request &req, kortex_driver::Base_Unsubscribe::Response &res) = 0; + virtual bool OnNotificationConfigurationChangeTopic(kortex_driver::OnNotificationConfigurationChangeTopic::Request &req, kortex_driver::OnNotificationConfigurationChangeTopic::Response &res) = 0; + virtual void cb_ConfigurationChangeTopic(Kinova::Api::Base::ConfigurationChangeNotification notif) = 0; + virtual bool OnNotificationMappingInfoTopic(kortex_driver::OnNotificationMappingInfoTopic::Request &req, kortex_driver::OnNotificationMappingInfoTopic::Response &res) = 0; + virtual void cb_MappingInfoTopic(Kinova::Api::Base::MappingInfoNotification notif) = 0; + virtual bool OnNotificationControlModeTopic(kortex_driver::OnNotificationControlModeTopic::Request &req, kortex_driver::OnNotificationControlModeTopic::Response &res) = 0; + virtual void cb_ControlModeTopic(Kinova::Api::Base::ControlModeNotification notif) = 0; + virtual bool OnNotificationOperatingModeTopic(kortex_driver::OnNotificationOperatingModeTopic::Request &req, kortex_driver::OnNotificationOperatingModeTopic::Response &res) = 0; + virtual void cb_OperatingModeTopic(Kinova::Api::Base::OperatingModeNotification notif) = 0; + virtual bool OnNotificationSequenceInfoTopic(kortex_driver::OnNotificationSequenceInfoTopic::Request &req, kortex_driver::OnNotificationSequenceInfoTopic::Response &res) = 0; + virtual void cb_SequenceInfoTopic(Kinova::Api::Base::SequenceInfoNotification notif) = 0; + virtual bool OnNotificationProtectionZoneTopic(kortex_driver::OnNotificationProtectionZoneTopic::Request &req, kortex_driver::OnNotificationProtectionZoneTopic::Response &res) = 0; + virtual void cb_ProtectionZoneTopic(Kinova::Api::Base::ProtectionZoneNotification notif) = 0; + virtual bool OnNotificationUserTopic(kortex_driver::OnNotificationUserTopic::Request &req, kortex_driver::OnNotificationUserTopic::Response &res) = 0; + virtual void cb_UserTopic(Kinova::Api::Base::UserNotification notif) = 0; + virtual bool OnNotificationControllerTopic(kortex_driver::OnNotificationControllerTopic::Request &req, kortex_driver::OnNotificationControllerTopic::Response &res) = 0; + virtual void cb_ControllerTopic(Kinova::Api::Base::ControllerNotification notif) = 0; + virtual bool OnNotificationActionTopic(kortex_driver::OnNotificationActionTopic::Request &req, kortex_driver::OnNotificationActionTopic::Response &res) = 0; + virtual void cb_ActionTopic(Kinova::Api::Base::ActionNotification notif) = 0; + virtual bool OnNotificationRobotEventTopic(kortex_driver::OnNotificationRobotEventTopic::Request &req, kortex_driver::OnNotificationRobotEventTopic::Response &res) = 0; + virtual void cb_RobotEventTopic(Kinova::Api::Base::RobotEventNotification notif) = 0; + virtual bool PlayCartesianTrajectory(kortex_driver::PlayCartesianTrajectory::Request &req, kortex_driver::PlayCartesianTrajectory::Response &res) = 0; + virtual bool PlayCartesianTrajectoryPosition(kortex_driver::PlayCartesianTrajectoryPosition::Request &req, kortex_driver::PlayCartesianTrajectoryPosition::Response &res) = 0; + virtual bool PlayCartesianTrajectoryOrientation(kortex_driver::PlayCartesianTrajectoryOrientation::Request &req, kortex_driver::PlayCartesianTrajectoryOrientation::Response &res) = 0; + virtual bool Stop(kortex_driver::Stop::Request &req, kortex_driver::Stop::Response &res) = 0; + virtual bool GetMeasuredCartesianPose(kortex_driver::GetMeasuredCartesianPose::Request &req, kortex_driver::GetMeasuredCartesianPose::Response &res) = 0; + virtual bool SendWrenchCommand(kortex_driver::SendWrenchCommand::Request &req, kortex_driver::SendWrenchCommand::Response &res) = 0; + virtual bool SendWrenchJoystickCommand(kortex_driver::SendWrenchJoystickCommand::Request &req, kortex_driver::SendWrenchJoystickCommand::Response &res) = 0; + virtual bool SendTwistJoystickCommand(kortex_driver::SendTwistJoystickCommand::Request &req, kortex_driver::SendTwistJoystickCommand::Response &res) = 0; + virtual bool SendTwistCommand(kortex_driver::SendTwistCommand::Request &req, kortex_driver::SendTwistCommand::Response &res) = 0; + virtual bool PlayJointTrajectory(kortex_driver::PlayJointTrajectory::Request &req, kortex_driver::PlayJointTrajectory::Response &res) = 0; + virtual bool PlaySelectedJointTrajectory(kortex_driver::PlaySelectedJointTrajectory::Request &req, kortex_driver::PlaySelectedJointTrajectory::Response &res) = 0; + virtual bool GetMeasuredJointAngles(kortex_driver::GetMeasuredJointAngles::Request &req, kortex_driver::GetMeasuredJointAngles::Response &res) = 0; + virtual bool SendJointSpeedsCommand(kortex_driver::SendJointSpeedsCommand::Request &req, kortex_driver::SendJointSpeedsCommand::Response &res) = 0; + virtual bool SendSelectedJointSpeedCommand(kortex_driver::SendSelectedJointSpeedCommand::Request &req, kortex_driver::SendSelectedJointSpeedCommand::Response &res) = 0; + virtual bool SendGripperCommand(kortex_driver::SendGripperCommand::Request &req, kortex_driver::SendGripperCommand::Response &res) = 0; + virtual bool GetMeasuredGripperMovement(kortex_driver::GetMeasuredGripperMovement::Request &req, kortex_driver::GetMeasuredGripperMovement::Response &res) = 0; + virtual bool SetAdmittance(kortex_driver::SetAdmittance::Request &req, kortex_driver::SetAdmittance::Response &res) = 0; + virtual bool SetOperatingMode(kortex_driver::SetOperatingMode::Request &req, kortex_driver::SetOperatingMode::Response &res) = 0; + virtual bool ApplyEmergencyStop(kortex_driver::ApplyEmergencyStop::Request &req, kortex_driver::ApplyEmergencyStop::Response &res) = 0; + virtual bool Base_ClearFaults(kortex_driver::Base_ClearFaults::Request &req, kortex_driver::Base_ClearFaults::Response &res) = 0; + virtual bool Base_GetControlMode(kortex_driver::Base_GetControlMode::Request &req, kortex_driver::Base_GetControlMode::Response &res) = 0; + virtual bool GetOperatingMode(kortex_driver::GetOperatingMode::Request &req, kortex_driver::GetOperatingMode::Response &res) = 0; + virtual bool SetServoingMode(kortex_driver::SetServoingMode::Request &req, kortex_driver::SetServoingMode::Response &res) = 0; + virtual bool GetServoingMode(kortex_driver::GetServoingMode::Request &req, kortex_driver::GetServoingMode::Response &res) = 0; + virtual bool OnNotificationServoingModeTopic(kortex_driver::OnNotificationServoingModeTopic::Request &req, kortex_driver::OnNotificationServoingModeTopic::Response &res) = 0; + virtual void cb_ServoingModeTopic(Kinova::Api::Base::ServoingModeNotification notif) = 0; + virtual bool RestoreFactorySettings(kortex_driver::RestoreFactorySettings::Request &req, kortex_driver::RestoreFactorySettings::Response &res) = 0; + virtual bool OnNotificationFactoryTopic(kortex_driver::OnNotificationFactoryTopic::Request &req, kortex_driver::OnNotificationFactoryTopic::Response &res) = 0; + virtual void cb_FactoryTopic(Kinova::Api::Base::FactoryNotification notif) = 0; + virtual bool GetAllConnectedControllers(kortex_driver::GetAllConnectedControllers::Request &req, kortex_driver::GetAllConnectedControllers::Response &res) = 0; + virtual bool GetControllerState(kortex_driver::GetControllerState::Request &req, kortex_driver::GetControllerState::Response &res) = 0; + virtual bool GetActuatorCount(kortex_driver::GetActuatorCount::Request &req, kortex_driver::GetActuatorCount::Response &res) = 0; + virtual bool StartWifiScan(kortex_driver::StartWifiScan::Request &req, kortex_driver::StartWifiScan::Response &res) = 0; + virtual bool GetConfiguredWifi(kortex_driver::GetConfiguredWifi::Request &req, kortex_driver::GetConfiguredWifi::Response &res) = 0; + virtual bool OnNotificationNetworkTopic(kortex_driver::OnNotificationNetworkTopic::Request &req, kortex_driver::OnNotificationNetworkTopic::Response &res) = 0; + virtual void cb_NetworkTopic(Kinova::Api::Base::NetworkNotification notif) = 0; + virtual bool GetArmState(kortex_driver::GetArmState::Request &req, kortex_driver::GetArmState::Response &res) = 0; + virtual bool OnNotificationArmStateTopic(kortex_driver::OnNotificationArmStateTopic::Request &req, kortex_driver::OnNotificationArmStateTopic::Response &res) = 0; + virtual void cb_ArmStateTopic(Kinova::Api::Base::ArmStateNotification notif) = 0; + virtual bool GetIPv4Information(kortex_driver::GetIPv4Information::Request &req, kortex_driver::GetIPv4Information::Response &res) = 0; + virtual bool SetWifiCountryCode(kortex_driver::SetWifiCountryCode::Request &req, kortex_driver::SetWifiCountryCode::Response &res) = 0; + virtual bool GetWifiCountryCode(kortex_driver::GetWifiCountryCode::Request &req, kortex_driver::GetWifiCountryCode::Response &res) = 0; + virtual bool Base_SetCapSenseConfig(kortex_driver::Base_SetCapSenseConfig::Request &req, kortex_driver::Base_SetCapSenseConfig::Response &res) = 0; + virtual bool Base_GetCapSenseConfig(kortex_driver::Base_GetCapSenseConfig::Request &req, kortex_driver::Base_GetCapSenseConfig::Response &res) = 0; + virtual bool GetAllJointsSpeedHardLimitation(kortex_driver::GetAllJointsSpeedHardLimitation::Request &req, kortex_driver::GetAllJointsSpeedHardLimitation::Response &res) = 0; + virtual bool GetAllJointsTorqueHardLimitation(kortex_driver::GetAllJointsTorqueHardLimitation::Request &req, kortex_driver::GetAllJointsTorqueHardLimitation::Response &res) = 0; + virtual bool GetTwistHardLimitation(kortex_driver::GetTwistHardLimitation::Request &req, kortex_driver::GetTwistHardLimitation::Response &res) = 0; + virtual bool GetWrenchHardLimitation(kortex_driver::GetWrenchHardLimitation::Request &req, kortex_driver::GetWrenchHardLimitation::Response &res) = 0; + virtual bool SendJointSpeedsJoystickCommand(kortex_driver::SendJointSpeedsJoystickCommand::Request &req, kortex_driver::SendJointSpeedsJoystickCommand::Response &res) = 0; + virtual bool SendSelectedJointSpeedJoystickCommand(kortex_driver::SendSelectedJointSpeedJoystickCommand::Request &req, kortex_driver::SendSelectedJointSpeedJoystickCommand::Response &res) = 0; + virtual bool EnableBridge(kortex_driver::EnableBridge::Request &req, kortex_driver::EnableBridge::Response &res) = 0; + virtual bool DisableBridge(kortex_driver::DisableBridge::Request &req, kortex_driver::DisableBridge::Response &res) = 0; + virtual bool GetBridgeList(kortex_driver::GetBridgeList::Request &req, kortex_driver::GetBridgeList::Response &res) = 0; + virtual bool GetBridgeConfig(kortex_driver::GetBridgeConfig::Request &req, kortex_driver::GetBridgeConfig::Response &res) = 0; + virtual bool PlayPreComputedJointTrajectory(kortex_driver::PlayPreComputedJointTrajectory::Request &req, kortex_driver::PlayPreComputedJointTrajectory::Response &res) = 0; + virtual bool GetProductConfiguration(kortex_driver::GetProductConfiguration::Request &req, kortex_driver::GetProductConfiguration::Response &res) = 0; + virtual bool UpdateEndEffectorTypeConfiguration(kortex_driver::UpdateEndEffectorTypeConfiguration::Request &req, kortex_driver::UpdateEndEffectorTypeConfiguration::Response &res) = 0; + virtual bool RestoreFactoryProductConfiguration(kortex_driver::RestoreFactoryProductConfiguration::Request &req, kortex_driver::RestoreFactoryProductConfiguration::Response &res) = 0; + virtual bool GetTrajectoryErrorReport(kortex_driver::GetTrajectoryErrorReport::Request &req, kortex_driver::GetTrajectoryErrorReport::Response &res) = 0; + virtual bool GetAllJointsSpeedSoftLimitation(kortex_driver::GetAllJointsSpeedSoftLimitation::Request &req, kortex_driver::GetAllJointsSpeedSoftLimitation::Response &res) = 0; + virtual bool GetAllJointsTorqueSoftLimitation(kortex_driver::GetAllJointsTorqueSoftLimitation::Request &req, kortex_driver::GetAllJointsTorqueSoftLimitation::Response &res) = 0; + virtual bool GetTwistSoftLimitation(kortex_driver::GetTwistSoftLimitation::Request &req, kortex_driver::GetTwistSoftLimitation::Response &res) = 0; + virtual bool GetWrenchSoftLimitation(kortex_driver::GetWrenchSoftLimitation::Request &req, kortex_driver::GetWrenchSoftLimitation::Response &res) = 0; + virtual bool SetControllerConfigurationMode(kortex_driver::SetControllerConfigurationMode::Request &req, kortex_driver::SetControllerConfigurationMode::Response &res) = 0; + virtual bool GetControllerConfigurationMode(kortex_driver::GetControllerConfigurationMode::Request &req, kortex_driver::GetControllerConfigurationMode::Response &res) = 0; + virtual bool StartTeaching(kortex_driver::StartTeaching::Request &req, kortex_driver::StartTeaching::Response &res) = 0; + virtual bool StopTeaching(kortex_driver::StopTeaching::Request &req, kortex_driver::StopTeaching::Response &res) = 0; + virtual bool AddSequenceTasks(kortex_driver::AddSequenceTasks::Request &req, kortex_driver::AddSequenceTasks::Response &res) = 0; + virtual bool UpdateSequenceTask(kortex_driver::UpdateSequenceTask::Request &req, kortex_driver::UpdateSequenceTask::Response &res) = 0; + virtual bool SwapSequenceTasks(kortex_driver::SwapSequenceTasks::Request &req, kortex_driver::SwapSequenceTasks::Response &res) = 0; + virtual bool ReadSequenceTask(kortex_driver::ReadSequenceTask::Request &req, kortex_driver::ReadSequenceTask::Response &res) = 0; + virtual bool ReadAllSequenceTasks(kortex_driver::ReadAllSequenceTasks::Request &req, kortex_driver::ReadAllSequenceTasks::Response &res) = 0; + virtual bool DeleteSequenceTask(kortex_driver::DeleteSequenceTask::Request &req, kortex_driver::DeleteSequenceTask::Response &res) = 0; + virtual bool DeleteAllSequenceTasks(kortex_driver::DeleteAllSequenceTasks::Request &req, kortex_driver::DeleteAllSequenceTasks::Response &res) = 0; + virtual bool TakeSnapshot(kortex_driver::TakeSnapshot::Request &req, kortex_driver::TakeSnapshot::Response &res) = 0; + virtual bool GetFirmwareBundleVersions(kortex_driver::GetFirmwareBundleVersions::Request &req, kortex_driver::GetFirmwareBundleVersions::Response &res) = 0; + virtual bool MoveSequenceTask(kortex_driver::MoveSequenceTask::Request &req, kortex_driver::MoveSequenceTask::Response &res) = 0; + virtual bool DuplicateMapping(kortex_driver::DuplicateMapping::Request &req, kortex_driver::DuplicateMapping::Response &res) = 0; + virtual bool DuplicateMap(kortex_driver::DuplicateMap::Request &req, kortex_driver::DuplicateMap::Response &res) = 0; + virtual bool SetControllerConfiguration(kortex_driver::SetControllerConfiguration::Request &req, kortex_driver::SetControllerConfiguration::Response &res) = 0; + virtual bool GetControllerConfiguration(kortex_driver::GetControllerConfiguration::Request &req, kortex_driver::GetControllerConfiguration::Response &res) = 0; + virtual bool GetAllControllerConfigurations(kortex_driver::GetAllControllerConfigurations::Request &req, kortex_driver::GetAllControllerConfigurations::Response &res) = 0; + +protected: + ros::NodeHandle m_node_handle; + ros::Publisher m_pub_Error; + ros::Publisher m_pub_ConfigurationChangeTopic; + bool m_is_activated_ConfigurationChangeTopic; + ros::Publisher m_pub_MappingInfoTopic; + bool m_is_activated_MappingInfoTopic; + ros::Publisher m_pub_ControlModeTopic; + bool m_is_activated_ControlModeTopic; + ros::Publisher m_pub_OperatingModeTopic; + bool m_is_activated_OperatingModeTopic; + ros::Publisher m_pub_SequenceInfoTopic; + bool m_is_activated_SequenceInfoTopic; + ros::Publisher m_pub_ProtectionZoneTopic; + bool m_is_activated_ProtectionZoneTopic; + ros::Publisher m_pub_UserTopic; + bool m_is_activated_UserTopic; + ros::Publisher m_pub_ControllerTopic; + bool m_is_activated_ControllerTopic; + ros::Publisher m_pub_ActionTopic; + bool m_is_activated_ActionTopic; + ros::Publisher m_pub_RobotEventTopic; + bool m_is_activated_RobotEventTopic; + ros::Publisher m_pub_ServoingModeTopic; + bool m_is_activated_ServoingModeTopic; + ros::Publisher m_pub_FactoryTopic; + bool m_is_activated_FactoryTopic; + ros::Publisher m_pub_NetworkTopic; + bool m_is_activated_NetworkTopic; + ros::Publisher m_pub_ArmStateTopic; + bool m_is_activated_ArmStateTopic; + + ros::ServiceServer m_serviceSetDeviceID; + ros::ServiceServer m_serviceSetApiOptions; + + ros::ServiceServer m_serviceCreateUserProfile; + ros::ServiceServer m_serviceUpdateUserProfile; + ros::ServiceServer m_serviceReadUserProfile; + ros::ServiceServer m_serviceDeleteUserProfile; + ros::ServiceServer m_serviceReadAllUserProfiles; + ros::ServiceServer m_serviceReadAllUsers; + ros::ServiceServer m_serviceChangePassword; + ros::ServiceServer m_serviceCreateSequence; + ros::ServiceServer m_serviceUpdateSequence; + ros::ServiceServer m_serviceReadSequence; + ros::ServiceServer m_serviceDeleteSequence; + ros::ServiceServer m_serviceReadAllSequences; + ros::ServiceServer m_servicePlaySequence; + ros::ServiceServer m_servicePlayAdvancedSequence; + ros::ServiceServer m_serviceStopSequence; + ros::ServiceServer m_servicePauseSequence; + ros::ServiceServer m_serviceResumeSequence; + ros::ServiceServer m_serviceCreateProtectionZone; + ros::ServiceServer m_serviceUpdateProtectionZone; + ros::ServiceServer m_serviceReadProtectionZone; + ros::ServiceServer m_serviceDeleteProtectionZone; + ros::ServiceServer m_serviceReadAllProtectionZones; + ros::ServiceServer m_serviceCreateMapping; + ros::ServiceServer m_serviceReadMapping; + ros::ServiceServer m_serviceUpdateMapping; + ros::ServiceServer m_serviceDeleteMapping; + ros::ServiceServer m_serviceReadAllMappings; + ros::ServiceServer m_serviceCreateMap; + ros::ServiceServer m_serviceReadMap; + ros::ServiceServer m_serviceUpdateMap; + ros::ServiceServer m_serviceDeleteMap; + ros::ServiceServer m_serviceReadAllMaps; + ros::ServiceServer m_serviceActivateMap; + ros::ServiceServer m_serviceCreateAction; + ros::ServiceServer m_serviceReadAction; + ros::ServiceServer m_serviceReadAllActions; + ros::ServiceServer m_serviceDeleteAction; + ros::ServiceServer m_serviceUpdateAction; + ros::ServiceServer m_serviceExecuteActionFromReference; + ros::ServiceServer m_serviceExecuteAction; + ros::ServiceServer m_servicePauseAction; + ros::ServiceServer m_serviceStopAction; + ros::ServiceServer m_serviceResumeAction; + ros::ServiceServer m_serviceGetIPv4Configuration; + ros::ServiceServer m_serviceSetIPv4Configuration; + ros::ServiceServer m_serviceSetCommunicationInterfaceEnable; + ros::ServiceServer m_serviceIsCommunicationInterfaceEnable; + ros::ServiceServer m_serviceGetAvailableWifi; + ros::ServiceServer m_serviceGetWifiInformation; + ros::ServiceServer m_serviceAddWifiConfiguration; + ros::ServiceServer m_serviceDeleteWifiConfiguration; + ros::ServiceServer m_serviceGetAllConfiguredWifis; + ros::ServiceServer m_serviceConnectWifi; + ros::ServiceServer m_serviceDisconnectWifi; + ros::ServiceServer m_serviceGetConnectedWifiInformation; + ros::ServiceServer m_serviceBase_Unsubscribe; + ros::ServiceServer m_serviceOnNotificationConfigurationChangeTopic; + ros::ServiceServer m_serviceOnNotificationMappingInfoTopic; + ros::ServiceServer m_serviceOnNotificationControlModeTopic; + ros::ServiceServer m_serviceOnNotificationOperatingModeTopic; + ros::ServiceServer m_serviceOnNotificationSequenceInfoTopic; + ros::ServiceServer m_serviceOnNotificationProtectionZoneTopic; + ros::ServiceServer m_serviceOnNotificationUserTopic; + ros::ServiceServer m_serviceOnNotificationControllerTopic; + ros::ServiceServer m_serviceOnNotificationActionTopic; + ros::ServiceServer m_serviceOnNotificationRobotEventTopic; + ros::ServiceServer m_servicePlayCartesianTrajectory; + ros::ServiceServer m_servicePlayCartesianTrajectoryPosition; + ros::ServiceServer m_servicePlayCartesianTrajectoryOrientation; + ros::ServiceServer m_serviceStop; + ros::ServiceServer m_serviceGetMeasuredCartesianPose; + ros::ServiceServer m_serviceSendWrenchCommand; + ros::ServiceServer m_serviceSendWrenchJoystickCommand; + ros::ServiceServer m_serviceSendTwistJoystickCommand; + ros::ServiceServer m_serviceSendTwistCommand; + ros::ServiceServer m_servicePlayJointTrajectory; + ros::ServiceServer m_servicePlaySelectedJointTrajectory; + ros::ServiceServer m_serviceGetMeasuredJointAngles; + ros::ServiceServer m_serviceSendJointSpeedsCommand; + ros::ServiceServer m_serviceSendSelectedJointSpeedCommand; + ros::ServiceServer m_serviceSendGripperCommand; + ros::ServiceServer m_serviceGetMeasuredGripperMovement; + ros::ServiceServer m_serviceSetAdmittance; + ros::ServiceServer m_serviceSetOperatingMode; + ros::ServiceServer m_serviceApplyEmergencyStop; + ros::ServiceServer m_serviceBase_ClearFaults; + ros::ServiceServer m_serviceBase_GetControlMode; + ros::ServiceServer m_serviceGetOperatingMode; + ros::ServiceServer m_serviceSetServoingMode; + ros::ServiceServer m_serviceGetServoingMode; + ros::ServiceServer m_serviceOnNotificationServoingModeTopic; + ros::ServiceServer m_serviceRestoreFactorySettings; + ros::ServiceServer m_serviceOnNotificationFactoryTopic; + ros::ServiceServer m_serviceGetAllConnectedControllers; + ros::ServiceServer m_serviceGetControllerState; + ros::ServiceServer m_serviceGetActuatorCount; + ros::ServiceServer m_serviceStartWifiScan; + ros::ServiceServer m_serviceGetConfiguredWifi; + ros::ServiceServer m_serviceOnNotificationNetworkTopic; + ros::ServiceServer m_serviceGetArmState; + ros::ServiceServer m_serviceOnNotificationArmStateTopic; + ros::ServiceServer m_serviceGetIPv4Information; + ros::ServiceServer m_serviceSetWifiCountryCode; + ros::ServiceServer m_serviceGetWifiCountryCode; + ros::ServiceServer m_serviceBase_SetCapSenseConfig; + ros::ServiceServer m_serviceBase_GetCapSenseConfig; + ros::ServiceServer m_serviceGetAllJointsSpeedHardLimitation; + ros::ServiceServer m_serviceGetAllJointsTorqueHardLimitation; + ros::ServiceServer m_serviceGetTwistHardLimitation; + ros::ServiceServer m_serviceGetWrenchHardLimitation; + ros::ServiceServer m_serviceSendJointSpeedsJoystickCommand; + ros::ServiceServer m_serviceSendSelectedJointSpeedJoystickCommand; + ros::ServiceServer m_serviceEnableBridge; + ros::ServiceServer m_serviceDisableBridge; + ros::ServiceServer m_serviceGetBridgeList; + ros::ServiceServer m_serviceGetBridgeConfig; + ros::ServiceServer m_servicePlayPreComputedJointTrajectory; + ros::ServiceServer m_serviceGetProductConfiguration; + ros::ServiceServer m_serviceUpdateEndEffectorTypeConfiguration; + ros::ServiceServer m_serviceRestoreFactoryProductConfiguration; + ros::ServiceServer m_serviceGetTrajectoryErrorReport; + ros::ServiceServer m_serviceGetAllJointsSpeedSoftLimitation; + ros::ServiceServer m_serviceGetAllJointsTorqueSoftLimitation; + ros::ServiceServer m_serviceGetTwistSoftLimitation; + ros::ServiceServer m_serviceGetWrenchSoftLimitation; + ros::ServiceServer m_serviceSetControllerConfigurationMode; + ros::ServiceServer m_serviceGetControllerConfigurationMode; + ros::ServiceServer m_serviceStartTeaching; + ros::ServiceServer m_serviceStopTeaching; + ros::ServiceServer m_serviceAddSequenceTasks; + ros::ServiceServer m_serviceUpdateSequenceTask; + ros::ServiceServer m_serviceSwapSequenceTasks; + ros::ServiceServer m_serviceReadSequenceTask; + ros::ServiceServer m_serviceReadAllSequenceTasks; + ros::ServiceServer m_serviceDeleteSequenceTask; + ros::ServiceServer m_serviceDeleteAllSequenceTasks; + ros::ServiceServer m_serviceTakeSnapshot; + ros::ServiceServer m_serviceGetFirmwareBundleVersions; + ros::ServiceServer m_serviceMoveSequenceTask; + ros::ServiceServer m_serviceDuplicateMapping; + ros::ServiceServer m_serviceDuplicateMap; + ros::ServiceServer m_serviceSetControllerConfiguration; + ros::ServiceServer m_serviceGetControllerConfiguration; + ros::ServiceServer m_serviceGetAllControllerConfigurations; +}; +#endif diff --git a/kortex_driver/include/kortex_driver/generated/interfaces/controlconfig_services_interface.h b/kortex_driver/include/kortex_driver/generated/interfaces/controlconfig_services_interface.h new file mode 100644 index 00000000..6475c3db --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/interfaces/controlconfig_services_interface.h @@ -0,0 +1,142 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_CONTROLCONFIG_SERVICES_INTERFACE_H_ +#define _KORTEX_CONTROLCONFIG_SERVICES_INTERFACE_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include +#include "kortex_driver/SetGravityVector.h" +#include "kortex_driver/GetGravityVector.h" +#include "kortex_driver/SetPayloadInformation.h" +#include "kortex_driver/GetPayloadInformation.h" +#include "kortex_driver/SetToolConfiguration.h" +#include "kortex_driver/GetToolConfiguration.h" +#include "kortex_driver/OnNotificationControlConfigurationTopic.h" +#include "kortex_driver/ControlConfigurationNotification.h" +#include "kortex_driver/ControlConfig_Unsubscribe.h" +#include "kortex_driver/SetCartesianReferenceFrame.h" +#include "kortex_driver/GetCartesianReferenceFrame.h" +#include "kortex_driver/ControlConfig_GetControlMode.h" +#include "kortex_driver/SetJointSpeedSoftLimits.h" +#include "kortex_driver/SetTwistLinearSoftLimit.h" +#include "kortex_driver/SetTwistAngularSoftLimit.h" +#include "kortex_driver/SetJointAccelerationSoftLimits.h" +#include "kortex_driver/GetKinematicHardLimits.h" +#include "kortex_driver/GetKinematicSoftLimits.h" +#include "kortex_driver/GetAllKinematicSoftLimits.h" +#include "kortex_driver/SetDesiredLinearTwist.h" +#include "kortex_driver/SetDesiredAngularTwist.h" +#include "kortex_driver/SetDesiredJointSpeeds.h" +#include "kortex_driver/GetDesiredSpeeds.h" +#include "kortex_driver/ResetGravityVector.h" +#include "kortex_driver/ResetPayloadInformation.h" +#include "kortex_driver/ResetToolConfiguration.h" +#include "kortex_driver/ResetJointSpeedSoftLimits.h" +#include "kortex_driver/ResetTwistLinearSoftLimit.h" +#include "kortex_driver/ResetTwistAngularSoftLimit.h" +#include "kortex_driver/ResetJointAccelerationSoftLimits.h" + +#include "kortex_driver/KortexError.h" +#include "kortex_driver/SetDeviceID.h" +#include "kortex_driver/SetApiOptions.h" +#include "kortex_driver/ApiOptions.h" + +using namespace std; + +class IControlConfigServices +{ + public: + IControlConfigServices(ros::NodeHandle& node_handle) : m_node_handle(node_handle) {} + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) = 0; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) = 0; + virtual bool SetGravityVector(kortex_driver::SetGravityVector::Request &req, kortex_driver::SetGravityVector::Response &res) = 0; + virtual bool GetGravityVector(kortex_driver::GetGravityVector::Request &req, kortex_driver::GetGravityVector::Response &res) = 0; + virtual bool SetPayloadInformation(kortex_driver::SetPayloadInformation::Request &req, kortex_driver::SetPayloadInformation::Response &res) = 0; + virtual bool GetPayloadInformation(kortex_driver::GetPayloadInformation::Request &req, kortex_driver::GetPayloadInformation::Response &res) = 0; + virtual bool SetToolConfiguration(kortex_driver::SetToolConfiguration::Request &req, kortex_driver::SetToolConfiguration::Response &res) = 0; + virtual bool GetToolConfiguration(kortex_driver::GetToolConfiguration::Request &req, kortex_driver::GetToolConfiguration::Response &res) = 0; + virtual bool OnNotificationControlConfigurationTopic(kortex_driver::OnNotificationControlConfigurationTopic::Request &req, kortex_driver::OnNotificationControlConfigurationTopic::Response &res) = 0; + virtual void cb_ControlConfigurationTopic(Kinova::Api::ControlConfig::ControlConfigurationNotification notif) = 0; + virtual bool ControlConfig_Unsubscribe(kortex_driver::ControlConfig_Unsubscribe::Request &req, kortex_driver::ControlConfig_Unsubscribe::Response &res) = 0; + virtual bool SetCartesianReferenceFrame(kortex_driver::SetCartesianReferenceFrame::Request &req, kortex_driver::SetCartesianReferenceFrame::Response &res) = 0; + virtual bool GetCartesianReferenceFrame(kortex_driver::GetCartesianReferenceFrame::Request &req, kortex_driver::GetCartesianReferenceFrame::Response &res) = 0; + virtual bool ControlConfig_GetControlMode(kortex_driver::ControlConfig_GetControlMode::Request &req, kortex_driver::ControlConfig_GetControlMode::Response &res) = 0; + virtual bool SetJointSpeedSoftLimits(kortex_driver::SetJointSpeedSoftLimits::Request &req, kortex_driver::SetJointSpeedSoftLimits::Response &res) = 0; + virtual bool SetTwistLinearSoftLimit(kortex_driver::SetTwistLinearSoftLimit::Request &req, kortex_driver::SetTwistLinearSoftLimit::Response &res) = 0; + virtual bool SetTwistAngularSoftLimit(kortex_driver::SetTwistAngularSoftLimit::Request &req, kortex_driver::SetTwistAngularSoftLimit::Response &res) = 0; + virtual bool SetJointAccelerationSoftLimits(kortex_driver::SetJointAccelerationSoftLimits::Request &req, kortex_driver::SetJointAccelerationSoftLimits::Response &res) = 0; + virtual bool GetKinematicHardLimits(kortex_driver::GetKinematicHardLimits::Request &req, kortex_driver::GetKinematicHardLimits::Response &res) = 0; + virtual bool GetKinematicSoftLimits(kortex_driver::GetKinematicSoftLimits::Request &req, kortex_driver::GetKinematicSoftLimits::Response &res) = 0; + virtual bool GetAllKinematicSoftLimits(kortex_driver::GetAllKinematicSoftLimits::Request &req, kortex_driver::GetAllKinematicSoftLimits::Response &res) = 0; + virtual bool SetDesiredLinearTwist(kortex_driver::SetDesiredLinearTwist::Request &req, kortex_driver::SetDesiredLinearTwist::Response &res) = 0; + virtual bool SetDesiredAngularTwist(kortex_driver::SetDesiredAngularTwist::Request &req, kortex_driver::SetDesiredAngularTwist::Response &res) = 0; + virtual bool SetDesiredJointSpeeds(kortex_driver::SetDesiredJointSpeeds::Request &req, kortex_driver::SetDesiredJointSpeeds::Response &res) = 0; + virtual bool GetDesiredSpeeds(kortex_driver::GetDesiredSpeeds::Request &req, kortex_driver::GetDesiredSpeeds::Response &res) = 0; + virtual bool ResetGravityVector(kortex_driver::ResetGravityVector::Request &req, kortex_driver::ResetGravityVector::Response &res) = 0; + virtual bool ResetPayloadInformation(kortex_driver::ResetPayloadInformation::Request &req, kortex_driver::ResetPayloadInformation::Response &res) = 0; + virtual bool ResetToolConfiguration(kortex_driver::ResetToolConfiguration::Request &req, kortex_driver::ResetToolConfiguration::Response &res) = 0; + virtual bool ResetJointSpeedSoftLimits(kortex_driver::ResetJointSpeedSoftLimits::Request &req, kortex_driver::ResetJointSpeedSoftLimits::Response &res) = 0; + virtual bool ResetTwistLinearSoftLimit(kortex_driver::ResetTwistLinearSoftLimit::Request &req, kortex_driver::ResetTwistLinearSoftLimit::Response &res) = 0; + virtual bool ResetTwistAngularSoftLimit(kortex_driver::ResetTwistAngularSoftLimit::Request &req, kortex_driver::ResetTwistAngularSoftLimit::Response &res) = 0; + virtual bool ResetJointAccelerationSoftLimits(kortex_driver::ResetJointAccelerationSoftLimits::Request &req, kortex_driver::ResetJointAccelerationSoftLimits::Response &res) = 0; + +protected: + ros::NodeHandle m_node_handle; + ros::Publisher m_pub_Error; + ros::Publisher m_pub_ControlConfigurationTopic; + bool m_is_activated_ControlConfigurationTopic; + + ros::ServiceServer m_serviceSetDeviceID; + ros::ServiceServer m_serviceSetApiOptions; + + ros::ServiceServer m_serviceSetGravityVector; + ros::ServiceServer m_serviceGetGravityVector; + ros::ServiceServer m_serviceSetPayloadInformation; + ros::ServiceServer m_serviceGetPayloadInformation; + ros::ServiceServer m_serviceSetToolConfiguration; + ros::ServiceServer m_serviceGetToolConfiguration; + ros::ServiceServer m_serviceOnNotificationControlConfigurationTopic; + ros::ServiceServer m_serviceControlConfig_Unsubscribe; + ros::ServiceServer m_serviceSetCartesianReferenceFrame; + ros::ServiceServer m_serviceGetCartesianReferenceFrame; + ros::ServiceServer m_serviceControlConfig_GetControlMode; + ros::ServiceServer m_serviceSetJointSpeedSoftLimits; + ros::ServiceServer m_serviceSetTwistLinearSoftLimit; + ros::ServiceServer m_serviceSetTwistAngularSoftLimit; + ros::ServiceServer m_serviceSetJointAccelerationSoftLimits; + ros::ServiceServer m_serviceGetKinematicHardLimits; + ros::ServiceServer m_serviceGetKinematicSoftLimits; + ros::ServiceServer m_serviceGetAllKinematicSoftLimits; + ros::ServiceServer m_serviceSetDesiredLinearTwist; + ros::ServiceServer m_serviceSetDesiredAngularTwist; + ros::ServiceServer m_serviceSetDesiredJointSpeeds; + ros::ServiceServer m_serviceGetDesiredSpeeds; + ros::ServiceServer m_serviceResetGravityVector; + ros::ServiceServer m_serviceResetPayloadInformation; + ros::ServiceServer m_serviceResetToolConfiguration; + ros::ServiceServer m_serviceResetJointSpeedSoftLimits; + ros::ServiceServer m_serviceResetTwistLinearSoftLimit; + ros::ServiceServer m_serviceResetTwistAngularSoftLimit; + ros::ServiceServer m_serviceResetJointAccelerationSoftLimits; +}; +#endif diff --git a/kortex_driver/include/kortex_driver/generated/interfaces/deviceconfig_services_interface.h b/kortex_driver/include/kortex_driver/generated/interfaces/deviceconfig_services_interface.h new file mode 100644 index 00000000..4f4b7d1e --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/interfaces/deviceconfig_services_interface.h @@ -0,0 +1,151 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_DEVICECONFIG_SERVICES_INTERFACE_H_ +#define _KORTEX_DEVICECONFIG_SERVICES_INTERFACE_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include +#include "kortex_driver/GetRunMode.h" +#include "kortex_driver/SetRunMode.h" +#include "kortex_driver/GetDeviceType.h" +#include "kortex_driver/GetFirmwareVersion.h" +#include "kortex_driver/GetBootloaderVersion.h" +#include "kortex_driver/GetModelNumber.h" +#include "kortex_driver/GetPartNumber.h" +#include "kortex_driver/GetSerialNumber.h" +#include "kortex_driver/GetMACAddress.h" +#include "kortex_driver/GetIPv4Settings.h" +#include "kortex_driver/SetIPv4Settings.h" +#include "kortex_driver/GetPartNumberRevision.h" +#include "kortex_driver/RebootRequest.h" +#include "kortex_driver/SetSafetyEnable.h" +#include "kortex_driver/SetSafetyErrorThreshold.h" +#include "kortex_driver/SetSafetyWarningThreshold.h" +#include "kortex_driver/SetSafetyConfiguration.h" +#include "kortex_driver/GetSafetyConfiguration.h" +#include "kortex_driver/GetSafetyInformation.h" +#include "kortex_driver/GetSafetyEnable.h" +#include "kortex_driver/GetSafetyStatus.h" +#include "kortex_driver/ClearAllSafetyStatus.h" +#include "kortex_driver/ClearSafetyStatus.h" +#include "kortex_driver/GetAllSafetyConfiguration.h" +#include "kortex_driver/GetAllSafetyInformation.h" +#include "kortex_driver/ResetSafetyDefaults.h" +#include "kortex_driver/OnNotificationSafetyTopic.h" +#include "kortex_driver/SafetyNotification.h" +#include "kortex_driver/ExecuteCalibration.h" +#include "kortex_driver/GetCalibrationResult.h" +#include "kortex_driver/StopCalibration.h" +#include "kortex_driver/DeviceConfig_SetCapSenseConfig.h" +#include "kortex_driver/DeviceConfig_GetCapSenseConfig.h" + +#include "kortex_driver/KortexError.h" +#include "kortex_driver/SetDeviceID.h" +#include "kortex_driver/SetApiOptions.h" +#include "kortex_driver/ApiOptions.h" + +using namespace std; + +class IDeviceConfigServices +{ + public: + IDeviceConfigServices(ros::NodeHandle& node_handle) : m_node_handle(node_handle) {} + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) = 0; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) = 0; + virtual bool GetRunMode(kortex_driver::GetRunMode::Request &req, kortex_driver::GetRunMode::Response &res) = 0; + virtual bool SetRunMode(kortex_driver::SetRunMode::Request &req, kortex_driver::SetRunMode::Response &res) = 0; + virtual bool GetDeviceType(kortex_driver::GetDeviceType::Request &req, kortex_driver::GetDeviceType::Response &res) = 0; + virtual bool GetFirmwareVersion(kortex_driver::GetFirmwareVersion::Request &req, kortex_driver::GetFirmwareVersion::Response &res) = 0; + virtual bool GetBootloaderVersion(kortex_driver::GetBootloaderVersion::Request &req, kortex_driver::GetBootloaderVersion::Response &res) = 0; + virtual bool GetModelNumber(kortex_driver::GetModelNumber::Request &req, kortex_driver::GetModelNumber::Response &res) = 0; + virtual bool GetPartNumber(kortex_driver::GetPartNumber::Request &req, kortex_driver::GetPartNumber::Response &res) = 0; + virtual bool GetSerialNumber(kortex_driver::GetSerialNumber::Request &req, kortex_driver::GetSerialNumber::Response &res) = 0; + virtual bool GetMACAddress(kortex_driver::GetMACAddress::Request &req, kortex_driver::GetMACAddress::Response &res) = 0; + virtual bool GetIPv4Settings(kortex_driver::GetIPv4Settings::Request &req, kortex_driver::GetIPv4Settings::Response &res) = 0; + virtual bool SetIPv4Settings(kortex_driver::SetIPv4Settings::Request &req, kortex_driver::SetIPv4Settings::Response &res) = 0; + virtual bool GetPartNumberRevision(kortex_driver::GetPartNumberRevision::Request &req, kortex_driver::GetPartNumberRevision::Response &res) = 0; + virtual bool RebootRequest(kortex_driver::RebootRequest::Request &req, kortex_driver::RebootRequest::Response &res) = 0; + virtual bool SetSafetyEnable(kortex_driver::SetSafetyEnable::Request &req, kortex_driver::SetSafetyEnable::Response &res) = 0; + virtual bool SetSafetyErrorThreshold(kortex_driver::SetSafetyErrorThreshold::Request &req, kortex_driver::SetSafetyErrorThreshold::Response &res) = 0; + virtual bool SetSafetyWarningThreshold(kortex_driver::SetSafetyWarningThreshold::Request &req, kortex_driver::SetSafetyWarningThreshold::Response &res) = 0; + virtual bool SetSafetyConfiguration(kortex_driver::SetSafetyConfiguration::Request &req, kortex_driver::SetSafetyConfiguration::Response &res) = 0; + virtual bool GetSafetyConfiguration(kortex_driver::GetSafetyConfiguration::Request &req, kortex_driver::GetSafetyConfiguration::Response &res) = 0; + virtual bool GetSafetyInformation(kortex_driver::GetSafetyInformation::Request &req, kortex_driver::GetSafetyInformation::Response &res) = 0; + virtual bool GetSafetyEnable(kortex_driver::GetSafetyEnable::Request &req, kortex_driver::GetSafetyEnable::Response &res) = 0; + virtual bool GetSafetyStatus(kortex_driver::GetSafetyStatus::Request &req, kortex_driver::GetSafetyStatus::Response &res) = 0; + virtual bool ClearAllSafetyStatus(kortex_driver::ClearAllSafetyStatus::Request &req, kortex_driver::ClearAllSafetyStatus::Response &res) = 0; + virtual bool ClearSafetyStatus(kortex_driver::ClearSafetyStatus::Request &req, kortex_driver::ClearSafetyStatus::Response &res) = 0; + virtual bool GetAllSafetyConfiguration(kortex_driver::GetAllSafetyConfiguration::Request &req, kortex_driver::GetAllSafetyConfiguration::Response &res) = 0; + virtual bool GetAllSafetyInformation(kortex_driver::GetAllSafetyInformation::Request &req, kortex_driver::GetAllSafetyInformation::Response &res) = 0; + virtual bool ResetSafetyDefaults(kortex_driver::ResetSafetyDefaults::Request &req, kortex_driver::ResetSafetyDefaults::Response &res) = 0; + virtual bool OnNotificationSafetyTopic(kortex_driver::OnNotificationSafetyTopic::Request &req, kortex_driver::OnNotificationSafetyTopic::Response &res) = 0; + virtual void cb_SafetyTopic(Kinova::Api::Common::SafetyNotification notif) = 0; + virtual bool ExecuteCalibration(kortex_driver::ExecuteCalibration::Request &req, kortex_driver::ExecuteCalibration::Response &res) = 0; + virtual bool GetCalibrationResult(kortex_driver::GetCalibrationResult::Request &req, kortex_driver::GetCalibrationResult::Response &res) = 0; + virtual bool StopCalibration(kortex_driver::StopCalibration::Request &req, kortex_driver::StopCalibration::Response &res) = 0; + virtual bool DeviceConfig_SetCapSenseConfig(kortex_driver::DeviceConfig_SetCapSenseConfig::Request &req, kortex_driver::DeviceConfig_SetCapSenseConfig::Response &res) = 0; + virtual bool DeviceConfig_GetCapSenseConfig(kortex_driver::DeviceConfig_GetCapSenseConfig::Request &req, kortex_driver::DeviceConfig_GetCapSenseConfig::Response &res) = 0; + +protected: + ros::NodeHandle m_node_handle; + ros::Publisher m_pub_Error; + ros::Publisher m_pub_SafetyTopic; + bool m_is_activated_SafetyTopic; + + ros::ServiceServer m_serviceSetDeviceID; + ros::ServiceServer m_serviceSetApiOptions; + + ros::ServiceServer m_serviceGetRunMode; + ros::ServiceServer m_serviceSetRunMode; + ros::ServiceServer m_serviceGetDeviceType; + ros::ServiceServer m_serviceGetFirmwareVersion; + ros::ServiceServer m_serviceGetBootloaderVersion; + ros::ServiceServer m_serviceGetModelNumber; + ros::ServiceServer m_serviceGetPartNumber; + ros::ServiceServer m_serviceGetSerialNumber; + ros::ServiceServer m_serviceGetMACAddress; + ros::ServiceServer m_serviceGetIPv4Settings; + ros::ServiceServer m_serviceSetIPv4Settings; + ros::ServiceServer m_serviceGetPartNumberRevision; + ros::ServiceServer m_serviceRebootRequest; + ros::ServiceServer m_serviceSetSafetyEnable; + ros::ServiceServer m_serviceSetSafetyErrorThreshold; + ros::ServiceServer m_serviceSetSafetyWarningThreshold; + ros::ServiceServer m_serviceSetSafetyConfiguration; + ros::ServiceServer m_serviceGetSafetyConfiguration; + ros::ServiceServer m_serviceGetSafetyInformation; + ros::ServiceServer m_serviceGetSafetyEnable; + ros::ServiceServer m_serviceGetSafetyStatus; + ros::ServiceServer m_serviceClearAllSafetyStatus; + ros::ServiceServer m_serviceClearSafetyStatus; + ros::ServiceServer m_serviceGetAllSafetyConfiguration; + ros::ServiceServer m_serviceGetAllSafetyInformation; + ros::ServiceServer m_serviceResetSafetyDefaults; + ros::ServiceServer m_serviceOnNotificationSafetyTopic; + ros::ServiceServer m_serviceExecuteCalibration; + ros::ServiceServer m_serviceGetCalibrationResult; + ros::ServiceServer m_serviceStopCalibration; + ros::ServiceServer m_serviceDeviceConfig_SetCapSenseConfig; + ros::ServiceServer m_serviceDeviceConfig_GetCapSenseConfig; +}; +#endif diff --git a/kortex_driver/include/kortex_driver/generated/interfaces/devicemanager_services_interface.h b/kortex_driver/include/kortex_driver/generated/interfaces/devicemanager_services_interface.h new file mode 100644 index 00000000..a2e544ed --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/interfaces/devicemanager_services_interface.h @@ -0,0 +1,54 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_DEVICEMANAGER_SERVICES_INTERFACE_H_ +#define _KORTEX_DEVICEMANAGER_SERVICES_INTERFACE_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include +#include "kortex_driver/ReadAllDevices.h" + +#include "kortex_driver/KortexError.h" +#include "kortex_driver/SetDeviceID.h" +#include "kortex_driver/SetApiOptions.h" +#include "kortex_driver/ApiOptions.h" + +using namespace std; + +class IDeviceManagerServices +{ + public: + IDeviceManagerServices(ros::NodeHandle& node_handle) : m_node_handle(node_handle) {} + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) = 0; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) = 0; + virtual bool ReadAllDevices(kortex_driver::ReadAllDevices::Request &req, kortex_driver::ReadAllDevices::Response &res) = 0; + +protected: + ros::NodeHandle m_node_handle; + ros::Publisher m_pub_Error; + + ros::ServiceServer m_serviceSetDeviceID; + ros::ServiceServer m_serviceSetApiOptions; + + ros::ServiceServer m_serviceReadAllDevices; +}; +#endif diff --git a/kortex_driver/include/kortex_driver/generated/interfaces/interconnectconfig_services_interface.h b/kortex_driver/include/kortex_driver/generated/interfaces/interconnectconfig_services_interface.h new file mode 100644 index 00000000..dc370d10 --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/interfaces/interconnectconfig_services_interface.h @@ -0,0 +1,93 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_INTERCONNECTCONFIG_SERVICES_INTERFACE_H_ +#define _KORTEX_INTERCONNECTCONFIG_SERVICES_INTERFACE_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include +#include "kortex_driver/GetUARTConfiguration.h" +#include "kortex_driver/SetUARTConfiguration.h" +#include "kortex_driver/GetEthernetConfiguration.h" +#include "kortex_driver/SetEthernetConfiguration.h" +#include "kortex_driver/GetGPIOConfiguration.h" +#include "kortex_driver/SetGPIOConfiguration.h" +#include "kortex_driver/GetGPIOState.h" +#include "kortex_driver/SetGPIOState.h" +#include "kortex_driver/GetI2CConfiguration.h" +#include "kortex_driver/SetI2CConfiguration.h" +#include "kortex_driver/I2CRead.h" +#include "kortex_driver/I2CReadRegister.h" +#include "kortex_driver/I2CWrite.h" +#include "kortex_driver/I2CWriteRegister.h" + +#include "kortex_driver/KortexError.h" +#include "kortex_driver/SetDeviceID.h" +#include "kortex_driver/SetApiOptions.h" +#include "kortex_driver/ApiOptions.h" + +using namespace std; + +class IInterconnectConfigServices +{ + public: + IInterconnectConfigServices(ros::NodeHandle& node_handle) : m_node_handle(node_handle) {} + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) = 0; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) = 0; + virtual bool GetUARTConfiguration(kortex_driver::GetUARTConfiguration::Request &req, kortex_driver::GetUARTConfiguration::Response &res) = 0; + virtual bool SetUARTConfiguration(kortex_driver::SetUARTConfiguration::Request &req, kortex_driver::SetUARTConfiguration::Response &res) = 0; + virtual bool GetEthernetConfiguration(kortex_driver::GetEthernetConfiguration::Request &req, kortex_driver::GetEthernetConfiguration::Response &res) = 0; + virtual bool SetEthernetConfiguration(kortex_driver::SetEthernetConfiguration::Request &req, kortex_driver::SetEthernetConfiguration::Response &res) = 0; + virtual bool GetGPIOConfiguration(kortex_driver::GetGPIOConfiguration::Request &req, kortex_driver::GetGPIOConfiguration::Response &res) = 0; + virtual bool SetGPIOConfiguration(kortex_driver::SetGPIOConfiguration::Request &req, kortex_driver::SetGPIOConfiguration::Response &res) = 0; + virtual bool GetGPIOState(kortex_driver::GetGPIOState::Request &req, kortex_driver::GetGPIOState::Response &res) = 0; + virtual bool SetGPIOState(kortex_driver::SetGPIOState::Request &req, kortex_driver::SetGPIOState::Response &res) = 0; + virtual bool GetI2CConfiguration(kortex_driver::GetI2CConfiguration::Request &req, kortex_driver::GetI2CConfiguration::Response &res) = 0; + virtual bool SetI2CConfiguration(kortex_driver::SetI2CConfiguration::Request &req, kortex_driver::SetI2CConfiguration::Response &res) = 0; + virtual bool I2CRead(kortex_driver::I2CRead::Request &req, kortex_driver::I2CRead::Response &res) = 0; + virtual bool I2CReadRegister(kortex_driver::I2CReadRegister::Request &req, kortex_driver::I2CReadRegister::Response &res) = 0; + virtual bool I2CWrite(kortex_driver::I2CWrite::Request &req, kortex_driver::I2CWrite::Response &res) = 0; + virtual bool I2CWriteRegister(kortex_driver::I2CWriteRegister::Request &req, kortex_driver::I2CWriteRegister::Response &res) = 0; + +protected: + ros::NodeHandle m_node_handle; + ros::Publisher m_pub_Error; + + ros::ServiceServer m_serviceSetDeviceID; + ros::ServiceServer m_serviceSetApiOptions; + + ros::ServiceServer m_serviceGetUARTConfiguration; + ros::ServiceServer m_serviceSetUARTConfiguration; + ros::ServiceServer m_serviceGetEthernetConfiguration; + ros::ServiceServer m_serviceSetEthernetConfiguration; + ros::ServiceServer m_serviceGetGPIOConfiguration; + ros::ServiceServer m_serviceSetGPIOConfiguration; + ros::ServiceServer m_serviceGetGPIOState; + ros::ServiceServer m_serviceSetGPIOState; + ros::ServiceServer m_serviceGetI2CConfiguration; + ros::ServiceServer m_serviceSetI2CConfiguration; + ros::ServiceServer m_serviceI2CRead; + ros::ServiceServer m_serviceI2CReadRegister; + ros::ServiceServer m_serviceI2CWrite; + ros::ServiceServer m_serviceI2CWriteRegister; +}; +#endif diff --git a/kortex_driver/include/kortex_driver/generated/interfaces/visionconfig_services_interface.h b/kortex_driver/include/kortex_driver/generated/interfaces/visionconfig_services_interface.h new file mode 100644 index 00000000..1bf98c7d --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/interfaces/visionconfig_services_interface.h @@ -0,0 +1,91 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_VISIONCONFIG_SERVICES_INTERFACE_H_ +#define _KORTEX_VISIONCONFIG_SERVICES_INTERFACE_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include +#include "kortex_driver/SetSensorSettings.h" +#include "kortex_driver/GetSensorSettings.h" +#include "kortex_driver/GetOptionValue.h" +#include "kortex_driver/SetOptionValue.h" +#include "kortex_driver/GetOptionInformation.h" +#include "kortex_driver/OnNotificationVisionTopic.h" +#include "kortex_driver/VisionNotification.h" +#include "kortex_driver/DoSensorFocusAction.h" +#include "kortex_driver/GetIntrinsicParameters.h" +#include "kortex_driver/GetIntrinsicParametersProfile.h" +#include "kortex_driver/SetIntrinsicParameters.h" +#include "kortex_driver/GetExtrinsicParameters.h" +#include "kortex_driver/SetExtrinsicParameters.h" + +#include "kortex_driver/KortexError.h" +#include "kortex_driver/SetDeviceID.h" +#include "kortex_driver/SetApiOptions.h" +#include "kortex_driver/ApiOptions.h" + +using namespace std; + +class IVisionConfigServices +{ + public: + IVisionConfigServices(ros::NodeHandle& node_handle) : m_node_handle(node_handle) {} + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) = 0; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) = 0; + virtual bool SetSensorSettings(kortex_driver::SetSensorSettings::Request &req, kortex_driver::SetSensorSettings::Response &res) = 0; + virtual bool GetSensorSettings(kortex_driver::GetSensorSettings::Request &req, kortex_driver::GetSensorSettings::Response &res) = 0; + virtual bool GetOptionValue(kortex_driver::GetOptionValue::Request &req, kortex_driver::GetOptionValue::Response &res) = 0; + virtual bool SetOptionValue(kortex_driver::SetOptionValue::Request &req, kortex_driver::SetOptionValue::Response &res) = 0; + virtual bool GetOptionInformation(kortex_driver::GetOptionInformation::Request &req, kortex_driver::GetOptionInformation::Response &res) = 0; + virtual bool OnNotificationVisionTopic(kortex_driver::OnNotificationVisionTopic::Request &req, kortex_driver::OnNotificationVisionTopic::Response &res) = 0; + virtual void cb_VisionTopic(Kinova::Api::VisionConfig::VisionNotification notif) = 0; + virtual bool DoSensorFocusAction(kortex_driver::DoSensorFocusAction::Request &req, kortex_driver::DoSensorFocusAction::Response &res) = 0; + virtual bool GetIntrinsicParameters(kortex_driver::GetIntrinsicParameters::Request &req, kortex_driver::GetIntrinsicParameters::Response &res) = 0; + virtual bool GetIntrinsicParametersProfile(kortex_driver::GetIntrinsicParametersProfile::Request &req, kortex_driver::GetIntrinsicParametersProfile::Response &res) = 0; + virtual bool SetIntrinsicParameters(kortex_driver::SetIntrinsicParameters::Request &req, kortex_driver::SetIntrinsicParameters::Response &res) = 0; + virtual bool GetExtrinsicParameters(kortex_driver::GetExtrinsicParameters::Request &req, kortex_driver::GetExtrinsicParameters::Response &res) = 0; + virtual bool SetExtrinsicParameters(kortex_driver::SetExtrinsicParameters::Request &req, kortex_driver::SetExtrinsicParameters::Response &res) = 0; + +protected: + ros::NodeHandle m_node_handle; + ros::Publisher m_pub_Error; + ros::Publisher m_pub_VisionTopic; + bool m_is_activated_VisionTopic; + + ros::ServiceServer m_serviceSetDeviceID; + ros::ServiceServer m_serviceSetApiOptions; + + ros::ServiceServer m_serviceSetSensorSettings; + ros::ServiceServer m_serviceGetSensorSettings; + ros::ServiceServer m_serviceGetOptionValue; + ros::ServiceServer m_serviceSetOptionValue; + ros::ServiceServer m_serviceGetOptionInformation; + ros::ServiceServer m_serviceOnNotificationVisionTopic; + ros::ServiceServer m_serviceDoSensorFocusAction; + ros::ServiceServer m_serviceGetIntrinsicParameters; + ros::ServiceServer m_serviceGetIntrinsicParametersProfile; + ros::ServiceServer m_serviceSetIntrinsicParameters; + ros::ServiceServer m_serviceGetExtrinsicParameters; + ros::ServiceServer m_serviceSetExtrinsicParameters; +}; +#endif diff --git a/kortex_driver/include/kortex_driver/generated/actuatorconfig_proto_converter.h b/kortex_driver/include/kortex_driver/generated/robot/actuatorconfig_proto_converter.h similarity index 80% rename from kortex_driver/include/kortex_driver/generated/actuatorconfig_proto_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/actuatorconfig_proto_converter.h index b5dd37e9..e91312bc 100644 --- a/kortex_driver/include/kortex_driver/generated/actuatorconfig_proto_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/actuatorconfig_proto_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_proto_converter.h" -#include "kortex_driver/generated/actuatorcyclic_proto_converter.h" -#include "kortex_driver/generated/productconfiguration_proto_converter.h" -#include "kortex_driver/generated/base_proto_converter.h" -#include "kortex_driver/generated/grippercyclic_proto_converter.h" -#include "kortex_driver/generated/interconnectcyclic_proto_converter.h" -#include "kortex_driver/generated/basecyclic_proto_converter.h" -#include "kortex_driver/generated/controlconfig_proto_converter.h" -#include "kortex_driver/generated/deviceconfig_proto_converter.h" -#include "kortex_driver/generated/devicemanager_proto_converter.h" -#include "kortex_driver/generated/interconnectconfig_proto_converter.h" -#include "kortex_driver/generated/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" #include "kortex_driver/AxisPosition.h" diff --git a/kortex_driver/include/kortex_driver/generated/actuatorconfig_ros_converter.h b/kortex_driver/include/kortex_driver/generated/robot/actuatorconfig_ros_converter.h similarity index 80% rename from kortex_driver/include/kortex_driver/generated/actuatorconfig_ros_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/actuatorconfig_ros_converter.h index 3412b504..024e75af 100644 --- a/kortex_driver/include/kortex_driver/generated/actuatorconfig_ros_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/actuatorconfig_ros_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_ros_converter.h" -#include "kortex_driver/generated/actuatorcyclic_ros_converter.h" -#include "kortex_driver/generated/productconfiguration_ros_converter.h" -#include "kortex_driver/generated/base_ros_converter.h" -#include "kortex_driver/generated/grippercyclic_ros_converter.h" -#include "kortex_driver/generated/interconnectcyclic_ros_converter.h" -#include "kortex_driver/generated/basecyclic_ros_converter.h" -#include "kortex_driver/generated/controlconfig_ros_converter.h" -#include "kortex_driver/generated/deviceconfig_ros_converter.h" -#include "kortex_driver/generated/devicemanager_ros_converter.h" -#include "kortex_driver/generated/interconnectconfig_ros_converter.h" -#include "kortex_driver/generated/visionconfig_ros_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" #include "kortex_driver/AxisPosition.h" diff --git a/kortex_driver/include/kortex_driver/generated/robot/actuatorconfig_services.h b/kortex_driver/include/kortex_driver/generated/robot/actuatorconfig_services.h new file mode 100644 index 00000000..0a7adacf --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/robot/actuatorconfig_services.h @@ -0,0 +1,61 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_ACTUATORCONFIG_ROBOT_SERVICES_H_ +#define _KORTEX_ACTUATORCONFIG_ROBOT_SERVICES_H_ + +#include "kortex_driver/generated/interfaces/actuatorconfig_services_interface.h" + +#include +#include + +using namespace std; + +class ActuatorConfigRobotServices : public IActuatorConfigServices +{ + public: + ActuatorConfigRobotServices(ros::NodeHandle& node_handle, Kinova::Api::ActuatorConfig::ActuatorConfigClient* actuatorconfig, uint32_t device_id, uint32_t timeout_ms); + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; + virtual bool GetAxisOffsets(kortex_driver::GetAxisOffsets::Request &req, kortex_driver::GetAxisOffsets::Response &res) override; + virtual bool SetAxisOffsets(kortex_driver::SetAxisOffsets::Request &req, kortex_driver::SetAxisOffsets::Response &res) override; + virtual bool SetTorqueOffset(kortex_driver::SetTorqueOffset::Request &req, kortex_driver::SetTorqueOffset::Response &res) override; + virtual bool ActuatorConfig_GetControlMode(kortex_driver::ActuatorConfig_GetControlMode::Request &req, kortex_driver::ActuatorConfig_GetControlMode::Response &res) override; + virtual bool SetControlMode(kortex_driver::SetControlMode::Request &req, kortex_driver::SetControlMode::Response &res) override; + virtual bool GetActivatedControlLoop(kortex_driver::GetActivatedControlLoop::Request &req, kortex_driver::GetActivatedControlLoop::Response &res) override; + virtual bool SetActivatedControlLoop(kortex_driver::SetActivatedControlLoop::Request &req, kortex_driver::SetActivatedControlLoop::Response &res) override; + virtual bool GetControlLoopParameters(kortex_driver::GetControlLoopParameters::Request &req, kortex_driver::GetControlLoopParameters::Response &res) override; + virtual bool SetControlLoopParameters(kortex_driver::SetControlLoopParameters::Request &req, kortex_driver::SetControlLoopParameters::Response &res) override; + virtual bool SelectCustomData(kortex_driver::SelectCustomData::Request &req, kortex_driver::SelectCustomData::Response &res) override; + virtual bool GetSelectedCustomData(kortex_driver::GetSelectedCustomData::Request &req, kortex_driver::GetSelectedCustomData::Response &res) override; + virtual bool SetCommandMode(kortex_driver::SetCommandMode::Request &req, kortex_driver::SetCommandMode::Response &res) override; + virtual bool ActuatorConfig_ClearFaults(kortex_driver::ActuatorConfig_ClearFaults::Request &req, kortex_driver::ActuatorConfig_ClearFaults::Response &res) override; + virtual bool SetServoing(kortex_driver::SetServoing::Request &req, kortex_driver::SetServoing::Response &res) override; + virtual bool MoveToPosition(kortex_driver::MoveToPosition::Request &req, kortex_driver::MoveToPosition::Response &res) override; + virtual bool GetCommandMode(kortex_driver::GetCommandMode::Request &req, kortex_driver::GetCommandMode::Response &res) override; + virtual bool GetServoing(kortex_driver::GetServoing::Request &req, kortex_driver::GetServoing::Response &res) override; + virtual bool GetTorqueOffset(kortex_driver::GetTorqueOffset::Request &req, kortex_driver::GetTorqueOffset::Response &res) override; + virtual bool SetCoggingFeedforwardMode(kortex_driver::SetCoggingFeedforwardMode::Request &req, kortex_driver::SetCoggingFeedforwardMode::Response &res) override; + virtual bool GetCoggingFeedforwardMode(kortex_driver::GetCoggingFeedforwardMode::Request &req, kortex_driver::GetCoggingFeedforwardMode::Response &res) override; + +private: + uint32_t m_current_device_id; + Kinova::Api::RouterClientSendOptions m_api_options; + + Kinova::Api::ActuatorConfig::ActuatorConfigClient* m_actuatorconfig; +}; +#endif diff --git a/kortex_driver/include/kortex_driver/generated/actuatorcyclic_proto_converter.h b/kortex_driver/include/kortex_driver/generated/robot/actuatorcyclic_proto_converter.h similarity index 59% rename from kortex_driver/include/kortex_driver/generated/actuatorcyclic_proto_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/actuatorcyclic_proto_converter.h index cff91c67..f463e1bc 100644 --- a/kortex_driver/include/kortex_driver/generated/actuatorcyclic_proto_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/actuatorcyclic_proto_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_proto_converter.h" -#include "kortex_driver/generated/actuatorconfig_proto_converter.h" -#include "kortex_driver/generated/productconfiguration_proto_converter.h" -#include "kortex_driver/generated/base_proto_converter.h" -#include "kortex_driver/generated/grippercyclic_proto_converter.h" -#include "kortex_driver/generated/interconnectcyclic_proto_converter.h" -#include "kortex_driver/generated/basecyclic_proto_converter.h" -#include "kortex_driver/generated/controlconfig_proto_converter.h" -#include "kortex_driver/generated/deviceconfig_proto_converter.h" -#include "kortex_driver/generated/devicemanager_proto_converter.h" -#include "kortex_driver/generated/interconnectconfig_proto_converter.h" -#include "kortex_driver/generated/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" #include "kortex_driver/ActuatorCyclic_MessageId.h" diff --git a/kortex_driver/include/kortex_driver/generated/actuatorcyclic_ros_converter.h b/kortex_driver/include/kortex_driver/generated/robot/actuatorcyclic_ros_converter.h similarity index 59% rename from kortex_driver/include/kortex_driver/generated/actuatorcyclic_ros_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/actuatorcyclic_ros_converter.h index 62be5681..02085e5b 100644 --- a/kortex_driver/include/kortex_driver/generated/actuatorcyclic_ros_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/actuatorcyclic_ros_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_ros_converter.h" -#include "kortex_driver/generated/actuatorconfig_ros_converter.h" -#include "kortex_driver/generated/productconfiguration_ros_converter.h" -#include "kortex_driver/generated/base_ros_converter.h" -#include "kortex_driver/generated/grippercyclic_ros_converter.h" -#include "kortex_driver/generated/interconnectcyclic_ros_converter.h" -#include "kortex_driver/generated/basecyclic_ros_converter.h" -#include "kortex_driver/generated/controlconfig_ros_converter.h" -#include "kortex_driver/generated/deviceconfig_ros_converter.h" -#include "kortex_driver/generated/devicemanager_ros_converter.h" -#include "kortex_driver/generated/interconnectconfig_ros_converter.h" -#include "kortex_driver/generated/visionconfig_ros_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" #include "kortex_driver/ActuatorCyclic_MessageId.h" diff --git a/kortex_driver/include/kortex_driver/generated/base_proto_converter.h b/kortex_driver/include/kortex_driver/generated/robot/base_proto_converter.h similarity index 96% rename from kortex_driver/include/kortex_driver/generated/base_proto_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/base_proto_converter.h index 52a4a7a5..23cc90cf 100644 --- a/kortex_driver/include/kortex_driver/generated/base_proto_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/base_proto_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_proto_converter.h" -#include "kortex_driver/generated/actuatorconfig_proto_converter.h" -#include "kortex_driver/generated/actuatorcyclic_proto_converter.h" -#include "kortex_driver/generated/productconfiguration_proto_converter.h" -#include "kortex_driver/generated/grippercyclic_proto_converter.h" -#include "kortex_driver/generated/interconnectcyclic_proto_converter.h" -#include "kortex_driver/generated/basecyclic_proto_converter.h" -#include "kortex_driver/generated/controlconfig_proto_converter.h" -#include "kortex_driver/generated/deviceconfig_proto_converter.h" -#include "kortex_driver/generated/devicemanager_proto_converter.h" -#include "kortex_driver/generated/interconnectconfig_proto_converter.h" -#include "kortex_driver/generated/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" #include "kortex_driver/FullUserProfile.h" diff --git a/kortex_driver/include/kortex_driver/generated/base_ros_converter.h b/kortex_driver/include/kortex_driver/generated/robot/base_ros_converter.h similarity index 96% rename from kortex_driver/include/kortex_driver/generated/base_ros_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/base_ros_converter.h index c05cd620..0cb57ed1 100644 --- a/kortex_driver/include/kortex_driver/generated/base_ros_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/base_ros_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_ros_converter.h" -#include "kortex_driver/generated/actuatorconfig_ros_converter.h" -#include "kortex_driver/generated/actuatorcyclic_ros_converter.h" -#include "kortex_driver/generated/productconfiguration_ros_converter.h" -#include "kortex_driver/generated/grippercyclic_ros_converter.h" -#include "kortex_driver/generated/interconnectcyclic_ros_converter.h" -#include "kortex_driver/generated/basecyclic_ros_converter.h" -#include "kortex_driver/generated/controlconfig_ros_converter.h" -#include "kortex_driver/generated/deviceconfig_ros_converter.h" -#include "kortex_driver/generated/devicemanager_ros_converter.h" -#include "kortex_driver/generated/interconnectconfig_ros_converter.h" -#include "kortex_driver/generated/visionconfig_ros_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" #include "kortex_driver/FullUserProfile.h" diff --git a/kortex_driver/include/kortex_driver/generated/robot/base_services.h b/kortex_driver/include/kortex_driver/generated/robot/base_services.h new file mode 100644 index 00000000..27c48320 --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/robot/base_services.h @@ -0,0 +1,199 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_BASE_ROBOT_SERVICES_H_ +#define _KORTEX_BASE_ROBOT_SERVICES_H_ + +#include "kortex_driver/generated/interfaces/base_services_interface.h" + +#include +#include + +using namespace std; + +class BaseRobotServices : public IBaseServices +{ + public: + BaseRobotServices(ros::NodeHandle& node_handle, Kinova::Api::Base::BaseClient* base, uint32_t device_id, uint32_t timeout_ms); + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; + virtual bool CreateUserProfile(kortex_driver::CreateUserProfile::Request &req, kortex_driver::CreateUserProfile::Response &res) override; + virtual bool UpdateUserProfile(kortex_driver::UpdateUserProfile::Request &req, kortex_driver::UpdateUserProfile::Response &res) override; + virtual bool ReadUserProfile(kortex_driver::ReadUserProfile::Request &req, kortex_driver::ReadUserProfile::Response &res) override; + virtual bool DeleteUserProfile(kortex_driver::DeleteUserProfile::Request &req, kortex_driver::DeleteUserProfile::Response &res) override; + virtual bool ReadAllUserProfiles(kortex_driver::ReadAllUserProfiles::Request &req, kortex_driver::ReadAllUserProfiles::Response &res) override; + virtual bool ReadAllUsers(kortex_driver::ReadAllUsers::Request &req, kortex_driver::ReadAllUsers::Response &res) override; + virtual bool ChangePassword(kortex_driver::ChangePassword::Request &req, kortex_driver::ChangePassword::Response &res) override; + virtual bool CreateSequence(kortex_driver::CreateSequence::Request &req, kortex_driver::CreateSequence::Response &res) override; + virtual bool UpdateSequence(kortex_driver::UpdateSequence::Request &req, kortex_driver::UpdateSequence::Response &res) override; + virtual bool ReadSequence(kortex_driver::ReadSequence::Request &req, kortex_driver::ReadSequence::Response &res) override; + virtual bool DeleteSequence(kortex_driver::DeleteSequence::Request &req, kortex_driver::DeleteSequence::Response &res) override; + virtual bool ReadAllSequences(kortex_driver::ReadAllSequences::Request &req, kortex_driver::ReadAllSequences::Response &res) override; + virtual bool PlaySequence(kortex_driver::PlaySequence::Request &req, kortex_driver::PlaySequence::Response &res) override; + virtual bool PlayAdvancedSequence(kortex_driver::PlayAdvancedSequence::Request &req, kortex_driver::PlayAdvancedSequence::Response &res) override; + virtual bool StopSequence(kortex_driver::StopSequence::Request &req, kortex_driver::StopSequence::Response &res) override; + virtual bool PauseSequence(kortex_driver::PauseSequence::Request &req, kortex_driver::PauseSequence::Response &res) override; + virtual bool ResumeSequence(kortex_driver::ResumeSequence::Request &req, kortex_driver::ResumeSequence::Response &res) override; + virtual bool CreateProtectionZone(kortex_driver::CreateProtectionZone::Request &req, kortex_driver::CreateProtectionZone::Response &res) override; + virtual bool UpdateProtectionZone(kortex_driver::UpdateProtectionZone::Request &req, kortex_driver::UpdateProtectionZone::Response &res) override; + virtual bool ReadProtectionZone(kortex_driver::ReadProtectionZone::Request &req, kortex_driver::ReadProtectionZone::Response &res) override; + virtual bool DeleteProtectionZone(kortex_driver::DeleteProtectionZone::Request &req, kortex_driver::DeleteProtectionZone::Response &res) override; + virtual bool ReadAllProtectionZones(kortex_driver::ReadAllProtectionZones::Request &req, kortex_driver::ReadAllProtectionZones::Response &res) override; + virtual bool CreateMapping(kortex_driver::CreateMapping::Request &req, kortex_driver::CreateMapping::Response &res) override; + virtual bool ReadMapping(kortex_driver::ReadMapping::Request &req, kortex_driver::ReadMapping::Response &res) override; + virtual bool UpdateMapping(kortex_driver::UpdateMapping::Request &req, kortex_driver::UpdateMapping::Response &res) override; + virtual bool DeleteMapping(kortex_driver::DeleteMapping::Request &req, kortex_driver::DeleteMapping::Response &res) override; + virtual bool ReadAllMappings(kortex_driver::ReadAllMappings::Request &req, kortex_driver::ReadAllMappings::Response &res) override; + virtual bool CreateMap(kortex_driver::CreateMap::Request &req, kortex_driver::CreateMap::Response &res) override; + virtual bool ReadMap(kortex_driver::ReadMap::Request &req, kortex_driver::ReadMap::Response &res) override; + virtual bool UpdateMap(kortex_driver::UpdateMap::Request &req, kortex_driver::UpdateMap::Response &res) override; + virtual bool DeleteMap(kortex_driver::DeleteMap::Request &req, kortex_driver::DeleteMap::Response &res) override; + virtual bool ReadAllMaps(kortex_driver::ReadAllMaps::Request &req, kortex_driver::ReadAllMaps::Response &res) override; + virtual bool ActivateMap(kortex_driver::ActivateMap::Request &req, kortex_driver::ActivateMap::Response &res) override; + virtual bool CreateAction(kortex_driver::CreateAction::Request &req, kortex_driver::CreateAction::Response &res) override; + virtual bool ReadAction(kortex_driver::ReadAction::Request &req, kortex_driver::ReadAction::Response &res) override; + virtual bool ReadAllActions(kortex_driver::ReadAllActions::Request &req, kortex_driver::ReadAllActions::Response &res) override; + virtual bool DeleteAction(kortex_driver::DeleteAction::Request &req, kortex_driver::DeleteAction::Response &res) override; + virtual bool UpdateAction(kortex_driver::UpdateAction::Request &req, kortex_driver::UpdateAction::Response &res) override; + virtual bool ExecuteActionFromReference(kortex_driver::ExecuteActionFromReference::Request &req, kortex_driver::ExecuteActionFromReference::Response &res) override; + virtual bool ExecuteAction(kortex_driver::ExecuteAction::Request &req, kortex_driver::ExecuteAction::Response &res) override; + virtual bool PauseAction(kortex_driver::PauseAction::Request &req, kortex_driver::PauseAction::Response &res) override; + virtual bool StopAction(kortex_driver::StopAction::Request &req, kortex_driver::StopAction::Response &res) override; + virtual bool ResumeAction(kortex_driver::ResumeAction::Request &req, kortex_driver::ResumeAction::Response &res) override; + virtual bool GetIPv4Configuration(kortex_driver::GetIPv4Configuration::Request &req, kortex_driver::GetIPv4Configuration::Response &res) override; + virtual bool SetIPv4Configuration(kortex_driver::SetIPv4Configuration::Request &req, kortex_driver::SetIPv4Configuration::Response &res) override; + virtual bool SetCommunicationInterfaceEnable(kortex_driver::SetCommunicationInterfaceEnable::Request &req, kortex_driver::SetCommunicationInterfaceEnable::Response &res) override; + virtual bool IsCommunicationInterfaceEnable(kortex_driver::IsCommunicationInterfaceEnable::Request &req, kortex_driver::IsCommunicationInterfaceEnable::Response &res) override; + virtual bool GetAvailableWifi(kortex_driver::GetAvailableWifi::Request &req, kortex_driver::GetAvailableWifi::Response &res) override; + virtual bool GetWifiInformation(kortex_driver::GetWifiInformation::Request &req, kortex_driver::GetWifiInformation::Response &res) override; + virtual bool AddWifiConfiguration(kortex_driver::AddWifiConfiguration::Request &req, kortex_driver::AddWifiConfiguration::Response &res) override; + virtual bool DeleteWifiConfiguration(kortex_driver::DeleteWifiConfiguration::Request &req, kortex_driver::DeleteWifiConfiguration::Response &res) override; + virtual bool GetAllConfiguredWifis(kortex_driver::GetAllConfiguredWifis::Request &req, kortex_driver::GetAllConfiguredWifis::Response &res) override; + virtual bool ConnectWifi(kortex_driver::ConnectWifi::Request &req, kortex_driver::ConnectWifi::Response &res) override; + virtual bool DisconnectWifi(kortex_driver::DisconnectWifi::Request &req, kortex_driver::DisconnectWifi::Response &res) override; + virtual bool GetConnectedWifiInformation(kortex_driver::GetConnectedWifiInformation::Request &req, kortex_driver::GetConnectedWifiInformation::Response &res) override; + virtual bool Base_Unsubscribe(kortex_driver::Base_Unsubscribe::Request &req, kortex_driver::Base_Unsubscribe::Response &res) override; + virtual bool OnNotificationConfigurationChangeTopic(kortex_driver::OnNotificationConfigurationChangeTopic::Request &req, kortex_driver::OnNotificationConfigurationChangeTopic::Response &res) override; + virtual void cb_ConfigurationChangeTopic(Kinova::Api::Base::ConfigurationChangeNotification notif) override; + virtual bool OnNotificationMappingInfoTopic(kortex_driver::OnNotificationMappingInfoTopic::Request &req, kortex_driver::OnNotificationMappingInfoTopic::Response &res) override; + virtual void cb_MappingInfoTopic(Kinova::Api::Base::MappingInfoNotification notif) override; + virtual bool OnNotificationControlModeTopic(kortex_driver::OnNotificationControlModeTopic::Request &req, kortex_driver::OnNotificationControlModeTopic::Response &res) override; + virtual void cb_ControlModeTopic(Kinova::Api::Base::ControlModeNotification notif) override; + virtual bool OnNotificationOperatingModeTopic(kortex_driver::OnNotificationOperatingModeTopic::Request &req, kortex_driver::OnNotificationOperatingModeTopic::Response &res) override; + virtual void cb_OperatingModeTopic(Kinova::Api::Base::OperatingModeNotification notif) override; + virtual bool OnNotificationSequenceInfoTopic(kortex_driver::OnNotificationSequenceInfoTopic::Request &req, kortex_driver::OnNotificationSequenceInfoTopic::Response &res) override; + virtual void cb_SequenceInfoTopic(Kinova::Api::Base::SequenceInfoNotification notif) override; + virtual bool OnNotificationProtectionZoneTopic(kortex_driver::OnNotificationProtectionZoneTopic::Request &req, kortex_driver::OnNotificationProtectionZoneTopic::Response &res) override; + virtual void cb_ProtectionZoneTopic(Kinova::Api::Base::ProtectionZoneNotification notif) override; + virtual bool OnNotificationUserTopic(kortex_driver::OnNotificationUserTopic::Request &req, kortex_driver::OnNotificationUserTopic::Response &res) override; + virtual void cb_UserTopic(Kinova::Api::Base::UserNotification notif) override; + virtual bool OnNotificationControllerTopic(kortex_driver::OnNotificationControllerTopic::Request &req, kortex_driver::OnNotificationControllerTopic::Response &res) override; + virtual void cb_ControllerTopic(Kinova::Api::Base::ControllerNotification notif) override; + virtual bool OnNotificationActionTopic(kortex_driver::OnNotificationActionTopic::Request &req, kortex_driver::OnNotificationActionTopic::Response &res) override; + virtual void cb_ActionTopic(Kinova::Api::Base::ActionNotification notif) override; + virtual bool OnNotificationRobotEventTopic(kortex_driver::OnNotificationRobotEventTopic::Request &req, kortex_driver::OnNotificationRobotEventTopic::Response &res) override; + virtual void cb_RobotEventTopic(Kinova::Api::Base::RobotEventNotification notif) override; + virtual bool PlayCartesianTrajectory(kortex_driver::PlayCartesianTrajectory::Request &req, kortex_driver::PlayCartesianTrajectory::Response &res) override; + virtual bool PlayCartesianTrajectoryPosition(kortex_driver::PlayCartesianTrajectoryPosition::Request &req, kortex_driver::PlayCartesianTrajectoryPosition::Response &res) override; + virtual bool PlayCartesianTrajectoryOrientation(kortex_driver::PlayCartesianTrajectoryOrientation::Request &req, kortex_driver::PlayCartesianTrajectoryOrientation::Response &res) override; + virtual bool Stop(kortex_driver::Stop::Request &req, kortex_driver::Stop::Response &res) override; + virtual bool GetMeasuredCartesianPose(kortex_driver::GetMeasuredCartesianPose::Request &req, kortex_driver::GetMeasuredCartesianPose::Response &res) override; + virtual bool SendWrenchCommand(kortex_driver::SendWrenchCommand::Request &req, kortex_driver::SendWrenchCommand::Response &res) override; + virtual bool SendWrenchJoystickCommand(kortex_driver::SendWrenchJoystickCommand::Request &req, kortex_driver::SendWrenchJoystickCommand::Response &res) override; + virtual bool SendTwistJoystickCommand(kortex_driver::SendTwistJoystickCommand::Request &req, kortex_driver::SendTwistJoystickCommand::Response &res) override; + virtual bool SendTwistCommand(kortex_driver::SendTwistCommand::Request &req, kortex_driver::SendTwistCommand::Response &res) override; + virtual bool PlayJointTrajectory(kortex_driver::PlayJointTrajectory::Request &req, kortex_driver::PlayJointTrajectory::Response &res) override; + virtual bool PlaySelectedJointTrajectory(kortex_driver::PlaySelectedJointTrajectory::Request &req, kortex_driver::PlaySelectedJointTrajectory::Response &res) override; + virtual bool GetMeasuredJointAngles(kortex_driver::GetMeasuredJointAngles::Request &req, kortex_driver::GetMeasuredJointAngles::Response &res) override; + virtual bool SendJointSpeedsCommand(kortex_driver::SendJointSpeedsCommand::Request &req, kortex_driver::SendJointSpeedsCommand::Response &res) override; + virtual bool SendSelectedJointSpeedCommand(kortex_driver::SendSelectedJointSpeedCommand::Request &req, kortex_driver::SendSelectedJointSpeedCommand::Response &res) override; + virtual bool SendGripperCommand(kortex_driver::SendGripperCommand::Request &req, kortex_driver::SendGripperCommand::Response &res) override; + virtual bool GetMeasuredGripperMovement(kortex_driver::GetMeasuredGripperMovement::Request &req, kortex_driver::GetMeasuredGripperMovement::Response &res) override; + virtual bool SetAdmittance(kortex_driver::SetAdmittance::Request &req, kortex_driver::SetAdmittance::Response &res) override; + virtual bool SetOperatingMode(kortex_driver::SetOperatingMode::Request &req, kortex_driver::SetOperatingMode::Response &res) override; + virtual bool ApplyEmergencyStop(kortex_driver::ApplyEmergencyStop::Request &req, kortex_driver::ApplyEmergencyStop::Response &res) override; + virtual bool Base_ClearFaults(kortex_driver::Base_ClearFaults::Request &req, kortex_driver::Base_ClearFaults::Response &res) override; + virtual bool Base_GetControlMode(kortex_driver::Base_GetControlMode::Request &req, kortex_driver::Base_GetControlMode::Response &res) override; + virtual bool GetOperatingMode(kortex_driver::GetOperatingMode::Request &req, kortex_driver::GetOperatingMode::Response &res) override; + virtual bool SetServoingMode(kortex_driver::SetServoingMode::Request &req, kortex_driver::SetServoingMode::Response &res) override; + virtual bool GetServoingMode(kortex_driver::GetServoingMode::Request &req, kortex_driver::GetServoingMode::Response &res) override; + virtual bool OnNotificationServoingModeTopic(kortex_driver::OnNotificationServoingModeTopic::Request &req, kortex_driver::OnNotificationServoingModeTopic::Response &res) override; + virtual void cb_ServoingModeTopic(Kinova::Api::Base::ServoingModeNotification notif) override; + virtual bool RestoreFactorySettings(kortex_driver::RestoreFactorySettings::Request &req, kortex_driver::RestoreFactorySettings::Response &res) override; + virtual bool OnNotificationFactoryTopic(kortex_driver::OnNotificationFactoryTopic::Request &req, kortex_driver::OnNotificationFactoryTopic::Response &res) override; + virtual void cb_FactoryTopic(Kinova::Api::Base::FactoryNotification notif) override; + virtual bool GetAllConnectedControllers(kortex_driver::GetAllConnectedControllers::Request &req, kortex_driver::GetAllConnectedControllers::Response &res) override; + virtual bool GetControllerState(kortex_driver::GetControllerState::Request &req, kortex_driver::GetControllerState::Response &res) override; + virtual bool GetActuatorCount(kortex_driver::GetActuatorCount::Request &req, kortex_driver::GetActuatorCount::Response &res) override; + virtual bool StartWifiScan(kortex_driver::StartWifiScan::Request &req, kortex_driver::StartWifiScan::Response &res) override; + virtual bool GetConfiguredWifi(kortex_driver::GetConfiguredWifi::Request &req, kortex_driver::GetConfiguredWifi::Response &res) override; + virtual bool OnNotificationNetworkTopic(kortex_driver::OnNotificationNetworkTopic::Request &req, kortex_driver::OnNotificationNetworkTopic::Response &res) override; + virtual void cb_NetworkTopic(Kinova::Api::Base::NetworkNotification notif) override; + virtual bool GetArmState(kortex_driver::GetArmState::Request &req, kortex_driver::GetArmState::Response &res) override; + virtual bool OnNotificationArmStateTopic(kortex_driver::OnNotificationArmStateTopic::Request &req, kortex_driver::OnNotificationArmStateTopic::Response &res) override; + virtual void cb_ArmStateTopic(Kinova::Api::Base::ArmStateNotification notif) override; + virtual bool GetIPv4Information(kortex_driver::GetIPv4Information::Request &req, kortex_driver::GetIPv4Information::Response &res) override; + virtual bool SetWifiCountryCode(kortex_driver::SetWifiCountryCode::Request &req, kortex_driver::SetWifiCountryCode::Response &res) override; + virtual bool GetWifiCountryCode(kortex_driver::GetWifiCountryCode::Request &req, kortex_driver::GetWifiCountryCode::Response &res) override; + virtual bool Base_SetCapSenseConfig(kortex_driver::Base_SetCapSenseConfig::Request &req, kortex_driver::Base_SetCapSenseConfig::Response &res) override; + virtual bool Base_GetCapSenseConfig(kortex_driver::Base_GetCapSenseConfig::Request &req, kortex_driver::Base_GetCapSenseConfig::Response &res) override; + virtual bool GetAllJointsSpeedHardLimitation(kortex_driver::GetAllJointsSpeedHardLimitation::Request &req, kortex_driver::GetAllJointsSpeedHardLimitation::Response &res) override; + virtual bool GetAllJointsTorqueHardLimitation(kortex_driver::GetAllJointsTorqueHardLimitation::Request &req, kortex_driver::GetAllJointsTorqueHardLimitation::Response &res) override; + virtual bool GetTwistHardLimitation(kortex_driver::GetTwistHardLimitation::Request &req, kortex_driver::GetTwistHardLimitation::Response &res) override; + virtual bool GetWrenchHardLimitation(kortex_driver::GetWrenchHardLimitation::Request &req, kortex_driver::GetWrenchHardLimitation::Response &res) override; + virtual bool SendJointSpeedsJoystickCommand(kortex_driver::SendJointSpeedsJoystickCommand::Request &req, kortex_driver::SendJointSpeedsJoystickCommand::Response &res) override; + virtual bool SendSelectedJointSpeedJoystickCommand(kortex_driver::SendSelectedJointSpeedJoystickCommand::Request &req, kortex_driver::SendSelectedJointSpeedJoystickCommand::Response &res) override; + virtual bool EnableBridge(kortex_driver::EnableBridge::Request &req, kortex_driver::EnableBridge::Response &res) override; + virtual bool DisableBridge(kortex_driver::DisableBridge::Request &req, kortex_driver::DisableBridge::Response &res) override; + virtual bool GetBridgeList(kortex_driver::GetBridgeList::Request &req, kortex_driver::GetBridgeList::Response &res) override; + virtual bool GetBridgeConfig(kortex_driver::GetBridgeConfig::Request &req, kortex_driver::GetBridgeConfig::Response &res) override; + virtual bool PlayPreComputedJointTrajectory(kortex_driver::PlayPreComputedJointTrajectory::Request &req, kortex_driver::PlayPreComputedJointTrajectory::Response &res) override; + virtual bool GetProductConfiguration(kortex_driver::GetProductConfiguration::Request &req, kortex_driver::GetProductConfiguration::Response &res) override; + virtual bool UpdateEndEffectorTypeConfiguration(kortex_driver::UpdateEndEffectorTypeConfiguration::Request &req, kortex_driver::UpdateEndEffectorTypeConfiguration::Response &res) override; + virtual bool RestoreFactoryProductConfiguration(kortex_driver::RestoreFactoryProductConfiguration::Request &req, kortex_driver::RestoreFactoryProductConfiguration::Response &res) override; + virtual bool GetTrajectoryErrorReport(kortex_driver::GetTrajectoryErrorReport::Request &req, kortex_driver::GetTrajectoryErrorReport::Response &res) override; + virtual bool GetAllJointsSpeedSoftLimitation(kortex_driver::GetAllJointsSpeedSoftLimitation::Request &req, kortex_driver::GetAllJointsSpeedSoftLimitation::Response &res) override; + virtual bool GetAllJointsTorqueSoftLimitation(kortex_driver::GetAllJointsTorqueSoftLimitation::Request &req, kortex_driver::GetAllJointsTorqueSoftLimitation::Response &res) override; + virtual bool GetTwistSoftLimitation(kortex_driver::GetTwistSoftLimitation::Request &req, kortex_driver::GetTwistSoftLimitation::Response &res) override; + virtual bool GetWrenchSoftLimitation(kortex_driver::GetWrenchSoftLimitation::Request &req, kortex_driver::GetWrenchSoftLimitation::Response &res) override; + virtual bool SetControllerConfigurationMode(kortex_driver::SetControllerConfigurationMode::Request &req, kortex_driver::SetControllerConfigurationMode::Response &res) override; + virtual bool GetControllerConfigurationMode(kortex_driver::GetControllerConfigurationMode::Request &req, kortex_driver::GetControllerConfigurationMode::Response &res) override; + virtual bool StartTeaching(kortex_driver::StartTeaching::Request &req, kortex_driver::StartTeaching::Response &res) override; + virtual bool StopTeaching(kortex_driver::StopTeaching::Request &req, kortex_driver::StopTeaching::Response &res) override; + virtual bool AddSequenceTasks(kortex_driver::AddSequenceTasks::Request &req, kortex_driver::AddSequenceTasks::Response &res) override; + virtual bool UpdateSequenceTask(kortex_driver::UpdateSequenceTask::Request &req, kortex_driver::UpdateSequenceTask::Response &res) override; + virtual bool SwapSequenceTasks(kortex_driver::SwapSequenceTasks::Request &req, kortex_driver::SwapSequenceTasks::Response &res) override; + virtual bool ReadSequenceTask(kortex_driver::ReadSequenceTask::Request &req, kortex_driver::ReadSequenceTask::Response &res) override; + virtual bool ReadAllSequenceTasks(kortex_driver::ReadAllSequenceTasks::Request &req, kortex_driver::ReadAllSequenceTasks::Response &res) override; + virtual bool DeleteSequenceTask(kortex_driver::DeleteSequenceTask::Request &req, kortex_driver::DeleteSequenceTask::Response &res) override; + virtual bool DeleteAllSequenceTasks(kortex_driver::DeleteAllSequenceTasks::Request &req, kortex_driver::DeleteAllSequenceTasks::Response &res) override; + virtual bool TakeSnapshot(kortex_driver::TakeSnapshot::Request &req, kortex_driver::TakeSnapshot::Response &res) override; + virtual bool GetFirmwareBundleVersions(kortex_driver::GetFirmwareBundleVersions::Request &req, kortex_driver::GetFirmwareBundleVersions::Response &res) override; + virtual bool MoveSequenceTask(kortex_driver::MoveSequenceTask::Request &req, kortex_driver::MoveSequenceTask::Response &res) override; + virtual bool DuplicateMapping(kortex_driver::DuplicateMapping::Request &req, kortex_driver::DuplicateMapping::Response &res) override; + virtual bool DuplicateMap(kortex_driver::DuplicateMap::Request &req, kortex_driver::DuplicateMap::Response &res) override; + virtual bool SetControllerConfiguration(kortex_driver::SetControllerConfiguration::Request &req, kortex_driver::SetControllerConfiguration::Response &res) override; + virtual bool GetControllerConfiguration(kortex_driver::GetControllerConfiguration::Request &req, kortex_driver::GetControllerConfiguration::Response &res) override; + virtual bool GetAllControllerConfigurations(kortex_driver::GetAllControllerConfigurations::Request &req, kortex_driver::GetAllControllerConfigurations::Response &res) override; + +private: + uint32_t m_current_device_id; + Kinova::Api::RouterClientSendOptions m_api_options; + + Kinova::Api::Base::BaseClient* m_base; +}; +#endif diff --git a/kortex_driver/include/kortex_driver/generated/basecyclic_proto_converter.h b/kortex_driver/include/kortex_driver/generated/robot/basecyclic_proto_converter.h similarity index 65% rename from kortex_driver/include/kortex_driver/generated/basecyclic_proto_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/basecyclic_proto_converter.h index 7d5b660e..ba500e29 100644 --- a/kortex_driver/include/kortex_driver/generated/basecyclic_proto_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/basecyclic_proto_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_proto_converter.h" -#include "kortex_driver/generated/actuatorconfig_proto_converter.h" -#include "kortex_driver/generated/actuatorcyclic_proto_converter.h" -#include "kortex_driver/generated/productconfiguration_proto_converter.h" -#include "kortex_driver/generated/base_proto_converter.h" -#include "kortex_driver/generated/grippercyclic_proto_converter.h" -#include "kortex_driver/generated/interconnectcyclic_proto_converter.h" -#include "kortex_driver/generated/controlconfig_proto_converter.h" -#include "kortex_driver/generated/deviceconfig_proto_converter.h" -#include "kortex_driver/generated/devicemanager_proto_converter.h" -#include "kortex_driver/generated/interconnectconfig_proto_converter.h" -#include "kortex_driver/generated/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" #include "kortex_driver/ActuatorCommand.h" diff --git a/kortex_driver/include/kortex_driver/generated/basecyclic_ros_converter.h b/kortex_driver/include/kortex_driver/generated/robot/basecyclic_ros_converter.h similarity index 65% rename from kortex_driver/include/kortex_driver/generated/basecyclic_ros_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/basecyclic_ros_converter.h index dde72627..d0f8a8a6 100644 --- a/kortex_driver/include/kortex_driver/generated/basecyclic_ros_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/basecyclic_ros_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_ros_converter.h" -#include "kortex_driver/generated/actuatorconfig_ros_converter.h" -#include "kortex_driver/generated/actuatorcyclic_ros_converter.h" -#include "kortex_driver/generated/productconfiguration_ros_converter.h" -#include "kortex_driver/generated/base_ros_converter.h" -#include "kortex_driver/generated/grippercyclic_ros_converter.h" -#include "kortex_driver/generated/interconnectcyclic_ros_converter.h" -#include "kortex_driver/generated/controlconfig_ros_converter.h" -#include "kortex_driver/generated/deviceconfig_ros_converter.h" -#include "kortex_driver/generated/devicemanager_ros_converter.h" -#include "kortex_driver/generated/interconnectconfig_ros_converter.h" -#include "kortex_driver/generated/visionconfig_ros_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" #include "kortex_driver/ActuatorCommand.h" diff --git a/kortex_driver/include/kortex_driver/generated/common_proto_converter.h b/kortex_driver/include/kortex_driver/generated/robot/common_proto_converter.h similarity index 72% rename from kortex_driver/include/kortex_driver/generated/common_proto_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/common_proto_converter.h index 8fef504b..786db33b 100644 --- a/kortex_driver/include/kortex_driver/generated/common_proto_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/common_proto_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/actuatorconfig_proto_converter.h" -#include "kortex_driver/generated/actuatorcyclic_proto_converter.h" -#include "kortex_driver/generated/productconfiguration_proto_converter.h" -#include "kortex_driver/generated/base_proto_converter.h" -#include "kortex_driver/generated/grippercyclic_proto_converter.h" -#include "kortex_driver/generated/interconnectcyclic_proto_converter.h" -#include "kortex_driver/generated/basecyclic_proto_converter.h" -#include "kortex_driver/generated/controlconfig_proto_converter.h" -#include "kortex_driver/generated/deviceconfig_proto_converter.h" -#include "kortex_driver/generated/devicemanager_proto_converter.h" -#include "kortex_driver/generated/interconnectconfig_proto_converter.h" -#include "kortex_driver/generated/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" #include "kortex_driver/DeviceHandle.h" diff --git a/kortex_driver/include/kortex_driver/generated/common_ros_converter.h b/kortex_driver/include/kortex_driver/generated/robot/common_ros_converter.h similarity index 72% rename from kortex_driver/include/kortex_driver/generated/common_ros_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/common_ros_converter.h index 0f1147b6..6aa14ec6 100644 --- a/kortex_driver/include/kortex_driver/generated/common_ros_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/common_ros_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/actuatorconfig_ros_converter.h" -#include "kortex_driver/generated/actuatorcyclic_ros_converter.h" -#include "kortex_driver/generated/productconfiguration_ros_converter.h" -#include "kortex_driver/generated/base_ros_converter.h" -#include "kortex_driver/generated/grippercyclic_ros_converter.h" -#include "kortex_driver/generated/interconnectcyclic_ros_converter.h" -#include "kortex_driver/generated/basecyclic_ros_converter.h" -#include "kortex_driver/generated/controlconfig_ros_converter.h" -#include "kortex_driver/generated/deviceconfig_ros_converter.h" -#include "kortex_driver/generated/devicemanager_ros_converter.h" -#include "kortex_driver/generated/interconnectconfig_ros_converter.h" -#include "kortex_driver/generated/visionconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" #include "kortex_driver/DeviceHandle.h" diff --git a/kortex_driver/include/kortex_driver/generated/controlconfig_proto_converter.h b/kortex_driver/include/kortex_driver/generated/robot/controlconfig_proto_converter.h similarity index 80% rename from kortex_driver/include/kortex_driver/generated/controlconfig_proto_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/controlconfig_proto_converter.h index c93da6a3..6b19dd05 100644 --- a/kortex_driver/include/kortex_driver/generated/controlconfig_proto_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/controlconfig_proto_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_proto_converter.h" -#include "kortex_driver/generated/actuatorconfig_proto_converter.h" -#include "kortex_driver/generated/actuatorcyclic_proto_converter.h" -#include "kortex_driver/generated/productconfiguration_proto_converter.h" -#include "kortex_driver/generated/base_proto_converter.h" -#include "kortex_driver/generated/grippercyclic_proto_converter.h" -#include "kortex_driver/generated/interconnectcyclic_proto_converter.h" -#include "kortex_driver/generated/basecyclic_proto_converter.h" -#include "kortex_driver/generated/deviceconfig_proto_converter.h" -#include "kortex_driver/generated/devicemanager_proto_converter.h" -#include "kortex_driver/generated/interconnectconfig_proto_converter.h" -#include "kortex_driver/generated/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" #include "kortex_driver/GravityVector.h" diff --git a/kortex_driver/include/kortex_driver/generated/controlconfig_ros_converter.h b/kortex_driver/include/kortex_driver/generated/robot/controlconfig_ros_converter.h similarity index 80% rename from kortex_driver/include/kortex_driver/generated/controlconfig_ros_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/controlconfig_ros_converter.h index c22531fb..6fd389fb 100644 --- a/kortex_driver/include/kortex_driver/generated/controlconfig_ros_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/controlconfig_ros_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_ros_converter.h" -#include "kortex_driver/generated/actuatorconfig_ros_converter.h" -#include "kortex_driver/generated/actuatorcyclic_ros_converter.h" -#include "kortex_driver/generated/productconfiguration_ros_converter.h" -#include "kortex_driver/generated/base_ros_converter.h" -#include "kortex_driver/generated/grippercyclic_ros_converter.h" -#include "kortex_driver/generated/interconnectcyclic_ros_converter.h" -#include "kortex_driver/generated/basecyclic_ros_converter.h" -#include "kortex_driver/generated/deviceconfig_ros_converter.h" -#include "kortex_driver/generated/devicemanager_ros_converter.h" -#include "kortex_driver/generated/interconnectconfig_ros_converter.h" -#include "kortex_driver/generated/visionconfig_ros_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" #include "kortex_driver/GravityVector.h" diff --git a/kortex_driver/include/kortex_driver/generated/robot/controlconfig_services.h b/kortex_driver/include/kortex_driver/generated/robot/controlconfig_services.h new file mode 100644 index 00000000..e4adde31 --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/robot/controlconfig_services.h @@ -0,0 +1,71 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_CONTROLCONFIG_ROBOT_SERVICES_H_ +#define _KORTEX_CONTROLCONFIG_ROBOT_SERVICES_H_ + +#include "kortex_driver/generated/interfaces/controlconfig_services_interface.h" + +#include +#include + +using namespace std; + +class ControlConfigRobotServices : public IControlConfigServices +{ + public: + ControlConfigRobotServices(ros::NodeHandle& node_handle, Kinova::Api::ControlConfig::ControlConfigClient* controlconfig, uint32_t device_id, uint32_t timeout_ms); + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; + virtual bool SetGravityVector(kortex_driver::SetGravityVector::Request &req, kortex_driver::SetGravityVector::Response &res) override; + virtual bool GetGravityVector(kortex_driver::GetGravityVector::Request &req, kortex_driver::GetGravityVector::Response &res) override; + virtual bool SetPayloadInformation(kortex_driver::SetPayloadInformation::Request &req, kortex_driver::SetPayloadInformation::Response &res) override; + virtual bool GetPayloadInformation(kortex_driver::GetPayloadInformation::Request &req, kortex_driver::GetPayloadInformation::Response &res) override; + virtual bool SetToolConfiguration(kortex_driver::SetToolConfiguration::Request &req, kortex_driver::SetToolConfiguration::Response &res) override; + virtual bool GetToolConfiguration(kortex_driver::GetToolConfiguration::Request &req, kortex_driver::GetToolConfiguration::Response &res) override; + virtual bool OnNotificationControlConfigurationTopic(kortex_driver::OnNotificationControlConfigurationTopic::Request &req, kortex_driver::OnNotificationControlConfigurationTopic::Response &res) override; + virtual void cb_ControlConfigurationTopic(Kinova::Api::ControlConfig::ControlConfigurationNotification notif) override; + virtual bool ControlConfig_Unsubscribe(kortex_driver::ControlConfig_Unsubscribe::Request &req, kortex_driver::ControlConfig_Unsubscribe::Response &res) override; + virtual bool SetCartesianReferenceFrame(kortex_driver::SetCartesianReferenceFrame::Request &req, kortex_driver::SetCartesianReferenceFrame::Response &res) override; + virtual bool GetCartesianReferenceFrame(kortex_driver::GetCartesianReferenceFrame::Request &req, kortex_driver::GetCartesianReferenceFrame::Response &res) override; + virtual bool ControlConfig_GetControlMode(kortex_driver::ControlConfig_GetControlMode::Request &req, kortex_driver::ControlConfig_GetControlMode::Response &res) override; + virtual bool SetJointSpeedSoftLimits(kortex_driver::SetJointSpeedSoftLimits::Request &req, kortex_driver::SetJointSpeedSoftLimits::Response &res) override; + virtual bool SetTwistLinearSoftLimit(kortex_driver::SetTwistLinearSoftLimit::Request &req, kortex_driver::SetTwistLinearSoftLimit::Response &res) override; + virtual bool SetTwistAngularSoftLimit(kortex_driver::SetTwistAngularSoftLimit::Request &req, kortex_driver::SetTwistAngularSoftLimit::Response &res) override; + virtual bool SetJointAccelerationSoftLimits(kortex_driver::SetJointAccelerationSoftLimits::Request &req, kortex_driver::SetJointAccelerationSoftLimits::Response &res) override; + virtual bool GetKinematicHardLimits(kortex_driver::GetKinematicHardLimits::Request &req, kortex_driver::GetKinematicHardLimits::Response &res) override; + virtual bool GetKinematicSoftLimits(kortex_driver::GetKinematicSoftLimits::Request &req, kortex_driver::GetKinematicSoftLimits::Response &res) override; + virtual bool GetAllKinematicSoftLimits(kortex_driver::GetAllKinematicSoftLimits::Request &req, kortex_driver::GetAllKinematicSoftLimits::Response &res) override; + virtual bool SetDesiredLinearTwist(kortex_driver::SetDesiredLinearTwist::Request &req, kortex_driver::SetDesiredLinearTwist::Response &res) override; + virtual bool SetDesiredAngularTwist(kortex_driver::SetDesiredAngularTwist::Request &req, kortex_driver::SetDesiredAngularTwist::Response &res) override; + virtual bool SetDesiredJointSpeeds(kortex_driver::SetDesiredJointSpeeds::Request &req, kortex_driver::SetDesiredJointSpeeds::Response &res) override; + virtual bool GetDesiredSpeeds(kortex_driver::GetDesiredSpeeds::Request &req, kortex_driver::GetDesiredSpeeds::Response &res) override; + virtual bool ResetGravityVector(kortex_driver::ResetGravityVector::Request &req, kortex_driver::ResetGravityVector::Response &res) override; + virtual bool ResetPayloadInformation(kortex_driver::ResetPayloadInformation::Request &req, kortex_driver::ResetPayloadInformation::Response &res) override; + virtual bool ResetToolConfiguration(kortex_driver::ResetToolConfiguration::Request &req, kortex_driver::ResetToolConfiguration::Response &res) override; + virtual bool ResetJointSpeedSoftLimits(kortex_driver::ResetJointSpeedSoftLimits::Request &req, kortex_driver::ResetJointSpeedSoftLimits::Response &res) override; + virtual bool ResetTwistLinearSoftLimit(kortex_driver::ResetTwistLinearSoftLimit::Request &req, kortex_driver::ResetTwistLinearSoftLimit::Response &res) override; + virtual bool ResetTwistAngularSoftLimit(kortex_driver::ResetTwistAngularSoftLimit::Request &req, kortex_driver::ResetTwistAngularSoftLimit::Response &res) override; + virtual bool ResetJointAccelerationSoftLimits(kortex_driver::ResetJointAccelerationSoftLimits::Request &req, kortex_driver::ResetJointAccelerationSoftLimits::Response &res) override; + +private: + uint32_t m_current_device_id; + Kinova::Api::RouterClientSendOptions m_api_options; + + Kinova::Api::ControlConfig::ControlConfigClient* m_controlconfig; +}; +#endif diff --git a/kortex_driver/include/kortex_driver/generated/deviceconfig_proto_converter.h b/kortex_driver/include/kortex_driver/generated/robot/deviceconfig_proto_converter.h similarity index 83% rename from kortex_driver/include/kortex_driver/generated/deviceconfig_proto_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/deviceconfig_proto_converter.h index 38e025c7..67448259 100644 --- a/kortex_driver/include/kortex_driver/generated/deviceconfig_proto_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/deviceconfig_proto_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_proto_converter.h" -#include "kortex_driver/generated/actuatorconfig_proto_converter.h" -#include "kortex_driver/generated/actuatorcyclic_proto_converter.h" -#include "kortex_driver/generated/productconfiguration_proto_converter.h" -#include "kortex_driver/generated/base_proto_converter.h" -#include "kortex_driver/generated/grippercyclic_proto_converter.h" -#include "kortex_driver/generated/interconnectcyclic_proto_converter.h" -#include "kortex_driver/generated/basecyclic_proto_converter.h" -#include "kortex_driver/generated/controlconfig_proto_converter.h" -#include "kortex_driver/generated/devicemanager_proto_converter.h" -#include "kortex_driver/generated/interconnectconfig_proto_converter.h" -#include "kortex_driver/generated/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" #include "kortex_driver/DeviceType.h" diff --git a/kortex_driver/include/kortex_driver/generated/deviceconfig_ros_converter.h b/kortex_driver/include/kortex_driver/generated/robot/deviceconfig_ros_converter.h similarity index 83% rename from kortex_driver/include/kortex_driver/generated/deviceconfig_ros_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/deviceconfig_ros_converter.h index 4e727c19..9dbe819a 100644 --- a/kortex_driver/include/kortex_driver/generated/deviceconfig_ros_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/deviceconfig_ros_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_ros_converter.h" -#include "kortex_driver/generated/actuatorconfig_ros_converter.h" -#include "kortex_driver/generated/actuatorcyclic_ros_converter.h" -#include "kortex_driver/generated/productconfiguration_ros_converter.h" -#include "kortex_driver/generated/base_ros_converter.h" -#include "kortex_driver/generated/grippercyclic_ros_converter.h" -#include "kortex_driver/generated/interconnectcyclic_ros_converter.h" -#include "kortex_driver/generated/basecyclic_ros_converter.h" -#include "kortex_driver/generated/controlconfig_ros_converter.h" -#include "kortex_driver/generated/devicemanager_ros_converter.h" -#include "kortex_driver/generated/interconnectconfig_ros_converter.h" -#include "kortex_driver/generated/visionconfig_ros_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" #include "kortex_driver/DeviceType.h" diff --git a/kortex_driver/include/kortex_driver/generated/robot/deviceconfig_services.h b/kortex_driver/include/kortex_driver/generated/robot/deviceconfig_services.h new file mode 100644 index 00000000..fb87a132 --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/robot/deviceconfig_services.h @@ -0,0 +1,74 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_DEVICECONFIG_ROBOT_SERVICES_H_ +#define _KORTEX_DEVICECONFIG_ROBOT_SERVICES_H_ + +#include "kortex_driver/generated/interfaces/deviceconfig_services_interface.h" + +#include +#include + +using namespace std; + +class DeviceConfigRobotServices : public IDeviceConfigServices +{ + public: + DeviceConfigRobotServices(ros::NodeHandle& node_handle, Kinova::Api::DeviceConfig::DeviceConfigClient* deviceconfig, uint32_t device_id, uint32_t timeout_ms); + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; + virtual bool GetRunMode(kortex_driver::GetRunMode::Request &req, kortex_driver::GetRunMode::Response &res) override; + virtual bool SetRunMode(kortex_driver::SetRunMode::Request &req, kortex_driver::SetRunMode::Response &res) override; + virtual bool GetDeviceType(kortex_driver::GetDeviceType::Request &req, kortex_driver::GetDeviceType::Response &res) override; + virtual bool GetFirmwareVersion(kortex_driver::GetFirmwareVersion::Request &req, kortex_driver::GetFirmwareVersion::Response &res) override; + virtual bool GetBootloaderVersion(kortex_driver::GetBootloaderVersion::Request &req, kortex_driver::GetBootloaderVersion::Response &res) override; + virtual bool GetModelNumber(kortex_driver::GetModelNumber::Request &req, kortex_driver::GetModelNumber::Response &res) override; + virtual bool GetPartNumber(kortex_driver::GetPartNumber::Request &req, kortex_driver::GetPartNumber::Response &res) override; + virtual bool GetSerialNumber(kortex_driver::GetSerialNumber::Request &req, kortex_driver::GetSerialNumber::Response &res) override; + virtual bool GetMACAddress(kortex_driver::GetMACAddress::Request &req, kortex_driver::GetMACAddress::Response &res) override; + virtual bool GetIPv4Settings(kortex_driver::GetIPv4Settings::Request &req, kortex_driver::GetIPv4Settings::Response &res) override; + virtual bool SetIPv4Settings(kortex_driver::SetIPv4Settings::Request &req, kortex_driver::SetIPv4Settings::Response &res) override; + virtual bool GetPartNumberRevision(kortex_driver::GetPartNumberRevision::Request &req, kortex_driver::GetPartNumberRevision::Response &res) override; + virtual bool RebootRequest(kortex_driver::RebootRequest::Request &req, kortex_driver::RebootRequest::Response &res) override; + virtual bool SetSafetyEnable(kortex_driver::SetSafetyEnable::Request &req, kortex_driver::SetSafetyEnable::Response &res) override; + virtual bool SetSafetyErrorThreshold(kortex_driver::SetSafetyErrorThreshold::Request &req, kortex_driver::SetSafetyErrorThreshold::Response &res) override; + virtual bool SetSafetyWarningThreshold(kortex_driver::SetSafetyWarningThreshold::Request &req, kortex_driver::SetSafetyWarningThreshold::Response &res) override; + virtual bool SetSafetyConfiguration(kortex_driver::SetSafetyConfiguration::Request &req, kortex_driver::SetSafetyConfiguration::Response &res) override; + virtual bool GetSafetyConfiguration(kortex_driver::GetSafetyConfiguration::Request &req, kortex_driver::GetSafetyConfiguration::Response &res) override; + virtual bool GetSafetyInformation(kortex_driver::GetSafetyInformation::Request &req, kortex_driver::GetSafetyInformation::Response &res) override; + virtual bool GetSafetyEnable(kortex_driver::GetSafetyEnable::Request &req, kortex_driver::GetSafetyEnable::Response &res) override; + virtual bool GetSafetyStatus(kortex_driver::GetSafetyStatus::Request &req, kortex_driver::GetSafetyStatus::Response &res) override; + virtual bool ClearAllSafetyStatus(kortex_driver::ClearAllSafetyStatus::Request &req, kortex_driver::ClearAllSafetyStatus::Response &res) override; + virtual bool ClearSafetyStatus(kortex_driver::ClearSafetyStatus::Request &req, kortex_driver::ClearSafetyStatus::Response &res) override; + virtual bool GetAllSafetyConfiguration(kortex_driver::GetAllSafetyConfiguration::Request &req, kortex_driver::GetAllSafetyConfiguration::Response &res) override; + virtual bool GetAllSafetyInformation(kortex_driver::GetAllSafetyInformation::Request &req, kortex_driver::GetAllSafetyInformation::Response &res) override; + virtual bool ResetSafetyDefaults(kortex_driver::ResetSafetyDefaults::Request &req, kortex_driver::ResetSafetyDefaults::Response &res) override; + virtual bool OnNotificationSafetyTopic(kortex_driver::OnNotificationSafetyTopic::Request &req, kortex_driver::OnNotificationSafetyTopic::Response &res) override; + virtual void cb_SafetyTopic(Kinova::Api::Common::SafetyNotification notif) override; + virtual bool ExecuteCalibration(kortex_driver::ExecuteCalibration::Request &req, kortex_driver::ExecuteCalibration::Response &res) override; + virtual bool GetCalibrationResult(kortex_driver::GetCalibrationResult::Request &req, kortex_driver::GetCalibrationResult::Response &res) override; + virtual bool StopCalibration(kortex_driver::StopCalibration::Request &req, kortex_driver::StopCalibration::Response &res) override; + virtual bool DeviceConfig_SetCapSenseConfig(kortex_driver::DeviceConfig_SetCapSenseConfig::Request &req, kortex_driver::DeviceConfig_SetCapSenseConfig::Response &res) override; + virtual bool DeviceConfig_GetCapSenseConfig(kortex_driver::DeviceConfig_GetCapSenseConfig::Request &req, kortex_driver::DeviceConfig_GetCapSenseConfig::Response &res) override; + +private: + uint32_t m_current_device_id; + Kinova::Api::RouterClientSendOptions m_api_options; + + Kinova::Api::DeviceConfig::DeviceConfigClient* m_deviceconfig; +}; +#endif diff --git a/kortex_driver/include/kortex_driver/generated/robot/devicemanager_proto_converter.h b/kortex_driver/include/kortex_driver/generated/robot/devicemanager_proto_converter.h new file mode 100644 index 00000000..42e63e7e --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/robot/devicemanager_proto_converter.h @@ -0,0 +1,49 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_DEVICEMANAGER_PROTO_CONVERTER_H_ +#define _KORTEX_DEVICEMANAGER_PROTO_CONVERTER_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include + +#include + +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" + + +#include "kortex_driver/DeviceHandles.h" + + +int ToProtoData(kortex_driver::DeviceHandles input, Kinova::Api::DeviceManager::DeviceHandles *output); + +#endif \ No newline at end of file diff --git a/kortex_driver/include/kortex_driver/generated/robot/devicemanager_ros_converter.h b/kortex_driver/include/kortex_driver/generated/robot/devicemanager_ros_converter.h new file mode 100644 index 00000000..3847a80b --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/robot/devicemanager_ros_converter.h @@ -0,0 +1,49 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_DEVICEMANAGER_ROS_CONVERTER_H_ +#define _KORTEX_DEVICEMANAGER_ROS_CONVERTER_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include + +#include + +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" + + +#include "kortex_driver/DeviceHandles.h" + + +int ToRosData(Kinova::Api::DeviceManager::DeviceHandles input, kortex_driver::DeviceHandles &output); + +#endif \ No newline at end of file diff --git a/kortex_driver/include/kortex_driver/generated/robot/devicemanager_services.h b/kortex_driver/include/kortex_driver/generated/robot/devicemanager_services.h new file mode 100644 index 00000000..26443eca --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/robot/devicemanager_services.h @@ -0,0 +1,42 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_DEVICEMANAGER_ROBOT_SERVICES_H_ +#define _KORTEX_DEVICEMANAGER_ROBOT_SERVICES_H_ + +#include "kortex_driver/generated/interfaces/devicemanager_services_interface.h" + +#include +#include + +using namespace std; + +class DeviceManagerRobotServices : public IDeviceManagerServices +{ + public: + DeviceManagerRobotServices(ros::NodeHandle& node_handle, Kinova::Api::DeviceManager::DeviceManagerClient* devicemanager, uint32_t device_id, uint32_t timeout_ms); + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; + virtual bool ReadAllDevices(kortex_driver::ReadAllDevices::Request &req, kortex_driver::ReadAllDevices::Response &res) override; + +private: + uint32_t m_current_device_id; + Kinova::Api::RouterClientSendOptions m_api_options; + + Kinova::Api::DeviceManager::DeviceManagerClient* m_devicemanager; +}; +#endif diff --git a/kortex_driver/include/kortex_driver/generated/grippercyclic_proto_converter.h b/kortex_driver/include/kortex_driver/generated/robot/grippercyclic_proto_converter.h similarity index 66% rename from kortex_driver/include/kortex_driver/generated/grippercyclic_proto_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/grippercyclic_proto_converter.h index fc5faad4..9a696f5f 100644 --- a/kortex_driver/include/kortex_driver/generated/grippercyclic_proto_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/grippercyclic_proto_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_proto_converter.h" -#include "kortex_driver/generated/actuatorconfig_proto_converter.h" -#include "kortex_driver/generated/actuatorcyclic_proto_converter.h" -#include "kortex_driver/generated/productconfiguration_proto_converter.h" -#include "kortex_driver/generated/base_proto_converter.h" -#include "kortex_driver/generated/interconnectcyclic_proto_converter.h" -#include "kortex_driver/generated/basecyclic_proto_converter.h" -#include "kortex_driver/generated/controlconfig_proto_converter.h" -#include "kortex_driver/generated/deviceconfig_proto_converter.h" -#include "kortex_driver/generated/devicemanager_proto_converter.h" -#include "kortex_driver/generated/interconnectconfig_proto_converter.h" -#include "kortex_driver/generated/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" #include "kortex_driver/GripperCyclic_MessageId.h" diff --git a/kortex_driver/include/kortex_driver/generated/grippercyclic_ros_converter.h b/kortex_driver/include/kortex_driver/generated/robot/grippercyclic_ros_converter.h similarity index 66% rename from kortex_driver/include/kortex_driver/generated/grippercyclic_ros_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/grippercyclic_ros_converter.h index e352379a..a584b91e 100644 --- a/kortex_driver/include/kortex_driver/generated/grippercyclic_ros_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/grippercyclic_ros_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_ros_converter.h" -#include "kortex_driver/generated/actuatorconfig_ros_converter.h" -#include "kortex_driver/generated/actuatorcyclic_ros_converter.h" -#include "kortex_driver/generated/productconfiguration_ros_converter.h" -#include "kortex_driver/generated/base_ros_converter.h" -#include "kortex_driver/generated/interconnectcyclic_ros_converter.h" -#include "kortex_driver/generated/basecyclic_ros_converter.h" -#include "kortex_driver/generated/controlconfig_ros_converter.h" -#include "kortex_driver/generated/deviceconfig_ros_converter.h" -#include "kortex_driver/generated/devicemanager_ros_converter.h" -#include "kortex_driver/generated/interconnectconfig_ros_converter.h" -#include "kortex_driver/generated/visionconfig_ros_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" #include "kortex_driver/GripperCyclic_MessageId.h" diff --git a/kortex_driver/include/kortex_driver/generated/interconnectconfig_proto_converter.h b/kortex_driver/include/kortex_driver/generated/robot/interconnectconfig_proto_converter.h similarity index 75% rename from kortex_driver/include/kortex_driver/generated/interconnectconfig_proto_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/interconnectconfig_proto_converter.h index 0f3d45cc..4b201c00 100644 --- a/kortex_driver/include/kortex_driver/generated/interconnectconfig_proto_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/interconnectconfig_proto_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_proto_converter.h" -#include "kortex_driver/generated/actuatorconfig_proto_converter.h" -#include "kortex_driver/generated/actuatorcyclic_proto_converter.h" -#include "kortex_driver/generated/productconfiguration_proto_converter.h" -#include "kortex_driver/generated/base_proto_converter.h" -#include "kortex_driver/generated/grippercyclic_proto_converter.h" -#include "kortex_driver/generated/interconnectcyclic_proto_converter.h" -#include "kortex_driver/generated/basecyclic_proto_converter.h" -#include "kortex_driver/generated/controlconfig_proto_converter.h" -#include "kortex_driver/generated/deviceconfig_proto_converter.h" -#include "kortex_driver/generated/devicemanager_proto_converter.h" -#include "kortex_driver/generated/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" #include "kortex_driver/EthernetDeviceIdentification.h" diff --git a/kortex_driver/include/kortex_driver/generated/interconnectconfig_ros_converter.h b/kortex_driver/include/kortex_driver/generated/robot/interconnectconfig_ros_converter.h similarity index 75% rename from kortex_driver/include/kortex_driver/generated/interconnectconfig_ros_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/interconnectconfig_ros_converter.h index 0f6187c3..f155cf65 100644 --- a/kortex_driver/include/kortex_driver/generated/interconnectconfig_ros_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/interconnectconfig_ros_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_ros_converter.h" -#include "kortex_driver/generated/actuatorconfig_ros_converter.h" -#include "kortex_driver/generated/actuatorcyclic_ros_converter.h" -#include "kortex_driver/generated/productconfiguration_ros_converter.h" -#include "kortex_driver/generated/base_ros_converter.h" -#include "kortex_driver/generated/grippercyclic_ros_converter.h" -#include "kortex_driver/generated/interconnectcyclic_ros_converter.h" -#include "kortex_driver/generated/basecyclic_ros_converter.h" -#include "kortex_driver/generated/controlconfig_ros_converter.h" -#include "kortex_driver/generated/deviceconfig_ros_converter.h" -#include "kortex_driver/generated/devicemanager_ros_converter.h" -#include "kortex_driver/generated/visionconfig_ros_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" #include "kortex_driver/EthernetDeviceIdentification.h" diff --git a/kortex_driver/include/kortex_driver/generated/robot/interconnectconfig_services.h b/kortex_driver/include/kortex_driver/generated/robot/interconnectconfig_services.h new file mode 100644 index 00000000..a4f360eb --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/robot/interconnectconfig_services.h @@ -0,0 +1,55 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_INTERCONNECTCONFIG_ROBOT_SERVICES_H_ +#define _KORTEX_INTERCONNECTCONFIG_ROBOT_SERVICES_H_ + +#include "kortex_driver/generated/interfaces/interconnectconfig_services_interface.h" + +#include +#include + +using namespace std; + +class InterconnectConfigRobotServices : public IInterconnectConfigServices +{ + public: + InterconnectConfigRobotServices(ros::NodeHandle& node_handle, Kinova::Api::InterconnectConfig::InterconnectConfigClient* interconnectconfig, uint32_t device_id, uint32_t timeout_ms); + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; + virtual bool GetUARTConfiguration(kortex_driver::GetUARTConfiguration::Request &req, kortex_driver::GetUARTConfiguration::Response &res) override; + virtual bool SetUARTConfiguration(kortex_driver::SetUARTConfiguration::Request &req, kortex_driver::SetUARTConfiguration::Response &res) override; + virtual bool GetEthernetConfiguration(kortex_driver::GetEthernetConfiguration::Request &req, kortex_driver::GetEthernetConfiguration::Response &res) override; + virtual bool SetEthernetConfiguration(kortex_driver::SetEthernetConfiguration::Request &req, kortex_driver::SetEthernetConfiguration::Response &res) override; + virtual bool GetGPIOConfiguration(kortex_driver::GetGPIOConfiguration::Request &req, kortex_driver::GetGPIOConfiguration::Response &res) override; + virtual bool SetGPIOConfiguration(kortex_driver::SetGPIOConfiguration::Request &req, kortex_driver::SetGPIOConfiguration::Response &res) override; + virtual bool GetGPIOState(kortex_driver::GetGPIOState::Request &req, kortex_driver::GetGPIOState::Response &res) override; + virtual bool SetGPIOState(kortex_driver::SetGPIOState::Request &req, kortex_driver::SetGPIOState::Response &res) override; + virtual bool GetI2CConfiguration(kortex_driver::GetI2CConfiguration::Request &req, kortex_driver::GetI2CConfiguration::Response &res) override; + virtual bool SetI2CConfiguration(kortex_driver::SetI2CConfiguration::Request &req, kortex_driver::SetI2CConfiguration::Response &res) override; + virtual bool I2CRead(kortex_driver::I2CRead::Request &req, kortex_driver::I2CRead::Response &res) override; + virtual bool I2CReadRegister(kortex_driver::I2CReadRegister::Request &req, kortex_driver::I2CReadRegister::Response &res) override; + virtual bool I2CWrite(kortex_driver::I2CWrite::Request &req, kortex_driver::I2CWrite::Response &res) override; + virtual bool I2CWriteRegister(kortex_driver::I2CWriteRegister::Request &req, kortex_driver::I2CWriteRegister::Response &res) override; + +private: + uint32_t m_current_device_id; + Kinova::Api::RouterClientSendOptions m_api_options; + + Kinova::Api::InterconnectConfig::InterconnectConfigClient* m_interconnectconfig; +}; +#endif diff --git a/kortex_driver/include/kortex_driver/generated/interconnectcyclic_proto_converter.h b/kortex_driver/include/kortex_driver/generated/robot/interconnectcyclic_proto_converter.h similarity index 60% rename from kortex_driver/include/kortex_driver/generated/interconnectcyclic_proto_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/interconnectcyclic_proto_converter.h index ba34f394..ad838be9 100644 --- a/kortex_driver/include/kortex_driver/generated/interconnectcyclic_proto_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/interconnectcyclic_proto_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_proto_converter.h" -#include "kortex_driver/generated/actuatorconfig_proto_converter.h" -#include "kortex_driver/generated/actuatorcyclic_proto_converter.h" -#include "kortex_driver/generated/productconfiguration_proto_converter.h" -#include "kortex_driver/generated/base_proto_converter.h" -#include "kortex_driver/generated/grippercyclic_proto_converter.h" -#include "kortex_driver/generated/basecyclic_proto_converter.h" -#include "kortex_driver/generated/controlconfig_proto_converter.h" -#include "kortex_driver/generated/deviceconfig_proto_converter.h" -#include "kortex_driver/generated/devicemanager_proto_converter.h" -#include "kortex_driver/generated/interconnectconfig_proto_converter.h" -#include "kortex_driver/generated/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" #include "kortex_driver/InterconnectCyclic_MessageId.h" diff --git a/kortex_driver/include/kortex_driver/generated/interconnectcyclic_ros_converter.h b/kortex_driver/include/kortex_driver/generated/robot/interconnectcyclic_ros_converter.h similarity index 60% rename from kortex_driver/include/kortex_driver/generated/interconnectcyclic_ros_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/interconnectcyclic_ros_converter.h index 2e89a38c..eefda5c7 100644 --- a/kortex_driver/include/kortex_driver/generated/interconnectcyclic_ros_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/interconnectcyclic_ros_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_ros_converter.h" -#include "kortex_driver/generated/actuatorconfig_ros_converter.h" -#include "kortex_driver/generated/actuatorcyclic_ros_converter.h" -#include "kortex_driver/generated/productconfiguration_ros_converter.h" -#include "kortex_driver/generated/base_ros_converter.h" -#include "kortex_driver/generated/grippercyclic_ros_converter.h" -#include "kortex_driver/generated/basecyclic_ros_converter.h" -#include "kortex_driver/generated/controlconfig_ros_converter.h" -#include "kortex_driver/generated/deviceconfig_ros_converter.h" -#include "kortex_driver/generated/devicemanager_ros_converter.h" -#include "kortex_driver/generated/interconnectconfig_ros_converter.h" -#include "kortex_driver/generated/visionconfig_ros_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" #include "kortex_driver/InterconnectCyclic_MessageId.h" diff --git a/kortex_driver/include/kortex_driver/generated/productconfiguration_proto_converter.h b/kortex_driver/include/kortex_driver/generated/robot/productconfiguration_proto_converter.h similarity index 54% rename from kortex_driver/include/kortex_driver/generated/productconfiguration_proto_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/productconfiguration_proto_converter.h index 213530f8..c40b896b 100644 --- a/kortex_driver/include/kortex_driver/generated/productconfiguration_proto_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/productconfiguration_proto_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_proto_converter.h" -#include "kortex_driver/generated/actuatorconfig_proto_converter.h" -#include "kortex_driver/generated/actuatorcyclic_proto_converter.h" -#include "kortex_driver/generated/base_proto_converter.h" -#include "kortex_driver/generated/grippercyclic_proto_converter.h" -#include "kortex_driver/generated/interconnectcyclic_proto_converter.h" -#include "kortex_driver/generated/basecyclic_proto_converter.h" -#include "kortex_driver/generated/controlconfig_proto_converter.h" -#include "kortex_driver/generated/deviceconfig_proto_converter.h" -#include "kortex_driver/generated/devicemanager_proto_converter.h" -#include "kortex_driver/generated/interconnectconfig_proto_converter.h" -#include "kortex_driver/generated/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" #include "kortex_driver/CompleteProductConfiguration.h" diff --git a/kortex_driver/include/kortex_driver/generated/productconfiguration_ros_converter.h b/kortex_driver/include/kortex_driver/generated/robot/productconfiguration_ros_converter.h similarity index 54% rename from kortex_driver/include/kortex_driver/generated/productconfiguration_ros_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/productconfiguration_ros_converter.h index 4998b0a2..757cc526 100644 --- a/kortex_driver/include/kortex_driver/generated/productconfiguration_ros_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/productconfiguration_ros_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_ros_converter.h" -#include "kortex_driver/generated/actuatorconfig_ros_converter.h" -#include "kortex_driver/generated/actuatorcyclic_ros_converter.h" -#include "kortex_driver/generated/base_ros_converter.h" -#include "kortex_driver/generated/grippercyclic_ros_converter.h" -#include "kortex_driver/generated/interconnectcyclic_ros_converter.h" -#include "kortex_driver/generated/basecyclic_ros_converter.h" -#include "kortex_driver/generated/controlconfig_ros_converter.h" -#include "kortex_driver/generated/deviceconfig_ros_converter.h" -#include "kortex_driver/generated/devicemanager_ros_converter.h" -#include "kortex_driver/generated/interconnectconfig_ros_converter.h" -#include "kortex_driver/generated/visionconfig_ros_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" #include "kortex_driver/CompleteProductConfiguration.h" diff --git a/kortex_driver/include/kortex_driver/generated/visionconfig_proto_converter.h b/kortex_driver/include/kortex_driver/generated/robot/visionconfig_proto_converter.h similarity index 78% rename from kortex_driver/include/kortex_driver/generated/visionconfig_proto_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/visionconfig_proto_converter.h index 2b643df0..ed757cda 100644 --- a/kortex_driver/include/kortex_driver/generated/visionconfig_proto_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/visionconfig_proto_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_proto_converter.h" -#include "kortex_driver/generated/actuatorconfig_proto_converter.h" -#include "kortex_driver/generated/actuatorcyclic_proto_converter.h" -#include "kortex_driver/generated/productconfiguration_proto_converter.h" -#include "kortex_driver/generated/base_proto_converter.h" -#include "kortex_driver/generated/grippercyclic_proto_converter.h" -#include "kortex_driver/generated/interconnectcyclic_proto_converter.h" -#include "kortex_driver/generated/basecyclic_proto_converter.h" -#include "kortex_driver/generated/controlconfig_proto_converter.h" -#include "kortex_driver/generated/deviceconfig_proto_converter.h" -#include "kortex_driver/generated/devicemanager_proto_converter.h" -#include "kortex_driver/generated/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" #include "kortex_driver/SensorSettings.h" diff --git a/kortex_driver/include/kortex_driver/generated/visionconfig_ros_converter.h b/kortex_driver/include/kortex_driver/generated/robot/visionconfig_ros_converter.h similarity index 78% rename from kortex_driver/include/kortex_driver/generated/visionconfig_ros_converter.h rename to kortex_driver/include/kortex_driver/generated/robot/visionconfig_ros_converter.h index 74cca282..3d4a961f 100644 --- a/kortex_driver/include/kortex_driver/generated/visionconfig_ros_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/visionconfig_ros_converter.h @@ -27,18 +27,18 @@ #include -#include "kortex_driver/generated/common_ros_converter.h" -#include "kortex_driver/generated/actuatorconfig_ros_converter.h" -#include "kortex_driver/generated/actuatorcyclic_ros_converter.h" -#include "kortex_driver/generated/productconfiguration_ros_converter.h" -#include "kortex_driver/generated/base_ros_converter.h" -#include "kortex_driver/generated/grippercyclic_ros_converter.h" -#include "kortex_driver/generated/interconnectcyclic_ros_converter.h" -#include "kortex_driver/generated/basecyclic_ros_converter.h" -#include "kortex_driver/generated/controlconfig_ros_converter.h" -#include "kortex_driver/generated/deviceconfig_ros_converter.h" -#include "kortex_driver/generated/devicemanager_ros_converter.h" -#include "kortex_driver/generated/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" #include "kortex_driver/SensorSettings.h" diff --git a/kortex_driver/include/kortex_driver/generated/robot/visionconfig_services.h b/kortex_driver/include/kortex_driver/generated/robot/visionconfig_services.h new file mode 100644 index 00000000..32e0129a --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/robot/visionconfig_services.h @@ -0,0 +1,54 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_VISIONCONFIG_ROBOT_SERVICES_H_ +#define _KORTEX_VISIONCONFIG_ROBOT_SERVICES_H_ + +#include "kortex_driver/generated/interfaces/visionconfig_services_interface.h" + +#include +#include + +using namespace std; + +class VisionConfigRobotServices : public IVisionConfigServices +{ + public: + VisionConfigRobotServices(ros::NodeHandle& node_handle, Kinova::Api::VisionConfig::VisionConfigClient* visionconfig, uint32_t device_id, uint32_t timeout_ms); + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; + virtual bool SetSensorSettings(kortex_driver::SetSensorSettings::Request &req, kortex_driver::SetSensorSettings::Response &res) override; + virtual bool GetSensorSettings(kortex_driver::GetSensorSettings::Request &req, kortex_driver::GetSensorSettings::Response &res) override; + virtual bool GetOptionValue(kortex_driver::GetOptionValue::Request &req, kortex_driver::GetOptionValue::Response &res) override; + virtual bool SetOptionValue(kortex_driver::SetOptionValue::Request &req, kortex_driver::SetOptionValue::Response &res) override; + virtual bool GetOptionInformation(kortex_driver::GetOptionInformation::Request &req, kortex_driver::GetOptionInformation::Response &res) override; + virtual bool OnNotificationVisionTopic(kortex_driver::OnNotificationVisionTopic::Request &req, kortex_driver::OnNotificationVisionTopic::Response &res) override; + virtual void cb_VisionTopic(Kinova::Api::VisionConfig::VisionNotification notif) override; + virtual bool DoSensorFocusAction(kortex_driver::DoSensorFocusAction::Request &req, kortex_driver::DoSensorFocusAction::Response &res) override; + virtual bool GetIntrinsicParameters(kortex_driver::GetIntrinsicParameters::Request &req, kortex_driver::GetIntrinsicParameters::Response &res) override; + virtual bool GetIntrinsicParametersProfile(kortex_driver::GetIntrinsicParametersProfile::Request &req, kortex_driver::GetIntrinsicParametersProfile::Response &res) override; + virtual bool SetIntrinsicParameters(kortex_driver::SetIntrinsicParameters::Request &req, kortex_driver::SetIntrinsicParameters::Response &res) override; + virtual bool GetExtrinsicParameters(kortex_driver::GetExtrinsicParameters::Request &req, kortex_driver::GetExtrinsicParameters::Response &res) override; + virtual bool SetExtrinsicParameters(kortex_driver::SetExtrinsicParameters::Request &req, kortex_driver::SetExtrinsicParameters::Response &res) override; + +private: + uint32_t m_current_device_id; + Kinova::Api::RouterClientSendOptions m_api_options; + + Kinova::Api::VisionConfig::VisionConfigClient* m_visionconfig; +}; +#endif diff --git a/kortex_driver/include/kortex_driver/generated/simulation/actuatorconfig_services.h b/kortex_driver/include/kortex_driver/generated/simulation/actuatorconfig_services.h new file mode 100644 index 00000000..e5a38be3 --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/simulation/actuatorconfig_services.h @@ -0,0 +1,73 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_ACTUATORCONFIG_SIMULATION_SERVICES_H_ +#define _KORTEX_ACTUATORCONFIG_SIMULATION_SERVICES_H_ + +#include "kortex_driver/generated/interfaces/actuatorconfig_services_interface.h" + +using namespace std; + +class ActuatorConfigSimulationServices : public IActuatorConfigServices +{ + public: + ActuatorConfigSimulationServices(ros::NodeHandle& node_handle); + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; + std::function GetAxisOffsetsHandler = nullptr; + virtual bool GetAxisOffsets(kortex_driver::GetAxisOffsets::Request &req, kortex_driver::GetAxisOffsets::Response &res) override; + std::function SetAxisOffsetsHandler = nullptr; + virtual bool SetAxisOffsets(kortex_driver::SetAxisOffsets::Request &req, kortex_driver::SetAxisOffsets::Response &res) override; + std::function SetTorqueOffsetHandler = nullptr; + virtual bool SetTorqueOffset(kortex_driver::SetTorqueOffset::Request &req, kortex_driver::SetTorqueOffset::Response &res) override; + std::function ActuatorConfig_GetControlModeHandler = nullptr; + virtual bool ActuatorConfig_GetControlMode(kortex_driver::ActuatorConfig_GetControlMode::Request &req, kortex_driver::ActuatorConfig_GetControlMode::Response &res) override; + std::function SetControlModeHandler = nullptr; + virtual bool SetControlMode(kortex_driver::SetControlMode::Request &req, kortex_driver::SetControlMode::Response &res) override; + std::function GetActivatedControlLoopHandler = nullptr; + virtual bool GetActivatedControlLoop(kortex_driver::GetActivatedControlLoop::Request &req, kortex_driver::GetActivatedControlLoop::Response &res) override; + std::function SetActivatedControlLoopHandler = nullptr; + virtual bool SetActivatedControlLoop(kortex_driver::SetActivatedControlLoop::Request &req, kortex_driver::SetActivatedControlLoop::Response &res) override; + std::function GetControlLoopParametersHandler = nullptr; + virtual bool GetControlLoopParameters(kortex_driver::GetControlLoopParameters::Request &req, kortex_driver::GetControlLoopParameters::Response &res) override; + std::function SetControlLoopParametersHandler = nullptr; + virtual bool SetControlLoopParameters(kortex_driver::SetControlLoopParameters::Request &req, kortex_driver::SetControlLoopParameters::Response &res) override; + std::function SelectCustomDataHandler = nullptr; + virtual bool SelectCustomData(kortex_driver::SelectCustomData::Request &req, kortex_driver::SelectCustomData::Response &res) override; + std::function GetSelectedCustomDataHandler = nullptr; + virtual bool GetSelectedCustomData(kortex_driver::GetSelectedCustomData::Request &req, kortex_driver::GetSelectedCustomData::Response &res) override; + std::function SetCommandModeHandler = nullptr; + virtual bool SetCommandMode(kortex_driver::SetCommandMode::Request &req, kortex_driver::SetCommandMode::Response &res) override; + std::function ActuatorConfig_ClearFaultsHandler = nullptr; + virtual bool ActuatorConfig_ClearFaults(kortex_driver::ActuatorConfig_ClearFaults::Request &req, kortex_driver::ActuatorConfig_ClearFaults::Response &res) override; + std::function SetServoingHandler = nullptr; + virtual bool SetServoing(kortex_driver::SetServoing::Request &req, kortex_driver::SetServoing::Response &res) override; + std::function MoveToPositionHandler = nullptr; + virtual bool MoveToPosition(kortex_driver::MoveToPosition::Request &req, kortex_driver::MoveToPosition::Response &res) override; + std::function GetCommandModeHandler = nullptr; + virtual bool GetCommandMode(kortex_driver::GetCommandMode::Request &req, kortex_driver::GetCommandMode::Response &res) override; + std::function GetServoingHandler = nullptr; + virtual bool GetServoing(kortex_driver::GetServoing::Request &req, kortex_driver::GetServoing::Response &res) override; + std::function GetTorqueOffsetHandler = nullptr; + virtual bool GetTorqueOffset(kortex_driver::GetTorqueOffset::Request &req, kortex_driver::GetTorqueOffset::Response &res) override; + std::function SetCoggingFeedforwardModeHandler = nullptr; + virtual bool SetCoggingFeedforwardMode(kortex_driver::SetCoggingFeedforwardMode::Request &req, kortex_driver::SetCoggingFeedforwardMode::Response &res) override; + std::function GetCoggingFeedforwardModeHandler = nullptr; + virtual bool GetCoggingFeedforwardMode(kortex_driver::GetCoggingFeedforwardMode::Request &req, kortex_driver::GetCoggingFeedforwardMode::Response &res) override; + +}; +#endif diff --git a/kortex_driver/include/kortex_driver/generated/simulation/base_services.h b/kortex_driver/include/kortex_driver/generated/simulation/base_services.h new file mode 100644 index 00000000..d38aa0b2 --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/simulation/base_services.h @@ -0,0 +1,335 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_BASE_SIMULATION_SERVICES_H_ +#define _KORTEX_BASE_SIMULATION_SERVICES_H_ + +#include "kortex_driver/generated/interfaces/base_services_interface.h" + +using namespace std; + +class BaseSimulationServices : public IBaseServices +{ + public: + BaseSimulationServices(ros::NodeHandle& node_handle); + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; + std::function CreateUserProfileHandler = nullptr; + virtual bool CreateUserProfile(kortex_driver::CreateUserProfile::Request &req, kortex_driver::CreateUserProfile::Response &res) override; + std::function UpdateUserProfileHandler = nullptr; + virtual bool UpdateUserProfile(kortex_driver::UpdateUserProfile::Request &req, kortex_driver::UpdateUserProfile::Response &res) override; + std::function ReadUserProfileHandler = nullptr; + virtual bool ReadUserProfile(kortex_driver::ReadUserProfile::Request &req, kortex_driver::ReadUserProfile::Response &res) override; + std::function DeleteUserProfileHandler = nullptr; + virtual bool DeleteUserProfile(kortex_driver::DeleteUserProfile::Request &req, kortex_driver::DeleteUserProfile::Response &res) override; + std::function ReadAllUserProfilesHandler = nullptr; + virtual bool ReadAllUserProfiles(kortex_driver::ReadAllUserProfiles::Request &req, kortex_driver::ReadAllUserProfiles::Response &res) override; + std::function ReadAllUsersHandler = nullptr; + virtual bool ReadAllUsers(kortex_driver::ReadAllUsers::Request &req, kortex_driver::ReadAllUsers::Response &res) override; + std::function ChangePasswordHandler = nullptr; + virtual bool ChangePassword(kortex_driver::ChangePassword::Request &req, kortex_driver::ChangePassword::Response &res) override; + std::function CreateSequenceHandler = nullptr; + virtual bool CreateSequence(kortex_driver::CreateSequence::Request &req, kortex_driver::CreateSequence::Response &res) override; + std::function UpdateSequenceHandler = nullptr; + virtual bool UpdateSequence(kortex_driver::UpdateSequence::Request &req, kortex_driver::UpdateSequence::Response &res) override; + std::function ReadSequenceHandler = nullptr; + virtual bool ReadSequence(kortex_driver::ReadSequence::Request &req, kortex_driver::ReadSequence::Response &res) override; + std::function DeleteSequenceHandler = nullptr; + virtual bool DeleteSequence(kortex_driver::DeleteSequence::Request &req, kortex_driver::DeleteSequence::Response &res) override; + std::function ReadAllSequencesHandler = nullptr; + virtual bool ReadAllSequences(kortex_driver::ReadAllSequences::Request &req, kortex_driver::ReadAllSequences::Response &res) override; + std::function PlaySequenceHandler = nullptr; + virtual bool PlaySequence(kortex_driver::PlaySequence::Request &req, kortex_driver::PlaySequence::Response &res) override; + std::function PlayAdvancedSequenceHandler = nullptr; + virtual bool PlayAdvancedSequence(kortex_driver::PlayAdvancedSequence::Request &req, kortex_driver::PlayAdvancedSequence::Response &res) override; + std::function StopSequenceHandler = nullptr; + virtual bool StopSequence(kortex_driver::StopSequence::Request &req, kortex_driver::StopSequence::Response &res) override; + std::function PauseSequenceHandler = nullptr; + virtual bool PauseSequence(kortex_driver::PauseSequence::Request &req, kortex_driver::PauseSequence::Response &res) override; + std::function ResumeSequenceHandler = nullptr; + virtual bool ResumeSequence(kortex_driver::ResumeSequence::Request &req, kortex_driver::ResumeSequence::Response &res) override; + std::function CreateProtectionZoneHandler = nullptr; + virtual bool CreateProtectionZone(kortex_driver::CreateProtectionZone::Request &req, kortex_driver::CreateProtectionZone::Response &res) override; + std::function UpdateProtectionZoneHandler = nullptr; + virtual bool UpdateProtectionZone(kortex_driver::UpdateProtectionZone::Request &req, kortex_driver::UpdateProtectionZone::Response &res) override; + std::function ReadProtectionZoneHandler = nullptr; + virtual bool ReadProtectionZone(kortex_driver::ReadProtectionZone::Request &req, kortex_driver::ReadProtectionZone::Response &res) override; + std::function DeleteProtectionZoneHandler = nullptr; + virtual bool DeleteProtectionZone(kortex_driver::DeleteProtectionZone::Request &req, kortex_driver::DeleteProtectionZone::Response &res) override; + std::function ReadAllProtectionZonesHandler = nullptr; + virtual bool ReadAllProtectionZones(kortex_driver::ReadAllProtectionZones::Request &req, kortex_driver::ReadAllProtectionZones::Response &res) override; + std::function CreateMappingHandler = nullptr; + virtual bool CreateMapping(kortex_driver::CreateMapping::Request &req, kortex_driver::CreateMapping::Response &res) override; + std::function ReadMappingHandler = nullptr; + virtual bool ReadMapping(kortex_driver::ReadMapping::Request &req, kortex_driver::ReadMapping::Response &res) override; + std::function UpdateMappingHandler = nullptr; + virtual bool UpdateMapping(kortex_driver::UpdateMapping::Request &req, kortex_driver::UpdateMapping::Response &res) override; + std::function DeleteMappingHandler = nullptr; + virtual bool DeleteMapping(kortex_driver::DeleteMapping::Request &req, kortex_driver::DeleteMapping::Response &res) override; + std::function ReadAllMappingsHandler = nullptr; + virtual bool ReadAllMappings(kortex_driver::ReadAllMappings::Request &req, kortex_driver::ReadAllMappings::Response &res) override; + std::function CreateMapHandler = nullptr; + virtual bool CreateMap(kortex_driver::CreateMap::Request &req, kortex_driver::CreateMap::Response &res) override; + std::function ReadMapHandler = nullptr; + virtual bool ReadMap(kortex_driver::ReadMap::Request &req, kortex_driver::ReadMap::Response &res) override; + std::function UpdateMapHandler = nullptr; + virtual bool UpdateMap(kortex_driver::UpdateMap::Request &req, kortex_driver::UpdateMap::Response &res) override; + std::function DeleteMapHandler = nullptr; + virtual bool DeleteMap(kortex_driver::DeleteMap::Request &req, kortex_driver::DeleteMap::Response &res) override; + std::function ReadAllMapsHandler = nullptr; + virtual bool ReadAllMaps(kortex_driver::ReadAllMaps::Request &req, kortex_driver::ReadAllMaps::Response &res) override; + std::function ActivateMapHandler = nullptr; + virtual bool ActivateMap(kortex_driver::ActivateMap::Request &req, kortex_driver::ActivateMap::Response &res) override; + std::function CreateActionHandler = nullptr; + virtual bool CreateAction(kortex_driver::CreateAction::Request &req, kortex_driver::CreateAction::Response &res) override; + std::function ReadActionHandler = nullptr; + virtual bool ReadAction(kortex_driver::ReadAction::Request &req, kortex_driver::ReadAction::Response &res) override; + std::function ReadAllActionsHandler = nullptr; + virtual bool ReadAllActions(kortex_driver::ReadAllActions::Request &req, kortex_driver::ReadAllActions::Response &res) override; + std::function DeleteActionHandler = nullptr; + virtual bool DeleteAction(kortex_driver::DeleteAction::Request &req, kortex_driver::DeleteAction::Response &res) override; + std::function UpdateActionHandler = nullptr; + virtual bool UpdateAction(kortex_driver::UpdateAction::Request &req, kortex_driver::UpdateAction::Response &res) override; + std::function ExecuteActionFromReferenceHandler = nullptr; + virtual bool ExecuteActionFromReference(kortex_driver::ExecuteActionFromReference::Request &req, kortex_driver::ExecuteActionFromReference::Response &res) override; + std::function ExecuteActionHandler = nullptr; + virtual bool ExecuteAction(kortex_driver::ExecuteAction::Request &req, kortex_driver::ExecuteAction::Response &res) override; + std::function PauseActionHandler = nullptr; + virtual bool PauseAction(kortex_driver::PauseAction::Request &req, kortex_driver::PauseAction::Response &res) override; + std::function StopActionHandler = nullptr; + virtual bool StopAction(kortex_driver::StopAction::Request &req, kortex_driver::StopAction::Response &res) override; + std::function ResumeActionHandler = nullptr; + virtual bool ResumeAction(kortex_driver::ResumeAction::Request &req, kortex_driver::ResumeAction::Response &res) override; + std::function GetIPv4ConfigurationHandler = nullptr; + virtual bool GetIPv4Configuration(kortex_driver::GetIPv4Configuration::Request &req, kortex_driver::GetIPv4Configuration::Response &res) override; + std::function SetIPv4ConfigurationHandler = nullptr; + virtual bool SetIPv4Configuration(kortex_driver::SetIPv4Configuration::Request &req, kortex_driver::SetIPv4Configuration::Response &res) override; + std::function SetCommunicationInterfaceEnableHandler = nullptr; + virtual bool SetCommunicationInterfaceEnable(kortex_driver::SetCommunicationInterfaceEnable::Request &req, kortex_driver::SetCommunicationInterfaceEnable::Response &res) override; + std::function IsCommunicationInterfaceEnableHandler = nullptr; + virtual bool IsCommunicationInterfaceEnable(kortex_driver::IsCommunicationInterfaceEnable::Request &req, kortex_driver::IsCommunicationInterfaceEnable::Response &res) override; + std::function GetAvailableWifiHandler = nullptr; + virtual bool GetAvailableWifi(kortex_driver::GetAvailableWifi::Request &req, kortex_driver::GetAvailableWifi::Response &res) override; + std::function GetWifiInformationHandler = nullptr; + virtual bool GetWifiInformation(kortex_driver::GetWifiInformation::Request &req, kortex_driver::GetWifiInformation::Response &res) override; + std::function AddWifiConfigurationHandler = nullptr; + virtual bool AddWifiConfiguration(kortex_driver::AddWifiConfiguration::Request &req, kortex_driver::AddWifiConfiguration::Response &res) override; + std::function DeleteWifiConfigurationHandler = nullptr; + virtual bool DeleteWifiConfiguration(kortex_driver::DeleteWifiConfiguration::Request &req, kortex_driver::DeleteWifiConfiguration::Response &res) override; + std::function GetAllConfiguredWifisHandler = nullptr; + virtual bool GetAllConfiguredWifis(kortex_driver::GetAllConfiguredWifis::Request &req, kortex_driver::GetAllConfiguredWifis::Response &res) override; + std::function ConnectWifiHandler = nullptr; + virtual bool ConnectWifi(kortex_driver::ConnectWifi::Request &req, kortex_driver::ConnectWifi::Response &res) override; + std::function DisconnectWifiHandler = nullptr; + virtual bool DisconnectWifi(kortex_driver::DisconnectWifi::Request &req, kortex_driver::DisconnectWifi::Response &res) override; + std::function GetConnectedWifiInformationHandler = nullptr; + virtual bool GetConnectedWifiInformation(kortex_driver::GetConnectedWifiInformation::Request &req, kortex_driver::GetConnectedWifiInformation::Response &res) override; + std::function Base_UnsubscribeHandler = nullptr; + virtual bool Base_Unsubscribe(kortex_driver::Base_Unsubscribe::Request &req, kortex_driver::Base_Unsubscribe::Response &res) override; + std::function OnNotificationConfigurationChangeTopicHandler = nullptr; + virtual bool OnNotificationConfigurationChangeTopic(kortex_driver::OnNotificationConfigurationChangeTopic::Request &req, kortex_driver::OnNotificationConfigurationChangeTopic::Response &res) override; + virtual void cb_ConfigurationChangeTopic(Kinova::Api::Base::ConfigurationChangeNotification notif) override; + std::function OnNotificationMappingInfoTopicHandler = nullptr; + virtual bool OnNotificationMappingInfoTopic(kortex_driver::OnNotificationMappingInfoTopic::Request &req, kortex_driver::OnNotificationMappingInfoTopic::Response &res) override; + virtual void cb_MappingInfoTopic(Kinova::Api::Base::MappingInfoNotification notif) override; + std::function OnNotificationControlModeTopicHandler = nullptr; + virtual bool OnNotificationControlModeTopic(kortex_driver::OnNotificationControlModeTopic::Request &req, kortex_driver::OnNotificationControlModeTopic::Response &res) override; + virtual void cb_ControlModeTopic(Kinova::Api::Base::ControlModeNotification notif) override; + std::function OnNotificationOperatingModeTopicHandler = nullptr; + virtual bool OnNotificationOperatingModeTopic(kortex_driver::OnNotificationOperatingModeTopic::Request &req, kortex_driver::OnNotificationOperatingModeTopic::Response &res) override; + virtual void cb_OperatingModeTopic(Kinova::Api::Base::OperatingModeNotification notif) override; + std::function OnNotificationSequenceInfoTopicHandler = nullptr; + virtual bool OnNotificationSequenceInfoTopic(kortex_driver::OnNotificationSequenceInfoTopic::Request &req, kortex_driver::OnNotificationSequenceInfoTopic::Response &res) override; + virtual void cb_SequenceInfoTopic(Kinova::Api::Base::SequenceInfoNotification notif) override; + std::function OnNotificationProtectionZoneTopicHandler = nullptr; + virtual bool OnNotificationProtectionZoneTopic(kortex_driver::OnNotificationProtectionZoneTopic::Request &req, kortex_driver::OnNotificationProtectionZoneTopic::Response &res) override; + virtual void cb_ProtectionZoneTopic(Kinova::Api::Base::ProtectionZoneNotification notif) override; + std::function OnNotificationUserTopicHandler = nullptr; + virtual bool OnNotificationUserTopic(kortex_driver::OnNotificationUserTopic::Request &req, kortex_driver::OnNotificationUserTopic::Response &res) override; + virtual void cb_UserTopic(Kinova::Api::Base::UserNotification notif) override; + std::function OnNotificationControllerTopicHandler = nullptr; + virtual bool OnNotificationControllerTopic(kortex_driver::OnNotificationControllerTopic::Request &req, kortex_driver::OnNotificationControllerTopic::Response &res) override; + virtual void cb_ControllerTopic(Kinova::Api::Base::ControllerNotification notif) override; + std::function OnNotificationActionTopicHandler = nullptr; + virtual bool OnNotificationActionTopic(kortex_driver::OnNotificationActionTopic::Request &req, kortex_driver::OnNotificationActionTopic::Response &res) override; + virtual void cb_ActionTopic(Kinova::Api::Base::ActionNotification notif) override; + std::function OnNotificationRobotEventTopicHandler = nullptr; + virtual bool OnNotificationRobotEventTopic(kortex_driver::OnNotificationRobotEventTopic::Request &req, kortex_driver::OnNotificationRobotEventTopic::Response &res) override; + virtual void cb_RobotEventTopic(Kinova::Api::Base::RobotEventNotification notif) override; + std::function PlayCartesianTrajectoryHandler = nullptr; + virtual bool PlayCartesianTrajectory(kortex_driver::PlayCartesianTrajectory::Request &req, kortex_driver::PlayCartesianTrajectory::Response &res) override; + std::function PlayCartesianTrajectoryPositionHandler = nullptr; + virtual bool PlayCartesianTrajectoryPosition(kortex_driver::PlayCartesianTrajectoryPosition::Request &req, kortex_driver::PlayCartesianTrajectoryPosition::Response &res) override; + std::function PlayCartesianTrajectoryOrientationHandler = nullptr; + virtual bool PlayCartesianTrajectoryOrientation(kortex_driver::PlayCartesianTrajectoryOrientation::Request &req, kortex_driver::PlayCartesianTrajectoryOrientation::Response &res) override; + std::function StopHandler = nullptr; + virtual bool Stop(kortex_driver::Stop::Request &req, kortex_driver::Stop::Response &res) override; + std::function GetMeasuredCartesianPoseHandler = nullptr; + virtual bool GetMeasuredCartesianPose(kortex_driver::GetMeasuredCartesianPose::Request &req, kortex_driver::GetMeasuredCartesianPose::Response &res) override; + std::function SendWrenchCommandHandler = nullptr; + virtual bool SendWrenchCommand(kortex_driver::SendWrenchCommand::Request &req, kortex_driver::SendWrenchCommand::Response &res) override; + std::function SendWrenchJoystickCommandHandler = nullptr; + virtual bool SendWrenchJoystickCommand(kortex_driver::SendWrenchJoystickCommand::Request &req, kortex_driver::SendWrenchJoystickCommand::Response &res) override; + std::function SendTwistJoystickCommandHandler = nullptr; + virtual bool SendTwistJoystickCommand(kortex_driver::SendTwistJoystickCommand::Request &req, kortex_driver::SendTwistJoystickCommand::Response &res) override; + std::function SendTwistCommandHandler = nullptr; + virtual bool SendTwistCommand(kortex_driver::SendTwistCommand::Request &req, kortex_driver::SendTwistCommand::Response &res) override; + std::function PlayJointTrajectoryHandler = nullptr; + virtual bool PlayJointTrajectory(kortex_driver::PlayJointTrajectory::Request &req, kortex_driver::PlayJointTrajectory::Response &res) override; + std::function PlaySelectedJointTrajectoryHandler = nullptr; + virtual bool PlaySelectedJointTrajectory(kortex_driver::PlaySelectedJointTrajectory::Request &req, kortex_driver::PlaySelectedJointTrajectory::Response &res) override; + std::function GetMeasuredJointAnglesHandler = nullptr; + virtual bool GetMeasuredJointAngles(kortex_driver::GetMeasuredJointAngles::Request &req, kortex_driver::GetMeasuredJointAngles::Response &res) override; + std::function SendJointSpeedsCommandHandler = nullptr; + virtual bool SendJointSpeedsCommand(kortex_driver::SendJointSpeedsCommand::Request &req, kortex_driver::SendJointSpeedsCommand::Response &res) override; + std::function SendSelectedJointSpeedCommandHandler = nullptr; + virtual bool SendSelectedJointSpeedCommand(kortex_driver::SendSelectedJointSpeedCommand::Request &req, kortex_driver::SendSelectedJointSpeedCommand::Response &res) override; + std::function SendGripperCommandHandler = nullptr; + virtual bool SendGripperCommand(kortex_driver::SendGripperCommand::Request &req, kortex_driver::SendGripperCommand::Response &res) override; + std::function GetMeasuredGripperMovementHandler = nullptr; + virtual bool GetMeasuredGripperMovement(kortex_driver::GetMeasuredGripperMovement::Request &req, kortex_driver::GetMeasuredGripperMovement::Response &res) override; + std::function SetAdmittanceHandler = nullptr; + virtual bool SetAdmittance(kortex_driver::SetAdmittance::Request &req, kortex_driver::SetAdmittance::Response &res) override; + std::function SetOperatingModeHandler = nullptr; + virtual bool SetOperatingMode(kortex_driver::SetOperatingMode::Request &req, kortex_driver::SetOperatingMode::Response &res) override; + std::function ApplyEmergencyStopHandler = nullptr; + virtual bool ApplyEmergencyStop(kortex_driver::ApplyEmergencyStop::Request &req, kortex_driver::ApplyEmergencyStop::Response &res) override; + std::function Base_ClearFaultsHandler = nullptr; + virtual bool Base_ClearFaults(kortex_driver::Base_ClearFaults::Request &req, kortex_driver::Base_ClearFaults::Response &res) override; + std::function Base_GetControlModeHandler = nullptr; + virtual bool Base_GetControlMode(kortex_driver::Base_GetControlMode::Request &req, kortex_driver::Base_GetControlMode::Response &res) override; + std::function GetOperatingModeHandler = nullptr; + virtual bool GetOperatingMode(kortex_driver::GetOperatingMode::Request &req, kortex_driver::GetOperatingMode::Response &res) override; + std::function SetServoingModeHandler = nullptr; + virtual bool SetServoingMode(kortex_driver::SetServoingMode::Request &req, kortex_driver::SetServoingMode::Response &res) override; + std::function GetServoingModeHandler = nullptr; + virtual bool GetServoingMode(kortex_driver::GetServoingMode::Request &req, kortex_driver::GetServoingMode::Response &res) override; + std::function OnNotificationServoingModeTopicHandler = nullptr; + virtual bool OnNotificationServoingModeTopic(kortex_driver::OnNotificationServoingModeTopic::Request &req, kortex_driver::OnNotificationServoingModeTopic::Response &res) override; + virtual void cb_ServoingModeTopic(Kinova::Api::Base::ServoingModeNotification notif) override; + std::function RestoreFactorySettingsHandler = nullptr; + virtual bool RestoreFactorySettings(kortex_driver::RestoreFactorySettings::Request &req, kortex_driver::RestoreFactorySettings::Response &res) override; + std::function OnNotificationFactoryTopicHandler = nullptr; + virtual bool OnNotificationFactoryTopic(kortex_driver::OnNotificationFactoryTopic::Request &req, kortex_driver::OnNotificationFactoryTopic::Response &res) override; + virtual void cb_FactoryTopic(Kinova::Api::Base::FactoryNotification notif) override; + std::function GetAllConnectedControllersHandler = nullptr; + virtual bool GetAllConnectedControllers(kortex_driver::GetAllConnectedControllers::Request &req, kortex_driver::GetAllConnectedControllers::Response &res) override; + std::function GetControllerStateHandler = nullptr; + virtual bool GetControllerState(kortex_driver::GetControllerState::Request &req, kortex_driver::GetControllerState::Response &res) override; + std::function GetActuatorCountHandler = nullptr; + virtual bool GetActuatorCount(kortex_driver::GetActuatorCount::Request &req, kortex_driver::GetActuatorCount::Response &res) override; + std::function StartWifiScanHandler = nullptr; + virtual bool StartWifiScan(kortex_driver::StartWifiScan::Request &req, kortex_driver::StartWifiScan::Response &res) override; + std::function GetConfiguredWifiHandler = nullptr; + virtual bool GetConfiguredWifi(kortex_driver::GetConfiguredWifi::Request &req, kortex_driver::GetConfiguredWifi::Response &res) override; + std::function OnNotificationNetworkTopicHandler = nullptr; + virtual bool OnNotificationNetworkTopic(kortex_driver::OnNotificationNetworkTopic::Request &req, kortex_driver::OnNotificationNetworkTopic::Response &res) override; + virtual void cb_NetworkTopic(Kinova::Api::Base::NetworkNotification notif) override; + std::function GetArmStateHandler = nullptr; + virtual bool GetArmState(kortex_driver::GetArmState::Request &req, kortex_driver::GetArmState::Response &res) override; + std::function OnNotificationArmStateTopicHandler = nullptr; + virtual bool OnNotificationArmStateTopic(kortex_driver::OnNotificationArmStateTopic::Request &req, kortex_driver::OnNotificationArmStateTopic::Response &res) override; + virtual void cb_ArmStateTopic(Kinova::Api::Base::ArmStateNotification notif) override; + std::function GetIPv4InformationHandler = nullptr; + virtual bool GetIPv4Information(kortex_driver::GetIPv4Information::Request &req, kortex_driver::GetIPv4Information::Response &res) override; + std::function SetWifiCountryCodeHandler = nullptr; + virtual bool SetWifiCountryCode(kortex_driver::SetWifiCountryCode::Request &req, kortex_driver::SetWifiCountryCode::Response &res) override; + std::function GetWifiCountryCodeHandler = nullptr; + virtual bool GetWifiCountryCode(kortex_driver::GetWifiCountryCode::Request &req, kortex_driver::GetWifiCountryCode::Response &res) override; + std::function Base_SetCapSenseConfigHandler = nullptr; + virtual bool Base_SetCapSenseConfig(kortex_driver::Base_SetCapSenseConfig::Request &req, kortex_driver::Base_SetCapSenseConfig::Response &res) override; + std::function Base_GetCapSenseConfigHandler = nullptr; + virtual bool Base_GetCapSenseConfig(kortex_driver::Base_GetCapSenseConfig::Request &req, kortex_driver::Base_GetCapSenseConfig::Response &res) override; + std::function GetAllJointsSpeedHardLimitationHandler = nullptr; + virtual bool GetAllJointsSpeedHardLimitation(kortex_driver::GetAllJointsSpeedHardLimitation::Request &req, kortex_driver::GetAllJointsSpeedHardLimitation::Response &res) override; + std::function GetAllJointsTorqueHardLimitationHandler = nullptr; + virtual bool GetAllJointsTorqueHardLimitation(kortex_driver::GetAllJointsTorqueHardLimitation::Request &req, kortex_driver::GetAllJointsTorqueHardLimitation::Response &res) override; + std::function GetTwistHardLimitationHandler = nullptr; + virtual bool GetTwistHardLimitation(kortex_driver::GetTwistHardLimitation::Request &req, kortex_driver::GetTwistHardLimitation::Response &res) override; + std::function GetWrenchHardLimitationHandler = nullptr; + virtual bool GetWrenchHardLimitation(kortex_driver::GetWrenchHardLimitation::Request &req, kortex_driver::GetWrenchHardLimitation::Response &res) override; + std::function SendJointSpeedsJoystickCommandHandler = nullptr; + virtual bool SendJointSpeedsJoystickCommand(kortex_driver::SendJointSpeedsJoystickCommand::Request &req, kortex_driver::SendJointSpeedsJoystickCommand::Response &res) override; + std::function SendSelectedJointSpeedJoystickCommandHandler = nullptr; + virtual bool SendSelectedJointSpeedJoystickCommand(kortex_driver::SendSelectedJointSpeedJoystickCommand::Request &req, kortex_driver::SendSelectedJointSpeedJoystickCommand::Response &res) override; + std::function EnableBridgeHandler = nullptr; + virtual bool EnableBridge(kortex_driver::EnableBridge::Request &req, kortex_driver::EnableBridge::Response &res) override; + std::function DisableBridgeHandler = nullptr; + virtual bool DisableBridge(kortex_driver::DisableBridge::Request &req, kortex_driver::DisableBridge::Response &res) override; + std::function GetBridgeListHandler = nullptr; + virtual bool GetBridgeList(kortex_driver::GetBridgeList::Request &req, kortex_driver::GetBridgeList::Response &res) override; + std::function GetBridgeConfigHandler = nullptr; + virtual bool GetBridgeConfig(kortex_driver::GetBridgeConfig::Request &req, kortex_driver::GetBridgeConfig::Response &res) override; + std::function PlayPreComputedJointTrajectoryHandler = nullptr; + virtual bool PlayPreComputedJointTrajectory(kortex_driver::PlayPreComputedJointTrajectory::Request &req, kortex_driver::PlayPreComputedJointTrajectory::Response &res) override; + std::function GetProductConfigurationHandler = nullptr; + virtual bool GetProductConfiguration(kortex_driver::GetProductConfiguration::Request &req, kortex_driver::GetProductConfiguration::Response &res) override; + std::function UpdateEndEffectorTypeConfigurationHandler = nullptr; + virtual bool UpdateEndEffectorTypeConfiguration(kortex_driver::UpdateEndEffectorTypeConfiguration::Request &req, kortex_driver::UpdateEndEffectorTypeConfiguration::Response &res) override; + std::function RestoreFactoryProductConfigurationHandler = nullptr; + virtual bool RestoreFactoryProductConfiguration(kortex_driver::RestoreFactoryProductConfiguration::Request &req, kortex_driver::RestoreFactoryProductConfiguration::Response &res) override; + std::function GetTrajectoryErrorReportHandler = nullptr; + virtual bool GetTrajectoryErrorReport(kortex_driver::GetTrajectoryErrorReport::Request &req, kortex_driver::GetTrajectoryErrorReport::Response &res) override; + std::function GetAllJointsSpeedSoftLimitationHandler = nullptr; + virtual bool GetAllJointsSpeedSoftLimitation(kortex_driver::GetAllJointsSpeedSoftLimitation::Request &req, kortex_driver::GetAllJointsSpeedSoftLimitation::Response &res) override; + std::function GetAllJointsTorqueSoftLimitationHandler = nullptr; + virtual bool GetAllJointsTorqueSoftLimitation(kortex_driver::GetAllJointsTorqueSoftLimitation::Request &req, kortex_driver::GetAllJointsTorqueSoftLimitation::Response &res) override; + std::function GetTwistSoftLimitationHandler = nullptr; + virtual bool GetTwistSoftLimitation(kortex_driver::GetTwistSoftLimitation::Request &req, kortex_driver::GetTwistSoftLimitation::Response &res) override; + std::function GetWrenchSoftLimitationHandler = nullptr; + virtual bool GetWrenchSoftLimitation(kortex_driver::GetWrenchSoftLimitation::Request &req, kortex_driver::GetWrenchSoftLimitation::Response &res) override; + std::function SetControllerConfigurationModeHandler = nullptr; + virtual bool SetControllerConfigurationMode(kortex_driver::SetControllerConfigurationMode::Request &req, kortex_driver::SetControllerConfigurationMode::Response &res) override; + std::function GetControllerConfigurationModeHandler = nullptr; + virtual bool GetControllerConfigurationMode(kortex_driver::GetControllerConfigurationMode::Request &req, kortex_driver::GetControllerConfigurationMode::Response &res) override; + std::function StartTeachingHandler = nullptr; + virtual bool StartTeaching(kortex_driver::StartTeaching::Request &req, kortex_driver::StartTeaching::Response &res) override; + std::function StopTeachingHandler = nullptr; + virtual bool StopTeaching(kortex_driver::StopTeaching::Request &req, kortex_driver::StopTeaching::Response &res) override; + std::function AddSequenceTasksHandler = nullptr; + virtual bool AddSequenceTasks(kortex_driver::AddSequenceTasks::Request &req, kortex_driver::AddSequenceTasks::Response &res) override; + std::function UpdateSequenceTaskHandler = nullptr; + virtual bool UpdateSequenceTask(kortex_driver::UpdateSequenceTask::Request &req, kortex_driver::UpdateSequenceTask::Response &res) override; + std::function SwapSequenceTasksHandler = nullptr; + virtual bool SwapSequenceTasks(kortex_driver::SwapSequenceTasks::Request &req, kortex_driver::SwapSequenceTasks::Response &res) override; + std::function ReadSequenceTaskHandler = nullptr; + virtual bool ReadSequenceTask(kortex_driver::ReadSequenceTask::Request &req, kortex_driver::ReadSequenceTask::Response &res) override; + std::function ReadAllSequenceTasksHandler = nullptr; + virtual bool ReadAllSequenceTasks(kortex_driver::ReadAllSequenceTasks::Request &req, kortex_driver::ReadAllSequenceTasks::Response &res) override; + std::function DeleteSequenceTaskHandler = nullptr; + virtual bool DeleteSequenceTask(kortex_driver::DeleteSequenceTask::Request &req, kortex_driver::DeleteSequenceTask::Response &res) override; + std::function DeleteAllSequenceTasksHandler = nullptr; + virtual bool DeleteAllSequenceTasks(kortex_driver::DeleteAllSequenceTasks::Request &req, kortex_driver::DeleteAllSequenceTasks::Response &res) override; + std::function TakeSnapshotHandler = nullptr; + virtual bool TakeSnapshot(kortex_driver::TakeSnapshot::Request &req, kortex_driver::TakeSnapshot::Response &res) override; + std::function GetFirmwareBundleVersionsHandler = nullptr; + virtual bool GetFirmwareBundleVersions(kortex_driver::GetFirmwareBundleVersions::Request &req, kortex_driver::GetFirmwareBundleVersions::Response &res) override; + std::function MoveSequenceTaskHandler = nullptr; + virtual bool MoveSequenceTask(kortex_driver::MoveSequenceTask::Request &req, kortex_driver::MoveSequenceTask::Response &res) override; + std::function DuplicateMappingHandler = nullptr; + virtual bool DuplicateMapping(kortex_driver::DuplicateMapping::Request &req, kortex_driver::DuplicateMapping::Response &res) override; + std::function DuplicateMapHandler = nullptr; + virtual bool DuplicateMap(kortex_driver::DuplicateMap::Request &req, kortex_driver::DuplicateMap::Response &res) override; + std::function SetControllerConfigurationHandler = nullptr; + virtual bool SetControllerConfiguration(kortex_driver::SetControllerConfiguration::Request &req, kortex_driver::SetControllerConfiguration::Response &res) override; + std::function GetControllerConfigurationHandler = nullptr; + virtual bool GetControllerConfiguration(kortex_driver::GetControllerConfiguration::Request &req, kortex_driver::GetControllerConfiguration::Response &res) override; + std::function GetAllControllerConfigurationsHandler = nullptr; + virtual bool GetAllControllerConfigurations(kortex_driver::GetAllControllerConfigurations::Request &req, kortex_driver::GetAllControllerConfigurations::Response &res) override; + +}; +#endif diff --git a/kortex_driver/include/kortex_driver/generated/simulation/controlconfig_services.h b/kortex_driver/include/kortex_driver/generated/simulation/controlconfig_services.h new file mode 100644 index 00000000..8e6ce705 --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/simulation/controlconfig_services.h @@ -0,0 +1,92 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_CONTROLCONFIG_SIMULATION_SERVICES_H_ +#define _KORTEX_CONTROLCONFIG_SIMULATION_SERVICES_H_ + +#include "kortex_driver/generated/interfaces/controlconfig_services_interface.h" + +using namespace std; + +class ControlConfigSimulationServices : public IControlConfigServices +{ + public: + ControlConfigSimulationServices(ros::NodeHandle& node_handle); + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; + std::function SetGravityVectorHandler = nullptr; + virtual bool SetGravityVector(kortex_driver::SetGravityVector::Request &req, kortex_driver::SetGravityVector::Response &res) override; + std::function GetGravityVectorHandler = nullptr; + virtual bool GetGravityVector(kortex_driver::GetGravityVector::Request &req, kortex_driver::GetGravityVector::Response &res) override; + std::function SetPayloadInformationHandler = nullptr; + virtual bool SetPayloadInformation(kortex_driver::SetPayloadInformation::Request &req, kortex_driver::SetPayloadInformation::Response &res) override; + std::function GetPayloadInformationHandler = nullptr; + virtual bool GetPayloadInformation(kortex_driver::GetPayloadInformation::Request &req, kortex_driver::GetPayloadInformation::Response &res) override; + std::function SetToolConfigurationHandler = nullptr; + virtual bool SetToolConfiguration(kortex_driver::SetToolConfiguration::Request &req, kortex_driver::SetToolConfiguration::Response &res) override; + std::function GetToolConfigurationHandler = nullptr; + virtual bool GetToolConfiguration(kortex_driver::GetToolConfiguration::Request &req, kortex_driver::GetToolConfiguration::Response &res) override; + std::function OnNotificationControlConfigurationTopicHandler = nullptr; + virtual bool OnNotificationControlConfigurationTopic(kortex_driver::OnNotificationControlConfigurationTopic::Request &req, kortex_driver::OnNotificationControlConfigurationTopic::Response &res) override; + virtual void cb_ControlConfigurationTopic(Kinova::Api::ControlConfig::ControlConfigurationNotification notif) override; + std::function ControlConfig_UnsubscribeHandler = nullptr; + virtual bool ControlConfig_Unsubscribe(kortex_driver::ControlConfig_Unsubscribe::Request &req, kortex_driver::ControlConfig_Unsubscribe::Response &res) override; + std::function SetCartesianReferenceFrameHandler = nullptr; + virtual bool SetCartesianReferenceFrame(kortex_driver::SetCartesianReferenceFrame::Request &req, kortex_driver::SetCartesianReferenceFrame::Response &res) override; + std::function GetCartesianReferenceFrameHandler = nullptr; + virtual bool GetCartesianReferenceFrame(kortex_driver::GetCartesianReferenceFrame::Request &req, kortex_driver::GetCartesianReferenceFrame::Response &res) override; + std::function ControlConfig_GetControlModeHandler = nullptr; + virtual bool ControlConfig_GetControlMode(kortex_driver::ControlConfig_GetControlMode::Request &req, kortex_driver::ControlConfig_GetControlMode::Response &res) override; + std::function SetJointSpeedSoftLimitsHandler = nullptr; + virtual bool SetJointSpeedSoftLimits(kortex_driver::SetJointSpeedSoftLimits::Request &req, kortex_driver::SetJointSpeedSoftLimits::Response &res) override; + std::function SetTwistLinearSoftLimitHandler = nullptr; + virtual bool SetTwistLinearSoftLimit(kortex_driver::SetTwistLinearSoftLimit::Request &req, kortex_driver::SetTwistLinearSoftLimit::Response &res) override; + std::function SetTwistAngularSoftLimitHandler = nullptr; + virtual bool SetTwistAngularSoftLimit(kortex_driver::SetTwistAngularSoftLimit::Request &req, kortex_driver::SetTwistAngularSoftLimit::Response &res) override; + std::function SetJointAccelerationSoftLimitsHandler = nullptr; + virtual bool SetJointAccelerationSoftLimits(kortex_driver::SetJointAccelerationSoftLimits::Request &req, kortex_driver::SetJointAccelerationSoftLimits::Response &res) override; + std::function GetKinematicHardLimitsHandler = nullptr; + virtual bool GetKinematicHardLimits(kortex_driver::GetKinematicHardLimits::Request &req, kortex_driver::GetKinematicHardLimits::Response &res) override; + std::function GetKinematicSoftLimitsHandler = nullptr; + virtual bool GetKinematicSoftLimits(kortex_driver::GetKinematicSoftLimits::Request &req, kortex_driver::GetKinematicSoftLimits::Response &res) override; + std::function GetAllKinematicSoftLimitsHandler = nullptr; + virtual bool GetAllKinematicSoftLimits(kortex_driver::GetAllKinematicSoftLimits::Request &req, kortex_driver::GetAllKinematicSoftLimits::Response &res) override; + std::function SetDesiredLinearTwistHandler = nullptr; + virtual bool SetDesiredLinearTwist(kortex_driver::SetDesiredLinearTwist::Request &req, kortex_driver::SetDesiredLinearTwist::Response &res) override; + std::function SetDesiredAngularTwistHandler = nullptr; + virtual bool SetDesiredAngularTwist(kortex_driver::SetDesiredAngularTwist::Request &req, kortex_driver::SetDesiredAngularTwist::Response &res) override; + std::function SetDesiredJointSpeedsHandler = nullptr; + virtual bool SetDesiredJointSpeeds(kortex_driver::SetDesiredJointSpeeds::Request &req, kortex_driver::SetDesiredJointSpeeds::Response &res) override; + std::function GetDesiredSpeedsHandler = nullptr; + virtual bool GetDesiredSpeeds(kortex_driver::GetDesiredSpeeds::Request &req, kortex_driver::GetDesiredSpeeds::Response &res) override; + std::function ResetGravityVectorHandler = nullptr; + virtual bool ResetGravityVector(kortex_driver::ResetGravityVector::Request &req, kortex_driver::ResetGravityVector::Response &res) override; + std::function ResetPayloadInformationHandler = nullptr; + virtual bool ResetPayloadInformation(kortex_driver::ResetPayloadInformation::Request &req, kortex_driver::ResetPayloadInformation::Response &res) override; + std::function ResetToolConfigurationHandler = nullptr; + virtual bool ResetToolConfiguration(kortex_driver::ResetToolConfiguration::Request &req, kortex_driver::ResetToolConfiguration::Response &res) override; + std::function ResetJointSpeedSoftLimitsHandler = nullptr; + virtual bool ResetJointSpeedSoftLimits(kortex_driver::ResetJointSpeedSoftLimits::Request &req, kortex_driver::ResetJointSpeedSoftLimits::Response &res) override; + std::function ResetTwistLinearSoftLimitHandler = nullptr; + virtual bool ResetTwistLinearSoftLimit(kortex_driver::ResetTwistLinearSoftLimit::Request &req, kortex_driver::ResetTwistLinearSoftLimit::Response &res) override; + std::function ResetTwistAngularSoftLimitHandler = nullptr; + virtual bool ResetTwistAngularSoftLimit(kortex_driver::ResetTwistAngularSoftLimit::Request &req, kortex_driver::ResetTwistAngularSoftLimit::Response &res) override; + std::function ResetJointAccelerationSoftLimitsHandler = nullptr; + virtual bool ResetJointAccelerationSoftLimits(kortex_driver::ResetJointAccelerationSoftLimits::Request &req, kortex_driver::ResetJointAccelerationSoftLimits::Response &res) override; + +}; +#endif diff --git a/kortex_driver/include/kortex_driver/generated/simulation/deviceconfig_services.h b/kortex_driver/include/kortex_driver/generated/simulation/deviceconfig_services.h new file mode 100644 index 00000000..cd3429ec --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/simulation/deviceconfig_services.h @@ -0,0 +1,98 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_DEVICECONFIG_SIMULATION_SERVICES_H_ +#define _KORTEX_DEVICECONFIG_SIMULATION_SERVICES_H_ + +#include "kortex_driver/generated/interfaces/deviceconfig_services_interface.h" + +using namespace std; + +class DeviceConfigSimulationServices : public IDeviceConfigServices +{ + public: + DeviceConfigSimulationServices(ros::NodeHandle& node_handle); + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; + std::function GetRunModeHandler = nullptr; + virtual bool GetRunMode(kortex_driver::GetRunMode::Request &req, kortex_driver::GetRunMode::Response &res) override; + std::function SetRunModeHandler = nullptr; + virtual bool SetRunMode(kortex_driver::SetRunMode::Request &req, kortex_driver::SetRunMode::Response &res) override; + std::function GetDeviceTypeHandler = nullptr; + virtual bool GetDeviceType(kortex_driver::GetDeviceType::Request &req, kortex_driver::GetDeviceType::Response &res) override; + std::function GetFirmwareVersionHandler = nullptr; + virtual bool GetFirmwareVersion(kortex_driver::GetFirmwareVersion::Request &req, kortex_driver::GetFirmwareVersion::Response &res) override; + std::function GetBootloaderVersionHandler = nullptr; + virtual bool GetBootloaderVersion(kortex_driver::GetBootloaderVersion::Request &req, kortex_driver::GetBootloaderVersion::Response &res) override; + std::function GetModelNumberHandler = nullptr; + virtual bool GetModelNumber(kortex_driver::GetModelNumber::Request &req, kortex_driver::GetModelNumber::Response &res) override; + std::function GetPartNumberHandler = nullptr; + virtual bool GetPartNumber(kortex_driver::GetPartNumber::Request &req, kortex_driver::GetPartNumber::Response &res) override; + std::function GetSerialNumberHandler = nullptr; + virtual bool GetSerialNumber(kortex_driver::GetSerialNumber::Request &req, kortex_driver::GetSerialNumber::Response &res) override; + std::function GetMACAddressHandler = nullptr; + virtual bool GetMACAddress(kortex_driver::GetMACAddress::Request &req, kortex_driver::GetMACAddress::Response &res) override; + std::function GetIPv4SettingsHandler = nullptr; + virtual bool GetIPv4Settings(kortex_driver::GetIPv4Settings::Request &req, kortex_driver::GetIPv4Settings::Response &res) override; + std::function SetIPv4SettingsHandler = nullptr; + virtual bool SetIPv4Settings(kortex_driver::SetIPv4Settings::Request &req, kortex_driver::SetIPv4Settings::Response &res) override; + std::function GetPartNumberRevisionHandler = nullptr; + virtual bool GetPartNumberRevision(kortex_driver::GetPartNumberRevision::Request &req, kortex_driver::GetPartNumberRevision::Response &res) override; + std::function RebootRequestHandler = nullptr; + virtual bool RebootRequest(kortex_driver::RebootRequest::Request &req, kortex_driver::RebootRequest::Response &res) override; + std::function SetSafetyEnableHandler = nullptr; + virtual bool SetSafetyEnable(kortex_driver::SetSafetyEnable::Request &req, kortex_driver::SetSafetyEnable::Response &res) override; + std::function SetSafetyErrorThresholdHandler = nullptr; + virtual bool SetSafetyErrorThreshold(kortex_driver::SetSafetyErrorThreshold::Request &req, kortex_driver::SetSafetyErrorThreshold::Response &res) override; + std::function SetSafetyWarningThresholdHandler = nullptr; + virtual bool SetSafetyWarningThreshold(kortex_driver::SetSafetyWarningThreshold::Request &req, kortex_driver::SetSafetyWarningThreshold::Response &res) override; + std::function SetSafetyConfigurationHandler = nullptr; + virtual bool SetSafetyConfiguration(kortex_driver::SetSafetyConfiguration::Request &req, kortex_driver::SetSafetyConfiguration::Response &res) override; + std::function GetSafetyConfigurationHandler = nullptr; + virtual bool GetSafetyConfiguration(kortex_driver::GetSafetyConfiguration::Request &req, kortex_driver::GetSafetyConfiguration::Response &res) override; + std::function GetSafetyInformationHandler = nullptr; + virtual bool GetSafetyInformation(kortex_driver::GetSafetyInformation::Request &req, kortex_driver::GetSafetyInformation::Response &res) override; + std::function GetSafetyEnableHandler = nullptr; + virtual bool GetSafetyEnable(kortex_driver::GetSafetyEnable::Request &req, kortex_driver::GetSafetyEnable::Response &res) override; + std::function GetSafetyStatusHandler = nullptr; + virtual bool GetSafetyStatus(kortex_driver::GetSafetyStatus::Request &req, kortex_driver::GetSafetyStatus::Response &res) override; + std::function ClearAllSafetyStatusHandler = nullptr; + virtual bool ClearAllSafetyStatus(kortex_driver::ClearAllSafetyStatus::Request &req, kortex_driver::ClearAllSafetyStatus::Response &res) override; + std::function ClearSafetyStatusHandler = nullptr; + virtual bool ClearSafetyStatus(kortex_driver::ClearSafetyStatus::Request &req, kortex_driver::ClearSafetyStatus::Response &res) override; + std::function GetAllSafetyConfigurationHandler = nullptr; + virtual bool GetAllSafetyConfiguration(kortex_driver::GetAllSafetyConfiguration::Request &req, kortex_driver::GetAllSafetyConfiguration::Response &res) override; + std::function GetAllSafetyInformationHandler = nullptr; + virtual bool GetAllSafetyInformation(kortex_driver::GetAllSafetyInformation::Request &req, kortex_driver::GetAllSafetyInformation::Response &res) override; + std::function ResetSafetyDefaultsHandler = nullptr; + virtual bool ResetSafetyDefaults(kortex_driver::ResetSafetyDefaults::Request &req, kortex_driver::ResetSafetyDefaults::Response &res) override; + std::function OnNotificationSafetyTopicHandler = nullptr; + virtual bool OnNotificationSafetyTopic(kortex_driver::OnNotificationSafetyTopic::Request &req, kortex_driver::OnNotificationSafetyTopic::Response &res) override; + virtual void cb_SafetyTopic(Kinova::Api::Common::SafetyNotification notif) override; + std::function ExecuteCalibrationHandler = nullptr; + virtual bool ExecuteCalibration(kortex_driver::ExecuteCalibration::Request &req, kortex_driver::ExecuteCalibration::Response &res) override; + std::function GetCalibrationResultHandler = nullptr; + virtual bool GetCalibrationResult(kortex_driver::GetCalibrationResult::Request &req, kortex_driver::GetCalibrationResult::Response &res) override; + std::function StopCalibrationHandler = nullptr; + virtual bool StopCalibration(kortex_driver::StopCalibration::Request &req, kortex_driver::StopCalibration::Response &res) override; + std::function DeviceConfig_SetCapSenseConfigHandler = nullptr; + virtual bool DeviceConfig_SetCapSenseConfig(kortex_driver::DeviceConfig_SetCapSenseConfig::Request &req, kortex_driver::DeviceConfig_SetCapSenseConfig::Response &res) override; + std::function DeviceConfig_GetCapSenseConfigHandler = nullptr; + virtual bool DeviceConfig_GetCapSenseConfig(kortex_driver::DeviceConfig_GetCapSenseConfig::Request &req, kortex_driver::DeviceConfig_GetCapSenseConfig::Response &res) override; + +}; +#endif diff --git a/kortex_driver/include/kortex_driver/generated/simulation/devicemanager_services.h b/kortex_driver/include/kortex_driver/generated/simulation/devicemanager_services.h new file mode 100644 index 00000000..c3305cad --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/simulation/devicemanager_services.h @@ -0,0 +1,35 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_DEVICEMANAGER_SIMULATION_SERVICES_H_ +#define _KORTEX_DEVICEMANAGER_SIMULATION_SERVICES_H_ + +#include "kortex_driver/generated/interfaces/devicemanager_services_interface.h" + +using namespace std; + +class DeviceManagerSimulationServices : public IDeviceManagerServices +{ + public: + DeviceManagerSimulationServices(ros::NodeHandle& node_handle); + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; + std::function ReadAllDevicesHandler = nullptr; + virtual bool ReadAllDevices(kortex_driver::ReadAllDevices::Request &req, kortex_driver::ReadAllDevices::Response &res) override; + +}; +#endif diff --git a/kortex_driver/include/kortex_driver/generated/simulation/interconnectconfig_services.h b/kortex_driver/include/kortex_driver/generated/simulation/interconnectconfig_services.h new file mode 100644 index 00000000..a5444818 --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/simulation/interconnectconfig_services.h @@ -0,0 +1,61 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_INTERCONNECTCONFIG_SIMULATION_SERVICES_H_ +#define _KORTEX_INTERCONNECTCONFIG_SIMULATION_SERVICES_H_ + +#include "kortex_driver/generated/interfaces/interconnectconfig_services_interface.h" + +using namespace std; + +class InterconnectConfigSimulationServices : public IInterconnectConfigServices +{ + public: + InterconnectConfigSimulationServices(ros::NodeHandle& node_handle); + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; + std::function GetUARTConfigurationHandler = nullptr; + virtual bool GetUARTConfiguration(kortex_driver::GetUARTConfiguration::Request &req, kortex_driver::GetUARTConfiguration::Response &res) override; + std::function SetUARTConfigurationHandler = nullptr; + virtual bool SetUARTConfiguration(kortex_driver::SetUARTConfiguration::Request &req, kortex_driver::SetUARTConfiguration::Response &res) override; + std::function GetEthernetConfigurationHandler = nullptr; + virtual bool GetEthernetConfiguration(kortex_driver::GetEthernetConfiguration::Request &req, kortex_driver::GetEthernetConfiguration::Response &res) override; + std::function SetEthernetConfigurationHandler = nullptr; + virtual bool SetEthernetConfiguration(kortex_driver::SetEthernetConfiguration::Request &req, kortex_driver::SetEthernetConfiguration::Response &res) override; + std::function GetGPIOConfigurationHandler = nullptr; + virtual bool GetGPIOConfiguration(kortex_driver::GetGPIOConfiguration::Request &req, kortex_driver::GetGPIOConfiguration::Response &res) override; + std::function SetGPIOConfigurationHandler = nullptr; + virtual bool SetGPIOConfiguration(kortex_driver::SetGPIOConfiguration::Request &req, kortex_driver::SetGPIOConfiguration::Response &res) override; + std::function GetGPIOStateHandler = nullptr; + virtual bool GetGPIOState(kortex_driver::GetGPIOState::Request &req, kortex_driver::GetGPIOState::Response &res) override; + std::function SetGPIOStateHandler = nullptr; + virtual bool SetGPIOState(kortex_driver::SetGPIOState::Request &req, kortex_driver::SetGPIOState::Response &res) override; + std::function GetI2CConfigurationHandler = nullptr; + virtual bool GetI2CConfiguration(kortex_driver::GetI2CConfiguration::Request &req, kortex_driver::GetI2CConfiguration::Response &res) override; + std::function SetI2CConfigurationHandler = nullptr; + virtual bool SetI2CConfiguration(kortex_driver::SetI2CConfiguration::Request &req, kortex_driver::SetI2CConfiguration::Response &res) override; + std::function I2CReadHandler = nullptr; + virtual bool I2CRead(kortex_driver::I2CRead::Request &req, kortex_driver::I2CRead::Response &res) override; + std::function I2CReadRegisterHandler = nullptr; + virtual bool I2CReadRegister(kortex_driver::I2CReadRegister::Request &req, kortex_driver::I2CReadRegister::Response &res) override; + std::function I2CWriteHandler = nullptr; + virtual bool I2CWrite(kortex_driver::I2CWrite::Request &req, kortex_driver::I2CWrite::Response &res) override; + std::function I2CWriteRegisterHandler = nullptr; + virtual bool I2CWriteRegister(kortex_driver::I2CWriteRegister::Request &req, kortex_driver::I2CWriteRegister::Response &res) override; + +}; +#endif diff --git a/kortex_driver/include/kortex_driver/generated/simulation/visionconfig_services.h b/kortex_driver/include/kortex_driver/generated/simulation/visionconfig_services.h new file mode 100644 index 00000000..57ea508b --- /dev/null +++ b/kortex_driver/include/kortex_driver/generated/simulation/visionconfig_services.h @@ -0,0 +1,58 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_VISIONCONFIG_SIMULATION_SERVICES_H_ +#define _KORTEX_VISIONCONFIG_SIMULATION_SERVICES_H_ + +#include "kortex_driver/generated/interfaces/visionconfig_services_interface.h" + +using namespace std; + +class VisionConfigSimulationServices : public IVisionConfigServices +{ + public: + VisionConfigSimulationServices(ros::NodeHandle& node_handle); + + virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; + virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; + std::function SetSensorSettingsHandler = nullptr; + virtual bool SetSensorSettings(kortex_driver::SetSensorSettings::Request &req, kortex_driver::SetSensorSettings::Response &res) override; + std::function GetSensorSettingsHandler = nullptr; + virtual bool GetSensorSettings(kortex_driver::GetSensorSettings::Request &req, kortex_driver::GetSensorSettings::Response &res) override; + std::function GetOptionValueHandler = nullptr; + virtual bool GetOptionValue(kortex_driver::GetOptionValue::Request &req, kortex_driver::GetOptionValue::Response &res) override; + std::function SetOptionValueHandler = nullptr; + virtual bool SetOptionValue(kortex_driver::SetOptionValue::Request &req, kortex_driver::SetOptionValue::Response &res) override; + std::function GetOptionInformationHandler = nullptr; + virtual bool GetOptionInformation(kortex_driver::GetOptionInformation::Request &req, kortex_driver::GetOptionInformation::Response &res) override; + std::function OnNotificationVisionTopicHandler = nullptr; + virtual bool OnNotificationVisionTopic(kortex_driver::OnNotificationVisionTopic::Request &req, kortex_driver::OnNotificationVisionTopic::Response &res) override; + virtual void cb_VisionTopic(Kinova::Api::VisionConfig::VisionNotification notif) override; + std::function DoSensorFocusActionHandler = nullptr; + virtual bool DoSensorFocusAction(kortex_driver::DoSensorFocusAction::Request &req, kortex_driver::DoSensorFocusAction::Response &res) override; + std::function GetIntrinsicParametersHandler = nullptr; + virtual bool GetIntrinsicParameters(kortex_driver::GetIntrinsicParameters::Request &req, kortex_driver::GetIntrinsicParameters::Response &res) override; + std::function GetIntrinsicParametersProfileHandler = nullptr; + virtual bool GetIntrinsicParametersProfile(kortex_driver::GetIntrinsicParametersProfile::Request &req, kortex_driver::GetIntrinsicParametersProfile::Response &res) override; + std::function SetIntrinsicParametersHandler = nullptr; + virtual bool SetIntrinsicParameters(kortex_driver::SetIntrinsicParameters::Request &req, kortex_driver::SetIntrinsicParameters::Response &res) override; + std::function GetExtrinsicParametersHandler = nullptr; + virtual bool GetExtrinsicParameters(kortex_driver::GetExtrinsicParameters::Request &req, kortex_driver::GetExtrinsicParameters::Response &res) override; + std::function SetExtrinsicParametersHandler = nullptr; + virtual bool SetExtrinsicParameters(kortex_driver::SetExtrinsicParameters::Request &req, kortex_driver::SetExtrinsicParameters::Response &res) override; + +}; +#endif diff --git a/kortex_driver/include/kortex_driver/generated/visionconfig_services.h b/kortex_driver/include/kortex_driver/generated/visionconfig_services.h deleted file mode 100644 index 1b4be317..00000000 --- a/kortex_driver/include/kortex_driver/generated/visionconfig_services.h +++ /dev/null @@ -1,99 +0,0 @@ -/* -* KINOVA (R) KORTEX (TM) -* -* Copyright (c) 2019 Kinova inc. All rights reserved. -* -* This software may be modified and distributed under the -* terms of the BSD 3-Clause license. -* -* Refer to the LICENSE file for details. -* -*/ - -/* - * This file has been auto-generated and should not be modified. - */ - -#ifndef _KORTEX_VISIONCONFIG_SERVICES_H_ -#define _KORTEX_VISIONCONFIG_SERVICES_H_ - -#include "ros/ros.h" - -#include -#include -#include -#include -#include - -#include -#include -#include "kortex_driver/SetSensorSettings.h" -#include "kortex_driver/GetSensorSettings.h" -#include "kortex_driver/GetOptionValue.h" -#include "kortex_driver/SetOptionValue.h" -#include "kortex_driver/GetOptionInformation.h" -#include "kortex_driver/OnNotificationVisionTopic.h" -#include "kortex_driver/VisionNotification.h" -#include "kortex_driver/DoSensorFocusAction.h" -#include "kortex_driver/GetIntrinsicParameters.h" -#include "kortex_driver/GetIntrinsicParametersProfile.h" -#include "kortex_driver/SetIntrinsicParameters.h" -#include "kortex_driver/GetExtrinsicParameters.h" -#include "kortex_driver/SetExtrinsicParameters.h" - -#include "kortex_driver/KortexError.h" -#include "kortex_driver/SetDeviceID.h" -#include "kortex_driver/SetApiOptions.h" -#include "kortex_driver/ApiOptions.h" - -using namespace std; - -class VisionConfigServices -{ - public: - VisionConfigServices(ros::NodeHandle& n, Kinova::Api::VisionConfig::VisionConfigClient* visionconfig, uint32_t device_id, uint32_t timeout_ms); - - bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res); - bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res); - bool SetSensorSettings(kortex_driver::SetSensorSettings::Request &req, kortex_driver::SetSensorSettings::Response &res); - bool GetSensorSettings(kortex_driver::GetSensorSettings::Request &req, kortex_driver::GetSensorSettings::Response &res); - bool GetOptionValue(kortex_driver::GetOptionValue::Request &req, kortex_driver::GetOptionValue::Response &res); - bool SetOptionValue(kortex_driver::SetOptionValue::Request &req, kortex_driver::SetOptionValue::Response &res); - bool GetOptionInformation(kortex_driver::GetOptionInformation::Request &req, kortex_driver::GetOptionInformation::Response &res); - bool OnNotificationVisionTopic(kortex_driver::OnNotificationVisionTopic::Request &req, kortex_driver::OnNotificationVisionTopic::Response &res); - void cb_VisionTopic(Kinova::Api::VisionConfig::VisionNotification notif); - bool DoSensorFocusAction(kortex_driver::DoSensorFocusAction::Request &req, kortex_driver::DoSensorFocusAction::Response &res); - bool GetIntrinsicParameters(kortex_driver::GetIntrinsicParameters::Request &req, kortex_driver::GetIntrinsicParameters::Response &res); - bool GetIntrinsicParametersProfile(kortex_driver::GetIntrinsicParametersProfile::Request &req, kortex_driver::GetIntrinsicParametersProfile::Response &res); - bool SetIntrinsicParameters(kortex_driver::SetIntrinsicParameters::Request &req, kortex_driver::SetIntrinsicParameters::Response &res); - bool GetExtrinsicParameters(kortex_driver::GetExtrinsicParameters::Request &req, kortex_driver::GetExtrinsicParameters::Response &res); - bool SetExtrinsicParameters(kortex_driver::SetExtrinsicParameters::Request &req, kortex_driver::SetExtrinsicParameters::Response &res); - -private: - uint32_t m_current_device_id; - Kinova::Api::RouterClientSendOptions m_api_options; - - Kinova::Api::VisionConfig::VisionConfigClient* m_visionconfig; - - ros::NodeHandle m_n; - ros::Publisher m_pub_Error; - ros::Publisher m_pub_VisionTopic; - bool m_is_activated_VisionTopic; - - ros::ServiceServer m_serviceSetDeviceID; - ros::ServiceServer m_serviceSetApiOptions; - - ros::ServiceServer m_serviceSetSensorSettings; - ros::ServiceServer m_serviceGetSensorSettings; - ros::ServiceServer m_serviceGetOptionValue; - ros::ServiceServer m_serviceSetOptionValue; - ros::ServiceServer m_serviceGetOptionInformation; - ros::ServiceServer m_serviceOnNotificationVisionTopic; - ros::ServiceServer m_serviceDoSensorFocusAction; - ros::ServiceServer m_serviceGetIntrinsicParameters; - ros::ServiceServer m_serviceGetIntrinsicParametersProfile; - ros::ServiceServer m_serviceSetIntrinsicParameters; - ros::ServiceServer m_serviceGetExtrinsicParameters; - ros::ServiceServer m_serviceSetExtrinsicParameters; -}; -#endif diff --git a/kortex_driver/src/generated/devicemanager_services.cpp b/kortex_driver/src/generated/devicemanager_services.cpp deleted file mode 100644 index c544c00e..00000000 --- a/kortex_driver/src/generated/devicemanager_services.cpp +++ /dev/null @@ -1,106 +0,0 @@ -/* -* KINOVA (R) KORTEX (TM) -* -* Copyright (c) 2019 Kinova inc. All rights reserved. -* -* This software may be modified and distributed under the -* terms of the BSD 3-Clause license. -* -* Refer to the LICENSE file for details. -* -*/ - -/* - * This file has been auto-generated and should not be modified. - */ - -#include "kortex_driver/generated/common_proto_converter.h" -#include "kortex_driver/generated/common_ros_converter.h" -#include "kortex_driver/generated/actuatorconfig_proto_converter.h" -#include "kortex_driver/generated/actuatorconfig_ros_converter.h" -#include "kortex_driver/generated/actuatorcyclic_proto_converter.h" -#include "kortex_driver/generated/actuatorcyclic_ros_converter.h" -#include "kortex_driver/generated/productconfiguration_proto_converter.h" -#include "kortex_driver/generated/productconfiguration_ros_converter.h" -#include "kortex_driver/generated/base_proto_converter.h" -#include "kortex_driver/generated/base_ros_converter.h" -#include "kortex_driver/generated/grippercyclic_proto_converter.h" -#include "kortex_driver/generated/grippercyclic_ros_converter.h" -#include "kortex_driver/generated/interconnectcyclic_proto_converter.h" -#include "kortex_driver/generated/interconnectcyclic_ros_converter.h" -#include "kortex_driver/generated/basecyclic_proto_converter.h" -#include "kortex_driver/generated/basecyclic_ros_converter.h" -#include "kortex_driver/generated/controlconfig_proto_converter.h" -#include "kortex_driver/generated/controlconfig_ros_converter.h" -#include "kortex_driver/generated/deviceconfig_proto_converter.h" -#include "kortex_driver/generated/deviceconfig_ros_converter.h" -#include "kortex_driver/generated/devicemanager_proto_converter.h" -#include "kortex_driver/generated/devicemanager_ros_converter.h" -#include "kortex_driver/generated/interconnectconfig_proto_converter.h" -#include "kortex_driver/generated/interconnectconfig_ros_converter.h" -#include "kortex_driver/generated/visionconfig_proto_converter.h" -#include "kortex_driver/generated/visionconfig_ros_converter.h" -#include "kortex_driver/generated/devicemanager_services.h" - -DeviceManagerServices::DeviceManagerServices(ros::NodeHandle& n, Kinova::Api::DeviceManager::DeviceManagerClient* devicemanager, uint32_t device_id, uint32_t timeout_ms): - m_n(n), - m_devicemanager(devicemanager), - m_current_device_id(device_id) -{ - m_api_options.timeout_ms = timeout_ms; - - m_pub_Error = m_n.advertise("kortex_error", 1000); - - m_serviceSetDeviceID = n.advertiseService("device_manager/set_device_id", &DeviceManagerServices::SetDeviceID, this); - m_serviceSetApiOptions = n.advertiseService("device_manager/set_api_options", &DeviceManagerServices::SetApiOptions, this); - - m_serviceReadAllDevices = m_n.advertiseService("device_manager/read_all_devices", &DeviceManagerServices::ReadAllDevices, this); -} - -bool DeviceManagerServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) -{ - m_current_device_id = req.device_id; - - return true; -} - -bool DeviceManagerServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) -{ - m_api_options.timeout_ms = req.input.timeout_ms; - - return true; -} - - -bool DeviceManagerServices::ReadAllDevices(kortex_driver::ReadAllDevices::Request &req, kortex_driver::ReadAllDevices::Response &res) -{ - - Kinova::Api::DeviceManager::DeviceHandles output; - - kortex_driver::KortexError result_error; - - try - { - output = m_devicemanager->ReadAllDevices(m_current_device_id, m_api_options); - } - - catch (Kinova::Api::KDetailedException& ex) - { - result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); - result_error.code = ex.getErrorInfo().getError().error_code(); - result_error.description = ex.toString(); - m_pub_Error.publish(result_error); - ROS_INFO("Kortex exception"); - ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); - ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); - ROS_INFO("KINOVA exception description: %s\n", ex.what()); - return false; - } - catch (std::runtime_error& ex2) - { - ROS_INFO("%s", ex2.what()); - return false; - } - ToRosData(output, res.output); - return true; -} diff --git a/kortex_driver/src/generated/actuatorconfig_proto_converter.cpp b/kortex_driver/src/generated/robot/actuatorconfig_proto_converter.cpp similarity index 98% rename from kortex_driver/src/generated/actuatorconfig_proto_converter.cpp rename to kortex_driver/src/generated/robot/actuatorconfig_proto_converter.cpp index 4d52e169..454a5faa 100644 --- a/kortex_driver/src/generated/actuatorconfig_proto_converter.cpp +++ b/kortex_driver/src/generated/robot/actuatorconfig_proto_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" int ToProtoData(kortex_driver::AxisPosition input, Kinova::Api::ActuatorConfig::AxisPosition *output) { diff --git a/kortex_driver/src/generated/actuatorconfig_ros_converter.cpp b/kortex_driver/src/generated/robot/actuatorconfig_ros_converter.cpp similarity index 98% rename from kortex_driver/src/generated/actuatorconfig_ros_converter.cpp rename to kortex_driver/src/generated/robot/actuatorconfig_ros_converter.cpp index 054b2ea4..d6fbfeab 100644 --- a/kortex_driver/src/generated/actuatorconfig_ros_converter.cpp +++ b/kortex_driver/src/generated/robot/actuatorconfig_ros_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" int ToRosData(Kinova::Api::ActuatorConfig::AxisPosition input, kortex_driver::AxisPosition &output) { diff --git a/kortex_driver/src/generated/actuatorconfig_services.cpp b/kortex_driver/src/generated/robot/actuatorconfig_services.cpp similarity index 67% rename from kortex_driver/src/generated/actuatorconfig_services.cpp rename to kortex_driver/src/generated/robot/actuatorconfig_services.cpp index 39aca451..e9a39715 100644 --- a/kortex_driver/src/generated/actuatorconfig_services.cpp +++ b/kortex_driver/src/generated/robot/actuatorconfig_services.cpp @@ -14,76 +14,76 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/common_proto_converter.h" -#include "kortex_driver/generated/common_ros_converter.h" -#include "kortex_driver/generated/actuatorconfig_proto_converter.h" -#include "kortex_driver/generated/actuatorconfig_ros_converter.h" -#include "kortex_driver/generated/actuatorcyclic_proto_converter.h" -#include "kortex_driver/generated/actuatorcyclic_ros_converter.h" -#include "kortex_driver/generated/productconfiguration_proto_converter.h" -#include "kortex_driver/generated/productconfiguration_ros_converter.h" -#include "kortex_driver/generated/base_proto_converter.h" -#include "kortex_driver/generated/base_ros_converter.h" -#include "kortex_driver/generated/grippercyclic_proto_converter.h" -#include "kortex_driver/generated/grippercyclic_ros_converter.h" -#include "kortex_driver/generated/interconnectcyclic_proto_converter.h" -#include "kortex_driver/generated/interconnectcyclic_ros_converter.h" -#include "kortex_driver/generated/basecyclic_proto_converter.h" -#include "kortex_driver/generated/basecyclic_ros_converter.h" -#include "kortex_driver/generated/controlconfig_proto_converter.h" -#include "kortex_driver/generated/controlconfig_ros_converter.h" -#include "kortex_driver/generated/deviceconfig_proto_converter.h" -#include "kortex_driver/generated/deviceconfig_ros_converter.h" -#include "kortex_driver/generated/devicemanager_proto_converter.h" -#include "kortex_driver/generated/devicemanager_ros_converter.h" -#include "kortex_driver/generated/interconnectconfig_proto_converter.h" -#include "kortex_driver/generated/interconnectconfig_ros_converter.h" -#include "kortex_driver/generated/visionconfig_proto_converter.h" -#include "kortex_driver/generated/visionconfig_ros_converter.h" -#include "kortex_driver/generated/actuatorconfig_services.h" - -ActuatorConfigServices::ActuatorConfigServices(ros::NodeHandle& n, Kinova::Api::ActuatorConfig::ActuatorConfigClient* actuatorconfig, uint32_t device_id, uint32_t timeout_ms): - m_n(n), +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_services.h" + +ActuatorConfigRobotServices::ActuatorConfigRobotServices(ros::NodeHandle& node_handle, Kinova::Api::ActuatorConfig::ActuatorConfigClient* actuatorconfig, uint32_t device_id, uint32_t timeout_ms): + IActuatorConfigServices(node_handle), m_actuatorconfig(actuatorconfig), m_current_device_id(device_id) { m_api_options.timeout_ms = timeout_ms; - m_pub_Error = m_n.advertise("kortex_error", 1000); - - m_serviceSetDeviceID = n.advertiseService("actuator_config/set_device_id", &ActuatorConfigServices::SetDeviceID, this); - m_serviceSetApiOptions = n.advertiseService("actuator_config/set_api_options", &ActuatorConfigServices::SetApiOptions, this); - - m_serviceGetAxisOffsets = m_n.advertiseService("actuator_config/get_axis_offsets", &ActuatorConfigServices::GetAxisOffsets, this); - m_serviceSetAxisOffsets = m_n.advertiseService("actuator_config/set_axis_offsets", &ActuatorConfigServices::SetAxisOffsets, this); - m_serviceSetTorqueOffset = m_n.advertiseService("actuator_config/set_torque_offset", &ActuatorConfigServices::SetTorqueOffset, this); - m_serviceActuatorConfig_GetControlMode = m_n.advertiseService("actuator_config/get_control_mode", &ActuatorConfigServices::ActuatorConfig_GetControlMode, this); - m_serviceSetControlMode = m_n.advertiseService("actuator_config/set_control_mode", &ActuatorConfigServices::SetControlMode, this); - m_serviceGetActivatedControlLoop = m_n.advertiseService("actuator_config/get_activated_control_loop", &ActuatorConfigServices::GetActivatedControlLoop, this); - m_serviceSetActivatedControlLoop = m_n.advertiseService("actuator_config/set_activated_control_loop", &ActuatorConfigServices::SetActivatedControlLoop, this); - m_serviceGetControlLoopParameters = m_n.advertiseService("actuator_config/get_control_loop_parameters", &ActuatorConfigServices::GetControlLoopParameters, this); - m_serviceSetControlLoopParameters = m_n.advertiseService("actuator_config/set_control_loop_parameters", &ActuatorConfigServices::SetControlLoopParameters, this); - m_serviceSelectCustomData = m_n.advertiseService("actuator_config/select_custom_data", &ActuatorConfigServices::SelectCustomData, this); - m_serviceGetSelectedCustomData = m_n.advertiseService("actuator_config/get_selected_custom_data", &ActuatorConfigServices::GetSelectedCustomData, this); - m_serviceSetCommandMode = m_n.advertiseService("actuator_config/set_command_mode", &ActuatorConfigServices::SetCommandMode, this); - m_serviceActuatorConfig_ClearFaults = m_n.advertiseService("actuator_config/clear_faults", &ActuatorConfigServices::ActuatorConfig_ClearFaults, this); - m_serviceSetServoing = m_n.advertiseService("actuator_config/set_servoing", &ActuatorConfigServices::SetServoing, this); - m_serviceMoveToPosition = m_n.advertiseService("actuator_config/move_to_position", &ActuatorConfigServices::MoveToPosition, this); - m_serviceGetCommandMode = m_n.advertiseService("actuator_config/get_command_mode", &ActuatorConfigServices::GetCommandMode, this); - m_serviceGetServoing = m_n.advertiseService("actuator_config/get_servoing", &ActuatorConfigServices::GetServoing, this); - m_serviceGetTorqueOffset = m_n.advertiseService("actuator_config/get_torque_offset", &ActuatorConfigServices::GetTorqueOffset, this); - m_serviceSetCoggingFeedforwardMode = m_n.advertiseService("actuator_config/set_cogging_feedforward_mode", &ActuatorConfigServices::SetCoggingFeedforwardMode, this); - m_serviceGetCoggingFeedforwardMode = m_n.advertiseService("actuator_config/get_cogging_feedforward_mode", &ActuatorConfigServices::GetCoggingFeedforwardMode, this); + m_pub_Error = m_node_handle.advertise("kortex_error", 1000); + + m_serviceSetDeviceID = m_node_handle.advertiseService("actuator_config/set_device_id", &ActuatorConfigRobotServices::SetDeviceID, this); + m_serviceSetApiOptions = m_node_handle.advertiseService("actuator_config/set_api_options", &ActuatorConfigRobotServices::SetApiOptions, this); + + m_serviceGetAxisOffsets = m_node_handle.advertiseService("actuator_config/get_axis_offsets", &ActuatorConfigRobotServices::GetAxisOffsets, this); + m_serviceSetAxisOffsets = m_node_handle.advertiseService("actuator_config/set_axis_offsets", &ActuatorConfigRobotServices::SetAxisOffsets, this); + m_serviceSetTorqueOffset = m_node_handle.advertiseService("actuator_config/set_torque_offset", &ActuatorConfigRobotServices::SetTorqueOffset, this); + m_serviceActuatorConfig_GetControlMode = m_node_handle.advertiseService("actuator_config/get_control_mode", &ActuatorConfigRobotServices::ActuatorConfig_GetControlMode, this); + m_serviceSetControlMode = m_node_handle.advertiseService("actuator_config/set_control_mode", &ActuatorConfigRobotServices::SetControlMode, this); + m_serviceGetActivatedControlLoop = m_node_handle.advertiseService("actuator_config/get_activated_control_loop", &ActuatorConfigRobotServices::GetActivatedControlLoop, this); + m_serviceSetActivatedControlLoop = m_node_handle.advertiseService("actuator_config/set_activated_control_loop", &ActuatorConfigRobotServices::SetActivatedControlLoop, this); + m_serviceGetControlLoopParameters = m_node_handle.advertiseService("actuator_config/get_control_loop_parameters", &ActuatorConfigRobotServices::GetControlLoopParameters, this); + m_serviceSetControlLoopParameters = m_node_handle.advertiseService("actuator_config/set_control_loop_parameters", &ActuatorConfigRobotServices::SetControlLoopParameters, this); + m_serviceSelectCustomData = m_node_handle.advertiseService("actuator_config/select_custom_data", &ActuatorConfigRobotServices::SelectCustomData, this); + m_serviceGetSelectedCustomData = m_node_handle.advertiseService("actuator_config/get_selected_custom_data", &ActuatorConfigRobotServices::GetSelectedCustomData, this); + m_serviceSetCommandMode = m_node_handle.advertiseService("actuator_config/set_command_mode", &ActuatorConfigRobotServices::SetCommandMode, this); + m_serviceActuatorConfig_ClearFaults = m_node_handle.advertiseService("actuator_config/clear_faults", &ActuatorConfigRobotServices::ActuatorConfig_ClearFaults, this); + m_serviceSetServoing = m_node_handle.advertiseService("actuator_config/set_servoing", &ActuatorConfigRobotServices::SetServoing, this); + m_serviceMoveToPosition = m_node_handle.advertiseService("actuator_config/move_to_position", &ActuatorConfigRobotServices::MoveToPosition, this); + m_serviceGetCommandMode = m_node_handle.advertiseService("actuator_config/get_command_mode", &ActuatorConfigRobotServices::GetCommandMode, this); + m_serviceGetServoing = m_node_handle.advertiseService("actuator_config/get_servoing", &ActuatorConfigRobotServices::GetServoing, this); + m_serviceGetTorqueOffset = m_node_handle.advertiseService("actuator_config/get_torque_offset", &ActuatorConfigRobotServices::GetTorqueOffset, this); + m_serviceSetCoggingFeedforwardMode = m_node_handle.advertiseService("actuator_config/set_cogging_feedforward_mode", &ActuatorConfigRobotServices::SetCoggingFeedforwardMode, this); + m_serviceGetCoggingFeedforwardMode = m_node_handle.advertiseService("actuator_config/get_cogging_feedforward_mode", &ActuatorConfigRobotServices::GetCoggingFeedforwardMode, this); } -bool ActuatorConfigServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) +bool ActuatorConfigRobotServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) { m_current_device_id = req.device_id; return true; } -bool ActuatorConfigServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) +bool ActuatorConfigRobotServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) { m_api_options.timeout_ms = req.input.timeout_ms; @@ -91,7 +91,7 @@ bool ActuatorConfigServices::SetApiOptions(kortex_driver::SetApiOptions::Request } -bool ActuatorConfigServices::GetAxisOffsets(kortex_driver::GetAxisOffsets::Request &req, kortex_driver::GetAxisOffsets::Response &res) +bool ActuatorConfigRobotServices::GetAxisOffsets(kortex_driver::GetAxisOffsets::Request &req, kortex_driver::GetAxisOffsets::Response &res) { Kinova::Api::ActuatorConfig::AxisOffsets output; @@ -124,7 +124,7 @@ bool ActuatorConfigServices::GetAxisOffsets(kortex_driver::GetAxisOffsets::Reque return true; } -bool ActuatorConfigServices::SetAxisOffsets(kortex_driver::SetAxisOffsets::Request &req, kortex_driver::SetAxisOffsets::Response &res) +bool ActuatorConfigRobotServices::SetAxisOffsets(kortex_driver::SetAxisOffsets::Request &req, kortex_driver::SetAxisOffsets::Response &res) { Kinova::Api::ActuatorConfig::AxisPosition input; @@ -156,7 +156,7 @@ bool ActuatorConfigServices::SetAxisOffsets(kortex_driver::SetAxisOffsets::Reque return true; } -bool ActuatorConfigServices::SetTorqueOffset(kortex_driver::SetTorqueOffset::Request &req, kortex_driver::SetTorqueOffset::Response &res) +bool ActuatorConfigRobotServices::SetTorqueOffset(kortex_driver::SetTorqueOffset::Request &req, kortex_driver::SetTorqueOffset::Response &res) { Kinova::Api::ActuatorConfig::TorqueOffset input; @@ -188,7 +188,7 @@ bool ActuatorConfigServices::SetTorqueOffset(kortex_driver::SetTorqueOffset::Req return true; } -bool ActuatorConfigServices::ActuatorConfig_GetControlMode(kortex_driver::ActuatorConfig_GetControlMode::Request &req, kortex_driver::ActuatorConfig_GetControlMode::Response &res) +bool ActuatorConfigRobotServices::ActuatorConfig_GetControlMode(kortex_driver::ActuatorConfig_GetControlMode::Request &req, kortex_driver::ActuatorConfig_GetControlMode::Response &res) { Kinova::Api::ActuatorConfig::ControlModeInformation output; @@ -221,7 +221,7 @@ bool ActuatorConfigServices::ActuatorConfig_GetControlMode(kortex_driver::Actuat return true; } -bool ActuatorConfigServices::SetControlMode(kortex_driver::SetControlMode::Request &req, kortex_driver::SetControlMode::Response &res) +bool ActuatorConfigRobotServices::SetControlMode(kortex_driver::SetControlMode::Request &req, kortex_driver::SetControlMode::Response &res) { Kinova::Api::ActuatorConfig::ControlModeInformation input; @@ -253,7 +253,7 @@ bool ActuatorConfigServices::SetControlMode(kortex_driver::SetControlMode::Reque return true; } -bool ActuatorConfigServices::GetActivatedControlLoop(kortex_driver::GetActivatedControlLoop::Request &req, kortex_driver::GetActivatedControlLoop::Response &res) +bool ActuatorConfigRobotServices::GetActivatedControlLoop(kortex_driver::GetActivatedControlLoop::Request &req, kortex_driver::GetActivatedControlLoop::Response &res) { Kinova::Api::ActuatorConfig::ControlLoop output; @@ -286,7 +286,7 @@ bool ActuatorConfigServices::GetActivatedControlLoop(kortex_driver::GetActivated return true; } -bool ActuatorConfigServices::SetActivatedControlLoop(kortex_driver::SetActivatedControlLoop::Request &req, kortex_driver::SetActivatedControlLoop::Response &res) +bool ActuatorConfigRobotServices::SetActivatedControlLoop(kortex_driver::SetActivatedControlLoop::Request &req, kortex_driver::SetActivatedControlLoop::Response &res) { Kinova::Api::ActuatorConfig::ControlLoop input; @@ -318,7 +318,7 @@ bool ActuatorConfigServices::SetActivatedControlLoop(kortex_driver::SetActivated return true; } -bool ActuatorConfigServices::GetControlLoopParameters(kortex_driver::GetControlLoopParameters::Request &req, kortex_driver::GetControlLoopParameters::Response &res) +bool ActuatorConfigRobotServices::GetControlLoopParameters(kortex_driver::GetControlLoopParameters::Request &req, kortex_driver::GetControlLoopParameters::Response &res) { Kinova::Api::ActuatorConfig::LoopSelection input; @@ -353,7 +353,7 @@ bool ActuatorConfigServices::GetControlLoopParameters(kortex_driver::GetControlL return true; } -bool ActuatorConfigServices::SetControlLoopParameters(kortex_driver::SetControlLoopParameters::Request &req, kortex_driver::SetControlLoopParameters::Response &res) +bool ActuatorConfigRobotServices::SetControlLoopParameters(kortex_driver::SetControlLoopParameters::Request &req, kortex_driver::SetControlLoopParameters::Response &res) { Kinova::Api::ActuatorConfig::ControlLoopParameters input; @@ -385,7 +385,7 @@ bool ActuatorConfigServices::SetControlLoopParameters(kortex_driver::SetControlL return true; } -bool ActuatorConfigServices::SelectCustomData(kortex_driver::SelectCustomData::Request &req, kortex_driver::SelectCustomData::Response &res) +bool ActuatorConfigRobotServices::SelectCustomData(kortex_driver::SelectCustomData::Request &req, kortex_driver::SelectCustomData::Response &res) { Kinova::Api::ActuatorConfig::CustomDataSelection input; @@ -417,7 +417,7 @@ bool ActuatorConfigServices::SelectCustomData(kortex_driver::SelectCustomData::R return true; } -bool ActuatorConfigServices::GetSelectedCustomData(kortex_driver::GetSelectedCustomData::Request &req, kortex_driver::GetSelectedCustomData::Response &res) +bool ActuatorConfigRobotServices::GetSelectedCustomData(kortex_driver::GetSelectedCustomData::Request &req, kortex_driver::GetSelectedCustomData::Response &res) { Kinova::Api::ActuatorConfig::CustomDataSelection output; @@ -450,7 +450,7 @@ bool ActuatorConfigServices::GetSelectedCustomData(kortex_driver::GetSelectedCus return true; } -bool ActuatorConfigServices::SetCommandMode(kortex_driver::SetCommandMode::Request &req, kortex_driver::SetCommandMode::Response &res) +bool ActuatorConfigRobotServices::SetCommandMode(kortex_driver::SetCommandMode::Request &req, kortex_driver::SetCommandMode::Response &res) { Kinova::Api::ActuatorConfig::CommandModeInformation input; @@ -482,7 +482,7 @@ bool ActuatorConfigServices::SetCommandMode(kortex_driver::SetCommandMode::Reque return true; } -bool ActuatorConfigServices::ActuatorConfig_ClearFaults(kortex_driver::ActuatorConfig_ClearFaults::Request &req, kortex_driver::ActuatorConfig_ClearFaults::Response &res) +bool ActuatorConfigRobotServices::ActuatorConfig_ClearFaults(kortex_driver::ActuatorConfig_ClearFaults::Request &req, kortex_driver::ActuatorConfig_ClearFaults::Response &res) { kortex_driver::KortexError result_error; @@ -512,7 +512,7 @@ bool ActuatorConfigServices::ActuatorConfig_ClearFaults(kortex_driver::ActuatorC return true; } -bool ActuatorConfigServices::SetServoing(kortex_driver::SetServoing::Request &req, kortex_driver::SetServoing::Response &res) +bool ActuatorConfigRobotServices::SetServoing(kortex_driver::SetServoing::Request &req, kortex_driver::SetServoing::Response &res) { Kinova::Api::ActuatorConfig::Servoing input; @@ -544,7 +544,7 @@ bool ActuatorConfigServices::SetServoing(kortex_driver::SetServoing::Request &r return true; } -bool ActuatorConfigServices::MoveToPosition(kortex_driver::MoveToPosition::Request &req, kortex_driver::MoveToPosition::Response &res) +bool ActuatorConfigRobotServices::MoveToPosition(kortex_driver::MoveToPosition::Request &req, kortex_driver::MoveToPosition::Response &res) { Kinova::Api::ActuatorConfig::PositionCommand input; @@ -576,7 +576,7 @@ bool ActuatorConfigServices::MoveToPosition(kortex_driver::MoveToPosition::Reque return true; } -bool ActuatorConfigServices::GetCommandMode(kortex_driver::GetCommandMode::Request &req, kortex_driver::GetCommandMode::Response &res) +bool ActuatorConfigRobotServices::GetCommandMode(kortex_driver::GetCommandMode::Request &req, kortex_driver::GetCommandMode::Response &res) { Kinova::Api::ActuatorConfig::CommandModeInformation output; @@ -609,7 +609,7 @@ bool ActuatorConfigServices::GetCommandMode(kortex_driver::GetCommandMode::Reque return true; } -bool ActuatorConfigServices::GetServoing(kortex_driver::GetServoing::Request &req, kortex_driver::GetServoing::Response &res) +bool ActuatorConfigRobotServices::GetServoing(kortex_driver::GetServoing::Request &req, kortex_driver::GetServoing::Response &res) { Kinova::Api::ActuatorConfig::Servoing output; @@ -642,7 +642,7 @@ bool ActuatorConfigServices::GetServoing(kortex_driver::GetServoing::Request &r return true; } -bool ActuatorConfigServices::GetTorqueOffset(kortex_driver::GetTorqueOffset::Request &req, kortex_driver::GetTorqueOffset::Response &res) +bool ActuatorConfigRobotServices::GetTorqueOffset(kortex_driver::GetTorqueOffset::Request &req, kortex_driver::GetTorqueOffset::Response &res) { Kinova::Api::ActuatorConfig::TorqueOffset output; @@ -675,7 +675,7 @@ bool ActuatorConfigServices::GetTorqueOffset(kortex_driver::GetTorqueOffset::Req return true; } -bool ActuatorConfigServices::SetCoggingFeedforwardMode(kortex_driver::SetCoggingFeedforwardMode::Request &req, kortex_driver::SetCoggingFeedforwardMode::Response &res) +bool ActuatorConfigRobotServices::SetCoggingFeedforwardMode(kortex_driver::SetCoggingFeedforwardMode::Request &req, kortex_driver::SetCoggingFeedforwardMode::Response &res) { Kinova::Api::ActuatorConfig::CoggingFeedforwardModeInformation input; @@ -707,7 +707,7 @@ bool ActuatorConfigServices::SetCoggingFeedforwardMode(kortex_driver::SetCogging return true; } -bool ActuatorConfigServices::GetCoggingFeedforwardMode(kortex_driver::GetCoggingFeedforwardMode::Request &req, kortex_driver::GetCoggingFeedforwardMode::Response &res) +bool ActuatorConfigRobotServices::GetCoggingFeedforwardMode(kortex_driver::GetCoggingFeedforwardMode::Request &req, kortex_driver::GetCoggingFeedforwardMode::Response &res) { Kinova::Api::ActuatorConfig::CoggingFeedforwardModeInformation output; diff --git a/kortex_driver/src/generated/actuatorcyclic_proto_converter.cpp b/kortex_driver/src/generated/robot/actuatorcyclic_proto_converter.cpp similarity index 97% rename from kortex_driver/src/generated/actuatorcyclic_proto_converter.cpp rename to kortex_driver/src/generated/robot/actuatorcyclic_proto_converter.cpp index 7a1ce565..6d7744fc 100644 --- a/kortex_driver/src/generated/actuatorcyclic_proto_converter.cpp +++ b/kortex_driver/src/generated/robot/actuatorcyclic_proto_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" int ToProtoData(kortex_driver::ActuatorCyclic_MessageId input, Kinova::Api::ActuatorCyclic::MessageId *output) { diff --git a/kortex_driver/src/generated/actuatorcyclic_ros_converter.cpp b/kortex_driver/src/generated/robot/actuatorcyclic_ros_converter.cpp similarity index 97% rename from kortex_driver/src/generated/actuatorcyclic_ros_converter.cpp rename to kortex_driver/src/generated/robot/actuatorcyclic_ros_converter.cpp index d54c6f17..4e2e5ef3 100644 --- a/kortex_driver/src/generated/actuatorcyclic_ros_converter.cpp +++ b/kortex_driver/src/generated/robot/actuatorcyclic_ros_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" int ToRosData(Kinova::Api::ActuatorCyclic::MessageId input, kortex_driver::ActuatorCyclic_MessageId &output) { diff --git a/kortex_driver/src/generated/base_proto_converter.cpp b/kortex_driver/src/generated/robot/base_proto_converter.cpp similarity index 99% rename from kortex_driver/src/generated/base_proto_converter.cpp rename to kortex_driver/src/generated/robot/base_proto_converter.cpp index 9e1d250f..1cefcb7b 100644 --- a/kortex_driver/src/generated/base_proto_converter.cpp +++ b/kortex_driver/src/generated/robot/base_proto_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/base_proto_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" int ToProtoData(kortex_driver::FullUserProfile input, Kinova::Api::Base::FullUserProfile *output) { diff --git a/kortex_driver/src/generated/base_ros_converter.cpp b/kortex_driver/src/generated/robot/base_ros_converter.cpp similarity index 99% rename from kortex_driver/src/generated/base_ros_converter.cpp rename to kortex_driver/src/generated/robot/base_ros_converter.cpp index e8c1f2d4..6ac426c1 100644 --- a/kortex_driver/src/generated/base_ros_converter.cpp +++ b/kortex_driver/src/generated/robot/base_ros_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/base_ros_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" int ToRosData(Kinova::Api::Base::FullUserProfile input, kortex_driver::FullUserProfile &output) { diff --git a/kortex_driver/src/generated/base_services.cpp b/kortex_driver/src/generated/robot/base_services.cpp similarity index 73% rename from kortex_driver/src/generated/base_services.cpp rename to kortex_driver/src/generated/robot/base_services.cpp index b40f51d2..1a8a6dd9 100644 --- a/kortex_driver/src/generated/base_services.cpp +++ b/kortex_driver/src/generated/robot/base_services.cpp @@ -14,228 +14,228 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/common_proto_converter.h" -#include "kortex_driver/generated/common_ros_converter.h" -#include "kortex_driver/generated/actuatorconfig_proto_converter.h" -#include "kortex_driver/generated/actuatorconfig_ros_converter.h" -#include "kortex_driver/generated/actuatorcyclic_proto_converter.h" -#include "kortex_driver/generated/actuatorcyclic_ros_converter.h" -#include "kortex_driver/generated/productconfiguration_proto_converter.h" -#include "kortex_driver/generated/productconfiguration_ros_converter.h" -#include "kortex_driver/generated/base_proto_converter.h" -#include "kortex_driver/generated/base_ros_converter.h" -#include "kortex_driver/generated/grippercyclic_proto_converter.h" -#include "kortex_driver/generated/grippercyclic_ros_converter.h" -#include "kortex_driver/generated/interconnectcyclic_proto_converter.h" -#include "kortex_driver/generated/interconnectcyclic_ros_converter.h" -#include "kortex_driver/generated/basecyclic_proto_converter.h" -#include "kortex_driver/generated/basecyclic_ros_converter.h" -#include "kortex_driver/generated/controlconfig_proto_converter.h" -#include "kortex_driver/generated/controlconfig_ros_converter.h" -#include "kortex_driver/generated/deviceconfig_proto_converter.h" -#include "kortex_driver/generated/deviceconfig_ros_converter.h" -#include "kortex_driver/generated/devicemanager_proto_converter.h" -#include "kortex_driver/generated/devicemanager_ros_converter.h" -#include "kortex_driver/generated/interconnectconfig_proto_converter.h" -#include "kortex_driver/generated/interconnectconfig_ros_converter.h" -#include "kortex_driver/generated/visionconfig_proto_converter.h" -#include "kortex_driver/generated/visionconfig_ros_converter.h" -#include "kortex_driver/generated/base_services.h" - -BaseServices::BaseServices(ros::NodeHandle& n, Kinova::Api::Base::BaseClient* base, uint32_t device_id, uint32_t timeout_ms): - m_n(n), +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" +#include "kortex_driver/generated/robot/base_services.h" + +BaseRobotServices::BaseRobotServices(ros::NodeHandle& node_handle, Kinova::Api::Base::BaseClient* base, uint32_t device_id, uint32_t timeout_ms): + IBaseServices(node_handle), m_base(base), m_current_device_id(device_id) { m_api_options.timeout_ms = timeout_ms; - m_pub_Error = m_n.advertise("kortex_error", 1000); - m_pub_ConfigurationChangeTopic = m_n.advertise("configuration_change_topic", 1000); + m_pub_Error = m_node_handle.advertise("kortex_error", 1000); + m_pub_ConfigurationChangeTopic = m_node_handle.advertise("configuration_change_topic", 1000); m_is_activated_ConfigurationChangeTopic = false; - m_pub_MappingInfoTopic = m_n.advertise("mapping_info_topic", 1000); + m_pub_MappingInfoTopic = m_node_handle.advertise("mapping_info_topic", 1000); m_is_activated_MappingInfoTopic = false; - m_pub_ControlModeTopic = m_n.advertise("control_mode_topic", 1000); + m_pub_ControlModeTopic = m_node_handle.advertise("control_mode_topic", 1000); m_is_activated_ControlModeTopic = false; - m_pub_OperatingModeTopic = m_n.advertise("operating_mode_topic", 1000); + m_pub_OperatingModeTopic = m_node_handle.advertise("operating_mode_topic", 1000); m_is_activated_OperatingModeTopic = false; - m_pub_SequenceInfoTopic = m_n.advertise("sequence_info_topic", 1000); + m_pub_SequenceInfoTopic = m_node_handle.advertise("sequence_info_topic", 1000); m_is_activated_SequenceInfoTopic = false; - m_pub_ProtectionZoneTopic = m_n.advertise("protection_zone_topic", 1000); + m_pub_ProtectionZoneTopic = m_node_handle.advertise("protection_zone_topic", 1000); m_is_activated_ProtectionZoneTopic = false; - m_pub_UserTopic = m_n.advertise("user_topic", 1000); + m_pub_UserTopic = m_node_handle.advertise("user_topic", 1000); m_is_activated_UserTopic = false; - m_pub_ControllerTopic = m_n.advertise("controller_topic", 1000); + m_pub_ControllerTopic = m_node_handle.advertise("controller_topic", 1000); m_is_activated_ControllerTopic = false; - m_pub_ActionTopic = m_n.advertise("action_topic", 1000); + m_pub_ActionTopic = m_node_handle.advertise("action_topic", 1000); m_is_activated_ActionTopic = false; - m_pub_RobotEventTopic = m_n.advertise("robot_event_topic", 1000); + m_pub_RobotEventTopic = m_node_handle.advertise("robot_event_topic", 1000); m_is_activated_RobotEventTopic = false; - m_pub_ServoingModeTopic = m_n.advertise("servoing_mode_topic", 1000); + m_pub_ServoingModeTopic = m_node_handle.advertise("servoing_mode_topic", 1000); m_is_activated_ServoingModeTopic = false; - m_pub_FactoryTopic = m_n.advertise("factory_topic", 1000); + m_pub_FactoryTopic = m_node_handle.advertise("factory_topic", 1000); m_is_activated_FactoryTopic = false; - m_pub_NetworkTopic = m_n.advertise("network_topic", 1000); + m_pub_NetworkTopic = m_node_handle.advertise("network_topic", 1000); m_is_activated_NetworkTopic = false; - m_pub_ArmStateTopic = m_n.advertise("arm_state_topic", 1000); + m_pub_ArmStateTopic = m_node_handle.advertise("arm_state_topic", 1000); m_is_activated_ArmStateTopic = false; - m_serviceSetDeviceID = n.advertiseService("base/set_device_id", &BaseServices::SetDeviceID, this); - m_serviceSetApiOptions = n.advertiseService("base/set_api_options", &BaseServices::SetApiOptions, this); - - m_serviceCreateUserProfile = m_n.advertiseService("base/create_user_profile", &BaseServices::CreateUserProfile, this); - m_serviceUpdateUserProfile = m_n.advertiseService("base/update_user_profile", &BaseServices::UpdateUserProfile, this); - m_serviceReadUserProfile = m_n.advertiseService("base/read_user_profile", &BaseServices::ReadUserProfile, this); - m_serviceDeleteUserProfile = m_n.advertiseService("base/delete_user_profile", &BaseServices::DeleteUserProfile, this); - m_serviceReadAllUserProfiles = m_n.advertiseService("base/read_all_user_profiles", &BaseServices::ReadAllUserProfiles, this); - m_serviceReadAllUsers = m_n.advertiseService("base/read_all_users", &BaseServices::ReadAllUsers, this); - m_serviceChangePassword = m_n.advertiseService("base/change_password", &BaseServices::ChangePassword, this); - m_serviceCreateSequence = m_n.advertiseService("base/create_sequence", &BaseServices::CreateSequence, this); - m_serviceUpdateSequence = m_n.advertiseService("base/update_sequence", &BaseServices::UpdateSequence, this); - m_serviceReadSequence = m_n.advertiseService("base/read_sequence", &BaseServices::ReadSequence, this); - m_serviceDeleteSequence = m_n.advertiseService("base/delete_sequence", &BaseServices::DeleteSequence, this); - m_serviceReadAllSequences = m_n.advertiseService("base/read_all_sequences", &BaseServices::ReadAllSequences, this); - m_servicePlaySequence = m_n.advertiseService("base/play_sequence", &BaseServices::PlaySequence, this); - m_servicePlayAdvancedSequence = m_n.advertiseService("base/play_advanced_sequence", &BaseServices::PlayAdvancedSequence, this); - m_serviceStopSequence = m_n.advertiseService("base/stop_sequence", &BaseServices::StopSequence, this); - m_servicePauseSequence = m_n.advertiseService("base/pause_sequence", &BaseServices::PauseSequence, this); - m_serviceResumeSequence = m_n.advertiseService("base/resume_sequence", &BaseServices::ResumeSequence, this); - m_serviceCreateProtectionZone = m_n.advertiseService("base/create_protection_zone", &BaseServices::CreateProtectionZone, this); - m_serviceUpdateProtectionZone = m_n.advertiseService("base/update_protection_zone", &BaseServices::UpdateProtectionZone, this); - m_serviceReadProtectionZone = m_n.advertiseService("base/read_protection_zone", &BaseServices::ReadProtectionZone, this); - m_serviceDeleteProtectionZone = m_n.advertiseService("base/delete_protection_zone", &BaseServices::DeleteProtectionZone, this); - m_serviceReadAllProtectionZones = m_n.advertiseService("base/read_all_protection_zones", &BaseServices::ReadAllProtectionZones, this); - m_serviceCreateMapping = m_n.advertiseService("base/create_mapping", &BaseServices::CreateMapping, this); - m_serviceReadMapping = m_n.advertiseService("base/read_mapping", &BaseServices::ReadMapping, this); - m_serviceUpdateMapping = m_n.advertiseService("base/update_mapping", &BaseServices::UpdateMapping, this); - m_serviceDeleteMapping = m_n.advertiseService("base/delete_mapping", &BaseServices::DeleteMapping, this); - m_serviceReadAllMappings = m_n.advertiseService("base/read_all_mappings", &BaseServices::ReadAllMappings, this); - m_serviceCreateMap = m_n.advertiseService("base/create_map", &BaseServices::CreateMap, this); - m_serviceReadMap = m_n.advertiseService("base/read_map", &BaseServices::ReadMap, this); - m_serviceUpdateMap = m_n.advertiseService("base/update_map", &BaseServices::UpdateMap, this); - m_serviceDeleteMap = m_n.advertiseService("base/delete_map", &BaseServices::DeleteMap, this); - m_serviceReadAllMaps = m_n.advertiseService("base/read_all_maps", &BaseServices::ReadAllMaps, this); - m_serviceActivateMap = m_n.advertiseService("base/activate_map", &BaseServices::ActivateMap, this); - m_serviceCreateAction = m_n.advertiseService("base/create_action", &BaseServices::CreateAction, this); - m_serviceReadAction = m_n.advertiseService("base/read_action", &BaseServices::ReadAction, this); - m_serviceReadAllActions = m_n.advertiseService("base/read_all_actions", &BaseServices::ReadAllActions, this); - m_serviceDeleteAction = m_n.advertiseService("base/delete_action", &BaseServices::DeleteAction, this); - m_serviceUpdateAction = m_n.advertiseService("base/update_action", &BaseServices::UpdateAction, this); - m_serviceExecuteActionFromReference = m_n.advertiseService("base/execute_action_from_reference", &BaseServices::ExecuteActionFromReference, this); - m_serviceExecuteAction = m_n.advertiseService("base/execute_action", &BaseServices::ExecuteAction, this); - m_servicePauseAction = m_n.advertiseService("base/pause_action", &BaseServices::PauseAction, this); - m_serviceStopAction = m_n.advertiseService("base/stop_action", &BaseServices::StopAction, this); - m_serviceResumeAction = m_n.advertiseService("base/resume_action", &BaseServices::ResumeAction, this); - m_serviceGetIPv4Configuration = m_n.advertiseService("base/get_i_pv4_configuration", &BaseServices::GetIPv4Configuration, this); - m_serviceSetIPv4Configuration = m_n.advertiseService("base/set_i_pv4_configuration", &BaseServices::SetIPv4Configuration, this); - m_serviceSetCommunicationInterfaceEnable = m_n.advertiseService("base/set_communication_interface_enable", &BaseServices::SetCommunicationInterfaceEnable, this); - m_serviceIsCommunicationInterfaceEnable = m_n.advertiseService("base/is_communication_interface_enable", &BaseServices::IsCommunicationInterfaceEnable, this); - m_serviceGetAvailableWifi = m_n.advertiseService("base/get_available_wifi", &BaseServices::GetAvailableWifi, this); - m_serviceGetWifiInformation = m_n.advertiseService("base/get_wifi_information", &BaseServices::GetWifiInformation, this); - m_serviceAddWifiConfiguration = m_n.advertiseService("base/add_wifi_configuration", &BaseServices::AddWifiConfiguration, this); - m_serviceDeleteWifiConfiguration = m_n.advertiseService("base/delete_wifi_configuration", &BaseServices::DeleteWifiConfiguration, this); - m_serviceGetAllConfiguredWifis = m_n.advertiseService("base/get_all_configured_wifis", &BaseServices::GetAllConfiguredWifis, this); - m_serviceConnectWifi = m_n.advertiseService("base/connect_wifi", &BaseServices::ConnectWifi, this); - m_serviceDisconnectWifi = m_n.advertiseService("base/disconnect_wifi", &BaseServices::DisconnectWifi, this); - m_serviceGetConnectedWifiInformation = m_n.advertiseService("base/get_connected_wifi_information", &BaseServices::GetConnectedWifiInformation, this); - m_serviceBase_Unsubscribe = m_n.advertiseService("base/unsubscribe", &BaseServices::Base_Unsubscribe, this); - m_serviceOnNotificationConfigurationChangeTopic = m_n.advertiseService("base/activate_publishing_of_configuration_change_topic", &BaseServices::OnNotificationConfigurationChangeTopic, this); - m_serviceOnNotificationMappingInfoTopic = m_n.advertiseService("base/activate_publishing_of_mapping_info_topic", &BaseServices::OnNotificationMappingInfoTopic, this); - m_serviceOnNotificationControlModeTopic = m_n.advertiseService("base/activate_publishing_of_control_mode_topic", &BaseServices::OnNotificationControlModeTopic, this); - m_serviceOnNotificationOperatingModeTopic = m_n.advertiseService("base/activate_publishing_of_operating_mode_topic", &BaseServices::OnNotificationOperatingModeTopic, this); - m_serviceOnNotificationSequenceInfoTopic = m_n.advertiseService("base/activate_publishing_of_sequence_info_topic", &BaseServices::OnNotificationSequenceInfoTopic, this); - m_serviceOnNotificationProtectionZoneTopic = m_n.advertiseService("base/activate_publishing_of_protection_zone_topic", &BaseServices::OnNotificationProtectionZoneTopic, this); - m_serviceOnNotificationUserTopic = m_n.advertiseService("base/activate_publishing_of_user_topic", &BaseServices::OnNotificationUserTopic, this); - m_serviceOnNotificationControllerTopic = m_n.advertiseService("base/activate_publishing_of_controller_topic", &BaseServices::OnNotificationControllerTopic, this); - m_serviceOnNotificationActionTopic = m_n.advertiseService("base/activate_publishing_of_action_topic", &BaseServices::OnNotificationActionTopic, this); - m_serviceOnNotificationRobotEventTopic = m_n.advertiseService("base/activate_publishing_of_robot_event_topic", &BaseServices::OnNotificationRobotEventTopic, this); - m_servicePlayCartesianTrajectory = m_n.advertiseService("base/play_cartesian_trajectory", &BaseServices::PlayCartesianTrajectory, this); - m_servicePlayCartesianTrajectoryPosition = m_n.advertiseService("base/play_cartesian_trajectory_position", &BaseServices::PlayCartesianTrajectoryPosition, this); - m_servicePlayCartesianTrajectoryOrientation = m_n.advertiseService("base/play_cartesian_trajectory_orientation", &BaseServices::PlayCartesianTrajectoryOrientation, this); - m_serviceStop = m_n.advertiseService("base/stop", &BaseServices::Stop, this); - m_serviceGetMeasuredCartesianPose = m_n.advertiseService("base/get_measured_cartesian_pose", &BaseServices::GetMeasuredCartesianPose, this); - m_serviceSendWrenchCommand = m_n.advertiseService("base/send_wrench_command", &BaseServices::SendWrenchCommand, this); - m_serviceSendWrenchJoystickCommand = m_n.advertiseService("base/send_wrench_joystick_command", &BaseServices::SendWrenchJoystickCommand, this); - m_serviceSendTwistJoystickCommand = m_n.advertiseService("base/send_twist_joystick_command", &BaseServices::SendTwistJoystickCommand, this); - m_serviceSendTwistCommand = m_n.advertiseService("base/send_twist_command", &BaseServices::SendTwistCommand, this); - m_servicePlayJointTrajectory = m_n.advertiseService("base/play_joint_trajectory", &BaseServices::PlayJointTrajectory, this); - m_servicePlaySelectedJointTrajectory = m_n.advertiseService("base/play_selected_joint_trajectory", &BaseServices::PlaySelectedJointTrajectory, this); - m_serviceGetMeasuredJointAngles = m_n.advertiseService("base/get_measured_joint_angles", &BaseServices::GetMeasuredJointAngles, this); - m_serviceSendJointSpeedsCommand = m_n.advertiseService("base/send_joint_speeds_command", &BaseServices::SendJointSpeedsCommand, this); - m_serviceSendSelectedJointSpeedCommand = m_n.advertiseService("base/send_selected_joint_speed_command", &BaseServices::SendSelectedJointSpeedCommand, this); - m_serviceSendGripperCommand = m_n.advertiseService("base/send_gripper_command", &BaseServices::SendGripperCommand, this); - m_serviceGetMeasuredGripperMovement = m_n.advertiseService("base/get_measured_gripper_movement", &BaseServices::GetMeasuredGripperMovement, this); - m_serviceSetAdmittance = m_n.advertiseService("base/set_admittance", &BaseServices::SetAdmittance, this); - m_serviceSetOperatingMode = m_n.advertiseService("base/set_operating_mode", &BaseServices::SetOperatingMode, this); - m_serviceApplyEmergencyStop = m_n.advertiseService("base/apply_emergency_stop", &BaseServices::ApplyEmergencyStop, this); - m_serviceBase_ClearFaults = m_n.advertiseService("base/clear_faults", &BaseServices::Base_ClearFaults, this); - m_serviceBase_GetControlMode = m_n.advertiseService("base/get_control_mode", &BaseServices::Base_GetControlMode, this); - m_serviceGetOperatingMode = m_n.advertiseService("base/get_operating_mode", &BaseServices::GetOperatingMode, this); - m_serviceSetServoingMode = m_n.advertiseService("base/set_servoing_mode", &BaseServices::SetServoingMode, this); - m_serviceGetServoingMode = m_n.advertiseService("base/get_servoing_mode", &BaseServices::GetServoingMode, this); - m_serviceOnNotificationServoingModeTopic = m_n.advertiseService("base/activate_publishing_of_servoing_mode_topic", &BaseServices::OnNotificationServoingModeTopic, this); - m_serviceRestoreFactorySettings = m_n.advertiseService("base/restore_factory_settings", &BaseServices::RestoreFactorySettings, this); - m_serviceOnNotificationFactoryTopic = m_n.advertiseService("base/activate_publishing_of_factory_topic", &BaseServices::OnNotificationFactoryTopic, this); - m_serviceGetAllConnectedControllers = m_n.advertiseService("base/get_all_connected_controllers", &BaseServices::GetAllConnectedControllers, this); - m_serviceGetControllerState = m_n.advertiseService("base/get_controller_state", &BaseServices::GetControllerState, this); - m_serviceGetActuatorCount = m_n.advertiseService("base/get_actuator_count", &BaseServices::GetActuatorCount, this); - m_serviceStartWifiScan = m_n.advertiseService("base/start_wifi_scan", &BaseServices::StartWifiScan, this); - m_serviceGetConfiguredWifi = m_n.advertiseService("base/get_configured_wifi", &BaseServices::GetConfiguredWifi, this); - m_serviceOnNotificationNetworkTopic = m_n.advertiseService("base/activate_publishing_of_network_topic", &BaseServices::OnNotificationNetworkTopic, this); - m_serviceGetArmState = m_n.advertiseService("base/get_arm_state", &BaseServices::GetArmState, this); - m_serviceOnNotificationArmStateTopic = m_n.advertiseService("base/activate_publishing_of_arm_state_topic", &BaseServices::OnNotificationArmStateTopic, this); - m_serviceGetIPv4Information = m_n.advertiseService("base/get_i_pv4_information", &BaseServices::GetIPv4Information, this); - m_serviceSetWifiCountryCode = m_n.advertiseService("base/set_wifi_country_code", &BaseServices::SetWifiCountryCode, this); - m_serviceGetWifiCountryCode = m_n.advertiseService("base/get_wifi_country_code", &BaseServices::GetWifiCountryCode, this); - m_serviceBase_SetCapSenseConfig = m_n.advertiseService("base/set_cap_sense_config", &BaseServices::Base_SetCapSenseConfig, this); - m_serviceBase_GetCapSenseConfig = m_n.advertiseService("base/get_cap_sense_config", &BaseServices::Base_GetCapSenseConfig, this); - m_serviceGetAllJointsSpeedHardLimitation = m_n.advertiseService("base/get_all_joints_speed_hard_limitation", &BaseServices::GetAllJointsSpeedHardLimitation, this); - m_serviceGetAllJointsTorqueHardLimitation = m_n.advertiseService("base/get_all_joints_torque_hard_limitation", &BaseServices::GetAllJointsTorqueHardLimitation, this); - m_serviceGetTwistHardLimitation = m_n.advertiseService("base/get_twist_hard_limitation", &BaseServices::GetTwistHardLimitation, this); - m_serviceGetWrenchHardLimitation = m_n.advertiseService("base/get_wrench_hard_limitation", &BaseServices::GetWrenchHardLimitation, this); - m_serviceSendJointSpeedsJoystickCommand = m_n.advertiseService("base/send_joint_speeds_joystick_command", &BaseServices::SendJointSpeedsJoystickCommand, this); - m_serviceSendSelectedJointSpeedJoystickCommand = m_n.advertiseService("base/send_selected_joint_speed_joystick_command", &BaseServices::SendSelectedJointSpeedJoystickCommand, this); - m_serviceEnableBridge = m_n.advertiseService("base/enable_bridge", &BaseServices::EnableBridge, this); - m_serviceDisableBridge = m_n.advertiseService("base/disable_bridge", &BaseServices::DisableBridge, this); - m_serviceGetBridgeList = m_n.advertiseService("base/get_bridge_list", &BaseServices::GetBridgeList, this); - m_serviceGetBridgeConfig = m_n.advertiseService("base/get_bridge_config", &BaseServices::GetBridgeConfig, this); - m_servicePlayPreComputedJointTrajectory = m_n.advertiseService("base/play_pre_computed_joint_trajectory", &BaseServices::PlayPreComputedJointTrajectory, this); - m_serviceGetProductConfiguration = m_n.advertiseService("base/get_product_configuration", &BaseServices::GetProductConfiguration, this); - m_serviceUpdateEndEffectorTypeConfiguration = m_n.advertiseService("base/update_end_effector_type_configuration", &BaseServices::UpdateEndEffectorTypeConfiguration, this); - m_serviceRestoreFactoryProductConfiguration = m_n.advertiseService("base/restore_factory_product_configuration", &BaseServices::RestoreFactoryProductConfiguration, this); - m_serviceGetTrajectoryErrorReport = m_n.advertiseService("base/get_trajectory_error_report", &BaseServices::GetTrajectoryErrorReport, this); - m_serviceGetAllJointsSpeedSoftLimitation = m_n.advertiseService("base/get_all_joints_speed_soft_limitation", &BaseServices::GetAllJointsSpeedSoftLimitation, this); - m_serviceGetAllJointsTorqueSoftLimitation = m_n.advertiseService("base/get_all_joints_torque_soft_limitation", &BaseServices::GetAllJointsTorqueSoftLimitation, this); - m_serviceGetTwistSoftLimitation = m_n.advertiseService("base/get_twist_soft_limitation", &BaseServices::GetTwistSoftLimitation, this); - m_serviceGetWrenchSoftLimitation = m_n.advertiseService("base/get_wrench_soft_limitation", &BaseServices::GetWrenchSoftLimitation, this); - m_serviceSetControllerConfigurationMode = m_n.advertiseService("base/set_controller_configuration_mode", &BaseServices::SetControllerConfigurationMode, this); - m_serviceGetControllerConfigurationMode = m_n.advertiseService("base/get_controller_configuration_mode", &BaseServices::GetControllerConfigurationMode, this); - m_serviceStartTeaching = m_n.advertiseService("base/start_teaching", &BaseServices::StartTeaching, this); - m_serviceStopTeaching = m_n.advertiseService("base/stop_teaching", &BaseServices::StopTeaching, this); - m_serviceAddSequenceTasks = m_n.advertiseService("base/add_sequence_tasks", &BaseServices::AddSequenceTasks, this); - m_serviceUpdateSequenceTask = m_n.advertiseService("base/update_sequence_task", &BaseServices::UpdateSequenceTask, this); - m_serviceSwapSequenceTasks = m_n.advertiseService("base/swap_sequence_tasks", &BaseServices::SwapSequenceTasks, this); - m_serviceReadSequenceTask = m_n.advertiseService("base/read_sequence_task", &BaseServices::ReadSequenceTask, this); - m_serviceReadAllSequenceTasks = m_n.advertiseService("base/read_all_sequence_tasks", &BaseServices::ReadAllSequenceTasks, this); - m_serviceDeleteSequenceTask = m_n.advertiseService("base/delete_sequence_task", &BaseServices::DeleteSequenceTask, this); - m_serviceDeleteAllSequenceTasks = m_n.advertiseService("base/delete_all_sequence_tasks", &BaseServices::DeleteAllSequenceTasks, this); - m_serviceTakeSnapshot = m_n.advertiseService("base/take_snapshot", &BaseServices::TakeSnapshot, this); - m_serviceGetFirmwareBundleVersions = m_n.advertiseService("base/get_firmware_bundle_versions", &BaseServices::GetFirmwareBundleVersions, this); - m_serviceMoveSequenceTask = m_n.advertiseService("base/move_sequence_task", &BaseServices::MoveSequenceTask, this); - m_serviceDuplicateMapping = m_n.advertiseService("base/duplicate_mapping", &BaseServices::DuplicateMapping, this); - m_serviceDuplicateMap = m_n.advertiseService("base/duplicate_map", &BaseServices::DuplicateMap, this); - m_serviceSetControllerConfiguration = m_n.advertiseService("base/set_controller_configuration", &BaseServices::SetControllerConfiguration, this); - m_serviceGetControllerConfiguration = m_n.advertiseService("base/get_controller_configuration", &BaseServices::GetControllerConfiguration, this); - m_serviceGetAllControllerConfigurations = m_n.advertiseService("base/get_all_controller_configurations", &BaseServices::GetAllControllerConfigurations, this); -} - -bool BaseServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) + m_serviceSetDeviceID = m_node_handle.advertiseService("base/set_device_id", &BaseRobotServices::SetDeviceID, this); + m_serviceSetApiOptions = m_node_handle.advertiseService("base/set_api_options", &BaseRobotServices::SetApiOptions, this); + + m_serviceCreateUserProfile = m_node_handle.advertiseService("base/create_user_profile", &BaseRobotServices::CreateUserProfile, this); + m_serviceUpdateUserProfile = m_node_handle.advertiseService("base/update_user_profile", &BaseRobotServices::UpdateUserProfile, this); + m_serviceReadUserProfile = m_node_handle.advertiseService("base/read_user_profile", &BaseRobotServices::ReadUserProfile, this); + m_serviceDeleteUserProfile = m_node_handle.advertiseService("base/delete_user_profile", &BaseRobotServices::DeleteUserProfile, this); + m_serviceReadAllUserProfiles = m_node_handle.advertiseService("base/read_all_user_profiles", &BaseRobotServices::ReadAllUserProfiles, this); + m_serviceReadAllUsers = m_node_handle.advertiseService("base/read_all_users", &BaseRobotServices::ReadAllUsers, this); + m_serviceChangePassword = m_node_handle.advertiseService("base/change_password", &BaseRobotServices::ChangePassword, this); + m_serviceCreateSequence = m_node_handle.advertiseService("base/create_sequence", &BaseRobotServices::CreateSequence, this); + m_serviceUpdateSequence = m_node_handle.advertiseService("base/update_sequence", &BaseRobotServices::UpdateSequence, this); + m_serviceReadSequence = m_node_handle.advertiseService("base/read_sequence", &BaseRobotServices::ReadSequence, this); + m_serviceDeleteSequence = m_node_handle.advertiseService("base/delete_sequence", &BaseRobotServices::DeleteSequence, this); + m_serviceReadAllSequences = m_node_handle.advertiseService("base/read_all_sequences", &BaseRobotServices::ReadAllSequences, this); + m_servicePlaySequence = m_node_handle.advertiseService("base/play_sequence", &BaseRobotServices::PlaySequence, this); + m_servicePlayAdvancedSequence = m_node_handle.advertiseService("base/play_advanced_sequence", &BaseRobotServices::PlayAdvancedSequence, this); + m_serviceStopSequence = m_node_handle.advertiseService("base/stop_sequence", &BaseRobotServices::StopSequence, this); + m_servicePauseSequence = m_node_handle.advertiseService("base/pause_sequence", &BaseRobotServices::PauseSequence, this); + m_serviceResumeSequence = m_node_handle.advertiseService("base/resume_sequence", &BaseRobotServices::ResumeSequence, this); + m_serviceCreateProtectionZone = m_node_handle.advertiseService("base/create_protection_zone", &BaseRobotServices::CreateProtectionZone, this); + m_serviceUpdateProtectionZone = m_node_handle.advertiseService("base/update_protection_zone", &BaseRobotServices::UpdateProtectionZone, this); + m_serviceReadProtectionZone = m_node_handle.advertiseService("base/read_protection_zone", &BaseRobotServices::ReadProtectionZone, this); + m_serviceDeleteProtectionZone = m_node_handle.advertiseService("base/delete_protection_zone", &BaseRobotServices::DeleteProtectionZone, this); + m_serviceReadAllProtectionZones = m_node_handle.advertiseService("base/read_all_protection_zones", &BaseRobotServices::ReadAllProtectionZones, this); + m_serviceCreateMapping = m_node_handle.advertiseService("base/create_mapping", &BaseRobotServices::CreateMapping, this); + m_serviceReadMapping = m_node_handle.advertiseService("base/read_mapping", &BaseRobotServices::ReadMapping, this); + m_serviceUpdateMapping = m_node_handle.advertiseService("base/update_mapping", &BaseRobotServices::UpdateMapping, this); + m_serviceDeleteMapping = m_node_handle.advertiseService("base/delete_mapping", &BaseRobotServices::DeleteMapping, this); + m_serviceReadAllMappings = m_node_handle.advertiseService("base/read_all_mappings", &BaseRobotServices::ReadAllMappings, this); + m_serviceCreateMap = m_node_handle.advertiseService("base/create_map", &BaseRobotServices::CreateMap, this); + m_serviceReadMap = m_node_handle.advertiseService("base/read_map", &BaseRobotServices::ReadMap, this); + m_serviceUpdateMap = m_node_handle.advertiseService("base/update_map", &BaseRobotServices::UpdateMap, this); + m_serviceDeleteMap = m_node_handle.advertiseService("base/delete_map", &BaseRobotServices::DeleteMap, this); + m_serviceReadAllMaps = m_node_handle.advertiseService("base/read_all_maps", &BaseRobotServices::ReadAllMaps, this); + m_serviceActivateMap = m_node_handle.advertiseService("base/activate_map", &BaseRobotServices::ActivateMap, this); + m_serviceCreateAction = m_node_handle.advertiseService("base/create_action", &BaseRobotServices::CreateAction, this); + m_serviceReadAction = m_node_handle.advertiseService("base/read_action", &BaseRobotServices::ReadAction, this); + m_serviceReadAllActions = m_node_handle.advertiseService("base/read_all_actions", &BaseRobotServices::ReadAllActions, this); + m_serviceDeleteAction = m_node_handle.advertiseService("base/delete_action", &BaseRobotServices::DeleteAction, this); + m_serviceUpdateAction = m_node_handle.advertiseService("base/update_action", &BaseRobotServices::UpdateAction, this); + m_serviceExecuteActionFromReference = m_node_handle.advertiseService("base/execute_action_from_reference", &BaseRobotServices::ExecuteActionFromReference, this); + m_serviceExecuteAction = m_node_handle.advertiseService("base/execute_action", &BaseRobotServices::ExecuteAction, this); + m_servicePauseAction = m_node_handle.advertiseService("base/pause_action", &BaseRobotServices::PauseAction, this); + m_serviceStopAction = m_node_handle.advertiseService("base/stop_action", &BaseRobotServices::StopAction, this); + m_serviceResumeAction = m_node_handle.advertiseService("base/resume_action", &BaseRobotServices::ResumeAction, this); + m_serviceGetIPv4Configuration = m_node_handle.advertiseService("base/get_i_pv4_configuration", &BaseRobotServices::GetIPv4Configuration, this); + m_serviceSetIPv4Configuration = m_node_handle.advertiseService("base/set_i_pv4_configuration", &BaseRobotServices::SetIPv4Configuration, this); + m_serviceSetCommunicationInterfaceEnable = m_node_handle.advertiseService("base/set_communication_interface_enable", &BaseRobotServices::SetCommunicationInterfaceEnable, this); + m_serviceIsCommunicationInterfaceEnable = m_node_handle.advertiseService("base/is_communication_interface_enable", &BaseRobotServices::IsCommunicationInterfaceEnable, this); + m_serviceGetAvailableWifi = m_node_handle.advertiseService("base/get_available_wifi", &BaseRobotServices::GetAvailableWifi, this); + m_serviceGetWifiInformation = m_node_handle.advertiseService("base/get_wifi_information", &BaseRobotServices::GetWifiInformation, this); + m_serviceAddWifiConfiguration = m_node_handle.advertiseService("base/add_wifi_configuration", &BaseRobotServices::AddWifiConfiguration, this); + m_serviceDeleteWifiConfiguration = m_node_handle.advertiseService("base/delete_wifi_configuration", &BaseRobotServices::DeleteWifiConfiguration, this); + m_serviceGetAllConfiguredWifis = m_node_handle.advertiseService("base/get_all_configured_wifis", &BaseRobotServices::GetAllConfiguredWifis, this); + m_serviceConnectWifi = m_node_handle.advertiseService("base/connect_wifi", &BaseRobotServices::ConnectWifi, this); + m_serviceDisconnectWifi = m_node_handle.advertiseService("base/disconnect_wifi", &BaseRobotServices::DisconnectWifi, this); + m_serviceGetConnectedWifiInformation = m_node_handle.advertiseService("base/get_connected_wifi_information", &BaseRobotServices::GetConnectedWifiInformation, this); + m_serviceBase_Unsubscribe = m_node_handle.advertiseService("base/unsubscribe", &BaseRobotServices::Base_Unsubscribe, this); + m_serviceOnNotificationConfigurationChangeTopic = m_node_handle.advertiseService("base/activate_publishing_of_configuration_change_topic", &BaseRobotServices::OnNotificationConfigurationChangeTopic, this); + m_serviceOnNotificationMappingInfoTopic = m_node_handle.advertiseService("base/activate_publishing_of_mapping_info_topic", &BaseRobotServices::OnNotificationMappingInfoTopic, this); + m_serviceOnNotificationControlModeTopic = m_node_handle.advertiseService("base/activate_publishing_of_control_mode_topic", &BaseRobotServices::OnNotificationControlModeTopic, this); + m_serviceOnNotificationOperatingModeTopic = m_node_handle.advertiseService("base/activate_publishing_of_operating_mode_topic", &BaseRobotServices::OnNotificationOperatingModeTopic, this); + m_serviceOnNotificationSequenceInfoTopic = m_node_handle.advertiseService("base/activate_publishing_of_sequence_info_topic", &BaseRobotServices::OnNotificationSequenceInfoTopic, this); + m_serviceOnNotificationProtectionZoneTopic = m_node_handle.advertiseService("base/activate_publishing_of_protection_zone_topic", &BaseRobotServices::OnNotificationProtectionZoneTopic, this); + m_serviceOnNotificationUserTopic = m_node_handle.advertiseService("base/activate_publishing_of_user_topic", &BaseRobotServices::OnNotificationUserTopic, this); + m_serviceOnNotificationControllerTopic = m_node_handle.advertiseService("base/activate_publishing_of_controller_topic", &BaseRobotServices::OnNotificationControllerTopic, this); + m_serviceOnNotificationActionTopic = m_node_handle.advertiseService("base/activate_publishing_of_action_topic", &BaseRobotServices::OnNotificationActionTopic, this); + m_serviceOnNotificationRobotEventTopic = m_node_handle.advertiseService("base/activate_publishing_of_robot_event_topic", &BaseRobotServices::OnNotificationRobotEventTopic, this); + m_servicePlayCartesianTrajectory = m_node_handle.advertiseService("base/play_cartesian_trajectory", &BaseRobotServices::PlayCartesianTrajectory, this); + m_servicePlayCartesianTrajectoryPosition = m_node_handle.advertiseService("base/play_cartesian_trajectory_position", &BaseRobotServices::PlayCartesianTrajectoryPosition, this); + m_servicePlayCartesianTrajectoryOrientation = m_node_handle.advertiseService("base/play_cartesian_trajectory_orientation", &BaseRobotServices::PlayCartesianTrajectoryOrientation, this); + m_serviceStop = m_node_handle.advertiseService("base/stop", &BaseRobotServices::Stop, this); + m_serviceGetMeasuredCartesianPose = m_node_handle.advertiseService("base/get_measured_cartesian_pose", &BaseRobotServices::GetMeasuredCartesianPose, this); + m_serviceSendWrenchCommand = m_node_handle.advertiseService("base/send_wrench_command", &BaseRobotServices::SendWrenchCommand, this); + m_serviceSendWrenchJoystickCommand = m_node_handle.advertiseService("base/send_wrench_joystick_command", &BaseRobotServices::SendWrenchJoystickCommand, this); + m_serviceSendTwistJoystickCommand = m_node_handle.advertiseService("base/send_twist_joystick_command", &BaseRobotServices::SendTwistJoystickCommand, this); + m_serviceSendTwistCommand = m_node_handle.advertiseService("base/send_twist_command", &BaseRobotServices::SendTwistCommand, this); + m_servicePlayJointTrajectory = m_node_handle.advertiseService("base/play_joint_trajectory", &BaseRobotServices::PlayJointTrajectory, this); + m_servicePlaySelectedJointTrajectory = m_node_handle.advertiseService("base/play_selected_joint_trajectory", &BaseRobotServices::PlaySelectedJointTrajectory, this); + m_serviceGetMeasuredJointAngles = m_node_handle.advertiseService("base/get_measured_joint_angles", &BaseRobotServices::GetMeasuredJointAngles, this); + m_serviceSendJointSpeedsCommand = m_node_handle.advertiseService("base/send_joint_speeds_command", &BaseRobotServices::SendJointSpeedsCommand, this); + m_serviceSendSelectedJointSpeedCommand = m_node_handle.advertiseService("base/send_selected_joint_speed_command", &BaseRobotServices::SendSelectedJointSpeedCommand, this); + m_serviceSendGripperCommand = m_node_handle.advertiseService("base/send_gripper_command", &BaseRobotServices::SendGripperCommand, this); + m_serviceGetMeasuredGripperMovement = m_node_handle.advertiseService("base/get_measured_gripper_movement", &BaseRobotServices::GetMeasuredGripperMovement, this); + m_serviceSetAdmittance = m_node_handle.advertiseService("base/set_admittance", &BaseRobotServices::SetAdmittance, this); + m_serviceSetOperatingMode = m_node_handle.advertiseService("base/set_operating_mode", &BaseRobotServices::SetOperatingMode, this); + m_serviceApplyEmergencyStop = m_node_handle.advertiseService("base/apply_emergency_stop", &BaseRobotServices::ApplyEmergencyStop, this); + m_serviceBase_ClearFaults = m_node_handle.advertiseService("base/clear_faults", &BaseRobotServices::Base_ClearFaults, this); + m_serviceBase_GetControlMode = m_node_handle.advertiseService("base/get_control_mode", &BaseRobotServices::Base_GetControlMode, this); + m_serviceGetOperatingMode = m_node_handle.advertiseService("base/get_operating_mode", &BaseRobotServices::GetOperatingMode, this); + m_serviceSetServoingMode = m_node_handle.advertiseService("base/set_servoing_mode", &BaseRobotServices::SetServoingMode, this); + m_serviceGetServoingMode = m_node_handle.advertiseService("base/get_servoing_mode", &BaseRobotServices::GetServoingMode, this); + m_serviceOnNotificationServoingModeTopic = m_node_handle.advertiseService("base/activate_publishing_of_servoing_mode_topic", &BaseRobotServices::OnNotificationServoingModeTopic, this); + m_serviceRestoreFactorySettings = m_node_handle.advertiseService("base/restore_factory_settings", &BaseRobotServices::RestoreFactorySettings, this); + m_serviceOnNotificationFactoryTopic = m_node_handle.advertiseService("base/activate_publishing_of_factory_topic", &BaseRobotServices::OnNotificationFactoryTopic, this); + m_serviceGetAllConnectedControllers = m_node_handle.advertiseService("base/get_all_connected_controllers", &BaseRobotServices::GetAllConnectedControllers, this); + m_serviceGetControllerState = m_node_handle.advertiseService("base/get_controller_state", &BaseRobotServices::GetControllerState, this); + m_serviceGetActuatorCount = m_node_handle.advertiseService("base/get_actuator_count", &BaseRobotServices::GetActuatorCount, this); + m_serviceStartWifiScan = m_node_handle.advertiseService("base/start_wifi_scan", &BaseRobotServices::StartWifiScan, this); + m_serviceGetConfiguredWifi = m_node_handle.advertiseService("base/get_configured_wifi", &BaseRobotServices::GetConfiguredWifi, this); + m_serviceOnNotificationNetworkTopic = m_node_handle.advertiseService("base/activate_publishing_of_network_topic", &BaseRobotServices::OnNotificationNetworkTopic, this); + m_serviceGetArmState = m_node_handle.advertiseService("base/get_arm_state", &BaseRobotServices::GetArmState, this); + m_serviceOnNotificationArmStateTopic = m_node_handle.advertiseService("base/activate_publishing_of_arm_state_topic", &BaseRobotServices::OnNotificationArmStateTopic, this); + m_serviceGetIPv4Information = m_node_handle.advertiseService("base/get_i_pv4_information", &BaseRobotServices::GetIPv4Information, this); + m_serviceSetWifiCountryCode = m_node_handle.advertiseService("base/set_wifi_country_code", &BaseRobotServices::SetWifiCountryCode, this); + m_serviceGetWifiCountryCode = m_node_handle.advertiseService("base/get_wifi_country_code", &BaseRobotServices::GetWifiCountryCode, this); + m_serviceBase_SetCapSenseConfig = m_node_handle.advertiseService("base/set_cap_sense_config", &BaseRobotServices::Base_SetCapSenseConfig, this); + m_serviceBase_GetCapSenseConfig = m_node_handle.advertiseService("base/get_cap_sense_config", &BaseRobotServices::Base_GetCapSenseConfig, this); + m_serviceGetAllJointsSpeedHardLimitation = m_node_handle.advertiseService("base/get_all_joints_speed_hard_limitation", &BaseRobotServices::GetAllJointsSpeedHardLimitation, this); + m_serviceGetAllJointsTorqueHardLimitation = m_node_handle.advertiseService("base/get_all_joints_torque_hard_limitation", &BaseRobotServices::GetAllJointsTorqueHardLimitation, this); + m_serviceGetTwistHardLimitation = m_node_handle.advertiseService("base/get_twist_hard_limitation", &BaseRobotServices::GetTwistHardLimitation, this); + m_serviceGetWrenchHardLimitation = m_node_handle.advertiseService("base/get_wrench_hard_limitation", &BaseRobotServices::GetWrenchHardLimitation, this); + m_serviceSendJointSpeedsJoystickCommand = m_node_handle.advertiseService("base/send_joint_speeds_joystick_command", &BaseRobotServices::SendJointSpeedsJoystickCommand, this); + m_serviceSendSelectedJointSpeedJoystickCommand = m_node_handle.advertiseService("base/send_selected_joint_speed_joystick_command", &BaseRobotServices::SendSelectedJointSpeedJoystickCommand, this); + m_serviceEnableBridge = m_node_handle.advertiseService("base/enable_bridge", &BaseRobotServices::EnableBridge, this); + m_serviceDisableBridge = m_node_handle.advertiseService("base/disable_bridge", &BaseRobotServices::DisableBridge, this); + m_serviceGetBridgeList = m_node_handle.advertiseService("base/get_bridge_list", &BaseRobotServices::GetBridgeList, this); + m_serviceGetBridgeConfig = m_node_handle.advertiseService("base/get_bridge_config", &BaseRobotServices::GetBridgeConfig, this); + m_servicePlayPreComputedJointTrajectory = m_node_handle.advertiseService("base/play_pre_computed_joint_trajectory", &BaseRobotServices::PlayPreComputedJointTrajectory, this); + m_serviceGetProductConfiguration = m_node_handle.advertiseService("base/get_product_configuration", &BaseRobotServices::GetProductConfiguration, this); + m_serviceUpdateEndEffectorTypeConfiguration = m_node_handle.advertiseService("base/update_end_effector_type_configuration", &BaseRobotServices::UpdateEndEffectorTypeConfiguration, this); + m_serviceRestoreFactoryProductConfiguration = m_node_handle.advertiseService("base/restore_factory_product_configuration", &BaseRobotServices::RestoreFactoryProductConfiguration, this); + m_serviceGetTrajectoryErrorReport = m_node_handle.advertiseService("base/get_trajectory_error_report", &BaseRobotServices::GetTrajectoryErrorReport, this); + m_serviceGetAllJointsSpeedSoftLimitation = m_node_handle.advertiseService("base/get_all_joints_speed_soft_limitation", &BaseRobotServices::GetAllJointsSpeedSoftLimitation, this); + m_serviceGetAllJointsTorqueSoftLimitation = m_node_handle.advertiseService("base/get_all_joints_torque_soft_limitation", &BaseRobotServices::GetAllJointsTorqueSoftLimitation, this); + m_serviceGetTwistSoftLimitation = m_node_handle.advertiseService("base/get_twist_soft_limitation", &BaseRobotServices::GetTwistSoftLimitation, this); + m_serviceGetWrenchSoftLimitation = m_node_handle.advertiseService("base/get_wrench_soft_limitation", &BaseRobotServices::GetWrenchSoftLimitation, this); + m_serviceSetControllerConfigurationMode = m_node_handle.advertiseService("base/set_controller_configuration_mode", &BaseRobotServices::SetControllerConfigurationMode, this); + m_serviceGetControllerConfigurationMode = m_node_handle.advertiseService("base/get_controller_configuration_mode", &BaseRobotServices::GetControllerConfigurationMode, this); + m_serviceStartTeaching = m_node_handle.advertiseService("base/start_teaching", &BaseRobotServices::StartTeaching, this); + m_serviceStopTeaching = m_node_handle.advertiseService("base/stop_teaching", &BaseRobotServices::StopTeaching, this); + m_serviceAddSequenceTasks = m_node_handle.advertiseService("base/add_sequence_tasks", &BaseRobotServices::AddSequenceTasks, this); + m_serviceUpdateSequenceTask = m_node_handle.advertiseService("base/update_sequence_task", &BaseRobotServices::UpdateSequenceTask, this); + m_serviceSwapSequenceTasks = m_node_handle.advertiseService("base/swap_sequence_tasks", &BaseRobotServices::SwapSequenceTasks, this); + m_serviceReadSequenceTask = m_node_handle.advertiseService("base/read_sequence_task", &BaseRobotServices::ReadSequenceTask, this); + m_serviceReadAllSequenceTasks = m_node_handle.advertiseService("base/read_all_sequence_tasks", &BaseRobotServices::ReadAllSequenceTasks, this); + m_serviceDeleteSequenceTask = m_node_handle.advertiseService("base/delete_sequence_task", &BaseRobotServices::DeleteSequenceTask, this); + m_serviceDeleteAllSequenceTasks = m_node_handle.advertiseService("base/delete_all_sequence_tasks", &BaseRobotServices::DeleteAllSequenceTasks, this); + m_serviceTakeSnapshot = m_node_handle.advertiseService("base/take_snapshot", &BaseRobotServices::TakeSnapshot, this); + m_serviceGetFirmwareBundleVersions = m_node_handle.advertiseService("base/get_firmware_bundle_versions", &BaseRobotServices::GetFirmwareBundleVersions, this); + m_serviceMoveSequenceTask = m_node_handle.advertiseService("base/move_sequence_task", &BaseRobotServices::MoveSequenceTask, this); + m_serviceDuplicateMapping = m_node_handle.advertiseService("base/duplicate_mapping", &BaseRobotServices::DuplicateMapping, this); + m_serviceDuplicateMap = m_node_handle.advertiseService("base/duplicate_map", &BaseRobotServices::DuplicateMap, this); + m_serviceSetControllerConfiguration = m_node_handle.advertiseService("base/set_controller_configuration", &BaseRobotServices::SetControllerConfiguration, this); + m_serviceGetControllerConfiguration = m_node_handle.advertiseService("base/get_controller_configuration", &BaseRobotServices::GetControllerConfiguration, this); + m_serviceGetAllControllerConfigurations = m_node_handle.advertiseService("base/get_all_controller_configurations", &BaseRobotServices::GetAllControllerConfigurations, this); +} + +bool BaseRobotServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) { m_current_device_id = req.device_id; return true; } -bool BaseServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) +bool BaseRobotServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) { m_api_options.timeout_ms = req.input.timeout_ms; @@ -243,7 +243,7 @@ bool BaseServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, ko } -bool BaseServices::CreateUserProfile(kortex_driver::CreateUserProfile::Request &req, kortex_driver::CreateUserProfile::Response &res) +bool BaseRobotServices::CreateUserProfile(kortex_driver::CreateUserProfile::Request &req, kortex_driver::CreateUserProfile::Response &res) { Kinova::Api::Base::FullUserProfile input; @@ -278,7 +278,7 @@ bool BaseServices::CreateUserProfile(kortex_driver::CreateUserProfile::Request return true; } -bool BaseServices::UpdateUserProfile(kortex_driver::UpdateUserProfile::Request &req, kortex_driver::UpdateUserProfile::Response &res) +bool BaseRobotServices::UpdateUserProfile(kortex_driver::UpdateUserProfile::Request &req, kortex_driver::UpdateUserProfile::Response &res) { Kinova::Api::Base::UserProfile input; @@ -310,7 +310,7 @@ bool BaseServices::UpdateUserProfile(kortex_driver::UpdateUserProfile::Request return true; } -bool BaseServices::ReadUserProfile(kortex_driver::ReadUserProfile::Request &req, kortex_driver::ReadUserProfile::Response &res) +bool BaseRobotServices::ReadUserProfile(kortex_driver::ReadUserProfile::Request &req, kortex_driver::ReadUserProfile::Response &res) { Kinova::Api::Common::UserProfileHandle input; @@ -345,7 +345,7 @@ bool BaseServices::ReadUserProfile(kortex_driver::ReadUserProfile::Request &req return true; } -bool BaseServices::DeleteUserProfile(kortex_driver::DeleteUserProfile::Request &req, kortex_driver::DeleteUserProfile::Response &res) +bool BaseRobotServices::DeleteUserProfile(kortex_driver::DeleteUserProfile::Request &req, kortex_driver::DeleteUserProfile::Response &res) { Kinova::Api::Common::UserProfileHandle input; @@ -377,7 +377,7 @@ bool BaseServices::DeleteUserProfile(kortex_driver::DeleteUserProfile::Request return true; } -bool BaseServices::ReadAllUserProfiles(kortex_driver::ReadAllUserProfiles::Request &req, kortex_driver::ReadAllUserProfiles::Response &res) +bool BaseRobotServices::ReadAllUserProfiles(kortex_driver::ReadAllUserProfiles::Request &req, kortex_driver::ReadAllUserProfiles::Response &res) { Kinova::Api::Base::UserProfileList output; @@ -410,7 +410,7 @@ bool BaseServices::ReadAllUserProfiles(kortex_driver::ReadAllUserProfiles::Reque return true; } -bool BaseServices::ReadAllUsers(kortex_driver::ReadAllUsers::Request &req, kortex_driver::ReadAllUsers::Response &res) +bool BaseRobotServices::ReadAllUsers(kortex_driver::ReadAllUsers::Request &req, kortex_driver::ReadAllUsers::Response &res) { Kinova::Api::Base::UserList output; @@ -443,7 +443,7 @@ bool BaseServices::ReadAllUsers(kortex_driver::ReadAllUsers::Request &req, kort return true; } -bool BaseServices::ChangePassword(kortex_driver::ChangePassword::Request &req, kortex_driver::ChangePassword::Response &res) +bool BaseRobotServices::ChangePassword(kortex_driver::ChangePassword::Request &req, kortex_driver::ChangePassword::Response &res) { Kinova::Api::Base::PasswordChange input; @@ -475,7 +475,7 @@ bool BaseServices::ChangePassword(kortex_driver::ChangePassword::Request &req, return true; } -bool BaseServices::CreateSequence(kortex_driver::CreateSequence::Request &req, kortex_driver::CreateSequence::Response &res) +bool BaseRobotServices::CreateSequence(kortex_driver::CreateSequence::Request &req, kortex_driver::CreateSequence::Response &res) { Kinova::Api::Base::Sequence input; @@ -510,7 +510,7 @@ bool BaseServices::CreateSequence(kortex_driver::CreateSequence::Request &req, return true; } -bool BaseServices::UpdateSequence(kortex_driver::UpdateSequence::Request &req, kortex_driver::UpdateSequence::Response &res) +bool BaseRobotServices::UpdateSequence(kortex_driver::UpdateSequence::Request &req, kortex_driver::UpdateSequence::Response &res) { Kinova::Api::Base::Sequence input; @@ -542,7 +542,7 @@ bool BaseServices::UpdateSequence(kortex_driver::UpdateSequence::Request &req, return true; } -bool BaseServices::ReadSequence(kortex_driver::ReadSequence::Request &req, kortex_driver::ReadSequence::Response &res) +bool BaseRobotServices::ReadSequence(kortex_driver::ReadSequence::Request &req, kortex_driver::ReadSequence::Response &res) { Kinova::Api::Base::SequenceHandle input; @@ -577,7 +577,7 @@ bool BaseServices::ReadSequence(kortex_driver::ReadSequence::Request &req, kort return true; } -bool BaseServices::DeleteSequence(kortex_driver::DeleteSequence::Request &req, kortex_driver::DeleteSequence::Response &res) +bool BaseRobotServices::DeleteSequence(kortex_driver::DeleteSequence::Request &req, kortex_driver::DeleteSequence::Response &res) { Kinova::Api::Base::SequenceHandle input; @@ -609,7 +609,7 @@ bool BaseServices::DeleteSequence(kortex_driver::DeleteSequence::Request &req, return true; } -bool BaseServices::ReadAllSequences(kortex_driver::ReadAllSequences::Request &req, kortex_driver::ReadAllSequences::Response &res) +bool BaseRobotServices::ReadAllSequences(kortex_driver::ReadAllSequences::Request &req, kortex_driver::ReadAllSequences::Response &res) { Kinova::Api::Base::SequenceList output; @@ -642,7 +642,7 @@ bool BaseServices::ReadAllSequences(kortex_driver::ReadAllSequences::Request &r return true; } -bool BaseServices::PlaySequence(kortex_driver::PlaySequence::Request &req, kortex_driver::PlaySequence::Response &res) +bool BaseRobotServices::PlaySequence(kortex_driver::PlaySequence::Request &req, kortex_driver::PlaySequence::Response &res) { Kinova::Api::Base::SequenceHandle input; @@ -674,7 +674,7 @@ bool BaseServices::PlaySequence(kortex_driver::PlaySequence::Request &req, kort return true; } -bool BaseServices::PlayAdvancedSequence(kortex_driver::PlayAdvancedSequence::Request &req, kortex_driver::PlayAdvancedSequence::Response &res) +bool BaseRobotServices::PlayAdvancedSequence(kortex_driver::PlayAdvancedSequence::Request &req, kortex_driver::PlayAdvancedSequence::Response &res) { Kinova::Api::Base::AdvancedSequenceHandle input; @@ -706,7 +706,7 @@ bool BaseServices::PlayAdvancedSequence(kortex_driver::PlayAdvancedSequence::Req return true; } -bool BaseServices::StopSequence(kortex_driver::StopSequence::Request &req, kortex_driver::StopSequence::Response &res) +bool BaseRobotServices::StopSequence(kortex_driver::StopSequence::Request &req, kortex_driver::StopSequence::Response &res) { kortex_driver::KortexError result_error; @@ -736,7 +736,7 @@ bool BaseServices::StopSequence(kortex_driver::StopSequence::Request &req, kort return true; } -bool BaseServices::PauseSequence(kortex_driver::PauseSequence::Request &req, kortex_driver::PauseSequence::Response &res) +bool BaseRobotServices::PauseSequence(kortex_driver::PauseSequence::Request &req, kortex_driver::PauseSequence::Response &res) { kortex_driver::KortexError result_error; @@ -766,7 +766,7 @@ bool BaseServices::PauseSequence(kortex_driver::PauseSequence::Request &req, ko return true; } -bool BaseServices::ResumeSequence(kortex_driver::ResumeSequence::Request &req, kortex_driver::ResumeSequence::Response &res) +bool BaseRobotServices::ResumeSequence(kortex_driver::ResumeSequence::Request &req, kortex_driver::ResumeSequence::Response &res) { kortex_driver::KortexError result_error; @@ -796,7 +796,7 @@ bool BaseServices::ResumeSequence(kortex_driver::ResumeSequence::Request &req, return true; } -bool BaseServices::CreateProtectionZone(kortex_driver::CreateProtectionZone::Request &req, kortex_driver::CreateProtectionZone::Response &res) +bool BaseRobotServices::CreateProtectionZone(kortex_driver::CreateProtectionZone::Request &req, kortex_driver::CreateProtectionZone::Response &res) { Kinova::Api::Base::ProtectionZone input; @@ -831,7 +831,7 @@ bool BaseServices::CreateProtectionZone(kortex_driver::CreateProtectionZone::Req return true; } -bool BaseServices::UpdateProtectionZone(kortex_driver::UpdateProtectionZone::Request &req, kortex_driver::UpdateProtectionZone::Response &res) +bool BaseRobotServices::UpdateProtectionZone(kortex_driver::UpdateProtectionZone::Request &req, kortex_driver::UpdateProtectionZone::Response &res) { Kinova::Api::Base::ProtectionZone input; @@ -863,7 +863,7 @@ bool BaseServices::UpdateProtectionZone(kortex_driver::UpdateProtectionZone::Req return true; } -bool BaseServices::ReadProtectionZone(kortex_driver::ReadProtectionZone::Request &req, kortex_driver::ReadProtectionZone::Response &res) +bool BaseRobotServices::ReadProtectionZone(kortex_driver::ReadProtectionZone::Request &req, kortex_driver::ReadProtectionZone::Response &res) { Kinova::Api::Base::ProtectionZoneHandle input; @@ -898,7 +898,7 @@ bool BaseServices::ReadProtectionZone(kortex_driver::ReadProtectionZone::Request return true; } -bool BaseServices::DeleteProtectionZone(kortex_driver::DeleteProtectionZone::Request &req, kortex_driver::DeleteProtectionZone::Response &res) +bool BaseRobotServices::DeleteProtectionZone(kortex_driver::DeleteProtectionZone::Request &req, kortex_driver::DeleteProtectionZone::Response &res) { Kinova::Api::Base::ProtectionZoneHandle input; @@ -930,7 +930,7 @@ bool BaseServices::DeleteProtectionZone(kortex_driver::DeleteProtectionZone::Req return true; } -bool BaseServices::ReadAllProtectionZones(kortex_driver::ReadAllProtectionZones::Request &req, kortex_driver::ReadAllProtectionZones::Response &res) +bool BaseRobotServices::ReadAllProtectionZones(kortex_driver::ReadAllProtectionZones::Request &req, kortex_driver::ReadAllProtectionZones::Response &res) { Kinova::Api::Base::ProtectionZoneList output; @@ -963,7 +963,7 @@ bool BaseServices::ReadAllProtectionZones(kortex_driver::ReadAllProtectionZones: return true; } -bool BaseServices::CreateMapping(kortex_driver::CreateMapping::Request &req, kortex_driver::CreateMapping::Response &res) +bool BaseRobotServices::CreateMapping(kortex_driver::CreateMapping::Request &req, kortex_driver::CreateMapping::Response &res) { Kinova::Api::Base::Mapping input; @@ -998,7 +998,7 @@ bool BaseServices::CreateMapping(kortex_driver::CreateMapping::Request &req, ko return true; } -bool BaseServices::ReadMapping(kortex_driver::ReadMapping::Request &req, kortex_driver::ReadMapping::Response &res) +bool BaseRobotServices::ReadMapping(kortex_driver::ReadMapping::Request &req, kortex_driver::ReadMapping::Response &res) { Kinova::Api::Base::MappingHandle input; @@ -1033,7 +1033,7 @@ bool BaseServices::ReadMapping(kortex_driver::ReadMapping::Request &req, kortex return true; } -bool BaseServices::UpdateMapping(kortex_driver::UpdateMapping::Request &req, kortex_driver::UpdateMapping::Response &res) +bool BaseRobotServices::UpdateMapping(kortex_driver::UpdateMapping::Request &req, kortex_driver::UpdateMapping::Response &res) { Kinova::Api::Base::Mapping input; @@ -1065,7 +1065,7 @@ bool BaseServices::UpdateMapping(kortex_driver::UpdateMapping::Request &req, ko return true; } -bool BaseServices::DeleteMapping(kortex_driver::DeleteMapping::Request &req, kortex_driver::DeleteMapping::Response &res) +bool BaseRobotServices::DeleteMapping(kortex_driver::DeleteMapping::Request &req, kortex_driver::DeleteMapping::Response &res) { Kinova::Api::Base::MappingHandle input; @@ -1097,7 +1097,7 @@ bool BaseServices::DeleteMapping(kortex_driver::DeleteMapping::Request &req, ko return true; } -bool BaseServices::ReadAllMappings(kortex_driver::ReadAllMappings::Request &req, kortex_driver::ReadAllMappings::Response &res) +bool BaseRobotServices::ReadAllMappings(kortex_driver::ReadAllMappings::Request &req, kortex_driver::ReadAllMappings::Response &res) { Kinova::Api::Base::MappingList output; @@ -1130,7 +1130,7 @@ bool BaseServices::ReadAllMappings(kortex_driver::ReadAllMappings::Request &req return true; } -bool BaseServices::CreateMap(kortex_driver::CreateMap::Request &req, kortex_driver::CreateMap::Response &res) +bool BaseRobotServices::CreateMap(kortex_driver::CreateMap::Request &req, kortex_driver::CreateMap::Response &res) { Kinova::Api::Base::Map input; @@ -1165,7 +1165,7 @@ bool BaseServices::CreateMap(kortex_driver::CreateMap::Request &req, kortex_dri return true; } -bool BaseServices::ReadMap(kortex_driver::ReadMap::Request &req, kortex_driver::ReadMap::Response &res) +bool BaseRobotServices::ReadMap(kortex_driver::ReadMap::Request &req, kortex_driver::ReadMap::Response &res) { Kinova::Api::Base::MapHandle input; @@ -1200,7 +1200,7 @@ bool BaseServices::ReadMap(kortex_driver::ReadMap::Request &req, kortex_driver: return true; } -bool BaseServices::UpdateMap(kortex_driver::UpdateMap::Request &req, kortex_driver::UpdateMap::Response &res) +bool BaseRobotServices::UpdateMap(kortex_driver::UpdateMap::Request &req, kortex_driver::UpdateMap::Response &res) { Kinova::Api::Base::Map input; @@ -1232,7 +1232,7 @@ bool BaseServices::UpdateMap(kortex_driver::UpdateMap::Request &req, kortex_dri return true; } -bool BaseServices::DeleteMap(kortex_driver::DeleteMap::Request &req, kortex_driver::DeleteMap::Response &res) +bool BaseRobotServices::DeleteMap(kortex_driver::DeleteMap::Request &req, kortex_driver::DeleteMap::Response &res) { Kinova::Api::Base::MapHandle input; @@ -1264,7 +1264,7 @@ bool BaseServices::DeleteMap(kortex_driver::DeleteMap::Request &req, kortex_dri return true; } -bool BaseServices::ReadAllMaps(kortex_driver::ReadAllMaps::Request &req, kortex_driver::ReadAllMaps::Response &res) +bool BaseRobotServices::ReadAllMaps(kortex_driver::ReadAllMaps::Request &req, kortex_driver::ReadAllMaps::Response &res) { Kinova::Api::Base::MappingHandle input; @@ -1299,7 +1299,7 @@ bool BaseServices::ReadAllMaps(kortex_driver::ReadAllMaps::Request &req, kortex return true; } -bool BaseServices::ActivateMap(kortex_driver::ActivateMap::Request &req, kortex_driver::ActivateMap::Response &res) +bool BaseRobotServices::ActivateMap(kortex_driver::ActivateMap::Request &req, kortex_driver::ActivateMap::Response &res) { Kinova::Api::Base::ActivateMapHandle input; @@ -1331,7 +1331,7 @@ bool BaseServices::ActivateMap(kortex_driver::ActivateMap::Request &req, kortex return true; } -bool BaseServices::CreateAction(kortex_driver::CreateAction::Request &req, kortex_driver::CreateAction::Response &res) +bool BaseRobotServices::CreateAction(kortex_driver::CreateAction::Request &req, kortex_driver::CreateAction::Response &res) { Kinova::Api::Base::Action input; @@ -1366,7 +1366,7 @@ bool BaseServices::CreateAction(kortex_driver::CreateAction::Request &req, kort return true; } -bool BaseServices::ReadAction(kortex_driver::ReadAction::Request &req, kortex_driver::ReadAction::Response &res) +bool BaseRobotServices::ReadAction(kortex_driver::ReadAction::Request &req, kortex_driver::ReadAction::Response &res) { Kinova::Api::Base::ActionHandle input; @@ -1401,7 +1401,7 @@ bool BaseServices::ReadAction(kortex_driver::ReadAction::Request &req, kortex_d return true; } -bool BaseServices::ReadAllActions(kortex_driver::ReadAllActions::Request &req, kortex_driver::ReadAllActions::Response &res) +bool BaseRobotServices::ReadAllActions(kortex_driver::ReadAllActions::Request &req, kortex_driver::ReadAllActions::Response &res) { Kinova::Api::Base::RequestedActionType input; @@ -1436,7 +1436,7 @@ bool BaseServices::ReadAllActions(kortex_driver::ReadAllActions::Request &req, return true; } -bool BaseServices::DeleteAction(kortex_driver::DeleteAction::Request &req, kortex_driver::DeleteAction::Response &res) +bool BaseRobotServices::DeleteAction(kortex_driver::DeleteAction::Request &req, kortex_driver::DeleteAction::Response &res) { Kinova::Api::Base::ActionHandle input; @@ -1468,7 +1468,7 @@ bool BaseServices::DeleteAction(kortex_driver::DeleteAction::Request &req, kort return true; } -bool BaseServices::UpdateAction(kortex_driver::UpdateAction::Request &req, kortex_driver::UpdateAction::Response &res) +bool BaseRobotServices::UpdateAction(kortex_driver::UpdateAction::Request &req, kortex_driver::UpdateAction::Response &res) { Kinova::Api::Base::Action input; @@ -1500,7 +1500,7 @@ bool BaseServices::UpdateAction(kortex_driver::UpdateAction::Request &req, kort return true; } -bool BaseServices::ExecuteActionFromReference(kortex_driver::ExecuteActionFromReference::Request &req, kortex_driver::ExecuteActionFromReference::Response &res) +bool BaseRobotServices::ExecuteActionFromReference(kortex_driver::ExecuteActionFromReference::Request &req, kortex_driver::ExecuteActionFromReference::Response &res) { Kinova::Api::Base::ActionHandle input; @@ -1532,7 +1532,7 @@ bool BaseServices::ExecuteActionFromReference(kortex_driver::ExecuteActionFromRe return true; } -bool BaseServices::ExecuteAction(kortex_driver::ExecuteAction::Request &req, kortex_driver::ExecuteAction::Response &res) +bool BaseRobotServices::ExecuteAction(kortex_driver::ExecuteAction::Request &req, kortex_driver::ExecuteAction::Response &res) { Kinova::Api::Base::Action input; @@ -1564,7 +1564,7 @@ bool BaseServices::ExecuteAction(kortex_driver::ExecuteAction::Request &req, ko return true; } -bool BaseServices::PauseAction(kortex_driver::PauseAction::Request &req, kortex_driver::PauseAction::Response &res) +bool BaseRobotServices::PauseAction(kortex_driver::PauseAction::Request &req, kortex_driver::PauseAction::Response &res) { kortex_driver::KortexError result_error; @@ -1594,7 +1594,7 @@ bool BaseServices::PauseAction(kortex_driver::PauseAction::Request &req, kortex return true; } -bool BaseServices::StopAction(kortex_driver::StopAction::Request &req, kortex_driver::StopAction::Response &res) +bool BaseRobotServices::StopAction(kortex_driver::StopAction::Request &req, kortex_driver::StopAction::Response &res) { kortex_driver::KortexError result_error; @@ -1624,7 +1624,7 @@ bool BaseServices::StopAction(kortex_driver::StopAction::Request &req, kortex_d return true; } -bool BaseServices::ResumeAction(kortex_driver::ResumeAction::Request &req, kortex_driver::ResumeAction::Response &res) +bool BaseRobotServices::ResumeAction(kortex_driver::ResumeAction::Request &req, kortex_driver::ResumeAction::Response &res) { kortex_driver::KortexError result_error; @@ -1654,7 +1654,7 @@ bool BaseServices::ResumeAction(kortex_driver::ResumeAction::Request &req, kort return true; } -bool BaseServices::GetIPv4Configuration(kortex_driver::GetIPv4Configuration::Request &req, kortex_driver::GetIPv4Configuration::Response &res) +bool BaseRobotServices::GetIPv4Configuration(kortex_driver::GetIPv4Configuration::Request &req, kortex_driver::GetIPv4Configuration::Response &res) { Kinova::Api::Base::NetworkHandle input; @@ -1689,7 +1689,7 @@ bool BaseServices::GetIPv4Configuration(kortex_driver::GetIPv4Configuration::Req return true; } -bool BaseServices::SetIPv4Configuration(kortex_driver::SetIPv4Configuration::Request &req, kortex_driver::SetIPv4Configuration::Response &res) +bool BaseRobotServices::SetIPv4Configuration(kortex_driver::SetIPv4Configuration::Request &req, kortex_driver::SetIPv4Configuration::Response &res) { Kinova::Api::Base::FullIPv4Configuration input; @@ -1721,7 +1721,7 @@ bool BaseServices::SetIPv4Configuration(kortex_driver::SetIPv4Configuration::Req return true; } -bool BaseServices::SetCommunicationInterfaceEnable(kortex_driver::SetCommunicationInterfaceEnable::Request &req, kortex_driver::SetCommunicationInterfaceEnable::Response &res) +bool BaseRobotServices::SetCommunicationInterfaceEnable(kortex_driver::SetCommunicationInterfaceEnable::Request &req, kortex_driver::SetCommunicationInterfaceEnable::Response &res) { Kinova::Api::Base::CommunicationInterfaceConfiguration input; @@ -1753,7 +1753,7 @@ bool BaseServices::SetCommunicationInterfaceEnable(kortex_driver::SetCommunicati return true; } -bool BaseServices::IsCommunicationInterfaceEnable(kortex_driver::IsCommunicationInterfaceEnable::Request &req, kortex_driver::IsCommunicationInterfaceEnable::Response &res) +bool BaseRobotServices::IsCommunicationInterfaceEnable(kortex_driver::IsCommunicationInterfaceEnable::Request &req, kortex_driver::IsCommunicationInterfaceEnable::Response &res) { Kinova::Api::Base::NetworkHandle input; @@ -1788,7 +1788,7 @@ bool BaseServices::IsCommunicationInterfaceEnable(kortex_driver::IsCommunication return true; } -bool BaseServices::GetAvailableWifi(kortex_driver::GetAvailableWifi::Request &req, kortex_driver::GetAvailableWifi::Response &res) +bool BaseRobotServices::GetAvailableWifi(kortex_driver::GetAvailableWifi::Request &req, kortex_driver::GetAvailableWifi::Response &res) { Kinova::Api::Base::WifiInformationList output; @@ -1821,7 +1821,7 @@ bool BaseServices::GetAvailableWifi(kortex_driver::GetAvailableWifi::Request &r return true; } -bool BaseServices::GetWifiInformation(kortex_driver::GetWifiInformation::Request &req, kortex_driver::GetWifiInformation::Response &res) +bool BaseRobotServices::GetWifiInformation(kortex_driver::GetWifiInformation::Request &req, kortex_driver::GetWifiInformation::Response &res) { Kinova::Api::Base::Ssid input; @@ -1856,7 +1856,7 @@ bool BaseServices::GetWifiInformation(kortex_driver::GetWifiInformation::Request return true; } -bool BaseServices::AddWifiConfiguration(kortex_driver::AddWifiConfiguration::Request &req, kortex_driver::AddWifiConfiguration::Response &res) +bool BaseRobotServices::AddWifiConfiguration(kortex_driver::AddWifiConfiguration::Request &req, kortex_driver::AddWifiConfiguration::Response &res) { Kinova::Api::Base::WifiConfiguration input; @@ -1888,7 +1888,7 @@ bool BaseServices::AddWifiConfiguration(kortex_driver::AddWifiConfiguration::Req return true; } -bool BaseServices::DeleteWifiConfiguration(kortex_driver::DeleteWifiConfiguration::Request &req, kortex_driver::DeleteWifiConfiguration::Response &res) +bool BaseRobotServices::DeleteWifiConfiguration(kortex_driver::DeleteWifiConfiguration::Request &req, kortex_driver::DeleteWifiConfiguration::Response &res) { Kinova::Api::Base::Ssid input; @@ -1920,7 +1920,7 @@ bool BaseServices::DeleteWifiConfiguration(kortex_driver::DeleteWifiConfiguratio return true; } -bool BaseServices::GetAllConfiguredWifis(kortex_driver::GetAllConfiguredWifis::Request &req, kortex_driver::GetAllConfiguredWifis::Response &res) +bool BaseRobotServices::GetAllConfiguredWifis(kortex_driver::GetAllConfiguredWifis::Request &req, kortex_driver::GetAllConfiguredWifis::Response &res) { Kinova::Api::Base::WifiConfigurationList output; @@ -1953,7 +1953,7 @@ bool BaseServices::GetAllConfiguredWifis(kortex_driver::GetAllConfiguredWifis::R return true; } -bool BaseServices::ConnectWifi(kortex_driver::ConnectWifi::Request &req, kortex_driver::ConnectWifi::Response &res) +bool BaseRobotServices::ConnectWifi(kortex_driver::ConnectWifi::Request &req, kortex_driver::ConnectWifi::Response &res) { Kinova::Api::Base::Ssid input; @@ -1985,7 +1985,7 @@ bool BaseServices::ConnectWifi(kortex_driver::ConnectWifi::Request &req, kortex return true; } -bool BaseServices::DisconnectWifi(kortex_driver::DisconnectWifi::Request &req, kortex_driver::DisconnectWifi::Response &res) +bool BaseRobotServices::DisconnectWifi(kortex_driver::DisconnectWifi::Request &req, kortex_driver::DisconnectWifi::Response &res) { kortex_driver::KortexError result_error; @@ -2015,7 +2015,7 @@ bool BaseServices::DisconnectWifi(kortex_driver::DisconnectWifi::Request &req, return true; } -bool BaseServices::GetConnectedWifiInformation(kortex_driver::GetConnectedWifiInformation::Request &req, kortex_driver::GetConnectedWifiInformation::Response &res) +bool BaseRobotServices::GetConnectedWifiInformation(kortex_driver::GetConnectedWifiInformation::Request &req, kortex_driver::GetConnectedWifiInformation::Response &res) { Kinova::Api::Base::WifiInformation output; @@ -2048,7 +2048,7 @@ bool BaseServices::GetConnectedWifiInformation(kortex_driver::GetConnectedWifiIn return true; } -bool BaseServices::Base_Unsubscribe(kortex_driver::Base_Unsubscribe::Request &req, kortex_driver::Base_Unsubscribe::Response &res) +bool BaseRobotServices::Base_Unsubscribe(kortex_driver::Base_Unsubscribe::Request &req, kortex_driver::Base_Unsubscribe::Response &res) { Kinova::Api::Common::NotificationHandle input; @@ -2080,7 +2080,7 @@ bool BaseServices::Base_Unsubscribe(kortex_driver::Base_Unsubscribe::Request &r return true; } -bool BaseServices::OnNotificationConfigurationChangeTopic(kortex_driver::OnNotificationConfigurationChangeTopic::Request &req, kortex_driver::OnNotificationConfigurationChangeTopic::Response &res) +bool BaseRobotServices::OnNotificationConfigurationChangeTopic(kortex_driver::OnNotificationConfigurationChangeTopic::Request &req, kortex_driver::OnNotificationConfigurationChangeTopic::Response &res) { // If the notification is already activated, don't activate multiple times @@ -2094,7 +2094,7 @@ bool BaseServices::OnNotificationConfigurationChangeTopic(kortex_driver::OnNotif try { - std::function< void (Kinova::Api::Base::ConfigurationChangeNotification) > callback = std::bind(&BaseServices::cb_ConfigurationChangeTopic, this, std::placeholders::_1); + std::function< void (Kinova::Api::Base::ConfigurationChangeNotification) > callback = std::bind(&BaseRobotServices::cb_ConfigurationChangeTopic, this, std::placeholders::_1); output = m_base->OnNotificationConfigurationChangeTopic(callback, input, m_current_device_id); m_is_activated_ConfigurationChangeTopic = true; } @@ -2119,14 +2119,14 @@ bool BaseServices::OnNotificationConfigurationChangeTopic(kortex_driver::OnNotif ToRosData(output, res.output); return true; } -void BaseServices::cb_ConfigurationChangeTopic(Kinova::Api::Base::ConfigurationChangeNotification notif) +void BaseRobotServices::cb_ConfigurationChangeTopic(Kinova::Api::Base::ConfigurationChangeNotification notif) { kortex_driver::ConfigurationChangeNotification ros_msg; ToRosData(notif, ros_msg); m_pub_ConfigurationChangeTopic.publish(ros_msg); } -bool BaseServices::OnNotificationMappingInfoTopic(kortex_driver::OnNotificationMappingInfoTopic::Request &req, kortex_driver::OnNotificationMappingInfoTopic::Response &res) +bool BaseRobotServices::OnNotificationMappingInfoTopic(kortex_driver::OnNotificationMappingInfoTopic::Request &req, kortex_driver::OnNotificationMappingInfoTopic::Response &res) { // If the notification is already activated, don't activate multiple times @@ -2140,7 +2140,7 @@ bool BaseServices::OnNotificationMappingInfoTopic(kortex_driver::OnNotificationM try { - std::function< void (Kinova::Api::Base::MappingInfoNotification) > callback = std::bind(&BaseServices::cb_MappingInfoTopic, this, std::placeholders::_1); + std::function< void (Kinova::Api::Base::MappingInfoNotification) > callback = std::bind(&BaseRobotServices::cb_MappingInfoTopic, this, std::placeholders::_1); output = m_base->OnNotificationMappingInfoTopic(callback, input, m_current_device_id); m_is_activated_MappingInfoTopic = true; } @@ -2165,14 +2165,14 @@ bool BaseServices::OnNotificationMappingInfoTopic(kortex_driver::OnNotificationM ToRosData(output, res.output); return true; } -void BaseServices::cb_MappingInfoTopic(Kinova::Api::Base::MappingInfoNotification notif) +void BaseRobotServices::cb_MappingInfoTopic(Kinova::Api::Base::MappingInfoNotification notif) { kortex_driver::MappingInfoNotification ros_msg; ToRosData(notif, ros_msg); m_pub_MappingInfoTopic.publish(ros_msg); } -bool BaseServices::OnNotificationControlModeTopic(kortex_driver::OnNotificationControlModeTopic::Request &req, kortex_driver::OnNotificationControlModeTopic::Response &res) +bool BaseRobotServices::OnNotificationControlModeTopic(kortex_driver::OnNotificationControlModeTopic::Request &req, kortex_driver::OnNotificationControlModeTopic::Response &res) { // If the notification is already activated, don't activate multiple times @@ -2186,7 +2186,7 @@ bool BaseServices::OnNotificationControlModeTopic(kortex_driver::OnNotificationC try { - std::function< void (Kinova::Api::Base::ControlModeNotification) > callback = std::bind(&BaseServices::cb_ControlModeTopic, this, std::placeholders::_1); + std::function< void (Kinova::Api::Base::ControlModeNotification) > callback = std::bind(&BaseRobotServices::cb_ControlModeTopic, this, std::placeholders::_1); output = m_base->OnNotificationControlModeTopic(callback, input, m_current_device_id); m_is_activated_ControlModeTopic = true; } @@ -2211,14 +2211,14 @@ bool BaseServices::OnNotificationControlModeTopic(kortex_driver::OnNotificationC ToRosData(output, res.output); return true; } -void BaseServices::cb_ControlModeTopic(Kinova::Api::Base::ControlModeNotification notif) +void BaseRobotServices::cb_ControlModeTopic(Kinova::Api::Base::ControlModeNotification notif) { kortex_driver::ControlModeNotification ros_msg; ToRosData(notif, ros_msg); m_pub_ControlModeTopic.publish(ros_msg); } -bool BaseServices::OnNotificationOperatingModeTopic(kortex_driver::OnNotificationOperatingModeTopic::Request &req, kortex_driver::OnNotificationOperatingModeTopic::Response &res) +bool BaseRobotServices::OnNotificationOperatingModeTopic(kortex_driver::OnNotificationOperatingModeTopic::Request &req, kortex_driver::OnNotificationOperatingModeTopic::Response &res) { // If the notification is already activated, don't activate multiple times @@ -2232,7 +2232,7 @@ bool BaseServices::OnNotificationOperatingModeTopic(kortex_driver::OnNotificatio try { - std::function< void (Kinova::Api::Base::OperatingModeNotification) > callback = std::bind(&BaseServices::cb_OperatingModeTopic, this, std::placeholders::_1); + std::function< void (Kinova::Api::Base::OperatingModeNotification) > callback = std::bind(&BaseRobotServices::cb_OperatingModeTopic, this, std::placeholders::_1); output = m_base->OnNotificationOperatingModeTopic(callback, input, m_current_device_id); m_is_activated_OperatingModeTopic = true; } @@ -2257,14 +2257,14 @@ bool BaseServices::OnNotificationOperatingModeTopic(kortex_driver::OnNotificatio ToRosData(output, res.output); return true; } -void BaseServices::cb_OperatingModeTopic(Kinova::Api::Base::OperatingModeNotification notif) +void BaseRobotServices::cb_OperatingModeTopic(Kinova::Api::Base::OperatingModeNotification notif) { kortex_driver::OperatingModeNotification ros_msg; ToRosData(notif, ros_msg); m_pub_OperatingModeTopic.publish(ros_msg); } -bool BaseServices::OnNotificationSequenceInfoTopic(kortex_driver::OnNotificationSequenceInfoTopic::Request &req, kortex_driver::OnNotificationSequenceInfoTopic::Response &res) +bool BaseRobotServices::OnNotificationSequenceInfoTopic(kortex_driver::OnNotificationSequenceInfoTopic::Request &req, kortex_driver::OnNotificationSequenceInfoTopic::Response &res) { // If the notification is already activated, don't activate multiple times @@ -2278,7 +2278,7 @@ bool BaseServices::OnNotificationSequenceInfoTopic(kortex_driver::OnNotification try { - std::function< void (Kinova::Api::Base::SequenceInfoNotification) > callback = std::bind(&BaseServices::cb_SequenceInfoTopic, this, std::placeholders::_1); + std::function< void (Kinova::Api::Base::SequenceInfoNotification) > callback = std::bind(&BaseRobotServices::cb_SequenceInfoTopic, this, std::placeholders::_1); output = m_base->OnNotificationSequenceInfoTopic(callback, input, m_current_device_id); m_is_activated_SequenceInfoTopic = true; } @@ -2303,14 +2303,14 @@ bool BaseServices::OnNotificationSequenceInfoTopic(kortex_driver::OnNotification ToRosData(output, res.output); return true; } -void BaseServices::cb_SequenceInfoTopic(Kinova::Api::Base::SequenceInfoNotification notif) +void BaseRobotServices::cb_SequenceInfoTopic(Kinova::Api::Base::SequenceInfoNotification notif) { kortex_driver::SequenceInfoNotification ros_msg; ToRosData(notif, ros_msg); m_pub_SequenceInfoTopic.publish(ros_msg); } -bool BaseServices::OnNotificationProtectionZoneTopic(kortex_driver::OnNotificationProtectionZoneTopic::Request &req, kortex_driver::OnNotificationProtectionZoneTopic::Response &res) +bool BaseRobotServices::OnNotificationProtectionZoneTopic(kortex_driver::OnNotificationProtectionZoneTopic::Request &req, kortex_driver::OnNotificationProtectionZoneTopic::Response &res) { // If the notification is already activated, don't activate multiple times @@ -2324,7 +2324,7 @@ bool BaseServices::OnNotificationProtectionZoneTopic(kortex_driver::OnNotificati try { - std::function< void (Kinova::Api::Base::ProtectionZoneNotification) > callback = std::bind(&BaseServices::cb_ProtectionZoneTopic, this, std::placeholders::_1); + std::function< void (Kinova::Api::Base::ProtectionZoneNotification) > callback = std::bind(&BaseRobotServices::cb_ProtectionZoneTopic, this, std::placeholders::_1); output = m_base->OnNotificationProtectionZoneTopic(callback, input, m_current_device_id); m_is_activated_ProtectionZoneTopic = true; } @@ -2349,14 +2349,14 @@ bool BaseServices::OnNotificationProtectionZoneTopic(kortex_driver::OnNotificati ToRosData(output, res.output); return true; } -void BaseServices::cb_ProtectionZoneTopic(Kinova::Api::Base::ProtectionZoneNotification notif) +void BaseRobotServices::cb_ProtectionZoneTopic(Kinova::Api::Base::ProtectionZoneNotification notif) { kortex_driver::ProtectionZoneNotification ros_msg; ToRosData(notif, ros_msg); m_pub_ProtectionZoneTopic.publish(ros_msg); } -bool BaseServices::OnNotificationUserTopic(kortex_driver::OnNotificationUserTopic::Request &req, kortex_driver::OnNotificationUserTopic::Response &res) +bool BaseRobotServices::OnNotificationUserTopic(kortex_driver::OnNotificationUserTopic::Request &req, kortex_driver::OnNotificationUserTopic::Response &res) { // If the notification is already activated, don't activate multiple times @@ -2370,7 +2370,7 @@ bool BaseServices::OnNotificationUserTopic(kortex_driver::OnNotificationUserTopi try { - std::function< void (Kinova::Api::Base::UserNotification) > callback = std::bind(&BaseServices::cb_UserTopic, this, std::placeholders::_1); + std::function< void (Kinova::Api::Base::UserNotification) > callback = std::bind(&BaseRobotServices::cb_UserTopic, this, std::placeholders::_1); output = m_base->OnNotificationUserTopic(callback, input, m_current_device_id); m_is_activated_UserTopic = true; } @@ -2395,14 +2395,14 @@ bool BaseServices::OnNotificationUserTopic(kortex_driver::OnNotificationUserTopi ToRosData(output, res.output); return true; } -void BaseServices::cb_UserTopic(Kinova::Api::Base::UserNotification notif) +void BaseRobotServices::cb_UserTopic(Kinova::Api::Base::UserNotification notif) { kortex_driver::UserNotification ros_msg; ToRosData(notif, ros_msg); m_pub_UserTopic.publish(ros_msg); } -bool BaseServices::OnNotificationControllerTopic(kortex_driver::OnNotificationControllerTopic::Request &req, kortex_driver::OnNotificationControllerTopic::Response &res) +bool BaseRobotServices::OnNotificationControllerTopic(kortex_driver::OnNotificationControllerTopic::Request &req, kortex_driver::OnNotificationControllerTopic::Response &res) { // If the notification is already activated, don't activate multiple times @@ -2416,7 +2416,7 @@ bool BaseServices::OnNotificationControllerTopic(kortex_driver::OnNotificationCo try { - std::function< void (Kinova::Api::Base::ControllerNotification) > callback = std::bind(&BaseServices::cb_ControllerTopic, this, std::placeholders::_1); + std::function< void (Kinova::Api::Base::ControllerNotification) > callback = std::bind(&BaseRobotServices::cb_ControllerTopic, this, std::placeholders::_1); output = m_base->OnNotificationControllerTopic(callback, input, m_current_device_id); m_is_activated_ControllerTopic = true; } @@ -2441,14 +2441,14 @@ bool BaseServices::OnNotificationControllerTopic(kortex_driver::OnNotificationCo ToRosData(output, res.output); return true; } -void BaseServices::cb_ControllerTopic(Kinova::Api::Base::ControllerNotification notif) +void BaseRobotServices::cb_ControllerTopic(Kinova::Api::Base::ControllerNotification notif) { kortex_driver::ControllerNotification ros_msg; ToRosData(notif, ros_msg); m_pub_ControllerTopic.publish(ros_msg); } -bool BaseServices::OnNotificationActionTopic(kortex_driver::OnNotificationActionTopic::Request &req, kortex_driver::OnNotificationActionTopic::Response &res) +bool BaseRobotServices::OnNotificationActionTopic(kortex_driver::OnNotificationActionTopic::Request &req, kortex_driver::OnNotificationActionTopic::Response &res) { // If the notification is already activated, don't activate multiple times @@ -2462,7 +2462,7 @@ bool BaseServices::OnNotificationActionTopic(kortex_driver::OnNotificationAction try { - std::function< void (Kinova::Api::Base::ActionNotification) > callback = std::bind(&BaseServices::cb_ActionTopic, this, std::placeholders::_1); + std::function< void (Kinova::Api::Base::ActionNotification) > callback = std::bind(&BaseRobotServices::cb_ActionTopic, this, std::placeholders::_1); output = m_base->OnNotificationActionTopic(callback, input, m_current_device_id); m_is_activated_ActionTopic = true; } @@ -2487,14 +2487,14 @@ bool BaseServices::OnNotificationActionTopic(kortex_driver::OnNotificationAction ToRosData(output, res.output); return true; } -void BaseServices::cb_ActionTopic(Kinova::Api::Base::ActionNotification notif) +void BaseRobotServices::cb_ActionTopic(Kinova::Api::Base::ActionNotification notif) { kortex_driver::ActionNotification ros_msg; ToRosData(notif, ros_msg); m_pub_ActionTopic.publish(ros_msg); } -bool BaseServices::OnNotificationRobotEventTopic(kortex_driver::OnNotificationRobotEventTopic::Request &req, kortex_driver::OnNotificationRobotEventTopic::Response &res) +bool BaseRobotServices::OnNotificationRobotEventTopic(kortex_driver::OnNotificationRobotEventTopic::Request &req, kortex_driver::OnNotificationRobotEventTopic::Response &res) { // If the notification is already activated, don't activate multiple times @@ -2508,7 +2508,7 @@ bool BaseServices::OnNotificationRobotEventTopic(kortex_driver::OnNotificationRo try { - std::function< void (Kinova::Api::Base::RobotEventNotification) > callback = std::bind(&BaseServices::cb_RobotEventTopic, this, std::placeholders::_1); + std::function< void (Kinova::Api::Base::RobotEventNotification) > callback = std::bind(&BaseRobotServices::cb_RobotEventTopic, this, std::placeholders::_1); output = m_base->OnNotificationRobotEventTopic(callback, input, m_current_device_id); m_is_activated_RobotEventTopic = true; } @@ -2533,14 +2533,14 @@ bool BaseServices::OnNotificationRobotEventTopic(kortex_driver::OnNotificationRo ToRosData(output, res.output); return true; } -void BaseServices::cb_RobotEventTopic(Kinova::Api::Base::RobotEventNotification notif) +void BaseRobotServices::cb_RobotEventTopic(Kinova::Api::Base::RobotEventNotification notif) { kortex_driver::RobotEventNotification ros_msg; ToRosData(notif, ros_msg); m_pub_RobotEventTopic.publish(ros_msg); } -bool BaseServices::PlayCartesianTrajectory(kortex_driver::PlayCartesianTrajectory::Request &req, kortex_driver::PlayCartesianTrajectory::Response &res) +bool BaseRobotServices::PlayCartesianTrajectory(kortex_driver::PlayCartesianTrajectory::Request &req, kortex_driver::PlayCartesianTrajectory::Response &res) { Kinova::Api::Base::ConstrainedPose input; @@ -2572,7 +2572,7 @@ bool BaseServices::PlayCartesianTrajectory(kortex_driver::PlayCartesianTrajector return true; } -bool BaseServices::PlayCartesianTrajectoryPosition(kortex_driver::PlayCartesianTrajectoryPosition::Request &req, kortex_driver::PlayCartesianTrajectoryPosition::Response &res) +bool BaseRobotServices::PlayCartesianTrajectoryPosition(kortex_driver::PlayCartesianTrajectoryPosition::Request &req, kortex_driver::PlayCartesianTrajectoryPosition::Response &res) { Kinova::Api::Base::ConstrainedPosition input; @@ -2604,7 +2604,7 @@ bool BaseServices::PlayCartesianTrajectoryPosition(kortex_driver::PlayCartesianT return true; } -bool BaseServices::PlayCartesianTrajectoryOrientation(kortex_driver::PlayCartesianTrajectoryOrientation::Request &req, kortex_driver::PlayCartesianTrajectoryOrientation::Response &res) +bool BaseRobotServices::PlayCartesianTrajectoryOrientation(kortex_driver::PlayCartesianTrajectoryOrientation::Request &req, kortex_driver::PlayCartesianTrajectoryOrientation::Response &res) { Kinova::Api::Base::ConstrainedOrientation input; @@ -2636,7 +2636,7 @@ bool BaseServices::PlayCartesianTrajectoryOrientation(kortex_driver::PlayCartesi return true; } -bool BaseServices::Stop(kortex_driver::Stop::Request &req, kortex_driver::Stop::Response &res) +bool BaseRobotServices::Stop(kortex_driver::Stop::Request &req, kortex_driver::Stop::Response &res) { kortex_driver::KortexError result_error; @@ -2666,7 +2666,7 @@ bool BaseServices::Stop(kortex_driver::Stop::Request &req, kortex_driver::Stop: return true; } -bool BaseServices::GetMeasuredCartesianPose(kortex_driver::GetMeasuredCartesianPose::Request &req, kortex_driver::GetMeasuredCartesianPose::Response &res) +bool BaseRobotServices::GetMeasuredCartesianPose(kortex_driver::GetMeasuredCartesianPose::Request &req, kortex_driver::GetMeasuredCartesianPose::Response &res) { Kinova::Api::Base::Pose output; @@ -2699,7 +2699,7 @@ bool BaseServices::GetMeasuredCartesianPose(kortex_driver::GetMeasuredCartesianP return true; } -bool BaseServices::SendWrenchCommand(kortex_driver::SendWrenchCommand::Request &req, kortex_driver::SendWrenchCommand::Response &res) +bool BaseRobotServices::SendWrenchCommand(kortex_driver::SendWrenchCommand::Request &req, kortex_driver::SendWrenchCommand::Response &res) { Kinova::Api::Base::WrenchCommand input; @@ -2731,7 +2731,7 @@ bool BaseServices::SendWrenchCommand(kortex_driver::SendWrenchCommand::Request return true; } -bool BaseServices::SendWrenchJoystickCommand(kortex_driver::SendWrenchJoystickCommand::Request &req, kortex_driver::SendWrenchJoystickCommand::Response &res) +bool BaseRobotServices::SendWrenchJoystickCommand(kortex_driver::SendWrenchJoystickCommand::Request &req, kortex_driver::SendWrenchJoystickCommand::Response &res) { Kinova::Api::Base::WrenchCommand input; @@ -2763,7 +2763,7 @@ bool BaseServices::SendWrenchJoystickCommand(kortex_driver::SendWrenchJoystickCo return true; } -bool BaseServices::SendTwistJoystickCommand(kortex_driver::SendTwistJoystickCommand::Request &req, kortex_driver::SendTwistJoystickCommand::Response &res) +bool BaseRobotServices::SendTwistJoystickCommand(kortex_driver::SendTwistJoystickCommand::Request &req, kortex_driver::SendTwistJoystickCommand::Response &res) { Kinova::Api::Base::TwistCommand input; @@ -2795,7 +2795,7 @@ bool BaseServices::SendTwistJoystickCommand(kortex_driver::SendTwistJoystickComm return true; } -bool BaseServices::SendTwistCommand(kortex_driver::SendTwistCommand::Request &req, kortex_driver::SendTwistCommand::Response &res) +bool BaseRobotServices::SendTwistCommand(kortex_driver::SendTwistCommand::Request &req, kortex_driver::SendTwistCommand::Response &res) { Kinova::Api::Base::TwistCommand input; @@ -2827,7 +2827,7 @@ bool BaseServices::SendTwistCommand(kortex_driver::SendTwistCommand::Request &r return true; } -bool BaseServices::PlayJointTrajectory(kortex_driver::PlayJointTrajectory::Request &req, kortex_driver::PlayJointTrajectory::Response &res) +bool BaseRobotServices::PlayJointTrajectory(kortex_driver::PlayJointTrajectory::Request &req, kortex_driver::PlayJointTrajectory::Response &res) { Kinova::Api::Base::ConstrainedJointAngles input; @@ -2859,7 +2859,7 @@ bool BaseServices::PlayJointTrajectory(kortex_driver::PlayJointTrajectory::Reque return true; } -bool BaseServices::PlaySelectedJointTrajectory(kortex_driver::PlaySelectedJointTrajectory::Request &req, kortex_driver::PlaySelectedJointTrajectory::Response &res) +bool BaseRobotServices::PlaySelectedJointTrajectory(kortex_driver::PlaySelectedJointTrajectory::Request &req, kortex_driver::PlaySelectedJointTrajectory::Response &res) { Kinova::Api::Base::ConstrainedJointAngle input; @@ -2891,7 +2891,7 @@ bool BaseServices::PlaySelectedJointTrajectory(kortex_driver::PlaySelectedJointT return true; } -bool BaseServices::GetMeasuredJointAngles(kortex_driver::GetMeasuredJointAngles::Request &req, kortex_driver::GetMeasuredJointAngles::Response &res) +bool BaseRobotServices::GetMeasuredJointAngles(kortex_driver::GetMeasuredJointAngles::Request &req, kortex_driver::GetMeasuredJointAngles::Response &res) { Kinova::Api::Base::JointAngles output; @@ -2924,7 +2924,7 @@ bool BaseServices::GetMeasuredJointAngles(kortex_driver::GetMeasuredJointAngles: return true; } -bool BaseServices::SendJointSpeedsCommand(kortex_driver::SendJointSpeedsCommand::Request &req, kortex_driver::SendJointSpeedsCommand::Response &res) +bool BaseRobotServices::SendJointSpeedsCommand(kortex_driver::SendJointSpeedsCommand::Request &req, kortex_driver::SendJointSpeedsCommand::Response &res) { Kinova::Api::Base::JointSpeeds input; @@ -2956,7 +2956,7 @@ bool BaseServices::SendJointSpeedsCommand(kortex_driver::SendJointSpeedsCommand: return true; } -bool BaseServices::SendSelectedJointSpeedCommand(kortex_driver::SendSelectedJointSpeedCommand::Request &req, kortex_driver::SendSelectedJointSpeedCommand::Response &res) +bool BaseRobotServices::SendSelectedJointSpeedCommand(kortex_driver::SendSelectedJointSpeedCommand::Request &req, kortex_driver::SendSelectedJointSpeedCommand::Response &res) { Kinova::Api::Base::JointSpeed input; @@ -2988,7 +2988,7 @@ bool BaseServices::SendSelectedJointSpeedCommand(kortex_driver::SendSelectedJoin return true; } -bool BaseServices::SendGripperCommand(kortex_driver::SendGripperCommand::Request &req, kortex_driver::SendGripperCommand::Response &res) +bool BaseRobotServices::SendGripperCommand(kortex_driver::SendGripperCommand::Request &req, kortex_driver::SendGripperCommand::Response &res) { Kinova::Api::Base::GripperCommand input; @@ -3020,7 +3020,7 @@ bool BaseServices::SendGripperCommand(kortex_driver::SendGripperCommand::Request return true; } -bool BaseServices::GetMeasuredGripperMovement(kortex_driver::GetMeasuredGripperMovement::Request &req, kortex_driver::GetMeasuredGripperMovement::Response &res) +bool BaseRobotServices::GetMeasuredGripperMovement(kortex_driver::GetMeasuredGripperMovement::Request &req, kortex_driver::GetMeasuredGripperMovement::Response &res) { Kinova::Api::Base::GripperRequest input; @@ -3055,7 +3055,7 @@ bool BaseServices::GetMeasuredGripperMovement(kortex_driver::GetMeasuredGripperM return true; } -bool BaseServices::SetAdmittance(kortex_driver::SetAdmittance::Request &req, kortex_driver::SetAdmittance::Response &res) +bool BaseRobotServices::SetAdmittance(kortex_driver::SetAdmittance::Request &req, kortex_driver::SetAdmittance::Response &res) { Kinova::Api::Base::Admittance input; @@ -3087,7 +3087,7 @@ bool BaseServices::SetAdmittance(kortex_driver::SetAdmittance::Request &req, ko return true; } -bool BaseServices::SetOperatingMode(kortex_driver::SetOperatingMode::Request &req, kortex_driver::SetOperatingMode::Response &res) +bool BaseRobotServices::SetOperatingMode(kortex_driver::SetOperatingMode::Request &req, kortex_driver::SetOperatingMode::Response &res) { Kinova::Api::Base::OperatingModeInformation input; @@ -3119,7 +3119,7 @@ bool BaseServices::SetOperatingMode(kortex_driver::SetOperatingMode::Request &r return true; } -bool BaseServices::ApplyEmergencyStop(kortex_driver::ApplyEmergencyStop::Request &req, kortex_driver::ApplyEmergencyStop::Response &res) +bool BaseRobotServices::ApplyEmergencyStop(kortex_driver::ApplyEmergencyStop::Request &req, kortex_driver::ApplyEmergencyStop::Response &res) { kortex_driver::KortexError result_error; @@ -3149,7 +3149,7 @@ bool BaseServices::ApplyEmergencyStop(kortex_driver::ApplyEmergencyStop::Request return true; } -bool BaseServices::Base_ClearFaults(kortex_driver::Base_ClearFaults::Request &req, kortex_driver::Base_ClearFaults::Response &res) +bool BaseRobotServices::Base_ClearFaults(kortex_driver::Base_ClearFaults::Request &req, kortex_driver::Base_ClearFaults::Response &res) { kortex_driver::KortexError result_error; @@ -3179,7 +3179,7 @@ bool BaseServices::Base_ClearFaults(kortex_driver::Base_ClearFaults::Request &r return true; } -bool BaseServices::Base_GetControlMode(kortex_driver::Base_GetControlMode::Request &req, kortex_driver::Base_GetControlMode::Response &res) +bool BaseRobotServices::Base_GetControlMode(kortex_driver::Base_GetControlMode::Request &req, kortex_driver::Base_GetControlMode::Response &res) { Kinova::Api::Base::ControlModeInformation output; @@ -3212,7 +3212,7 @@ bool BaseServices::Base_GetControlMode(kortex_driver::Base_GetControlMode::Reque return true; } -bool BaseServices::GetOperatingMode(kortex_driver::GetOperatingMode::Request &req, kortex_driver::GetOperatingMode::Response &res) +bool BaseRobotServices::GetOperatingMode(kortex_driver::GetOperatingMode::Request &req, kortex_driver::GetOperatingMode::Response &res) { Kinova::Api::Base::OperatingModeInformation output; @@ -3245,7 +3245,7 @@ bool BaseServices::GetOperatingMode(kortex_driver::GetOperatingMode::Request &r return true; } -bool BaseServices::SetServoingMode(kortex_driver::SetServoingMode::Request &req, kortex_driver::SetServoingMode::Response &res) +bool BaseRobotServices::SetServoingMode(kortex_driver::SetServoingMode::Request &req, kortex_driver::SetServoingMode::Response &res) { Kinova::Api::Base::ServoingModeInformation input; @@ -3277,7 +3277,7 @@ bool BaseServices::SetServoingMode(kortex_driver::SetServoingMode::Request &req return true; } -bool BaseServices::GetServoingMode(kortex_driver::GetServoingMode::Request &req, kortex_driver::GetServoingMode::Response &res) +bool BaseRobotServices::GetServoingMode(kortex_driver::GetServoingMode::Request &req, kortex_driver::GetServoingMode::Response &res) { Kinova::Api::Base::ServoingModeInformation output; @@ -3310,7 +3310,7 @@ bool BaseServices::GetServoingMode(kortex_driver::GetServoingMode::Request &req return true; } -bool BaseServices::OnNotificationServoingModeTopic(kortex_driver::OnNotificationServoingModeTopic::Request &req, kortex_driver::OnNotificationServoingModeTopic::Response &res) +bool BaseRobotServices::OnNotificationServoingModeTopic(kortex_driver::OnNotificationServoingModeTopic::Request &req, kortex_driver::OnNotificationServoingModeTopic::Response &res) { // If the notification is already activated, don't activate multiple times @@ -3324,7 +3324,7 @@ bool BaseServices::OnNotificationServoingModeTopic(kortex_driver::OnNotification try { - std::function< void (Kinova::Api::Base::ServoingModeNotification) > callback = std::bind(&BaseServices::cb_ServoingModeTopic, this, std::placeholders::_1); + std::function< void (Kinova::Api::Base::ServoingModeNotification) > callback = std::bind(&BaseRobotServices::cb_ServoingModeTopic, this, std::placeholders::_1); output = m_base->OnNotificationServoingModeTopic(callback, input, m_current_device_id); m_is_activated_ServoingModeTopic = true; } @@ -3349,14 +3349,14 @@ bool BaseServices::OnNotificationServoingModeTopic(kortex_driver::OnNotification ToRosData(output, res.output); return true; } -void BaseServices::cb_ServoingModeTopic(Kinova::Api::Base::ServoingModeNotification notif) +void BaseRobotServices::cb_ServoingModeTopic(Kinova::Api::Base::ServoingModeNotification notif) { kortex_driver::ServoingModeNotification ros_msg; ToRosData(notif, ros_msg); m_pub_ServoingModeTopic.publish(ros_msg); } -bool BaseServices::RestoreFactorySettings(kortex_driver::RestoreFactorySettings::Request &req, kortex_driver::RestoreFactorySettings::Response &res) +bool BaseRobotServices::RestoreFactorySettings(kortex_driver::RestoreFactorySettings::Request &req, kortex_driver::RestoreFactorySettings::Response &res) { kortex_driver::KortexError result_error; @@ -3386,7 +3386,7 @@ bool BaseServices::RestoreFactorySettings(kortex_driver::RestoreFactorySettings: return true; } -bool BaseServices::OnNotificationFactoryTopic(kortex_driver::OnNotificationFactoryTopic::Request &req, kortex_driver::OnNotificationFactoryTopic::Response &res) +bool BaseRobotServices::OnNotificationFactoryTopic(kortex_driver::OnNotificationFactoryTopic::Request &req, kortex_driver::OnNotificationFactoryTopic::Response &res) { // If the notification is already activated, don't activate multiple times @@ -3400,7 +3400,7 @@ bool BaseServices::OnNotificationFactoryTopic(kortex_driver::OnNotificationFacto try { - std::function< void (Kinova::Api::Base::FactoryNotification) > callback = std::bind(&BaseServices::cb_FactoryTopic, this, std::placeholders::_1); + std::function< void (Kinova::Api::Base::FactoryNotification) > callback = std::bind(&BaseRobotServices::cb_FactoryTopic, this, std::placeholders::_1); output = m_base->OnNotificationFactoryTopic(callback, input, m_current_device_id); m_is_activated_FactoryTopic = true; } @@ -3425,14 +3425,14 @@ bool BaseServices::OnNotificationFactoryTopic(kortex_driver::OnNotificationFacto ToRosData(output, res.output); return true; } -void BaseServices::cb_FactoryTopic(Kinova::Api::Base::FactoryNotification notif) +void BaseRobotServices::cb_FactoryTopic(Kinova::Api::Base::FactoryNotification notif) { kortex_driver::FactoryNotification ros_msg; ToRosData(notif, ros_msg); m_pub_FactoryTopic.publish(ros_msg); } -bool BaseServices::GetAllConnectedControllers(kortex_driver::GetAllConnectedControllers::Request &req, kortex_driver::GetAllConnectedControllers::Response &res) +bool BaseRobotServices::GetAllConnectedControllers(kortex_driver::GetAllConnectedControllers::Request &req, kortex_driver::GetAllConnectedControllers::Response &res) { Kinova::Api::Base::ControllerList output; @@ -3465,7 +3465,7 @@ bool BaseServices::GetAllConnectedControllers(kortex_driver::GetAllConnectedCont return true; } -bool BaseServices::GetControllerState(kortex_driver::GetControllerState::Request &req, kortex_driver::GetControllerState::Response &res) +bool BaseRobotServices::GetControllerState(kortex_driver::GetControllerState::Request &req, kortex_driver::GetControllerState::Response &res) { Kinova::Api::Base::ControllerHandle input; @@ -3500,7 +3500,7 @@ bool BaseServices::GetControllerState(kortex_driver::GetControllerState::Request return true; } -bool BaseServices::GetActuatorCount(kortex_driver::GetActuatorCount::Request &req, kortex_driver::GetActuatorCount::Response &res) +bool BaseRobotServices::GetActuatorCount(kortex_driver::GetActuatorCount::Request &req, kortex_driver::GetActuatorCount::Response &res) { Kinova::Api::Base::ActuatorInformation output; @@ -3533,7 +3533,7 @@ bool BaseServices::GetActuatorCount(kortex_driver::GetActuatorCount::Request &r return true; } -bool BaseServices::StartWifiScan(kortex_driver::StartWifiScan::Request &req, kortex_driver::StartWifiScan::Response &res) +bool BaseRobotServices::StartWifiScan(kortex_driver::StartWifiScan::Request &req, kortex_driver::StartWifiScan::Response &res) { kortex_driver::KortexError result_error; @@ -3563,7 +3563,7 @@ bool BaseServices::StartWifiScan(kortex_driver::StartWifiScan::Request &req, ko return true; } -bool BaseServices::GetConfiguredWifi(kortex_driver::GetConfiguredWifi::Request &req, kortex_driver::GetConfiguredWifi::Response &res) +bool BaseRobotServices::GetConfiguredWifi(kortex_driver::GetConfiguredWifi::Request &req, kortex_driver::GetConfiguredWifi::Response &res) { Kinova::Api::Base::Ssid input; @@ -3598,7 +3598,7 @@ bool BaseServices::GetConfiguredWifi(kortex_driver::GetConfiguredWifi::Request return true; } -bool BaseServices::OnNotificationNetworkTopic(kortex_driver::OnNotificationNetworkTopic::Request &req, kortex_driver::OnNotificationNetworkTopic::Response &res) +bool BaseRobotServices::OnNotificationNetworkTopic(kortex_driver::OnNotificationNetworkTopic::Request &req, kortex_driver::OnNotificationNetworkTopic::Response &res) { // If the notification is already activated, don't activate multiple times @@ -3612,7 +3612,7 @@ bool BaseServices::OnNotificationNetworkTopic(kortex_driver::OnNotificationNetwo try { - std::function< void (Kinova::Api::Base::NetworkNotification) > callback = std::bind(&BaseServices::cb_NetworkTopic, this, std::placeholders::_1); + std::function< void (Kinova::Api::Base::NetworkNotification) > callback = std::bind(&BaseRobotServices::cb_NetworkTopic, this, std::placeholders::_1); output = m_base->OnNotificationNetworkTopic(callback, input, m_current_device_id); m_is_activated_NetworkTopic = true; } @@ -3637,14 +3637,14 @@ bool BaseServices::OnNotificationNetworkTopic(kortex_driver::OnNotificationNetwo ToRosData(output, res.output); return true; } -void BaseServices::cb_NetworkTopic(Kinova::Api::Base::NetworkNotification notif) +void BaseRobotServices::cb_NetworkTopic(Kinova::Api::Base::NetworkNotification notif) { kortex_driver::NetworkNotification ros_msg; ToRosData(notif, ros_msg); m_pub_NetworkTopic.publish(ros_msg); } -bool BaseServices::GetArmState(kortex_driver::GetArmState::Request &req, kortex_driver::GetArmState::Response &res) +bool BaseRobotServices::GetArmState(kortex_driver::GetArmState::Request &req, kortex_driver::GetArmState::Response &res) { Kinova::Api::Base::ArmStateInformation output; @@ -3677,7 +3677,7 @@ bool BaseServices::GetArmState(kortex_driver::GetArmState::Request &req, kortex return true; } -bool BaseServices::OnNotificationArmStateTopic(kortex_driver::OnNotificationArmStateTopic::Request &req, kortex_driver::OnNotificationArmStateTopic::Response &res) +bool BaseRobotServices::OnNotificationArmStateTopic(kortex_driver::OnNotificationArmStateTopic::Request &req, kortex_driver::OnNotificationArmStateTopic::Response &res) { // If the notification is already activated, don't activate multiple times @@ -3691,7 +3691,7 @@ bool BaseServices::OnNotificationArmStateTopic(kortex_driver::OnNotificationArmS try { - std::function< void (Kinova::Api::Base::ArmStateNotification) > callback = std::bind(&BaseServices::cb_ArmStateTopic, this, std::placeholders::_1); + std::function< void (Kinova::Api::Base::ArmStateNotification) > callback = std::bind(&BaseRobotServices::cb_ArmStateTopic, this, std::placeholders::_1); output = m_base->OnNotificationArmStateTopic(callback, input, m_current_device_id); m_is_activated_ArmStateTopic = true; } @@ -3716,14 +3716,14 @@ bool BaseServices::OnNotificationArmStateTopic(kortex_driver::OnNotificationArmS ToRosData(output, res.output); return true; } -void BaseServices::cb_ArmStateTopic(Kinova::Api::Base::ArmStateNotification notif) +void BaseRobotServices::cb_ArmStateTopic(Kinova::Api::Base::ArmStateNotification notif) { kortex_driver::ArmStateNotification ros_msg; ToRosData(notif, ros_msg); m_pub_ArmStateTopic.publish(ros_msg); } -bool BaseServices::GetIPv4Information(kortex_driver::GetIPv4Information::Request &req, kortex_driver::GetIPv4Information::Response &res) +bool BaseRobotServices::GetIPv4Information(kortex_driver::GetIPv4Information::Request &req, kortex_driver::GetIPv4Information::Response &res) { Kinova::Api::Base::NetworkHandle input; @@ -3758,7 +3758,7 @@ bool BaseServices::GetIPv4Information(kortex_driver::GetIPv4Information::Request return true; } -bool BaseServices::SetWifiCountryCode(kortex_driver::SetWifiCountryCode::Request &req, kortex_driver::SetWifiCountryCode::Response &res) +bool BaseRobotServices::SetWifiCountryCode(kortex_driver::SetWifiCountryCode::Request &req, kortex_driver::SetWifiCountryCode::Response &res) { Kinova::Api::Common::CountryCode input; @@ -3790,7 +3790,7 @@ bool BaseServices::SetWifiCountryCode(kortex_driver::SetWifiCountryCode::Request return true; } -bool BaseServices::GetWifiCountryCode(kortex_driver::GetWifiCountryCode::Request &req, kortex_driver::GetWifiCountryCode::Response &res) +bool BaseRobotServices::GetWifiCountryCode(kortex_driver::GetWifiCountryCode::Request &req, kortex_driver::GetWifiCountryCode::Response &res) { Kinova::Api::Common::CountryCode output; @@ -3823,7 +3823,7 @@ bool BaseServices::GetWifiCountryCode(kortex_driver::GetWifiCountryCode::Request return true; } -bool BaseServices::Base_SetCapSenseConfig(kortex_driver::Base_SetCapSenseConfig::Request &req, kortex_driver::Base_SetCapSenseConfig::Response &res) +bool BaseRobotServices::Base_SetCapSenseConfig(kortex_driver::Base_SetCapSenseConfig::Request &req, kortex_driver::Base_SetCapSenseConfig::Response &res) { Kinova::Api::Base::CapSenseConfig input; @@ -3855,7 +3855,7 @@ bool BaseServices::Base_SetCapSenseConfig(kortex_driver::Base_SetCapSenseConfig: return true; } -bool BaseServices::Base_GetCapSenseConfig(kortex_driver::Base_GetCapSenseConfig::Request &req, kortex_driver::Base_GetCapSenseConfig::Response &res) +bool BaseRobotServices::Base_GetCapSenseConfig(kortex_driver::Base_GetCapSenseConfig::Request &req, kortex_driver::Base_GetCapSenseConfig::Response &res) { Kinova::Api::Base::CapSenseConfig output; @@ -3888,9 +3888,9 @@ bool BaseServices::Base_GetCapSenseConfig(kortex_driver::Base_GetCapSenseConfig: return true; } -bool BaseServices::GetAllJointsSpeedHardLimitation(kortex_driver::GetAllJointsSpeedHardLimitation::Request &req, kortex_driver::GetAllJointsSpeedHardLimitation::Response &res) +bool BaseRobotServices::GetAllJointsSpeedHardLimitation(kortex_driver::GetAllJointsSpeedHardLimitation::Request &req, kortex_driver::GetAllJointsSpeedHardLimitation::Response &res) { - ROS_WARN("The base/get_all_joints_speed_hard_limitation service will be deprecated in a future release."); + ROS_WARN("The base/get_all_joints_speed_hard_limitation service is now deprecated and will be removed in a future release."); Kinova::Api::Base::JointsLimitationsList output; @@ -3922,9 +3922,9 @@ bool BaseServices::GetAllJointsSpeedHardLimitation(kortex_driver::GetAllJointsSp return true; } -bool BaseServices::GetAllJointsTorqueHardLimitation(kortex_driver::GetAllJointsTorqueHardLimitation::Request &req, kortex_driver::GetAllJointsTorqueHardLimitation::Response &res) +bool BaseRobotServices::GetAllJointsTorqueHardLimitation(kortex_driver::GetAllJointsTorqueHardLimitation::Request &req, kortex_driver::GetAllJointsTorqueHardLimitation::Response &res) { - ROS_WARN("The base/get_all_joints_torque_hard_limitation service will be deprecated in a future release."); + ROS_WARN("The base/get_all_joints_torque_hard_limitation service is now deprecated and will be removed in a future release."); Kinova::Api::Base::JointsLimitationsList output; @@ -3956,9 +3956,9 @@ bool BaseServices::GetAllJointsTorqueHardLimitation(kortex_driver::GetAllJointsT return true; } -bool BaseServices::GetTwistHardLimitation(kortex_driver::GetTwistHardLimitation::Request &req, kortex_driver::GetTwistHardLimitation::Response &res) +bool BaseRobotServices::GetTwistHardLimitation(kortex_driver::GetTwistHardLimitation::Request &req, kortex_driver::GetTwistHardLimitation::Response &res) { - ROS_WARN("The base/get_twist_hard_limitation service will be deprecated in a future release."); + ROS_WARN("The base/get_twist_hard_limitation service is now deprecated and will be removed in a future release."); Kinova::Api::Base::TwistLimitation output; @@ -3990,9 +3990,9 @@ bool BaseServices::GetTwistHardLimitation(kortex_driver::GetTwistHardLimitation: return true; } -bool BaseServices::GetWrenchHardLimitation(kortex_driver::GetWrenchHardLimitation::Request &req, kortex_driver::GetWrenchHardLimitation::Response &res) +bool BaseRobotServices::GetWrenchHardLimitation(kortex_driver::GetWrenchHardLimitation::Request &req, kortex_driver::GetWrenchHardLimitation::Response &res) { - ROS_WARN("The base/get_wrench_hard_limitation service will be deprecated in a future release."); + ROS_WARN("The base/get_wrench_hard_limitation service is now deprecated and will be removed in a future release."); Kinova::Api::Base::WrenchLimitation output; @@ -4024,7 +4024,7 @@ bool BaseServices::GetWrenchHardLimitation(kortex_driver::GetWrenchHardLimitatio return true; } -bool BaseServices::SendJointSpeedsJoystickCommand(kortex_driver::SendJointSpeedsJoystickCommand::Request &req, kortex_driver::SendJointSpeedsJoystickCommand::Response &res) +bool BaseRobotServices::SendJointSpeedsJoystickCommand(kortex_driver::SendJointSpeedsJoystickCommand::Request &req, kortex_driver::SendJointSpeedsJoystickCommand::Response &res) { Kinova::Api::Base::JointSpeeds input; @@ -4056,7 +4056,7 @@ bool BaseServices::SendJointSpeedsJoystickCommand(kortex_driver::SendJointSpeeds return true; } -bool BaseServices::SendSelectedJointSpeedJoystickCommand(kortex_driver::SendSelectedJointSpeedJoystickCommand::Request &req, kortex_driver::SendSelectedJointSpeedJoystickCommand::Response &res) +bool BaseRobotServices::SendSelectedJointSpeedJoystickCommand(kortex_driver::SendSelectedJointSpeedJoystickCommand::Request &req, kortex_driver::SendSelectedJointSpeedJoystickCommand::Response &res) { Kinova::Api::Base::JointSpeed input; @@ -4088,7 +4088,7 @@ bool BaseServices::SendSelectedJointSpeedJoystickCommand(kortex_driver::SendSele return true; } -bool BaseServices::EnableBridge(kortex_driver::EnableBridge::Request &req, kortex_driver::EnableBridge::Response &res) +bool BaseRobotServices::EnableBridge(kortex_driver::EnableBridge::Request &req, kortex_driver::EnableBridge::Response &res) { Kinova::Api::Base::BridgeConfig input; @@ -4123,7 +4123,7 @@ bool BaseServices::EnableBridge(kortex_driver::EnableBridge::Request &req, kort return true; } -bool BaseServices::DisableBridge(kortex_driver::DisableBridge::Request &req, kortex_driver::DisableBridge::Response &res) +bool BaseRobotServices::DisableBridge(kortex_driver::DisableBridge::Request &req, kortex_driver::DisableBridge::Response &res) { Kinova::Api::Base::BridgeIdentifier input; @@ -4158,7 +4158,7 @@ bool BaseServices::DisableBridge(kortex_driver::DisableBridge::Request &req, ko return true; } -bool BaseServices::GetBridgeList(kortex_driver::GetBridgeList::Request &req, kortex_driver::GetBridgeList::Response &res) +bool BaseRobotServices::GetBridgeList(kortex_driver::GetBridgeList::Request &req, kortex_driver::GetBridgeList::Response &res) { Kinova::Api::Base::BridgeList output; @@ -4191,7 +4191,7 @@ bool BaseServices::GetBridgeList(kortex_driver::GetBridgeList::Request &req, ko return true; } -bool BaseServices::GetBridgeConfig(kortex_driver::GetBridgeConfig::Request &req, kortex_driver::GetBridgeConfig::Response &res) +bool BaseRobotServices::GetBridgeConfig(kortex_driver::GetBridgeConfig::Request &req, kortex_driver::GetBridgeConfig::Response &res) { Kinova::Api::Base::BridgeIdentifier input; @@ -4226,7 +4226,7 @@ bool BaseServices::GetBridgeConfig(kortex_driver::GetBridgeConfig::Request &req return true; } -bool BaseServices::PlayPreComputedJointTrajectory(kortex_driver::PlayPreComputedJointTrajectory::Request &req, kortex_driver::PlayPreComputedJointTrajectory::Response &res) +bool BaseRobotServices::PlayPreComputedJointTrajectory(kortex_driver::PlayPreComputedJointTrajectory::Request &req, kortex_driver::PlayPreComputedJointTrajectory::Response &res) { Kinova::Api::Base::PreComputedJointTrajectory input; @@ -4258,7 +4258,7 @@ bool BaseServices::PlayPreComputedJointTrajectory(kortex_driver::PlayPreComputed return true; } -bool BaseServices::GetProductConfiguration(kortex_driver::GetProductConfiguration::Request &req, kortex_driver::GetProductConfiguration::Response &res) +bool BaseRobotServices::GetProductConfiguration(kortex_driver::GetProductConfiguration::Request &req, kortex_driver::GetProductConfiguration::Response &res) { Kinova::Api::ProductConfiguration::CompleteProductConfiguration output; @@ -4291,7 +4291,7 @@ bool BaseServices::GetProductConfiguration(kortex_driver::GetProductConfiguratio return true; } -bool BaseServices::UpdateEndEffectorTypeConfiguration(kortex_driver::UpdateEndEffectorTypeConfiguration::Request &req, kortex_driver::UpdateEndEffectorTypeConfiguration::Response &res) +bool BaseRobotServices::UpdateEndEffectorTypeConfiguration(kortex_driver::UpdateEndEffectorTypeConfiguration::Request &req, kortex_driver::UpdateEndEffectorTypeConfiguration::Response &res) { Kinova::Api::ProductConfiguration::ProductConfigurationEndEffectorType input; @@ -4323,7 +4323,7 @@ bool BaseServices::UpdateEndEffectorTypeConfiguration(kortex_driver::UpdateEndEf return true; } -bool BaseServices::RestoreFactoryProductConfiguration(kortex_driver::RestoreFactoryProductConfiguration::Request &req, kortex_driver::RestoreFactoryProductConfiguration::Response &res) +bool BaseRobotServices::RestoreFactoryProductConfiguration(kortex_driver::RestoreFactoryProductConfiguration::Request &req, kortex_driver::RestoreFactoryProductConfiguration::Response &res) { kortex_driver::KortexError result_error; @@ -4353,7 +4353,7 @@ bool BaseServices::RestoreFactoryProductConfiguration(kortex_driver::RestoreFact return true; } -bool BaseServices::GetTrajectoryErrorReport(kortex_driver::GetTrajectoryErrorReport::Request &req, kortex_driver::GetTrajectoryErrorReport::Response &res) +bool BaseRobotServices::GetTrajectoryErrorReport(kortex_driver::GetTrajectoryErrorReport::Request &req, kortex_driver::GetTrajectoryErrorReport::Response &res) { Kinova::Api::Base::TrajectoryErrorReport output; @@ -4386,9 +4386,9 @@ bool BaseServices::GetTrajectoryErrorReport(kortex_driver::GetTrajectoryErrorRep return true; } -bool BaseServices::GetAllJointsSpeedSoftLimitation(kortex_driver::GetAllJointsSpeedSoftLimitation::Request &req, kortex_driver::GetAllJointsSpeedSoftLimitation::Response &res) +bool BaseRobotServices::GetAllJointsSpeedSoftLimitation(kortex_driver::GetAllJointsSpeedSoftLimitation::Request &req, kortex_driver::GetAllJointsSpeedSoftLimitation::Response &res) { - ROS_WARN("The base/get_all_joints_speed_soft_limitation service will be deprecated in a future release."); + ROS_WARN("The base/get_all_joints_speed_soft_limitation service is now deprecated and will be removed in a future release."); Kinova::Api::Base::JointsLimitationsList output; @@ -4420,9 +4420,9 @@ bool BaseServices::GetAllJointsSpeedSoftLimitation(kortex_driver::GetAllJointsSp return true; } -bool BaseServices::GetAllJointsTorqueSoftLimitation(kortex_driver::GetAllJointsTorqueSoftLimitation::Request &req, kortex_driver::GetAllJointsTorqueSoftLimitation::Response &res) +bool BaseRobotServices::GetAllJointsTorqueSoftLimitation(kortex_driver::GetAllJointsTorqueSoftLimitation::Request &req, kortex_driver::GetAllJointsTorqueSoftLimitation::Response &res) { - ROS_WARN("The base/get_all_joints_torque_soft_limitation service will be deprecated in a future release."); + ROS_WARN("The base/get_all_joints_torque_soft_limitation service is now deprecated and will be removed in a future release."); Kinova::Api::Base::JointsLimitationsList output; @@ -4454,9 +4454,9 @@ bool BaseServices::GetAllJointsTorqueSoftLimitation(kortex_driver::GetAllJointsT return true; } -bool BaseServices::GetTwistSoftLimitation(kortex_driver::GetTwistSoftLimitation::Request &req, kortex_driver::GetTwistSoftLimitation::Response &res) +bool BaseRobotServices::GetTwistSoftLimitation(kortex_driver::GetTwistSoftLimitation::Request &req, kortex_driver::GetTwistSoftLimitation::Response &res) { - ROS_WARN("The base/get_twist_soft_limitation service will be deprecated in a future release."); + ROS_WARN("The base/get_twist_soft_limitation service is now deprecated and will be removed in a future release."); Kinova::Api::Base::TwistLimitation output; @@ -4488,9 +4488,9 @@ bool BaseServices::GetTwistSoftLimitation(kortex_driver::GetTwistSoftLimitation: return true; } -bool BaseServices::GetWrenchSoftLimitation(kortex_driver::GetWrenchSoftLimitation::Request &req, kortex_driver::GetWrenchSoftLimitation::Response &res) +bool BaseRobotServices::GetWrenchSoftLimitation(kortex_driver::GetWrenchSoftLimitation::Request &req, kortex_driver::GetWrenchSoftLimitation::Response &res) { - ROS_WARN("The base/get_wrench_soft_limitation service will be deprecated in a future release."); + ROS_WARN("The base/get_wrench_soft_limitation service is now deprecated and will be removed in a future release."); Kinova::Api::Base::WrenchLimitation output; @@ -4522,7 +4522,7 @@ bool BaseServices::GetWrenchSoftLimitation(kortex_driver::GetWrenchSoftLimitatio return true; } -bool BaseServices::SetControllerConfigurationMode(kortex_driver::SetControllerConfigurationMode::Request &req, kortex_driver::SetControllerConfigurationMode::Response &res) +bool BaseRobotServices::SetControllerConfigurationMode(kortex_driver::SetControllerConfigurationMode::Request &req, kortex_driver::SetControllerConfigurationMode::Response &res) { Kinova::Api::Base::ControllerConfigurationMode input; @@ -4554,7 +4554,7 @@ bool BaseServices::SetControllerConfigurationMode(kortex_driver::SetControllerCo return true; } -bool BaseServices::GetControllerConfigurationMode(kortex_driver::GetControllerConfigurationMode::Request &req, kortex_driver::GetControllerConfigurationMode::Response &res) +bool BaseRobotServices::GetControllerConfigurationMode(kortex_driver::GetControllerConfigurationMode::Request &req, kortex_driver::GetControllerConfigurationMode::Response &res) { Kinova::Api::Base::ControllerConfigurationMode output; @@ -4587,7 +4587,7 @@ bool BaseServices::GetControllerConfigurationMode(kortex_driver::GetControllerCo return true; } -bool BaseServices::StartTeaching(kortex_driver::StartTeaching::Request &req, kortex_driver::StartTeaching::Response &res) +bool BaseRobotServices::StartTeaching(kortex_driver::StartTeaching::Request &req, kortex_driver::StartTeaching::Response &res) { Kinova::Api::Base::SequenceTaskHandle input; @@ -4619,7 +4619,7 @@ bool BaseServices::StartTeaching(kortex_driver::StartTeaching::Request &req, ko return true; } -bool BaseServices::StopTeaching(kortex_driver::StopTeaching::Request &req, kortex_driver::StopTeaching::Response &res) +bool BaseRobotServices::StopTeaching(kortex_driver::StopTeaching::Request &req, kortex_driver::StopTeaching::Response &res) { kortex_driver::KortexError result_error; @@ -4649,7 +4649,7 @@ bool BaseServices::StopTeaching(kortex_driver::StopTeaching::Request &req, kort return true; } -bool BaseServices::AddSequenceTasks(kortex_driver::AddSequenceTasks::Request &req, kortex_driver::AddSequenceTasks::Response &res) +bool BaseRobotServices::AddSequenceTasks(kortex_driver::AddSequenceTasks::Request &req, kortex_driver::AddSequenceTasks::Response &res) { Kinova::Api::Base::SequenceTasksConfiguration input; @@ -4684,7 +4684,7 @@ bool BaseServices::AddSequenceTasks(kortex_driver::AddSequenceTasks::Request &r return true; } -bool BaseServices::UpdateSequenceTask(kortex_driver::UpdateSequenceTask::Request &req, kortex_driver::UpdateSequenceTask::Response &res) +bool BaseRobotServices::UpdateSequenceTask(kortex_driver::UpdateSequenceTask::Request &req, kortex_driver::UpdateSequenceTask::Response &res) { Kinova::Api::Base::SequenceTaskConfiguration input; @@ -4716,7 +4716,7 @@ bool BaseServices::UpdateSequenceTask(kortex_driver::UpdateSequenceTask::Request return true; } -bool BaseServices::SwapSequenceTasks(kortex_driver::SwapSequenceTasks::Request &req, kortex_driver::SwapSequenceTasks::Response &res) +bool BaseRobotServices::SwapSequenceTasks(kortex_driver::SwapSequenceTasks::Request &req, kortex_driver::SwapSequenceTasks::Response &res) { Kinova::Api::Base::SequenceTasksPair input; @@ -4748,7 +4748,7 @@ bool BaseServices::SwapSequenceTasks(kortex_driver::SwapSequenceTasks::Request return true; } -bool BaseServices::ReadSequenceTask(kortex_driver::ReadSequenceTask::Request &req, kortex_driver::ReadSequenceTask::Response &res) +bool BaseRobotServices::ReadSequenceTask(kortex_driver::ReadSequenceTask::Request &req, kortex_driver::ReadSequenceTask::Response &res) { Kinova::Api::Base::SequenceTaskHandle input; @@ -4783,7 +4783,7 @@ bool BaseServices::ReadSequenceTask(kortex_driver::ReadSequenceTask::Request &r return true; } -bool BaseServices::ReadAllSequenceTasks(kortex_driver::ReadAllSequenceTasks::Request &req, kortex_driver::ReadAllSequenceTasks::Response &res) +bool BaseRobotServices::ReadAllSequenceTasks(kortex_driver::ReadAllSequenceTasks::Request &req, kortex_driver::ReadAllSequenceTasks::Response &res) { Kinova::Api::Base::SequenceHandle input; @@ -4818,7 +4818,7 @@ bool BaseServices::ReadAllSequenceTasks(kortex_driver::ReadAllSequenceTasks::Req return true; } -bool BaseServices::DeleteSequenceTask(kortex_driver::DeleteSequenceTask::Request &req, kortex_driver::DeleteSequenceTask::Response &res) +bool BaseRobotServices::DeleteSequenceTask(kortex_driver::DeleteSequenceTask::Request &req, kortex_driver::DeleteSequenceTask::Response &res) { Kinova::Api::Base::SequenceTaskHandle input; @@ -4850,7 +4850,7 @@ bool BaseServices::DeleteSequenceTask(kortex_driver::DeleteSequenceTask::Request return true; } -bool BaseServices::DeleteAllSequenceTasks(kortex_driver::DeleteAllSequenceTasks::Request &req, kortex_driver::DeleteAllSequenceTasks::Response &res) +bool BaseRobotServices::DeleteAllSequenceTasks(kortex_driver::DeleteAllSequenceTasks::Request &req, kortex_driver::DeleteAllSequenceTasks::Response &res) { Kinova::Api::Base::SequenceHandle input; @@ -4882,7 +4882,7 @@ bool BaseServices::DeleteAllSequenceTasks(kortex_driver::DeleteAllSequenceTasks: return true; } -bool BaseServices::TakeSnapshot(kortex_driver::TakeSnapshot::Request &req, kortex_driver::TakeSnapshot::Response &res) +bool BaseRobotServices::TakeSnapshot(kortex_driver::TakeSnapshot::Request &req, kortex_driver::TakeSnapshot::Response &res) { Kinova::Api::Base::Snapshot input; @@ -4914,7 +4914,7 @@ bool BaseServices::TakeSnapshot(kortex_driver::TakeSnapshot::Request &req, kort return true; } -bool BaseServices::GetFirmwareBundleVersions(kortex_driver::GetFirmwareBundleVersions::Request &req, kortex_driver::GetFirmwareBundleVersions::Response &res) +bool BaseRobotServices::GetFirmwareBundleVersions(kortex_driver::GetFirmwareBundleVersions::Request &req, kortex_driver::GetFirmwareBundleVersions::Response &res) { Kinova::Api::Base::FirmwareBundleVersions output; @@ -4947,7 +4947,7 @@ bool BaseServices::GetFirmwareBundleVersions(kortex_driver::GetFirmwareBundleVer return true; } -bool BaseServices::MoveSequenceTask(kortex_driver::MoveSequenceTask::Request &req, kortex_driver::MoveSequenceTask::Response &res) +bool BaseRobotServices::MoveSequenceTask(kortex_driver::MoveSequenceTask::Request &req, kortex_driver::MoveSequenceTask::Response &res) { Kinova::Api::Base::SequenceTasksPair input; @@ -4979,7 +4979,7 @@ bool BaseServices::MoveSequenceTask(kortex_driver::MoveSequenceTask::Request &r return true; } -bool BaseServices::DuplicateMapping(kortex_driver::DuplicateMapping::Request &req, kortex_driver::DuplicateMapping::Response &res) +bool BaseRobotServices::DuplicateMapping(kortex_driver::DuplicateMapping::Request &req, kortex_driver::DuplicateMapping::Response &res) { Kinova::Api::Base::MappingHandle input; @@ -5014,7 +5014,7 @@ bool BaseServices::DuplicateMapping(kortex_driver::DuplicateMapping::Request &r return true; } -bool BaseServices::DuplicateMap(kortex_driver::DuplicateMap::Request &req, kortex_driver::DuplicateMap::Response &res) +bool BaseRobotServices::DuplicateMap(kortex_driver::DuplicateMap::Request &req, kortex_driver::DuplicateMap::Response &res) { Kinova::Api::Base::MapHandle input; @@ -5049,7 +5049,7 @@ bool BaseServices::DuplicateMap(kortex_driver::DuplicateMap::Request &req, kort return true; } -bool BaseServices::SetControllerConfiguration(kortex_driver::SetControllerConfiguration::Request &req, kortex_driver::SetControllerConfiguration::Response &res) +bool BaseRobotServices::SetControllerConfiguration(kortex_driver::SetControllerConfiguration::Request &req, kortex_driver::SetControllerConfiguration::Response &res) { Kinova::Api::Base::ControllerConfiguration input; @@ -5081,7 +5081,7 @@ bool BaseServices::SetControllerConfiguration(kortex_driver::SetControllerConfig return true; } -bool BaseServices::GetControllerConfiguration(kortex_driver::GetControllerConfiguration::Request &req, kortex_driver::GetControllerConfiguration::Response &res) +bool BaseRobotServices::GetControllerConfiguration(kortex_driver::GetControllerConfiguration::Request &req, kortex_driver::GetControllerConfiguration::Response &res) { Kinova::Api::Base::ControllerHandle input; @@ -5116,7 +5116,7 @@ bool BaseServices::GetControllerConfiguration(kortex_driver::GetControllerConfig return true; } -bool BaseServices::GetAllControllerConfigurations(kortex_driver::GetAllControllerConfigurations::Request &req, kortex_driver::GetAllControllerConfigurations::Response &res) +bool BaseRobotServices::GetAllControllerConfigurations(kortex_driver::GetAllControllerConfigurations::Request &req, kortex_driver::GetAllControllerConfigurations::Response &res) { Kinova::Api::Base::ControllerConfigurationList output; diff --git a/kortex_driver/src/generated/basecyclic_proto_converter.cpp b/kortex_driver/src/generated/robot/basecyclic_proto_converter.cpp similarity index 98% rename from kortex_driver/src/generated/basecyclic_proto_converter.cpp rename to kortex_driver/src/generated/robot/basecyclic_proto_converter.cpp index 321ab90f..30f9bc4e 100644 --- a/kortex_driver/src/generated/basecyclic_proto_converter.cpp +++ b/kortex_driver/src/generated/robot/basecyclic_proto_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" int ToProtoData(kortex_driver::ActuatorCommand input, Kinova::Api::BaseCyclic::ActuatorCommand *output) { diff --git a/kortex_driver/src/generated/basecyclic_ros_converter.cpp b/kortex_driver/src/generated/robot/basecyclic_ros_converter.cpp similarity index 98% rename from kortex_driver/src/generated/basecyclic_ros_converter.cpp rename to kortex_driver/src/generated/robot/basecyclic_ros_converter.cpp index 6399d0b7..e064758e 100644 --- a/kortex_driver/src/generated/basecyclic_ros_converter.cpp +++ b/kortex_driver/src/generated/robot/basecyclic_ros_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" int ToRosData(Kinova::Api::BaseCyclic::ActuatorCommand input, kortex_driver::ActuatorCommand &output) { diff --git a/kortex_driver/src/generated/common_proto_converter.cpp b/kortex_driver/src/generated/robot/common_proto_converter.cpp similarity index 98% rename from kortex_driver/src/generated/common_proto_converter.cpp rename to kortex_driver/src/generated/robot/common_proto_converter.cpp index e0211113..d1357870 100644 --- a/kortex_driver/src/generated/common_proto_converter.cpp +++ b/kortex_driver/src/generated/robot/common_proto_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/common_proto_converter.h" +#include "kortex_driver/generated/robot/common_proto_converter.h" int ToProtoData(kortex_driver::DeviceHandle input, Kinova::Api::Common::DeviceHandle *output) { diff --git a/kortex_driver/src/generated/common_ros_converter.cpp b/kortex_driver/src/generated/robot/common_ros_converter.cpp similarity index 97% rename from kortex_driver/src/generated/common_ros_converter.cpp rename to kortex_driver/src/generated/robot/common_ros_converter.cpp index d9cb8e82..df98ecba 100644 --- a/kortex_driver/src/generated/common_ros_converter.cpp +++ b/kortex_driver/src/generated/robot/common_ros_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/common_ros_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" int ToRosData(Kinova::Api::Common::DeviceHandle input, kortex_driver::DeviceHandle &output) { diff --git a/kortex_driver/src/generated/controlconfig_proto_converter.cpp b/kortex_driver/src/generated/robot/controlconfig_proto_converter.cpp similarity index 98% rename from kortex_driver/src/generated/controlconfig_proto_converter.cpp rename to kortex_driver/src/generated/robot/controlconfig_proto_converter.cpp index c6541423..844f103d 100644 --- a/kortex_driver/src/generated/controlconfig_proto_converter.cpp +++ b/kortex_driver/src/generated/robot/controlconfig_proto_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" int ToProtoData(kortex_driver::GravityVector input, Kinova::Api::ControlConfig::GravityVector *output) { diff --git a/kortex_driver/src/generated/controlconfig_ros_converter.cpp b/kortex_driver/src/generated/robot/controlconfig_ros_converter.cpp similarity index 98% rename from kortex_driver/src/generated/controlconfig_ros_converter.cpp rename to kortex_driver/src/generated/robot/controlconfig_ros_converter.cpp index 531310d7..5032d465 100644 --- a/kortex_driver/src/generated/controlconfig_ros_converter.cpp +++ b/kortex_driver/src/generated/robot/controlconfig_ros_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" int ToRosData(Kinova::Api::ControlConfig::GravityVector input, kortex_driver::GravityVector &output) { diff --git a/kortex_driver/src/generated/controlconfig_services.cpp b/kortex_driver/src/generated/robot/controlconfig_services.cpp similarity index 68% rename from kortex_driver/src/generated/controlconfig_services.cpp rename to kortex_driver/src/generated/robot/controlconfig_services.cpp index eb06f4f2..0e75bf8f 100644 --- a/kortex_driver/src/generated/controlconfig_services.cpp +++ b/kortex_driver/src/generated/robot/controlconfig_services.cpp @@ -14,87 +14,87 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/common_proto_converter.h" -#include "kortex_driver/generated/common_ros_converter.h" -#include "kortex_driver/generated/actuatorconfig_proto_converter.h" -#include "kortex_driver/generated/actuatorconfig_ros_converter.h" -#include "kortex_driver/generated/actuatorcyclic_proto_converter.h" -#include "kortex_driver/generated/actuatorcyclic_ros_converter.h" -#include "kortex_driver/generated/productconfiguration_proto_converter.h" -#include "kortex_driver/generated/productconfiguration_ros_converter.h" -#include "kortex_driver/generated/base_proto_converter.h" -#include "kortex_driver/generated/base_ros_converter.h" -#include "kortex_driver/generated/grippercyclic_proto_converter.h" -#include "kortex_driver/generated/grippercyclic_ros_converter.h" -#include "kortex_driver/generated/interconnectcyclic_proto_converter.h" -#include "kortex_driver/generated/interconnectcyclic_ros_converter.h" -#include "kortex_driver/generated/basecyclic_proto_converter.h" -#include "kortex_driver/generated/basecyclic_ros_converter.h" -#include "kortex_driver/generated/controlconfig_proto_converter.h" -#include "kortex_driver/generated/controlconfig_ros_converter.h" -#include "kortex_driver/generated/deviceconfig_proto_converter.h" -#include "kortex_driver/generated/deviceconfig_ros_converter.h" -#include "kortex_driver/generated/devicemanager_proto_converter.h" -#include "kortex_driver/generated/devicemanager_ros_converter.h" -#include "kortex_driver/generated/interconnectconfig_proto_converter.h" -#include "kortex_driver/generated/interconnectconfig_ros_converter.h" -#include "kortex_driver/generated/visionconfig_proto_converter.h" -#include "kortex_driver/generated/visionconfig_ros_converter.h" -#include "kortex_driver/generated/controlconfig_services.h" - -ControlConfigServices::ControlConfigServices(ros::NodeHandle& n, Kinova::Api::ControlConfig::ControlConfigClient* controlconfig, uint32_t device_id, uint32_t timeout_ms): - m_n(n), +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_services.h" + +ControlConfigRobotServices::ControlConfigRobotServices(ros::NodeHandle& node_handle, Kinova::Api::ControlConfig::ControlConfigClient* controlconfig, uint32_t device_id, uint32_t timeout_ms): + IControlConfigServices(node_handle), m_controlconfig(controlconfig), m_current_device_id(device_id) { m_api_options.timeout_ms = timeout_ms; - m_pub_Error = m_n.advertise("kortex_error", 1000); - m_pub_ControlConfigurationTopic = m_n.advertise("control_configuration_topic", 1000); + m_pub_Error = m_node_handle.advertise("kortex_error", 1000); + m_pub_ControlConfigurationTopic = m_node_handle.advertise("control_configuration_topic", 1000); m_is_activated_ControlConfigurationTopic = false; - m_serviceSetDeviceID = n.advertiseService("control_config/set_device_id", &ControlConfigServices::SetDeviceID, this); - m_serviceSetApiOptions = n.advertiseService("control_config/set_api_options", &ControlConfigServices::SetApiOptions, this); - - m_serviceSetGravityVector = m_n.advertiseService("control_config/set_gravity_vector", &ControlConfigServices::SetGravityVector, this); - m_serviceGetGravityVector = m_n.advertiseService("control_config/get_gravity_vector", &ControlConfigServices::GetGravityVector, this); - m_serviceSetPayloadInformation = m_n.advertiseService("control_config/set_payload_information", &ControlConfigServices::SetPayloadInformation, this); - m_serviceGetPayloadInformation = m_n.advertiseService("control_config/get_payload_information", &ControlConfigServices::GetPayloadInformation, this); - m_serviceSetToolConfiguration = m_n.advertiseService("control_config/set_tool_configuration", &ControlConfigServices::SetToolConfiguration, this); - m_serviceGetToolConfiguration = m_n.advertiseService("control_config/get_tool_configuration", &ControlConfigServices::GetToolConfiguration, this); - m_serviceOnNotificationControlConfigurationTopic = m_n.advertiseService("control_config/activate_publishing_of_control_configuration_topic", &ControlConfigServices::OnNotificationControlConfigurationTopic, this); - m_serviceControlConfig_Unsubscribe = m_n.advertiseService("control_config/unsubscribe", &ControlConfigServices::ControlConfig_Unsubscribe, this); - m_serviceSetCartesianReferenceFrame = m_n.advertiseService("control_config/set_cartesian_reference_frame", &ControlConfigServices::SetCartesianReferenceFrame, this); - m_serviceGetCartesianReferenceFrame = m_n.advertiseService("control_config/get_cartesian_reference_frame", &ControlConfigServices::GetCartesianReferenceFrame, this); - m_serviceControlConfig_GetControlMode = m_n.advertiseService("control_config/get_control_mode", &ControlConfigServices::ControlConfig_GetControlMode, this); - m_serviceSetJointSpeedSoftLimits = m_n.advertiseService("control_config/set_joint_speed_soft_limits", &ControlConfigServices::SetJointSpeedSoftLimits, this); - m_serviceSetTwistLinearSoftLimit = m_n.advertiseService("control_config/set_twist_linear_soft_limit", &ControlConfigServices::SetTwistLinearSoftLimit, this); - m_serviceSetTwistAngularSoftLimit = m_n.advertiseService("control_config/set_twist_angular_soft_limit", &ControlConfigServices::SetTwistAngularSoftLimit, this); - m_serviceSetJointAccelerationSoftLimits = m_n.advertiseService("control_config/set_joint_acceleration_soft_limits", &ControlConfigServices::SetJointAccelerationSoftLimits, this); - m_serviceGetKinematicHardLimits = m_n.advertiseService("control_config/get_kinematic_hard_limits", &ControlConfigServices::GetKinematicHardLimits, this); - m_serviceGetKinematicSoftLimits = m_n.advertiseService("control_config/get_kinematic_soft_limits", &ControlConfigServices::GetKinematicSoftLimits, this); - m_serviceGetAllKinematicSoftLimits = m_n.advertiseService("control_config/get_all_kinematic_soft_limits", &ControlConfigServices::GetAllKinematicSoftLimits, this); - m_serviceSetDesiredLinearTwist = m_n.advertiseService("control_config/set_desired_linear_twist", &ControlConfigServices::SetDesiredLinearTwist, this); - m_serviceSetDesiredAngularTwist = m_n.advertiseService("control_config/set_desired_angular_twist", &ControlConfigServices::SetDesiredAngularTwist, this); - m_serviceSetDesiredJointSpeeds = m_n.advertiseService("control_config/set_desired_joint_speeds", &ControlConfigServices::SetDesiredJointSpeeds, this); - m_serviceGetDesiredSpeeds = m_n.advertiseService("control_config/get_desired_speeds", &ControlConfigServices::GetDesiredSpeeds, this); - m_serviceResetGravityVector = m_n.advertiseService("control_config/reset_gravity_vector", &ControlConfigServices::ResetGravityVector, this); - m_serviceResetPayloadInformation = m_n.advertiseService("control_config/reset_payload_information", &ControlConfigServices::ResetPayloadInformation, this); - m_serviceResetToolConfiguration = m_n.advertiseService("control_config/reset_tool_configuration", &ControlConfigServices::ResetToolConfiguration, this); - m_serviceResetJointSpeedSoftLimits = m_n.advertiseService("control_config/reset_joint_speed_soft_limits", &ControlConfigServices::ResetJointSpeedSoftLimits, this); - m_serviceResetTwistLinearSoftLimit = m_n.advertiseService("control_config/reset_twist_linear_soft_limit", &ControlConfigServices::ResetTwistLinearSoftLimit, this); - m_serviceResetTwistAngularSoftLimit = m_n.advertiseService("control_config/reset_twist_angular_soft_limit", &ControlConfigServices::ResetTwistAngularSoftLimit, this); - m_serviceResetJointAccelerationSoftLimits = m_n.advertiseService("control_config/reset_joint_acceleration_soft_limits", &ControlConfigServices::ResetJointAccelerationSoftLimits, this); + m_serviceSetDeviceID = m_node_handle.advertiseService("control_config/set_device_id", &ControlConfigRobotServices::SetDeviceID, this); + m_serviceSetApiOptions = m_node_handle.advertiseService("control_config/set_api_options", &ControlConfigRobotServices::SetApiOptions, this); + + m_serviceSetGravityVector = m_node_handle.advertiseService("control_config/set_gravity_vector", &ControlConfigRobotServices::SetGravityVector, this); + m_serviceGetGravityVector = m_node_handle.advertiseService("control_config/get_gravity_vector", &ControlConfigRobotServices::GetGravityVector, this); + m_serviceSetPayloadInformation = m_node_handle.advertiseService("control_config/set_payload_information", &ControlConfigRobotServices::SetPayloadInformation, this); + m_serviceGetPayloadInformation = m_node_handle.advertiseService("control_config/get_payload_information", &ControlConfigRobotServices::GetPayloadInformation, this); + m_serviceSetToolConfiguration = m_node_handle.advertiseService("control_config/set_tool_configuration", &ControlConfigRobotServices::SetToolConfiguration, this); + m_serviceGetToolConfiguration = m_node_handle.advertiseService("control_config/get_tool_configuration", &ControlConfigRobotServices::GetToolConfiguration, this); + m_serviceOnNotificationControlConfigurationTopic = m_node_handle.advertiseService("control_config/activate_publishing_of_control_configuration_topic", &ControlConfigRobotServices::OnNotificationControlConfigurationTopic, this); + m_serviceControlConfig_Unsubscribe = m_node_handle.advertiseService("control_config/unsubscribe", &ControlConfigRobotServices::ControlConfig_Unsubscribe, this); + m_serviceSetCartesianReferenceFrame = m_node_handle.advertiseService("control_config/set_cartesian_reference_frame", &ControlConfigRobotServices::SetCartesianReferenceFrame, this); + m_serviceGetCartesianReferenceFrame = m_node_handle.advertiseService("control_config/get_cartesian_reference_frame", &ControlConfigRobotServices::GetCartesianReferenceFrame, this); + m_serviceControlConfig_GetControlMode = m_node_handle.advertiseService("control_config/get_control_mode", &ControlConfigRobotServices::ControlConfig_GetControlMode, this); + m_serviceSetJointSpeedSoftLimits = m_node_handle.advertiseService("control_config/set_joint_speed_soft_limits", &ControlConfigRobotServices::SetJointSpeedSoftLimits, this); + m_serviceSetTwistLinearSoftLimit = m_node_handle.advertiseService("control_config/set_twist_linear_soft_limit", &ControlConfigRobotServices::SetTwistLinearSoftLimit, this); + m_serviceSetTwistAngularSoftLimit = m_node_handle.advertiseService("control_config/set_twist_angular_soft_limit", &ControlConfigRobotServices::SetTwistAngularSoftLimit, this); + m_serviceSetJointAccelerationSoftLimits = m_node_handle.advertiseService("control_config/set_joint_acceleration_soft_limits", &ControlConfigRobotServices::SetJointAccelerationSoftLimits, this); + m_serviceGetKinematicHardLimits = m_node_handle.advertiseService("control_config/get_kinematic_hard_limits", &ControlConfigRobotServices::GetKinematicHardLimits, this); + m_serviceGetKinematicSoftLimits = m_node_handle.advertiseService("control_config/get_kinematic_soft_limits", &ControlConfigRobotServices::GetKinematicSoftLimits, this); + m_serviceGetAllKinematicSoftLimits = m_node_handle.advertiseService("control_config/get_all_kinematic_soft_limits", &ControlConfigRobotServices::GetAllKinematicSoftLimits, this); + m_serviceSetDesiredLinearTwist = m_node_handle.advertiseService("control_config/set_desired_linear_twist", &ControlConfigRobotServices::SetDesiredLinearTwist, this); + m_serviceSetDesiredAngularTwist = m_node_handle.advertiseService("control_config/set_desired_angular_twist", &ControlConfigRobotServices::SetDesiredAngularTwist, this); + m_serviceSetDesiredJointSpeeds = m_node_handle.advertiseService("control_config/set_desired_joint_speeds", &ControlConfigRobotServices::SetDesiredJointSpeeds, this); + m_serviceGetDesiredSpeeds = m_node_handle.advertiseService("control_config/get_desired_speeds", &ControlConfigRobotServices::GetDesiredSpeeds, this); + m_serviceResetGravityVector = m_node_handle.advertiseService("control_config/reset_gravity_vector", &ControlConfigRobotServices::ResetGravityVector, this); + m_serviceResetPayloadInformation = m_node_handle.advertiseService("control_config/reset_payload_information", &ControlConfigRobotServices::ResetPayloadInformation, this); + m_serviceResetToolConfiguration = m_node_handle.advertiseService("control_config/reset_tool_configuration", &ControlConfigRobotServices::ResetToolConfiguration, this); + m_serviceResetJointSpeedSoftLimits = m_node_handle.advertiseService("control_config/reset_joint_speed_soft_limits", &ControlConfigRobotServices::ResetJointSpeedSoftLimits, this); + m_serviceResetTwistLinearSoftLimit = m_node_handle.advertiseService("control_config/reset_twist_linear_soft_limit", &ControlConfigRobotServices::ResetTwistLinearSoftLimit, this); + m_serviceResetTwistAngularSoftLimit = m_node_handle.advertiseService("control_config/reset_twist_angular_soft_limit", &ControlConfigRobotServices::ResetTwistAngularSoftLimit, this); + m_serviceResetJointAccelerationSoftLimits = m_node_handle.advertiseService("control_config/reset_joint_acceleration_soft_limits", &ControlConfigRobotServices::ResetJointAccelerationSoftLimits, this); } -bool ControlConfigServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) +bool ControlConfigRobotServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) { m_current_device_id = req.device_id; return true; } -bool ControlConfigServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) +bool ControlConfigRobotServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) { m_api_options.timeout_ms = req.input.timeout_ms; @@ -102,7 +102,7 @@ bool ControlConfigServices::SetApiOptions(kortex_driver::SetApiOptions::Request } -bool ControlConfigServices::SetGravityVector(kortex_driver::SetGravityVector::Request &req, kortex_driver::SetGravityVector::Response &res) +bool ControlConfigRobotServices::SetGravityVector(kortex_driver::SetGravityVector::Request &req, kortex_driver::SetGravityVector::Response &res) { Kinova::Api::ControlConfig::GravityVector input; @@ -134,7 +134,7 @@ bool ControlConfigServices::SetGravityVector(kortex_driver::SetGravityVector::Re return true; } -bool ControlConfigServices::GetGravityVector(kortex_driver::GetGravityVector::Request &req, kortex_driver::GetGravityVector::Response &res) +bool ControlConfigRobotServices::GetGravityVector(kortex_driver::GetGravityVector::Request &req, kortex_driver::GetGravityVector::Response &res) { Kinova::Api::ControlConfig::GravityVector output; @@ -167,7 +167,7 @@ bool ControlConfigServices::GetGravityVector(kortex_driver::GetGravityVector::Re return true; } -bool ControlConfigServices::SetPayloadInformation(kortex_driver::SetPayloadInformation::Request &req, kortex_driver::SetPayloadInformation::Response &res) +bool ControlConfigRobotServices::SetPayloadInformation(kortex_driver::SetPayloadInformation::Request &req, kortex_driver::SetPayloadInformation::Response &res) { Kinova::Api::ControlConfig::PayloadInformation input; @@ -199,7 +199,7 @@ bool ControlConfigServices::SetPayloadInformation(kortex_driver::SetPayloadInfor return true; } -bool ControlConfigServices::GetPayloadInformation(kortex_driver::GetPayloadInformation::Request &req, kortex_driver::GetPayloadInformation::Response &res) +bool ControlConfigRobotServices::GetPayloadInformation(kortex_driver::GetPayloadInformation::Request &req, kortex_driver::GetPayloadInformation::Response &res) { Kinova::Api::ControlConfig::PayloadInformation output; @@ -232,7 +232,7 @@ bool ControlConfigServices::GetPayloadInformation(kortex_driver::GetPayloadInfor return true; } -bool ControlConfigServices::SetToolConfiguration(kortex_driver::SetToolConfiguration::Request &req, kortex_driver::SetToolConfiguration::Response &res) +bool ControlConfigRobotServices::SetToolConfiguration(kortex_driver::SetToolConfiguration::Request &req, kortex_driver::SetToolConfiguration::Response &res) { Kinova::Api::ControlConfig::ToolConfiguration input; @@ -264,7 +264,7 @@ bool ControlConfigServices::SetToolConfiguration(kortex_driver::SetToolConfigura return true; } -bool ControlConfigServices::GetToolConfiguration(kortex_driver::GetToolConfiguration::Request &req, kortex_driver::GetToolConfiguration::Response &res) +bool ControlConfigRobotServices::GetToolConfiguration(kortex_driver::GetToolConfiguration::Request &req, kortex_driver::GetToolConfiguration::Response &res) { Kinova::Api::ControlConfig::ToolConfiguration output; @@ -297,7 +297,7 @@ bool ControlConfigServices::GetToolConfiguration(kortex_driver::GetToolConfigura return true; } -bool ControlConfigServices::OnNotificationControlConfigurationTopic(kortex_driver::OnNotificationControlConfigurationTopic::Request &req, kortex_driver::OnNotificationControlConfigurationTopic::Response &res) +bool ControlConfigRobotServices::OnNotificationControlConfigurationTopic(kortex_driver::OnNotificationControlConfigurationTopic::Request &req, kortex_driver::OnNotificationControlConfigurationTopic::Response &res) { // If the notification is already activated, don't activate multiple times @@ -311,7 +311,7 @@ bool ControlConfigServices::OnNotificationControlConfigurationTopic(kortex_drive try { - std::function< void (Kinova::Api::ControlConfig::ControlConfigurationNotification) > callback = std::bind(&ControlConfigServices::cb_ControlConfigurationTopic, this, std::placeholders::_1); + std::function< void (Kinova::Api::ControlConfig::ControlConfigurationNotification) > callback = std::bind(&ControlConfigRobotServices::cb_ControlConfigurationTopic, this, std::placeholders::_1); output = m_controlconfig->OnNotificationControlConfigurationTopic(callback, input, m_current_device_id); m_is_activated_ControlConfigurationTopic = true; } @@ -336,14 +336,14 @@ bool ControlConfigServices::OnNotificationControlConfigurationTopic(kortex_drive ToRosData(output, res.output); return true; } -void ControlConfigServices::cb_ControlConfigurationTopic(Kinova::Api::ControlConfig::ControlConfigurationNotification notif) +void ControlConfigRobotServices::cb_ControlConfigurationTopic(Kinova::Api::ControlConfig::ControlConfigurationNotification notif) { kortex_driver::ControlConfigurationNotification ros_msg; ToRosData(notif, ros_msg); m_pub_ControlConfigurationTopic.publish(ros_msg); } -bool ControlConfigServices::ControlConfig_Unsubscribe(kortex_driver::ControlConfig_Unsubscribe::Request &req, kortex_driver::ControlConfig_Unsubscribe::Response &res) +bool ControlConfigRobotServices::ControlConfig_Unsubscribe(kortex_driver::ControlConfig_Unsubscribe::Request &req, kortex_driver::ControlConfig_Unsubscribe::Response &res) { Kinova::Api::Common::NotificationHandle input; @@ -375,7 +375,7 @@ bool ControlConfigServices::ControlConfig_Unsubscribe(kortex_driver::ControlConf return true; } -bool ControlConfigServices::SetCartesianReferenceFrame(kortex_driver::SetCartesianReferenceFrame::Request &req, kortex_driver::SetCartesianReferenceFrame::Response &res) +bool ControlConfigRobotServices::SetCartesianReferenceFrame(kortex_driver::SetCartesianReferenceFrame::Request &req, kortex_driver::SetCartesianReferenceFrame::Response &res) { Kinova::Api::ControlConfig::CartesianReferenceFrameInfo input; @@ -407,7 +407,7 @@ bool ControlConfigServices::SetCartesianReferenceFrame(kortex_driver::SetCartesi return true; } -bool ControlConfigServices::GetCartesianReferenceFrame(kortex_driver::GetCartesianReferenceFrame::Request &req, kortex_driver::GetCartesianReferenceFrame::Response &res) +bool ControlConfigRobotServices::GetCartesianReferenceFrame(kortex_driver::GetCartesianReferenceFrame::Request &req, kortex_driver::GetCartesianReferenceFrame::Response &res) { Kinova::Api::ControlConfig::CartesianReferenceFrameInfo output; @@ -440,7 +440,7 @@ bool ControlConfigServices::GetCartesianReferenceFrame(kortex_driver::GetCartesi return true; } -bool ControlConfigServices::ControlConfig_GetControlMode(kortex_driver::ControlConfig_GetControlMode::Request &req, kortex_driver::ControlConfig_GetControlMode::Response &res) +bool ControlConfigRobotServices::ControlConfig_GetControlMode(kortex_driver::ControlConfig_GetControlMode::Request &req, kortex_driver::ControlConfig_GetControlMode::Response &res) { Kinova::Api::ControlConfig::ControlModeInformation output; @@ -473,7 +473,7 @@ bool ControlConfigServices::ControlConfig_GetControlMode(kortex_driver::ControlC return true; } -bool ControlConfigServices::SetJointSpeedSoftLimits(kortex_driver::SetJointSpeedSoftLimits::Request &req, kortex_driver::SetJointSpeedSoftLimits::Response &res) +bool ControlConfigRobotServices::SetJointSpeedSoftLimits(kortex_driver::SetJointSpeedSoftLimits::Request &req, kortex_driver::SetJointSpeedSoftLimits::Response &res) { Kinova::Api::ControlConfig::JointSpeedSoftLimits input; @@ -505,7 +505,7 @@ bool ControlConfigServices::SetJointSpeedSoftLimits(kortex_driver::SetJointSpeed return true; } -bool ControlConfigServices::SetTwistLinearSoftLimit(kortex_driver::SetTwistLinearSoftLimit::Request &req, kortex_driver::SetTwistLinearSoftLimit::Response &res) +bool ControlConfigRobotServices::SetTwistLinearSoftLimit(kortex_driver::SetTwistLinearSoftLimit::Request &req, kortex_driver::SetTwistLinearSoftLimit::Response &res) { Kinova::Api::ControlConfig::TwistLinearSoftLimit input; @@ -537,7 +537,7 @@ bool ControlConfigServices::SetTwistLinearSoftLimit(kortex_driver::SetTwistLinea return true; } -bool ControlConfigServices::SetTwistAngularSoftLimit(kortex_driver::SetTwistAngularSoftLimit::Request &req, kortex_driver::SetTwistAngularSoftLimit::Response &res) +bool ControlConfigRobotServices::SetTwistAngularSoftLimit(kortex_driver::SetTwistAngularSoftLimit::Request &req, kortex_driver::SetTwistAngularSoftLimit::Response &res) { Kinova::Api::ControlConfig::TwistAngularSoftLimit input; @@ -569,7 +569,7 @@ bool ControlConfigServices::SetTwistAngularSoftLimit(kortex_driver::SetTwistAngu return true; } -bool ControlConfigServices::SetJointAccelerationSoftLimits(kortex_driver::SetJointAccelerationSoftLimits::Request &req, kortex_driver::SetJointAccelerationSoftLimits::Response &res) +bool ControlConfigRobotServices::SetJointAccelerationSoftLimits(kortex_driver::SetJointAccelerationSoftLimits::Request &req, kortex_driver::SetJointAccelerationSoftLimits::Response &res) { Kinova::Api::ControlConfig::JointAccelerationSoftLimits input; @@ -601,7 +601,7 @@ bool ControlConfigServices::SetJointAccelerationSoftLimits(kortex_driver::SetJoi return true; } -bool ControlConfigServices::GetKinematicHardLimits(kortex_driver::GetKinematicHardLimits::Request &req, kortex_driver::GetKinematicHardLimits::Response &res) +bool ControlConfigRobotServices::GetKinematicHardLimits(kortex_driver::GetKinematicHardLimits::Request &req, kortex_driver::GetKinematicHardLimits::Response &res) { Kinova::Api::ControlConfig::KinematicLimits output; @@ -634,7 +634,7 @@ bool ControlConfigServices::GetKinematicHardLimits(kortex_driver::GetKinematicHa return true; } -bool ControlConfigServices::GetKinematicSoftLimits(kortex_driver::GetKinematicSoftLimits::Request &req, kortex_driver::GetKinematicSoftLimits::Response &res) +bool ControlConfigRobotServices::GetKinematicSoftLimits(kortex_driver::GetKinematicSoftLimits::Request &req, kortex_driver::GetKinematicSoftLimits::Response &res) { Kinova::Api::ControlConfig::ControlModeInformation input; @@ -669,7 +669,7 @@ bool ControlConfigServices::GetKinematicSoftLimits(kortex_driver::GetKinematicSo return true; } -bool ControlConfigServices::GetAllKinematicSoftLimits(kortex_driver::GetAllKinematicSoftLimits::Request &req, kortex_driver::GetAllKinematicSoftLimits::Response &res) +bool ControlConfigRobotServices::GetAllKinematicSoftLimits(kortex_driver::GetAllKinematicSoftLimits::Request &req, kortex_driver::GetAllKinematicSoftLimits::Response &res) { Kinova::Api::ControlConfig::KinematicLimitsList output; @@ -702,7 +702,7 @@ bool ControlConfigServices::GetAllKinematicSoftLimits(kortex_driver::GetAllKinem return true; } -bool ControlConfigServices::SetDesiredLinearTwist(kortex_driver::SetDesiredLinearTwist::Request &req, kortex_driver::SetDesiredLinearTwist::Response &res) +bool ControlConfigRobotServices::SetDesiredLinearTwist(kortex_driver::SetDesiredLinearTwist::Request &req, kortex_driver::SetDesiredLinearTwist::Response &res) { Kinova::Api::ControlConfig::LinearTwist input; @@ -734,7 +734,7 @@ bool ControlConfigServices::SetDesiredLinearTwist(kortex_driver::SetDesiredLinea return true; } -bool ControlConfigServices::SetDesiredAngularTwist(kortex_driver::SetDesiredAngularTwist::Request &req, kortex_driver::SetDesiredAngularTwist::Response &res) +bool ControlConfigRobotServices::SetDesiredAngularTwist(kortex_driver::SetDesiredAngularTwist::Request &req, kortex_driver::SetDesiredAngularTwist::Response &res) { Kinova::Api::ControlConfig::AngularTwist input; @@ -766,7 +766,7 @@ bool ControlConfigServices::SetDesiredAngularTwist(kortex_driver::SetDesiredAngu return true; } -bool ControlConfigServices::SetDesiredJointSpeeds(kortex_driver::SetDesiredJointSpeeds::Request &req, kortex_driver::SetDesiredJointSpeeds::Response &res) +bool ControlConfigRobotServices::SetDesiredJointSpeeds(kortex_driver::SetDesiredJointSpeeds::Request &req, kortex_driver::SetDesiredJointSpeeds::Response &res) { Kinova::Api::ControlConfig::JointSpeeds input; @@ -798,7 +798,7 @@ bool ControlConfigServices::SetDesiredJointSpeeds(kortex_driver::SetDesiredJoint return true; } -bool ControlConfigServices::GetDesiredSpeeds(kortex_driver::GetDesiredSpeeds::Request &req, kortex_driver::GetDesiredSpeeds::Response &res) +bool ControlConfigRobotServices::GetDesiredSpeeds(kortex_driver::GetDesiredSpeeds::Request &req, kortex_driver::GetDesiredSpeeds::Response &res) { Kinova::Api::ControlConfig::DesiredSpeeds output; @@ -831,7 +831,7 @@ bool ControlConfigServices::GetDesiredSpeeds(kortex_driver::GetDesiredSpeeds::Re return true; } -bool ControlConfigServices::ResetGravityVector(kortex_driver::ResetGravityVector::Request &req, kortex_driver::ResetGravityVector::Response &res) +bool ControlConfigRobotServices::ResetGravityVector(kortex_driver::ResetGravityVector::Request &req, kortex_driver::ResetGravityVector::Response &res) { Kinova::Api::ControlConfig::GravityVector output; @@ -864,7 +864,7 @@ bool ControlConfigServices::ResetGravityVector(kortex_driver::ResetGravityVector return true; } -bool ControlConfigServices::ResetPayloadInformation(kortex_driver::ResetPayloadInformation::Request &req, kortex_driver::ResetPayloadInformation::Response &res) +bool ControlConfigRobotServices::ResetPayloadInformation(kortex_driver::ResetPayloadInformation::Request &req, kortex_driver::ResetPayloadInformation::Response &res) { Kinova::Api::ControlConfig::PayloadInformation output; @@ -897,7 +897,7 @@ bool ControlConfigServices::ResetPayloadInformation(kortex_driver::ResetPayloadI return true; } -bool ControlConfigServices::ResetToolConfiguration(kortex_driver::ResetToolConfiguration::Request &req, kortex_driver::ResetToolConfiguration::Response &res) +bool ControlConfigRobotServices::ResetToolConfiguration(kortex_driver::ResetToolConfiguration::Request &req, kortex_driver::ResetToolConfiguration::Response &res) { Kinova::Api::ControlConfig::ToolConfiguration output; @@ -930,7 +930,7 @@ bool ControlConfigServices::ResetToolConfiguration(kortex_driver::ResetToolConfi return true; } -bool ControlConfigServices::ResetJointSpeedSoftLimits(kortex_driver::ResetJointSpeedSoftLimits::Request &req, kortex_driver::ResetJointSpeedSoftLimits::Response &res) +bool ControlConfigRobotServices::ResetJointSpeedSoftLimits(kortex_driver::ResetJointSpeedSoftLimits::Request &req, kortex_driver::ResetJointSpeedSoftLimits::Response &res) { Kinova::Api::ControlConfig::ControlModeInformation input; @@ -965,7 +965,7 @@ bool ControlConfigServices::ResetJointSpeedSoftLimits(kortex_driver::ResetJointS return true; } -bool ControlConfigServices::ResetTwistLinearSoftLimit(kortex_driver::ResetTwistLinearSoftLimit::Request &req, kortex_driver::ResetTwistLinearSoftLimit::Response &res) +bool ControlConfigRobotServices::ResetTwistLinearSoftLimit(kortex_driver::ResetTwistLinearSoftLimit::Request &req, kortex_driver::ResetTwistLinearSoftLimit::Response &res) { Kinova::Api::ControlConfig::ControlModeInformation input; @@ -1000,7 +1000,7 @@ bool ControlConfigServices::ResetTwistLinearSoftLimit(kortex_driver::ResetTwistL return true; } -bool ControlConfigServices::ResetTwistAngularSoftLimit(kortex_driver::ResetTwistAngularSoftLimit::Request &req, kortex_driver::ResetTwistAngularSoftLimit::Response &res) +bool ControlConfigRobotServices::ResetTwistAngularSoftLimit(kortex_driver::ResetTwistAngularSoftLimit::Request &req, kortex_driver::ResetTwistAngularSoftLimit::Response &res) { Kinova::Api::ControlConfig::ControlModeInformation input; @@ -1035,7 +1035,7 @@ bool ControlConfigServices::ResetTwistAngularSoftLimit(kortex_driver::ResetTwist return true; } -bool ControlConfigServices::ResetJointAccelerationSoftLimits(kortex_driver::ResetJointAccelerationSoftLimits::Request &req, kortex_driver::ResetJointAccelerationSoftLimits::Response &res) +bool ControlConfigRobotServices::ResetJointAccelerationSoftLimits(kortex_driver::ResetJointAccelerationSoftLimits::Request &req, kortex_driver::ResetJointAccelerationSoftLimits::Response &res) { Kinova::Api::ControlConfig::ControlModeInformation input; diff --git a/kortex_driver/src/generated/deviceconfig_proto_converter.cpp b/kortex_driver/src/generated/robot/deviceconfig_proto_converter.cpp similarity index 98% rename from kortex_driver/src/generated/deviceconfig_proto_converter.cpp rename to kortex_driver/src/generated/robot/deviceconfig_proto_converter.cpp index 636e6201..143b5902 100644 --- a/kortex_driver/src/generated/deviceconfig_proto_converter.cpp +++ b/kortex_driver/src/generated/robot/deviceconfig_proto_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" int ToProtoData(kortex_driver::DeviceType input, Kinova::Api::DeviceConfig::DeviceType *output) { diff --git a/kortex_driver/src/generated/deviceconfig_ros_converter.cpp b/kortex_driver/src/generated/robot/deviceconfig_ros_converter.cpp similarity index 98% rename from kortex_driver/src/generated/deviceconfig_ros_converter.cpp rename to kortex_driver/src/generated/robot/deviceconfig_ros_converter.cpp index 49829cc8..15e0ef16 100644 --- a/kortex_driver/src/generated/deviceconfig_ros_converter.cpp +++ b/kortex_driver/src/generated/robot/deviceconfig_ros_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" int ToRosData(Kinova::Api::DeviceConfig::DeviceType input, kortex_driver::DeviceType &output) { diff --git a/kortex_driver/src/generated/deviceconfig_services.cpp b/kortex_driver/src/generated/robot/deviceconfig_services.cpp similarity index 70% rename from kortex_driver/src/generated/deviceconfig_services.cpp rename to kortex_driver/src/generated/robot/deviceconfig_services.cpp index 1380c329..8476cff6 100644 --- a/kortex_driver/src/generated/deviceconfig_services.cpp +++ b/kortex_driver/src/generated/robot/deviceconfig_services.cpp @@ -14,90 +14,90 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/common_proto_converter.h" -#include "kortex_driver/generated/common_ros_converter.h" -#include "kortex_driver/generated/actuatorconfig_proto_converter.h" -#include "kortex_driver/generated/actuatorconfig_ros_converter.h" -#include "kortex_driver/generated/actuatorcyclic_proto_converter.h" -#include "kortex_driver/generated/actuatorcyclic_ros_converter.h" -#include "kortex_driver/generated/productconfiguration_proto_converter.h" -#include "kortex_driver/generated/productconfiguration_ros_converter.h" -#include "kortex_driver/generated/base_proto_converter.h" -#include "kortex_driver/generated/base_ros_converter.h" -#include "kortex_driver/generated/grippercyclic_proto_converter.h" -#include "kortex_driver/generated/grippercyclic_ros_converter.h" -#include "kortex_driver/generated/interconnectcyclic_proto_converter.h" -#include "kortex_driver/generated/interconnectcyclic_ros_converter.h" -#include "kortex_driver/generated/basecyclic_proto_converter.h" -#include "kortex_driver/generated/basecyclic_ros_converter.h" -#include "kortex_driver/generated/controlconfig_proto_converter.h" -#include "kortex_driver/generated/controlconfig_ros_converter.h" -#include "kortex_driver/generated/deviceconfig_proto_converter.h" -#include "kortex_driver/generated/deviceconfig_ros_converter.h" -#include "kortex_driver/generated/devicemanager_proto_converter.h" -#include "kortex_driver/generated/devicemanager_ros_converter.h" -#include "kortex_driver/generated/interconnectconfig_proto_converter.h" -#include "kortex_driver/generated/interconnectconfig_ros_converter.h" -#include "kortex_driver/generated/visionconfig_proto_converter.h" -#include "kortex_driver/generated/visionconfig_ros_converter.h" -#include "kortex_driver/generated/deviceconfig_services.h" - -DeviceConfigServices::DeviceConfigServices(ros::NodeHandle& n, Kinova::Api::DeviceConfig::DeviceConfigClient* deviceconfig, uint32_t device_id, uint32_t timeout_ms): - m_n(n), +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_services.h" + +DeviceConfigRobotServices::DeviceConfigRobotServices(ros::NodeHandle& node_handle, Kinova::Api::DeviceConfig::DeviceConfigClient* deviceconfig, uint32_t device_id, uint32_t timeout_ms): + IDeviceConfigServices(node_handle), m_deviceconfig(deviceconfig), m_current_device_id(device_id) { m_api_options.timeout_ms = timeout_ms; - m_pub_Error = m_n.advertise("kortex_error", 1000); - m_pub_SafetyTopic = m_n.advertise("safety_topic", 1000); + m_pub_Error = m_node_handle.advertise("kortex_error", 1000); + m_pub_SafetyTopic = m_node_handle.advertise("safety_topic", 1000); m_is_activated_SafetyTopic = false; - m_serviceSetDeviceID = n.advertiseService("device_config/set_device_id", &DeviceConfigServices::SetDeviceID, this); - m_serviceSetApiOptions = n.advertiseService("device_config/set_api_options", &DeviceConfigServices::SetApiOptions, this); - - m_serviceGetRunMode = m_n.advertiseService("device_config/get_run_mode", &DeviceConfigServices::GetRunMode, this); - m_serviceSetRunMode = m_n.advertiseService("device_config/set_run_mode", &DeviceConfigServices::SetRunMode, this); - m_serviceGetDeviceType = m_n.advertiseService("device_config/get_device_type", &DeviceConfigServices::GetDeviceType, this); - m_serviceGetFirmwareVersion = m_n.advertiseService("device_config/get_firmware_version", &DeviceConfigServices::GetFirmwareVersion, this); - m_serviceGetBootloaderVersion = m_n.advertiseService("device_config/get_bootloader_version", &DeviceConfigServices::GetBootloaderVersion, this); - m_serviceGetModelNumber = m_n.advertiseService("device_config/get_model_number", &DeviceConfigServices::GetModelNumber, this); - m_serviceGetPartNumber = m_n.advertiseService("device_config/get_part_number", &DeviceConfigServices::GetPartNumber, this); - m_serviceGetSerialNumber = m_n.advertiseService("device_config/get_serial_number", &DeviceConfigServices::GetSerialNumber, this); - m_serviceGetMACAddress = m_n.advertiseService("device_config/get_m_a_c_address", &DeviceConfigServices::GetMACAddress, this); - m_serviceGetIPv4Settings = m_n.advertiseService("device_config/get_i_pv4_settings", &DeviceConfigServices::GetIPv4Settings, this); - m_serviceSetIPv4Settings = m_n.advertiseService("device_config/set_i_pv4_settings", &DeviceConfigServices::SetIPv4Settings, this); - m_serviceGetPartNumberRevision = m_n.advertiseService("device_config/get_part_number_revision", &DeviceConfigServices::GetPartNumberRevision, this); - m_serviceRebootRequest = m_n.advertiseService("device_config/reboot_request", &DeviceConfigServices::RebootRequest, this); - m_serviceSetSafetyEnable = m_n.advertiseService("device_config/set_safety_enable", &DeviceConfigServices::SetSafetyEnable, this); - m_serviceSetSafetyErrorThreshold = m_n.advertiseService("device_config/set_safety_error_threshold", &DeviceConfigServices::SetSafetyErrorThreshold, this); - m_serviceSetSafetyWarningThreshold = m_n.advertiseService("device_config/set_safety_warning_threshold", &DeviceConfigServices::SetSafetyWarningThreshold, this); - m_serviceSetSafetyConfiguration = m_n.advertiseService("device_config/set_safety_configuration", &DeviceConfigServices::SetSafetyConfiguration, this); - m_serviceGetSafetyConfiguration = m_n.advertiseService("device_config/get_safety_configuration", &DeviceConfigServices::GetSafetyConfiguration, this); - m_serviceGetSafetyInformation = m_n.advertiseService("device_config/get_safety_information", &DeviceConfigServices::GetSafetyInformation, this); - m_serviceGetSafetyEnable = m_n.advertiseService("device_config/get_safety_enable", &DeviceConfigServices::GetSafetyEnable, this); - m_serviceGetSafetyStatus = m_n.advertiseService("device_config/get_safety_status", &DeviceConfigServices::GetSafetyStatus, this); - m_serviceClearAllSafetyStatus = m_n.advertiseService("device_config/clear_all_safety_status", &DeviceConfigServices::ClearAllSafetyStatus, this); - m_serviceClearSafetyStatus = m_n.advertiseService("device_config/clear_safety_status", &DeviceConfigServices::ClearSafetyStatus, this); - m_serviceGetAllSafetyConfiguration = m_n.advertiseService("device_config/get_all_safety_configuration", &DeviceConfigServices::GetAllSafetyConfiguration, this); - m_serviceGetAllSafetyInformation = m_n.advertiseService("device_config/get_all_safety_information", &DeviceConfigServices::GetAllSafetyInformation, this); - m_serviceResetSafetyDefaults = m_n.advertiseService("device_config/reset_safety_defaults", &DeviceConfigServices::ResetSafetyDefaults, this); - m_serviceOnNotificationSafetyTopic = m_n.advertiseService("device_config/activate_publishing_of_safety_topic", &DeviceConfigServices::OnNotificationSafetyTopic, this); - m_serviceExecuteCalibration = m_n.advertiseService("device_config/execute_calibration", &DeviceConfigServices::ExecuteCalibration, this); - m_serviceGetCalibrationResult = m_n.advertiseService("device_config/get_calibration_result", &DeviceConfigServices::GetCalibrationResult, this); - m_serviceStopCalibration = m_n.advertiseService("device_config/stop_calibration", &DeviceConfigServices::StopCalibration, this); - m_serviceDeviceConfig_SetCapSenseConfig = m_n.advertiseService("device_config/set_cap_sense_config", &DeviceConfigServices::DeviceConfig_SetCapSenseConfig, this); - m_serviceDeviceConfig_GetCapSenseConfig = m_n.advertiseService("device_config/get_cap_sense_config", &DeviceConfigServices::DeviceConfig_GetCapSenseConfig, this); + m_serviceSetDeviceID = m_node_handle.advertiseService("device_config/set_device_id", &DeviceConfigRobotServices::SetDeviceID, this); + m_serviceSetApiOptions = m_node_handle.advertiseService("device_config/set_api_options", &DeviceConfigRobotServices::SetApiOptions, this); + + m_serviceGetRunMode = m_node_handle.advertiseService("device_config/get_run_mode", &DeviceConfigRobotServices::GetRunMode, this); + m_serviceSetRunMode = m_node_handle.advertiseService("device_config/set_run_mode", &DeviceConfigRobotServices::SetRunMode, this); + m_serviceGetDeviceType = m_node_handle.advertiseService("device_config/get_device_type", &DeviceConfigRobotServices::GetDeviceType, this); + m_serviceGetFirmwareVersion = m_node_handle.advertiseService("device_config/get_firmware_version", &DeviceConfigRobotServices::GetFirmwareVersion, this); + m_serviceGetBootloaderVersion = m_node_handle.advertiseService("device_config/get_bootloader_version", &DeviceConfigRobotServices::GetBootloaderVersion, this); + m_serviceGetModelNumber = m_node_handle.advertiseService("device_config/get_model_number", &DeviceConfigRobotServices::GetModelNumber, this); + m_serviceGetPartNumber = m_node_handle.advertiseService("device_config/get_part_number", &DeviceConfigRobotServices::GetPartNumber, this); + m_serviceGetSerialNumber = m_node_handle.advertiseService("device_config/get_serial_number", &DeviceConfigRobotServices::GetSerialNumber, this); + m_serviceGetMACAddress = m_node_handle.advertiseService("device_config/get_m_a_c_address", &DeviceConfigRobotServices::GetMACAddress, this); + m_serviceGetIPv4Settings = m_node_handle.advertiseService("device_config/get_i_pv4_settings", &DeviceConfigRobotServices::GetIPv4Settings, this); + m_serviceSetIPv4Settings = m_node_handle.advertiseService("device_config/set_i_pv4_settings", &DeviceConfigRobotServices::SetIPv4Settings, this); + m_serviceGetPartNumberRevision = m_node_handle.advertiseService("device_config/get_part_number_revision", &DeviceConfigRobotServices::GetPartNumberRevision, this); + m_serviceRebootRequest = m_node_handle.advertiseService("device_config/reboot_request", &DeviceConfigRobotServices::RebootRequest, this); + m_serviceSetSafetyEnable = m_node_handle.advertiseService("device_config/set_safety_enable", &DeviceConfigRobotServices::SetSafetyEnable, this); + m_serviceSetSafetyErrorThreshold = m_node_handle.advertiseService("device_config/set_safety_error_threshold", &DeviceConfigRobotServices::SetSafetyErrorThreshold, this); + m_serviceSetSafetyWarningThreshold = m_node_handle.advertiseService("device_config/set_safety_warning_threshold", &DeviceConfigRobotServices::SetSafetyWarningThreshold, this); + m_serviceSetSafetyConfiguration = m_node_handle.advertiseService("device_config/set_safety_configuration", &DeviceConfigRobotServices::SetSafetyConfiguration, this); + m_serviceGetSafetyConfiguration = m_node_handle.advertiseService("device_config/get_safety_configuration", &DeviceConfigRobotServices::GetSafetyConfiguration, this); + m_serviceGetSafetyInformation = m_node_handle.advertiseService("device_config/get_safety_information", &DeviceConfigRobotServices::GetSafetyInformation, this); + m_serviceGetSafetyEnable = m_node_handle.advertiseService("device_config/get_safety_enable", &DeviceConfigRobotServices::GetSafetyEnable, this); + m_serviceGetSafetyStatus = m_node_handle.advertiseService("device_config/get_safety_status", &DeviceConfigRobotServices::GetSafetyStatus, this); + m_serviceClearAllSafetyStatus = m_node_handle.advertiseService("device_config/clear_all_safety_status", &DeviceConfigRobotServices::ClearAllSafetyStatus, this); + m_serviceClearSafetyStatus = m_node_handle.advertiseService("device_config/clear_safety_status", &DeviceConfigRobotServices::ClearSafetyStatus, this); + m_serviceGetAllSafetyConfiguration = m_node_handle.advertiseService("device_config/get_all_safety_configuration", &DeviceConfigRobotServices::GetAllSafetyConfiguration, this); + m_serviceGetAllSafetyInformation = m_node_handle.advertiseService("device_config/get_all_safety_information", &DeviceConfigRobotServices::GetAllSafetyInformation, this); + m_serviceResetSafetyDefaults = m_node_handle.advertiseService("device_config/reset_safety_defaults", &DeviceConfigRobotServices::ResetSafetyDefaults, this); + m_serviceOnNotificationSafetyTopic = m_node_handle.advertiseService("device_config/activate_publishing_of_safety_topic", &DeviceConfigRobotServices::OnNotificationSafetyTopic, this); + m_serviceExecuteCalibration = m_node_handle.advertiseService("device_config/execute_calibration", &DeviceConfigRobotServices::ExecuteCalibration, this); + m_serviceGetCalibrationResult = m_node_handle.advertiseService("device_config/get_calibration_result", &DeviceConfigRobotServices::GetCalibrationResult, this); + m_serviceStopCalibration = m_node_handle.advertiseService("device_config/stop_calibration", &DeviceConfigRobotServices::StopCalibration, this); + m_serviceDeviceConfig_SetCapSenseConfig = m_node_handle.advertiseService("device_config/set_cap_sense_config", &DeviceConfigRobotServices::DeviceConfig_SetCapSenseConfig, this); + m_serviceDeviceConfig_GetCapSenseConfig = m_node_handle.advertiseService("device_config/get_cap_sense_config", &DeviceConfigRobotServices::DeviceConfig_GetCapSenseConfig, this); } -bool DeviceConfigServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) +bool DeviceConfigRobotServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) { m_current_device_id = req.device_id; return true; } -bool DeviceConfigServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) +bool DeviceConfigRobotServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) { m_api_options.timeout_ms = req.input.timeout_ms; @@ -105,7 +105,7 @@ bool DeviceConfigServices::SetApiOptions(kortex_driver::SetApiOptions::Request } -bool DeviceConfigServices::GetRunMode(kortex_driver::GetRunMode::Request &req, kortex_driver::GetRunMode::Response &res) +bool DeviceConfigRobotServices::GetRunMode(kortex_driver::GetRunMode::Request &req, kortex_driver::GetRunMode::Response &res) { Kinova::Api::DeviceConfig::RunMode output; @@ -138,7 +138,7 @@ bool DeviceConfigServices::GetRunMode(kortex_driver::GetRunMode::Request &req, return true; } -bool DeviceConfigServices::SetRunMode(kortex_driver::SetRunMode::Request &req, kortex_driver::SetRunMode::Response &res) +bool DeviceConfigRobotServices::SetRunMode(kortex_driver::SetRunMode::Request &req, kortex_driver::SetRunMode::Response &res) { Kinova::Api::DeviceConfig::RunMode input; @@ -170,7 +170,7 @@ bool DeviceConfigServices::SetRunMode(kortex_driver::SetRunMode::Request &req, return true; } -bool DeviceConfigServices::GetDeviceType(kortex_driver::GetDeviceType::Request &req, kortex_driver::GetDeviceType::Response &res) +bool DeviceConfigRobotServices::GetDeviceType(kortex_driver::GetDeviceType::Request &req, kortex_driver::GetDeviceType::Response &res) { Kinova::Api::DeviceConfig::DeviceType output; @@ -203,7 +203,7 @@ bool DeviceConfigServices::GetDeviceType(kortex_driver::GetDeviceType::Request return true; } -bool DeviceConfigServices::GetFirmwareVersion(kortex_driver::GetFirmwareVersion::Request &req, kortex_driver::GetFirmwareVersion::Response &res) +bool DeviceConfigRobotServices::GetFirmwareVersion(kortex_driver::GetFirmwareVersion::Request &req, kortex_driver::GetFirmwareVersion::Response &res) { Kinova::Api::DeviceConfig::FirmwareVersion output; @@ -236,7 +236,7 @@ bool DeviceConfigServices::GetFirmwareVersion(kortex_driver::GetFirmwareVersion: return true; } -bool DeviceConfigServices::GetBootloaderVersion(kortex_driver::GetBootloaderVersion::Request &req, kortex_driver::GetBootloaderVersion::Response &res) +bool DeviceConfigRobotServices::GetBootloaderVersion(kortex_driver::GetBootloaderVersion::Request &req, kortex_driver::GetBootloaderVersion::Response &res) { Kinova::Api::DeviceConfig::BootloaderVersion output; @@ -269,7 +269,7 @@ bool DeviceConfigServices::GetBootloaderVersion(kortex_driver::GetBootloaderVers return true; } -bool DeviceConfigServices::GetModelNumber(kortex_driver::GetModelNumber::Request &req, kortex_driver::GetModelNumber::Response &res) +bool DeviceConfigRobotServices::GetModelNumber(kortex_driver::GetModelNumber::Request &req, kortex_driver::GetModelNumber::Response &res) { Kinova::Api::DeviceConfig::ModelNumber output; @@ -302,7 +302,7 @@ bool DeviceConfigServices::GetModelNumber(kortex_driver::GetModelNumber::Request return true; } -bool DeviceConfigServices::GetPartNumber(kortex_driver::GetPartNumber::Request &req, kortex_driver::GetPartNumber::Response &res) +bool DeviceConfigRobotServices::GetPartNumber(kortex_driver::GetPartNumber::Request &req, kortex_driver::GetPartNumber::Response &res) { Kinova::Api::DeviceConfig::PartNumber output; @@ -335,7 +335,7 @@ bool DeviceConfigServices::GetPartNumber(kortex_driver::GetPartNumber::Request return true; } -bool DeviceConfigServices::GetSerialNumber(kortex_driver::GetSerialNumber::Request &req, kortex_driver::GetSerialNumber::Response &res) +bool DeviceConfigRobotServices::GetSerialNumber(kortex_driver::GetSerialNumber::Request &req, kortex_driver::GetSerialNumber::Response &res) { Kinova::Api::DeviceConfig::SerialNumber output; @@ -368,7 +368,7 @@ bool DeviceConfigServices::GetSerialNumber(kortex_driver::GetSerialNumber::Reque return true; } -bool DeviceConfigServices::GetMACAddress(kortex_driver::GetMACAddress::Request &req, kortex_driver::GetMACAddress::Response &res) +bool DeviceConfigRobotServices::GetMACAddress(kortex_driver::GetMACAddress::Request &req, kortex_driver::GetMACAddress::Response &res) { Kinova::Api::DeviceConfig::MACAddress output; @@ -401,7 +401,7 @@ bool DeviceConfigServices::GetMACAddress(kortex_driver::GetMACAddress::Request return true; } -bool DeviceConfigServices::GetIPv4Settings(kortex_driver::GetIPv4Settings::Request &req, kortex_driver::GetIPv4Settings::Response &res) +bool DeviceConfigRobotServices::GetIPv4Settings(kortex_driver::GetIPv4Settings::Request &req, kortex_driver::GetIPv4Settings::Response &res) { Kinova::Api::DeviceConfig::IPv4Settings output; @@ -434,7 +434,7 @@ bool DeviceConfigServices::GetIPv4Settings(kortex_driver::GetIPv4Settings::Reque return true; } -bool DeviceConfigServices::SetIPv4Settings(kortex_driver::SetIPv4Settings::Request &req, kortex_driver::SetIPv4Settings::Response &res) +bool DeviceConfigRobotServices::SetIPv4Settings(kortex_driver::SetIPv4Settings::Request &req, kortex_driver::SetIPv4Settings::Response &res) { Kinova::Api::DeviceConfig::IPv4Settings input; @@ -466,7 +466,7 @@ bool DeviceConfigServices::SetIPv4Settings(kortex_driver::SetIPv4Settings::Reque return true; } -bool DeviceConfigServices::GetPartNumberRevision(kortex_driver::GetPartNumberRevision::Request &req, kortex_driver::GetPartNumberRevision::Response &res) +bool DeviceConfigRobotServices::GetPartNumberRevision(kortex_driver::GetPartNumberRevision::Request &req, kortex_driver::GetPartNumberRevision::Response &res) { Kinova::Api::DeviceConfig::PartNumberRevision output; @@ -499,7 +499,7 @@ bool DeviceConfigServices::GetPartNumberRevision(kortex_driver::GetPartNumberRev return true; } -bool DeviceConfigServices::RebootRequest(kortex_driver::RebootRequest::Request &req, kortex_driver::RebootRequest::Response &res) +bool DeviceConfigRobotServices::RebootRequest(kortex_driver::RebootRequest::Request &req, kortex_driver::RebootRequest::Response &res) { Kinova::Api::DeviceConfig::RebootRqst input; @@ -531,7 +531,7 @@ bool DeviceConfigServices::RebootRequest(kortex_driver::RebootRequest::Request return true; } -bool DeviceConfigServices::SetSafetyEnable(kortex_driver::SetSafetyEnable::Request &req, kortex_driver::SetSafetyEnable::Response &res) +bool DeviceConfigRobotServices::SetSafetyEnable(kortex_driver::SetSafetyEnable::Request &req, kortex_driver::SetSafetyEnable::Response &res) { Kinova::Api::DeviceConfig::SafetyEnable input; @@ -563,7 +563,7 @@ bool DeviceConfigServices::SetSafetyEnable(kortex_driver::SetSafetyEnable::Reque return true; } -bool DeviceConfigServices::SetSafetyErrorThreshold(kortex_driver::SetSafetyErrorThreshold::Request &req, kortex_driver::SetSafetyErrorThreshold::Response &res) +bool DeviceConfigRobotServices::SetSafetyErrorThreshold(kortex_driver::SetSafetyErrorThreshold::Request &req, kortex_driver::SetSafetyErrorThreshold::Response &res) { Kinova::Api::DeviceConfig::SafetyThreshold input; @@ -595,7 +595,7 @@ bool DeviceConfigServices::SetSafetyErrorThreshold(kortex_driver::SetSafetyError return true; } -bool DeviceConfigServices::SetSafetyWarningThreshold(kortex_driver::SetSafetyWarningThreshold::Request &req, kortex_driver::SetSafetyWarningThreshold::Response &res) +bool DeviceConfigRobotServices::SetSafetyWarningThreshold(kortex_driver::SetSafetyWarningThreshold::Request &req, kortex_driver::SetSafetyWarningThreshold::Response &res) { Kinova::Api::DeviceConfig::SafetyThreshold input; @@ -627,7 +627,7 @@ bool DeviceConfigServices::SetSafetyWarningThreshold(kortex_driver::SetSafetyWar return true; } -bool DeviceConfigServices::SetSafetyConfiguration(kortex_driver::SetSafetyConfiguration::Request &req, kortex_driver::SetSafetyConfiguration::Response &res) +bool DeviceConfigRobotServices::SetSafetyConfiguration(kortex_driver::SetSafetyConfiguration::Request &req, kortex_driver::SetSafetyConfiguration::Response &res) { Kinova::Api::DeviceConfig::SafetyConfiguration input; @@ -659,7 +659,7 @@ bool DeviceConfigServices::SetSafetyConfiguration(kortex_driver::SetSafetyConfig return true; } -bool DeviceConfigServices::GetSafetyConfiguration(kortex_driver::GetSafetyConfiguration::Request &req, kortex_driver::GetSafetyConfiguration::Response &res) +bool DeviceConfigRobotServices::GetSafetyConfiguration(kortex_driver::GetSafetyConfiguration::Request &req, kortex_driver::GetSafetyConfiguration::Response &res) { Kinova::Api::Common::SafetyHandle input; @@ -694,7 +694,7 @@ bool DeviceConfigServices::GetSafetyConfiguration(kortex_driver::GetSafetyConfig return true; } -bool DeviceConfigServices::GetSafetyInformation(kortex_driver::GetSafetyInformation::Request &req, kortex_driver::GetSafetyInformation::Response &res) +bool DeviceConfigRobotServices::GetSafetyInformation(kortex_driver::GetSafetyInformation::Request &req, kortex_driver::GetSafetyInformation::Response &res) { Kinova::Api::Common::SafetyHandle input; @@ -729,7 +729,7 @@ bool DeviceConfigServices::GetSafetyInformation(kortex_driver::GetSafetyInformat return true; } -bool DeviceConfigServices::GetSafetyEnable(kortex_driver::GetSafetyEnable::Request &req, kortex_driver::GetSafetyEnable::Response &res) +bool DeviceConfigRobotServices::GetSafetyEnable(kortex_driver::GetSafetyEnable::Request &req, kortex_driver::GetSafetyEnable::Response &res) { Kinova::Api::Common::SafetyHandle input; @@ -764,7 +764,7 @@ bool DeviceConfigServices::GetSafetyEnable(kortex_driver::GetSafetyEnable::Reque return true; } -bool DeviceConfigServices::GetSafetyStatus(kortex_driver::GetSafetyStatus::Request &req, kortex_driver::GetSafetyStatus::Response &res) +bool DeviceConfigRobotServices::GetSafetyStatus(kortex_driver::GetSafetyStatus::Request &req, kortex_driver::GetSafetyStatus::Response &res) { Kinova::Api::Common::SafetyHandle input; @@ -799,7 +799,7 @@ bool DeviceConfigServices::GetSafetyStatus(kortex_driver::GetSafetyStatus::Reque return true; } -bool DeviceConfigServices::ClearAllSafetyStatus(kortex_driver::ClearAllSafetyStatus::Request &req, kortex_driver::ClearAllSafetyStatus::Response &res) +bool DeviceConfigRobotServices::ClearAllSafetyStatus(kortex_driver::ClearAllSafetyStatus::Request &req, kortex_driver::ClearAllSafetyStatus::Response &res) { kortex_driver::KortexError result_error; @@ -829,7 +829,7 @@ bool DeviceConfigServices::ClearAllSafetyStatus(kortex_driver::ClearAllSafetySta return true; } -bool DeviceConfigServices::ClearSafetyStatus(kortex_driver::ClearSafetyStatus::Request &req, kortex_driver::ClearSafetyStatus::Response &res) +bool DeviceConfigRobotServices::ClearSafetyStatus(kortex_driver::ClearSafetyStatus::Request &req, kortex_driver::ClearSafetyStatus::Response &res) { Kinova::Api::Common::SafetyHandle input; @@ -861,7 +861,7 @@ bool DeviceConfigServices::ClearSafetyStatus(kortex_driver::ClearSafetyStatus::R return true; } -bool DeviceConfigServices::GetAllSafetyConfiguration(kortex_driver::GetAllSafetyConfiguration::Request &req, kortex_driver::GetAllSafetyConfiguration::Response &res) +bool DeviceConfigRobotServices::GetAllSafetyConfiguration(kortex_driver::GetAllSafetyConfiguration::Request &req, kortex_driver::GetAllSafetyConfiguration::Response &res) { Kinova::Api::DeviceConfig::SafetyConfigurationList output; @@ -894,7 +894,7 @@ bool DeviceConfigServices::GetAllSafetyConfiguration(kortex_driver::GetAllSafety return true; } -bool DeviceConfigServices::GetAllSafetyInformation(kortex_driver::GetAllSafetyInformation::Request &req, kortex_driver::GetAllSafetyInformation::Response &res) +bool DeviceConfigRobotServices::GetAllSafetyInformation(kortex_driver::GetAllSafetyInformation::Request &req, kortex_driver::GetAllSafetyInformation::Response &res) { Kinova::Api::DeviceConfig::SafetyInformationList output; @@ -927,7 +927,7 @@ bool DeviceConfigServices::GetAllSafetyInformation(kortex_driver::GetAllSafetyIn return true; } -bool DeviceConfigServices::ResetSafetyDefaults(kortex_driver::ResetSafetyDefaults::Request &req, kortex_driver::ResetSafetyDefaults::Response &res) +bool DeviceConfigRobotServices::ResetSafetyDefaults(kortex_driver::ResetSafetyDefaults::Request &req, kortex_driver::ResetSafetyDefaults::Response &res) { kortex_driver::KortexError result_error; @@ -957,7 +957,7 @@ bool DeviceConfigServices::ResetSafetyDefaults(kortex_driver::ResetSafetyDefault return true; } -bool DeviceConfigServices::OnNotificationSafetyTopic(kortex_driver::OnNotificationSafetyTopic::Request &req, kortex_driver::OnNotificationSafetyTopic::Response &res) +bool DeviceConfigRobotServices::OnNotificationSafetyTopic(kortex_driver::OnNotificationSafetyTopic::Request &req, kortex_driver::OnNotificationSafetyTopic::Response &res) { // If the notification is already activated, don't activate multiple times @@ -971,7 +971,7 @@ bool DeviceConfigServices::OnNotificationSafetyTopic(kortex_driver::OnNotificati try { - std::function< void (Kinova::Api::Common::SafetyNotification) > callback = std::bind(&DeviceConfigServices::cb_SafetyTopic, this, std::placeholders::_1); + std::function< void (Kinova::Api::Common::SafetyNotification) > callback = std::bind(&DeviceConfigRobotServices::cb_SafetyTopic, this, std::placeholders::_1); output = m_deviceconfig->OnNotificationSafetyTopic(callback, input, m_current_device_id); m_is_activated_SafetyTopic = true; } @@ -996,14 +996,14 @@ bool DeviceConfigServices::OnNotificationSafetyTopic(kortex_driver::OnNotificati ToRosData(output, res.output); return true; } -void DeviceConfigServices::cb_SafetyTopic(Kinova::Api::Common::SafetyNotification notif) +void DeviceConfigRobotServices::cb_SafetyTopic(Kinova::Api::Common::SafetyNotification notif) { kortex_driver::SafetyNotification ros_msg; ToRosData(notif, ros_msg); m_pub_SafetyTopic.publish(ros_msg); } -bool DeviceConfigServices::ExecuteCalibration(kortex_driver::ExecuteCalibration::Request &req, kortex_driver::ExecuteCalibration::Response &res) +bool DeviceConfigRobotServices::ExecuteCalibration(kortex_driver::ExecuteCalibration::Request &req, kortex_driver::ExecuteCalibration::Response &res) { Kinova::Api::DeviceConfig::Calibration input; @@ -1035,7 +1035,7 @@ bool DeviceConfigServices::ExecuteCalibration(kortex_driver::ExecuteCalibration: return true; } -bool DeviceConfigServices::GetCalibrationResult(kortex_driver::GetCalibrationResult::Request &req, kortex_driver::GetCalibrationResult::Response &res) +bool DeviceConfigRobotServices::GetCalibrationResult(kortex_driver::GetCalibrationResult::Request &req, kortex_driver::GetCalibrationResult::Response &res) { Kinova::Api::DeviceConfig::CalibrationElement input; @@ -1070,7 +1070,7 @@ bool DeviceConfigServices::GetCalibrationResult(kortex_driver::GetCalibrationRes return true; } -bool DeviceConfigServices::StopCalibration(kortex_driver::StopCalibration::Request &req, kortex_driver::StopCalibration::Response &res) +bool DeviceConfigRobotServices::StopCalibration(kortex_driver::StopCalibration::Request &req, kortex_driver::StopCalibration::Response &res) { Kinova::Api::DeviceConfig::Calibration input; @@ -1105,7 +1105,7 @@ bool DeviceConfigServices::StopCalibration(kortex_driver::StopCalibration::Reque return true; } -bool DeviceConfigServices::DeviceConfig_SetCapSenseConfig(kortex_driver::DeviceConfig_SetCapSenseConfig::Request &req, kortex_driver::DeviceConfig_SetCapSenseConfig::Response &res) +bool DeviceConfigRobotServices::DeviceConfig_SetCapSenseConfig(kortex_driver::DeviceConfig_SetCapSenseConfig::Request &req, kortex_driver::DeviceConfig_SetCapSenseConfig::Response &res) { Kinova::Api::DeviceConfig::CapSenseConfig input; @@ -1137,7 +1137,7 @@ bool DeviceConfigServices::DeviceConfig_SetCapSenseConfig(kortex_driver::DeviceC return true; } -bool DeviceConfigServices::DeviceConfig_GetCapSenseConfig(kortex_driver::DeviceConfig_GetCapSenseConfig::Request &req, kortex_driver::DeviceConfig_GetCapSenseConfig::Response &res) +bool DeviceConfigRobotServices::DeviceConfig_GetCapSenseConfig(kortex_driver::DeviceConfig_GetCapSenseConfig::Request &req, kortex_driver::DeviceConfig_GetCapSenseConfig::Response &res) { Kinova::Api::DeviceConfig::CapSenseConfig output; diff --git a/kortex_driver/src/generated/devicemanager_proto_converter.cpp b/kortex_driver/src/generated/robot/devicemanager_proto_converter.cpp similarity index 88% rename from kortex_driver/src/generated/devicemanager_proto_converter.cpp rename to kortex_driver/src/generated/robot/devicemanager_proto_converter.cpp index 31785e68..22cda421 100644 --- a/kortex_driver/src/generated/devicemanager_proto_converter.cpp +++ b/kortex_driver/src/generated/robot/devicemanager_proto_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" int ToProtoData(kortex_driver::DeviceHandles input, Kinova::Api::DeviceManager::DeviceHandles *output) { diff --git a/kortex_driver/src/generated/devicemanager_ros_converter.cpp b/kortex_driver/src/generated/robot/devicemanager_ros_converter.cpp similarity index 89% rename from kortex_driver/src/generated/devicemanager_ros_converter.cpp rename to kortex_driver/src/generated/robot/devicemanager_ros_converter.cpp index c9a2115d..3b5e95f6 100644 --- a/kortex_driver/src/generated/devicemanager_ros_converter.cpp +++ b/kortex_driver/src/generated/robot/devicemanager_ros_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" int ToRosData(Kinova::Api::DeviceManager::DeviceHandles input, kortex_driver::DeviceHandles &output) { diff --git a/kortex_driver/src/generated/robot/devicemanager_services.cpp b/kortex_driver/src/generated/robot/devicemanager_services.cpp new file mode 100644 index 00000000..b649cf2b --- /dev/null +++ b/kortex_driver/src/generated/robot/devicemanager_services.cpp @@ -0,0 +1,106 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_services.h" + +DeviceManagerRobotServices::DeviceManagerRobotServices(ros::NodeHandle& node_handle, Kinova::Api::DeviceManager::DeviceManagerClient* devicemanager, uint32_t device_id, uint32_t timeout_ms): + IDeviceManagerServices(node_handle), + m_devicemanager(devicemanager), + m_current_device_id(device_id) +{ + m_api_options.timeout_ms = timeout_ms; + + m_pub_Error = m_node_handle.advertise("kortex_error", 1000); + + m_serviceSetDeviceID = m_node_handle.advertiseService("device_manager/set_device_id", &DeviceManagerRobotServices::SetDeviceID, this); + m_serviceSetApiOptions = m_node_handle.advertiseService("device_manager/set_api_options", &DeviceManagerRobotServices::SetApiOptions, this); + + m_serviceReadAllDevices = m_node_handle.advertiseService("device_manager/read_all_devices", &DeviceManagerRobotServices::ReadAllDevices, this); +} + +bool DeviceManagerRobotServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) +{ + m_current_device_id = req.device_id; + + return true; +} + +bool DeviceManagerRobotServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) +{ + m_api_options.timeout_ms = req.input.timeout_ms; + + return true; +} + + +bool DeviceManagerRobotServices::ReadAllDevices(kortex_driver::ReadAllDevices::Request &req, kortex_driver::ReadAllDevices::Response &res) +{ + + Kinova::Api::DeviceManager::DeviceHandles output; + + kortex_driver::KortexError result_error; + + try + { + output = m_devicemanager->ReadAllDevices(m_current_device_id, m_api_options); + } + + catch (Kinova::Api::KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + ROS_INFO("%s", ex2.what()); + return false; + } + ToRosData(output, res.output); + return true; +} diff --git a/kortex_driver/src/generated/grippercyclic_proto_converter.cpp b/kortex_driver/src/generated/robot/grippercyclic_proto_converter.cpp similarity index 97% rename from kortex_driver/src/generated/grippercyclic_proto_converter.cpp rename to kortex_driver/src/generated/robot/grippercyclic_proto_converter.cpp index f5840258..fe1962d9 100644 --- a/kortex_driver/src/generated/grippercyclic_proto_converter.cpp +++ b/kortex_driver/src/generated/robot/grippercyclic_proto_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" int ToProtoData(kortex_driver::GripperCyclic_MessageId input, Kinova::Api::GripperCyclic::MessageId *output) { diff --git a/kortex_driver/src/generated/grippercyclic_ros_converter.cpp b/kortex_driver/src/generated/robot/grippercyclic_ros_converter.cpp similarity index 97% rename from kortex_driver/src/generated/grippercyclic_ros_converter.cpp rename to kortex_driver/src/generated/robot/grippercyclic_ros_converter.cpp index 5dd097e2..09523044 100644 --- a/kortex_driver/src/generated/grippercyclic_ros_converter.cpp +++ b/kortex_driver/src/generated/robot/grippercyclic_ros_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" int ToRosData(Kinova::Api::GripperCyclic::MessageId input, kortex_driver::GripperCyclic_MessageId &output) { diff --git a/kortex_driver/src/generated/interconnectconfig_proto_converter.cpp b/kortex_driver/src/generated/robot/interconnectconfig_proto_converter.cpp similarity index 98% rename from kortex_driver/src/generated/interconnectconfig_proto_converter.cpp rename to kortex_driver/src/generated/robot/interconnectconfig_proto_converter.cpp index e210b6a0..1c0bc732 100644 --- a/kortex_driver/src/generated/interconnectconfig_proto_converter.cpp +++ b/kortex_driver/src/generated/robot/interconnectconfig_proto_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" int ToProtoData(kortex_driver::EthernetDeviceIdentification input, Kinova::Api::InterconnectConfig::EthernetDeviceIdentification *output) { diff --git a/kortex_driver/src/generated/interconnectconfig_ros_converter.cpp b/kortex_driver/src/generated/robot/interconnectconfig_ros_converter.cpp similarity index 97% rename from kortex_driver/src/generated/interconnectconfig_ros_converter.cpp rename to kortex_driver/src/generated/robot/interconnectconfig_ros_converter.cpp index 356d96e4..bc2f660b 100644 --- a/kortex_driver/src/generated/interconnectconfig_ros_converter.cpp +++ b/kortex_driver/src/generated/robot/interconnectconfig_ros_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" int ToRosData(Kinova::Api::InterconnectConfig::EthernetDeviceIdentification input, kortex_driver::EthernetDeviceIdentification &output) { diff --git a/kortex_driver/src/generated/interconnectconfig_services.cpp b/kortex_driver/src/generated/robot/interconnectconfig_services.cpp similarity index 65% rename from kortex_driver/src/generated/interconnectconfig_services.cpp rename to kortex_driver/src/generated/robot/interconnectconfig_services.cpp index c55847c2..bd696c48 100644 --- a/kortex_driver/src/generated/interconnectconfig_services.cpp +++ b/kortex_driver/src/generated/robot/interconnectconfig_services.cpp @@ -14,70 +14,70 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/common_proto_converter.h" -#include "kortex_driver/generated/common_ros_converter.h" -#include "kortex_driver/generated/actuatorconfig_proto_converter.h" -#include "kortex_driver/generated/actuatorconfig_ros_converter.h" -#include "kortex_driver/generated/actuatorcyclic_proto_converter.h" -#include "kortex_driver/generated/actuatorcyclic_ros_converter.h" -#include "kortex_driver/generated/productconfiguration_proto_converter.h" -#include "kortex_driver/generated/productconfiguration_ros_converter.h" -#include "kortex_driver/generated/base_proto_converter.h" -#include "kortex_driver/generated/base_ros_converter.h" -#include "kortex_driver/generated/grippercyclic_proto_converter.h" -#include "kortex_driver/generated/grippercyclic_ros_converter.h" -#include "kortex_driver/generated/interconnectcyclic_proto_converter.h" -#include "kortex_driver/generated/interconnectcyclic_ros_converter.h" -#include "kortex_driver/generated/basecyclic_proto_converter.h" -#include "kortex_driver/generated/basecyclic_ros_converter.h" -#include "kortex_driver/generated/controlconfig_proto_converter.h" -#include "kortex_driver/generated/controlconfig_ros_converter.h" -#include "kortex_driver/generated/deviceconfig_proto_converter.h" -#include "kortex_driver/generated/deviceconfig_ros_converter.h" -#include "kortex_driver/generated/devicemanager_proto_converter.h" -#include "kortex_driver/generated/devicemanager_ros_converter.h" -#include "kortex_driver/generated/interconnectconfig_proto_converter.h" -#include "kortex_driver/generated/interconnectconfig_ros_converter.h" -#include "kortex_driver/generated/visionconfig_proto_converter.h" -#include "kortex_driver/generated/visionconfig_ros_converter.h" -#include "kortex_driver/generated/interconnectconfig_services.h" - -InterconnectConfigServices::InterconnectConfigServices(ros::NodeHandle& n, Kinova::Api::InterconnectConfig::InterconnectConfigClient* interconnectconfig, uint32_t device_id, uint32_t timeout_ms): - m_n(n), +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_services.h" + +InterconnectConfigRobotServices::InterconnectConfigRobotServices(ros::NodeHandle& node_handle, Kinova::Api::InterconnectConfig::InterconnectConfigClient* interconnectconfig, uint32_t device_id, uint32_t timeout_ms): + IInterconnectConfigServices(node_handle), m_interconnectconfig(interconnectconfig), m_current_device_id(device_id) { m_api_options.timeout_ms = timeout_ms; - m_pub_Error = m_n.advertise("kortex_error", 1000); - - m_serviceSetDeviceID = n.advertiseService("interconnect_config/set_device_id", &InterconnectConfigServices::SetDeviceID, this); - m_serviceSetApiOptions = n.advertiseService("interconnect_config/set_api_options", &InterconnectConfigServices::SetApiOptions, this); - - m_serviceGetUARTConfiguration = m_n.advertiseService("interconnect_config/get_u_a_r_t_configuration", &InterconnectConfigServices::GetUARTConfiguration, this); - m_serviceSetUARTConfiguration = m_n.advertiseService("interconnect_config/set_u_a_r_t_configuration", &InterconnectConfigServices::SetUARTConfiguration, this); - m_serviceGetEthernetConfiguration = m_n.advertiseService("interconnect_config/get_ethernet_configuration", &InterconnectConfigServices::GetEthernetConfiguration, this); - m_serviceSetEthernetConfiguration = m_n.advertiseService("interconnect_config/set_ethernet_configuration", &InterconnectConfigServices::SetEthernetConfiguration, this); - m_serviceGetGPIOConfiguration = m_n.advertiseService("interconnect_config/get_g_p_i_o_configuration", &InterconnectConfigServices::GetGPIOConfiguration, this); - m_serviceSetGPIOConfiguration = m_n.advertiseService("interconnect_config/set_g_p_i_o_configuration", &InterconnectConfigServices::SetGPIOConfiguration, this); - m_serviceGetGPIOState = m_n.advertiseService("interconnect_config/get_g_p_i_o_state", &InterconnectConfigServices::GetGPIOState, this); - m_serviceSetGPIOState = m_n.advertiseService("interconnect_config/set_g_p_i_o_state", &InterconnectConfigServices::SetGPIOState, this); - m_serviceGetI2CConfiguration = m_n.advertiseService("interconnect_config/get_i2_c_configuration", &InterconnectConfigServices::GetI2CConfiguration, this); - m_serviceSetI2CConfiguration = m_n.advertiseService("interconnect_config/set_i2_c_configuration", &InterconnectConfigServices::SetI2CConfiguration, this); - m_serviceI2CRead = m_n.advertiseService("interconnect_config/i2_c_read", &InterconnectConfigServices::I2CRead, this); - m_serviceI2CReadRegister = m_n.advertiseService("interconnect_config/i2_c_read_register", &InterconnectConfigServices::I2CReadRegister, this); - m_serviceI2CWrite = m_n.advertiseService("interconnect_config/i2_c_write", &InterconnectConfigServices::I2CWrite, this); - m_serviceI2CWriteRegister = m_n.advertiseService("interconnect_config/i2_c_write_register", &InterconnectConfigServices::I2CWriteRegister, this); + m_pub_Error = m_node_handle.advertise("kortex_error", 1000); + + m_serviceSetDeviceID = m_node_handle.advertiseService("interconnect_config/set_device_id", &InterconnectConfigRobotServices::SetDeviceID, this); + m_serviceSetApiOptions = m_node_handle.advertiseService("interconnect_config/set_api_options", &InterconnectConfigRobotServices::SetApiOptions, this); + + m_serviceGetUARTConfiguration = m_node_handle.advertiseService("interconnect_config/get_u_a_r_t_configuration", &InterconnectConfigRobotServices::GetUARTConfiguration, this); + m_serviceSetUARTConfiguration = m_node_handle.advertiseService("interconnect_config/set_u_a_r_t_configuration", &InterconnectConfigRobotServices::SetUARTConfiguration, this); + m_serviceGetEthernetConfiguration = m_node_handle.advertiseService("interconnect_config/get_ethernet_configuration", &InterconnectConfigRobotServices::GetEthernetConfiguration, this); + m_serviceSetEthernetConfiguration = m_node_handle.advertiseService("interconnect_config/set_ethernet_configuration", &InterconnectConfigRobotServices::SetEthernetConfiguration, this); + m_serviceGetGPIOConfiguration = m_node_handle.advertiseService("interconnect_config/get_g_p_i_o_configuration", &InterconnectConfigRobotServices::GetGPIOConfiguration, this); + m_serviceSetGPIOConfiguration = m_node_handle.advertiseService("interconnect_config/set_g_p_i_o_configuration", &InterconnectConfigRobotServices::SetGPIOConfiguration, this); + m_serviceGetGPIOState = m_node_handle.advertiseService("interconnect_config/get_g_p_i_o_state", &InterconnectConfigRobotServices::GetGPIOState, this); + m_serviceSetGPIOState = m_node_handle.advertiseService("interconnect_config/set_g_p_i_o_state", &InterconnectConfigRobotServices::SetGPIOState, this); + m_serviceGetI2CConfiguration = m_node_handle.advertiseService("interconnect_config/get_i2_c_configuration", &InterconnectConfigRobotServices::GetI2CConfiguration, this); + m_serviceSetI2CConfiguration = m_node_handle.advertiseService("interconnect_config/set_i2_c_configuration", &InterconnectConfigRobotServices::SetI2CConfiguration, this); + m_serviceI2CRead = m_node_handle.advertiseService("interconnect_config/i2_c_read", &InterconnectConfigRobotServices::I2CRead, this); + m_serviceI2CReadRegister = m_node_handle.advertiseService("interconnect_config/i2_c_read_register", &InterconnectConfigRobotServices::I2CReadRegister, this); + m_serviceI2CWrite = m_node_handle.advertiseService("interconnect_config/i2_c_write", &InterconnectConfigRobotServices::I2CWrite, this); + m_serviceI2CWriteRegister = m_node_handle.advertiseService("interconnect_config/i2_c_write_register", &InterconnectConfigRobotServices::I2CWriteRegister, this); } -bool InterconnectConfigServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) +bool InterconnectConfigRobotServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) { m_current_device_id = req.device_id; return true; } -bool InterconnectConfigServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) +bool InterconnectConfigRobotServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) { m_api_options.timeout_ms = req.input.timeout_ms; @@ -85,7 +85,7 @@ bool InterconnectConfigServices::SetApiOptions(kortex_driver::SetApiOptions::Req } -bool InterconnectConfigServices::GetUARTConfiguration(kortex_driver::GetUARTConfiguration::Request &req, kortex_driver::GetUARTConfiguration::Response &res) +bool InterconnectConfigRobotServices::GetUARTConfiguration(kortex_driver::GetUARTConfiguration::Request &req, kortex_driver::GetUARTConfiguration::Response &res) { Kinova::Api::Common::UARTDeviceIdentification input; @@ -120,7 +120,7 @@ bool InterconnectConfigServices::GetUARTConfiguration(kortex_driver::GetUARTConf return true; } -bool InterconnectConfigServices::SetUARTConfiguration(kortex_driver::SetUARTConfiguration::Request &req, kortex_driver::SetUARTConfiguration::Response &res) +bool InterconnectConfigRobotServices::SetUARTConfiguration(kortex_driver::SetUARTConfiguration::Request &req, kortex_driver::SetUARTConfiguration::Response &res) { Kinova::Api::Common::UARTConfiguration input; @@ -152,7 +152,7 @@ bool InterconnectConfigServices::SetUARTConfiguration(kortex_driver::SetUARTConf return true; } -bool InterconnectConfigServices::GetEthernetConfiguration(kortex_driver::GetEthernetConfiguration::Request &req, kortex_driver::GetEthernetConfiguration::Response &res) +bool InterconnectConfigRobotServices::GetEthernetConfiguration(kortex_driver::GetEthernetConfiguration::Request &req, kortex_driver::GetEthernetConfiguration::Response &res) { Kinova::Api::InterconnectConfig::EthernetDeviceIdentification input; @@ -187,7 +187,7 @@ bool InterconnectConfigServices::GetEthernetConfiguration(kortex_driver::GetEthe return true; } -bool InterconnectConfigServices::SetEthernetConfiguration(kortex_driver::SetEthernetConfiguration::Request &req, kortex_driver::SetEthernetConfiguration::Response &res) +bool InterconnectConfigRobotServices::SetEthernetConfiguration(kortex_driver::SetEthernetConfiguration::Request &req, kortex_driver::SetEthernetConfiguration::Response &res) { Kinova::Api::InterconnectConfig::EthernetConfiguration input; @@ -219,7 +219,7 @@ bool InterconnectConfigServices::SetEthernetConfiguration(kortex_driver::SetEthe return true; } -bool InterconnectConfigServices::GetGPIOConfiguration(kortex_driver::GetGPIOConfiguration::Request &req, kortex_driver::GetGPIOConfiguration::Response &res) +bool InterconnectConfigRobotServices::GetGPIOConfiguration(kortex_driver::GetGPIOConfiguration::Request &req, kortex_driver::GetGPIOConfiguration::Response &res) { Kinova::Api::InterconnectConfig::GPIOIdentification input; @@ -254,7 +254,7 @@ bool InterconnectConfigServices::GetGPIOConfiguration(kortex_driver::GetGPIOConf return true; } -bool InterconnectConfigServices::SetGPIOConfiguration(kortex_driver::SetGPIOConfiguration::Request &req, kortex_driver::SetGPIOConfiguration::Response &res) +bool InterconnectConfigRobotServices::SetGPIOConfiguration(kortex_driver::SetGPIOConfiguration::Request &req, kortex_driver::SetGPIOConfiguration::Response &res) { Kinova::Api::InterconnectConfig::GPIOConfiguration input; @@ -286,7 +286,7 @@ bool InterconnectConfigServices::SetGPIOConfiguration(kortex_driver::SetGPIOConf return true; } -bool InterconnectConfigServices::GetGPIOState(kortex_driver::GetGPIOState::Request &req, kortex_driver::GetGPIOState::Response &res) +bool InterconnectConfigRobotServices::GetGPIOState(kortex_driver::GetGPIOState::Request &req, kortex_driver::GetGPIOState::Response &res) { Kinova::Api::InterconnectConfig::GPIOIdentification input; @@ -321,7 +321,7 @@ bool InterconnectConfigServices::GetGPIOState(kortex_driver::GetGPIOState::Reque return true; } -bool InterconnectConfigServices::SetGPIOState(kortex_driver::SetGPIOState::Request &req, kortex_driver::SetGPIOState::Response &res) +bool InterconnectConfigRobotServices::SetGPIOState(kortex_driver::SetGPIOState::Request &req, kortex_driver::SetGPIOState::Response &res) { Kinova::Api::InterconnectConfig::GPIOState input; @@ -353,7 +353,7 @@ bool InterconnectConfigServices::SetGPIOState(kortex_driver::SetGPIOState::Reque return true; } -bool InterconnectConfigServices::GetI2CConfiguration(kortex_driver::GetI2CConfiguration::Request &req, kortex_driver::GetI2CConfiguration::Response &res) +bool InterconnectConfigRobotServices::GetI2CConfiguration(kortex_driver::GetI2CConfiguration::Request &req, kortex_driver::GetI2CConfiguration::Response &res) { Kinova::Api::InterconnectConfig::I2CDeviceIdentification input; @@ -388,7 +388,7 @@ bool InterconnectConfigServices::GetI2CConfiguration(kortex_driver::GetI2CConfig return true; } -bool InterconnectConfigServices::SetI2CConfiguration(kortex_driver::SetI2CConfiguration::Request &req, kortex_driver::SetI2CConfiguration::Response &res) +bool InterconnectConfigRobotServices::SetI2CConfiguration(kortex_driver::SetI2CConfiguration::Request &req, kortex_driver::SetI2CConfiguration::Response &res) { Kinova::Api::InterconnectConfig::I2CConfiguration input; @@ -420,7 +420,7 @@ bool InterconnectConfigServices::SetI2CConfiguration(kortex_driver::SetI2CConfig return true; } -bool InterconnectConfigServices::I2CRead(kortex_driver::I2CRead::Request &req, kortex_driver::I2CRead::Response &res) +bool InterconnectConfigRobotServices::I2CRead(kortex_driver::I2CRead::Request &req, kortex_driver::I2CRead::Response &res) { Kinova::Api::InterconnectConfig::I2CReadParameter input; @@ -455,7 +455,7 @@ bool InterconnectConfigServices::I2CRead(kortex_driver::I2CRead::Request &req, return true; } -bool InterconnectConfigServices::I2CReadRegister(kortex_driver::I2CReadRegister::Request &req, kortex_driver::I2CReadRegister::Response &res) +bool InterconnectConfigRobotServices::I2CReadRegister(kortex_driver::I2CReadRegister::Request &req, kortex_driver::I2CReadRegister::Response &res) { Kinova::Api::InterconnectConfig::I2CReadRegisterParameter input; @@ -490,7 +490,7 @@ bool InterconnectConfigServices::I2CReadRegister(kortex_driver::I2CReadRegister: return true; } -bool InterconnectConfigServices::I2CWrite(kortex_driver::I2CWrite::Request &req, kortex_driver::I2CWrite::Response &res) +bool InterconnectConfigRobotServices::I2CWrite(kortex_driver::I2CWrite::Request &req, kortex_driver::I2CWrite::Response &res) { Kinova::Api::InterconnectConfig::I2CWriteParameter input; @@ -522,7 +522,7 @@ bool InterconnectConfigServices::I2CWrite(kortex_driver::I2CWrite::Request &req return true; } -bool InterconnectConfigServices::I2CWriteRegister(kortex_driver::I2CWriteRegister::Request &req, kortex_driver::I2CWriteRegister::Response &res) +bool InterconnectConfigRobotServices::I2CWriteRegister(kortex_driver::I2CWriteRegister::Request &req, kortex_driver::I2CWriteRegister::Response &res) { Kinova::Api::InterconnectConfig::I2CWriteRegisterParameter input; diff --git a/kortex_driver/src/generated/interconnectcyclic_proto_converter.cpp b/kortex_driver/src/generated/robot/interconnectcyclic_proto_converter.cpp similarity index 97% rename from kortex_driver/src/generated/interconnectcyclic_proto_converter.cpp rename to kortex_driver/src/generated/robot/interconnectcyclic_proto_converter.cpp index 2698cc16..a2aef974 100644 --- a/kortex_driver/src/generated/interconnectcyclic_proto_converter.cpp +++ b/kortex_driver/src/generated/robot/interconnectcyclic_proto_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" int ToProtoData(kortex_driver::InterconnectCyclic_MessageId input, Kinova::Api::InterconnectCyclic::MessageId *output) { diff --git a/kortex_driver/src/generated/interconnectcyclic_ros_converter.cpp b/kortex_driver/src/generated/robot/interconnectcyclic_ros_converter.cpp similarity index 97% rename from kortex_driver/src/generated/interconnectcyclic_ros_converter.cpp rename to kortex_driver/src/generated/robot/interconnectcyclic_ros_converter.cpp index 46688c53..bdda1db3 100644 --- a/kortex_driver/src/generated/interconnectcyclic_ros_converter.cpp +++ b/kortex_driver/src/generated/robot/interconnectcyclic_ros_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" int ToRosData(Kinova::Api::InterconnectCyclic::MessageId input, kortex_driver::InterconnectCyclic_MessageId &output) { diff --git a/kortex_driver/src/generated/productconfiguration_proto_converter.cpp b/kortex_driver/src/generated/robot/productconfiguration_proto_converter.cpp similarity index 95% rename from kortex_driver/src/generated/productconfiguration_proto_converter.cpp rename to kortex_driver/src/generated/robot/productconfiguration_proto_converter.cpp index 7fd28374..c93d9b8c 100644 --- a/kortex_driver/src/generated/productconfiguration_proto_converter.cpp +++ b/kortex_driver/src/generated/robot/productconfiguration_proto_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" int ToProtoData(kortex_driver::CompleteProductConfiguration input, Kinova::Api::ProductConfiguration::CompleteProductConfiguration *output) { diff --git a/kortex_driver/src/generated/productconfiguration_ros_converter.cpp b/kortex_driver/src/generated/robot/productconfiguration_ros_converter.cpp similarity index 94% rename from kortex_driver/src/generated/productconfiguration_ros_converter.cpp rename to kortex_driver/src/generated/robot/productconfiguration_ros_converter.cpp index 106fc819..4eeb2ad4 100644 --- a/kortex_driver/src/generated/productconfiguration_ros_converter.cpp +++ b/kortex_driver/src/generated/robot/productconfiguration_ros_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" int ToRosData(Kinova::Api::ProductConfiguration::CompleteProductConfiguration input, kortex_driver::CompleteProductConfiguration &output) { diff --git a/kortex_driver/src/generated/visionconfig_proto_converter.cpp b/kortex_driver/src/generated/robot/visionconfig_proto_converter.cpp similarity index 98% rename from kortex_driver/src/generated/visionconfig_proto_converter.cpp rename to kortex_driver/src/generated/robot/visionconfig_proto_converter.cpp index 233a91c4..39009bbf 100644 --- a/kortex_driver/src/generated/visionconfig_proto_converter.cpp +++ b/kortex_driver/src/generated/robot/visionconfig_proto_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" int ToProtoData(kortex_driver::SensorSettings input, Kinova::Api::VisionConfig::SensorSettings *output) { diff --git a/kortex_driver/src/generated/visionconfig_ros_converter.cpp b/kortex_driver/src/generated/robot/visionconfig_ros_converter.cpp similarity index 98% rename from kortex_driver/src/generated/visionconfig_ros_converter.cpp rename to kortex_driver/src/generated/robot/visionconfig_ros_converter.cpp index ff83444c..caee31c4 100644 --- a/kortex_driver/src/generated/visionconfig_ros_converter.cpp +++ b/kortex_driver/src/generated/robot/visionconfig_ros_converter.cpp @@ -14,7 +14,7 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/visionconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" int ToRosData(Kinova::Api::VisionConfig::SensorSettings input, kortex_driver::SensorSettings &output) { diff --git a/kortex_driver/src/generated/visionconfig_services.cpp b/kortex_driver/src/generated/robot/visionconfig_services.cpp similarity index 64% rename from kortex_driver/src/generated/visionconfig_services.cpp rename to kortex_driver/src/generated/robot/visionconfig_services.cpp index d5cba3a7..a128bfb7 100644 --- a/kortex_driver/src/generated/visionconfig_services.cpp +++ b/kortex_driver/src/generated/robot/visionconfig_services.cpp @@ -14,70 +14,70 @@ * This file has been auto-generated and should not be modified. */ -#include "kortex_driver/generated/common_proto_converter.h" -#include "kortex_driver/generated/common_ros_converter.h" -#include "kortex_driver/generated/actuatorconfig_proto_converter.h" -#include "kortex_driver/generated/actuatorconfig_ros_converter.h" -#include "kortex_driver/generated/actuatorcyclic_proto_converter.h" -#include "kortex_driver/generated/actuatorcyclic_ros_converter.h" -#include "kortex_driver/generated/productconfiguration_proto_converter.h" -#include "kortex_driver/generated/productconfiguration_ros_converter.h" -#include "kortex_driver/generated/base_proto_converter.h" -#include "kortex_driver/generated/base_ros_converter.h" -#include "kortex_driver/generated/grippercyclic_proto_converter.h" -#include "kortex_driver/generated/grippercyclic_ros_converter.h" -#include "kortex_driver/generated/interconnectcyclic_proto_converter.h" -#include "kortex_driver/generated/interconnectcyclic_ros_converter.h" -#include "kortex_driver/generated/basecyclic_proto_converter.h" -#include "kortex_driver/generated/basecyclic_ros_converter.h" -#include "kortex_driver/generated/controlconfig_proto_converter.h" -#include "kortex_driver/generated/controlconfig_ros_converter.h" -#include "kortex_driver/generated/deviceconfig_proto_converter.h" -#include "kortex_driver/generated/deviceconfig_ros_converter.h" -#include "kortex_driver/generated/devicemanager_proto_converter.h" -#include "kortex_driver/generated/devicemanager_ros_converter.h" -#include "kortex_driver/generated/interconnectconfig_proto_converter.h" -#include "kortex_driver/generated/interconnectconfig_ros_converter.h" -#include "kortex_driver/generated/visionconfig_proto_converter.h" -#include "kortex_driver/generated/visionconfig_ros_converter.h" -#include "kortex_driver/generated/visionconfig_services.h" - -VisionConfigServices::VisionConfigServices(ros::NodeHandle& n, Kinova::Api::VisionConfig::VisionConfigClient* visionconfig, uint32_t device_id, uint32_t timeout_ms): - m_n(n), +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_services.h" + +VisionConfigRobotServices::VisionConfigRobotServices(ros::NodeHandle& node_handle, Kinova::Api::VisionConfig::VisionConfigClient* visionconfig, uint32_t device_id, uint32_t timeout_ms): + IVisionConfigServices(node_handle), m_visionconfig(visionconfig), m_current_device_id(device_id) { m_api_options.timeout_ms = timeout_ms; - m_pub_Error = m_n.advertise("kortex_error", 1000); - m_pub_VisionTopic = m_n.advertise("vision_topic", 1000); + m_pub_Error = m_node_handle.advertise("kortex_error", 1000); + m_pub_VisionTopic = m_node_handle.advertise("vision_topic", 1000); m_is_activated_VisionTopic = false; - m_serviceSetDeviceID = n.advertiseService("vision_config/set_device_id", &VisionConfigServices::SetDeviceID, this); - m_serviceSetApiOptions = n.advertiseService("vision_config/set_api_options", &VisionConfigServices::SetApiOptions, this); - - m_serviceSetSensorSettings = m_n.advertiseService("vision_config/set_sensor_settings", &VisionConfigServices::SetSensorSettings, this); - m_serviceGetSensorSettings = m_n.advertiseService("vision_config/get_sensor_settings", &VisionConfigServices::GetSensorSettings, this); - m_serviceGetOptionValue = m_n.advertiseService("vision_config/get_option_value", &VisionConfigServices::GetOptionValue, this); - m_serviceSetOptionValue = m_n.advertiseService("vision_config/set_option_value", &VisionConfigServices::SetOptionValue, this); - m_serviceGetOptionInformation = m_n.advertiseService("vision_config/get_option_information", &VisionConfigServices::GetOptionInformation, this); - m_serviceOnNotificationVisionTopic = m_n.advertiseService("vision_config/activate_publishing_of_vision_topic", &VisionConfigServices::OnNotificationVisionTopic, this); - m_serviceDoSensorFocusAction = m_n.advertiseService("vision_config/do_sensor_focus_action", &VisionConfigServices::DoSensorFocusAction, this); - m_serviceGetIntrinsicParameters = m_n.advertiseService("vision_config/get_intrinsic_parameters", &VisionConfigServices::GetIntrinsicParameters, this); - m_serviceGetIntrinsicParametersProfile = m_n.advertiseService("vision_config/get_intrinsic_parameters_profile", &VisionConfigServices::GetIntrinsicParametersProfile, this); - m_serviceSetIntrinsicParameters = m_n.advertiseService("vision_config/set_intrinsic_parameters", &VisionConfigServices::SetIntrinsicParameters, this); - m_serviceGetExtrinsicParameters = m_n.advertiseService("vision_config/get_extrinsic_parameters", &VisionConfigServices::GetExtrinsicParameters, this); - m_serviceSetExtrinsicParameters = m_n.advertiseService("vision_config/set_extrinsic_parameters", &VisionConfigServices::SetExtrinsicParameters, this); + m_serviceSetDeviceID = m_node_handle.advertiseService("vision_config/set_device_id", &VisionConfigRobotServices::SetDeviceID, this); + m_serviceSetApiOptions = m_node_handle.advertiseService("vision_config/set_api_options", &VisionConfigRobotServices::SetApiOptions, this); + + m_serviceSetSensorSettings = m_node_handle.advertiseService("vision_config/set_sensor_settings", &VisionConfigRobotServices::SetSensorSettings, this); + m_serviceGetSensorSettings = m_node_handle.advertiseService("vision_config/get_sensor_settings", &VisionConfigRobotServices::GetSensorSettings, this); + m_serviceGetOptionValue = m_node_handle.advertiseService("vision_config/get_option_value", &VisionConfigRobotServices::GetOptionValue, this); + m_serviceSetOptionValue = m_node_handle.advertiseService("vision_config/set_option_value", &VisionConfigRobotServices::SetOptionValue, this); + m_serviceGetOptionInformation = m_node_handle.advertiseService("vision_config/get_option_information", &VisionConfigRobotServices::GetOptionInformation, this); + m_serviceOnNotificationVisionTopic = m_node_handle.advertiseService("vision_config/activate_publishing_of_vision_topic", &VisionConfigRobotServices::OnNotificationVisionTopic, this); + m_serviceDoSensorFocusAction = m_node_handle.advertiseService("vision_config/do_sensor_focus_action", &VisionConfigRobotServices::DoSensorFocusAction, this); + m_serviceGetIntrinsicParameters = m_node_handle.advertiseService("vision_config/get_intrinsic_parameters", &VisionConfigRobotServices::GetIntrinsicParameters, this); + m_serviceGetIntrinsicParametersProfile = m_node_handle.advertiseService("vision_config/get_intrinsic_parameters_profile", &VisionConfigRobotServices::GetIntrinsicParametersProfile, this); + m_serviceSetIntrinsicParameters = m_node_handle.advertiseService("vision_config/set_intrinsic_parameters", &VisionConfigRobotServices::SetIntrinsicParameters, this); + m_serviceGetExtrinsicParameters = m_node_handle.advertiseService("vision_config/get_extrinsic_parameters", &VisionConfigRobotServices::GetExtrinsicParameters, this); + m_serviceSetExtrinsicParameters = m_node_handle.advertiseService("vision_config/set_extrinsic_parameters", &VisionConfigRobotServices::SetExtrinsicParameters, this); } -bool VisionConfigServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) +bool VisionConfigRobotServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) { m_current_device_id = req.device_id; return true; } -bool VisionConfigServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) +bool VisionConfigRobotServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) { m_api_options.timeout_ms = req.input.timeout_ms; @@ -85,7 +85,7 @@ bool VisionConfigServices::SetApiOptions(kortex_driver::SetApiOptions::Request } -bool VisionConfigServices::SetSensorSettings(kortex_driver::SetSensorSettings::Request &req, kortex_driver::SetSensorSettings::Response &res) +bool VisionConfigRobotServices::SetSensorSettings(kortex_driver::SetSensorSettings::Request &req, kortex_driver::SetSensorSettings::Response &res) { Kinova::Api::VisionConfig::SensorSettings input; @@ -117,7 +117,7 @@ bool VisionConfigServices::SetSensorSettings(kortex_driver::SetSensorSettings::R return true; } -bool VisionConfigServices::GetSensorSettings(kortex_driver::GetSensorSettings::Request &req, kortex_driver::GetSensorSettings::Response &res) +bool VisionConfigRobotServices::GetSensorSettings(kortex_driver::GetSensorSettings::Request &req, kortex_driver::GetSensorSettings::Response &res) { Kinova::Api::VisionConfig::SensorIdentifier input; @@ -152,7 +152,7 @@ bool VisionConfigServices::GetSensorSettings(kortex_driver::GetSensorSettings::R return true; } -bool VisionConfigServices::GetOptionValue(kortex_driver::GetOptionValue::Request &req, kortex_driver::GetOptionValue::Response &res) +bool VisionConfigRobotServices::GetOptionValue(kortex_driver::GetOptionValue::Request &req, kortex_driver::GetOptionValue::Response &res) { Kinova::Api::VisionConfig::OptionIdentifier input; @@ -187,7 +187,7 @@ bool VisionConfigServices::GetOptionValue(kortex_driver::GetOptionValue::Request return true; } -bool VisionConfigServices::SetOptionValue(kortex_driver::SetOptionValue::Request &req, kortex_driver::SetOptionValue::Response &res) +bool VisionConfigRobotServices::SetOptionValue(kortex_driver::SetOptionValue::Request &req, kortex_driver::SetOptionValue::Response &res) { Kinova::Api::VisionConfig::OptionValue input; @@ -219,7 +219,7 @@ bool VisionConfigServices::SetOptionValue(kortex_driver::SetOptionValue::Request return true; } -bool VisionConfigServices::GetOptionInformation(kortex_driver::GetOptionInformation::Request &req, kortex_driver::GetOptionInformation::Response &res) +bool VisionConfigRobotServices::GetOptionInformation(kortex_driver::GetOptionInformation::Request &req, kortex_driver::GetOptionInformation::Response &res) { Kinova::Api::VisionConfig::OptionIdentifier input; @@ -254,7 +254,7 @@ bool VisionConfigServices::GetOptionInformation(kortex_driver::GetOptionInformat return true; } -bool VisionConfigServices::OnNotificationVisionTopic(kortex_driver::OnNotificationVisionTopic::Request &req, kortex_driver::OnNotificationVisionTopic::Response &res) +bool VisionConfigRobotServices::OnNotificationVisionTopic(kortex_driver::OnNotificationVisionTopic::Request &req, kortex_driver::OnNotificationVisionTopic::Response &res) { // If the notification is already activated, don't activate multiple times @@ -268,7 +268,7 @@ bool VisionConfigServices::OnNotificationVisionTopic(kortex_driver::OnNotificati try { - std::function< void (Kinova::Api::VisionConfig::VisionNotification) > callback = std::bind(&VisionConfigServices::cb_VisionTopic, this, std::placeholders::_1); + std::function< void (Kinova::Api::VisionConfig::VisionNotification) > callback = std::bind(&VisionConfigRobotServices::cb_VisionTopic, this, std::placeholders::_1); output = m_visionconfig->OnNotificationVisionTopic(callback, input, m_current_device_id); m_is_activated_VisionTopic = true; } @@ -293,14 +293,14 @@ bool VisionConfigServices::OnNotificationVisionTopic(kortex_driver::OnNotificati ToRosData(output, res.output); return true; } -void VisionConfigServices::cb_VisionTopic(Kinova::Api::VisionConfig::VisionNotification notif) +void VisionConfigRobotServices::cb_VisionTopic(Kinova::Api::VisionConfig::VisionNotification notif) { kortex_driver::VisionNotification ros_msg; ToRosData(notif, ros_msg); m_pub_VisionTopic.publish(ros_msg); } -bool VisionConfigServices::DoSensorFocusAction(kortex_driver::DoSensorFocusAction::Request &req, kortex_driver::DoSensorFocusAction::Response &res) +bool VisionConfigRobotServices::DoSensorFocusAction(kortex_driver::DoSensorFocusAction::Request &req, kortex_driver::DoSensorFocusAction::Response &res) { Kinova::Api::VisionConfig::SensorFocusAction input; @@ -332,7 +332,7 @@ bool VisionConfigServices::DoSensorFocusAction(kortex_driver::DoSensorFocusActio return true; } -bool VisionConfigServices::GetIntrinsicParameters(kortex_driver::GetIntrinsicParameters::Request &req, kortex_driver::GetIntrinsicParameters::Response &res) +bool VisionConfigRobotServices::GetIntrinsicParameters(kortex_driver::GetIntrinsicParameters::Request &req, kortex_driver::GetIntrinsicParameters::Response &res) { Kinova::Api::VisionConfig::SensorIdentifier input; @@ -367,7 +367,7 @@ bool VisionConfigServices::GetIntrinsicParameters(kortex_driver::GetIntrinsicPar return true; } -bool VisionConfigServices::GetIntrinsicParametersProfile(kortex_driver::GetIntrinsicParametersProfile::Request &req, kortex_driver::GetIntrinsicParametersProfile::Response &res) +bool VisionConfigRobotServices::GetIntrinsicParametersProfile(kortex_driver::GetIntrinsicParametersProfile::Request &req, kortex_driver::GetIntrinsicParametersProfile::Response &res) { Kinova::Api::VisionConfig::IntrinsicProfileIdentifier input; @@ -402,7 +402,7 @@ bool VisionConfigServices::GetIntrinsicParametersProfile(kortex_driver::GetIntri return true; } -bool VisionConfigServices::SetIntrinsicParameters(kortex_driver::SetIntrinsicParameters::Request &req, kortex_driver::SetIntrinsicParameters::Response &res) +bool VisionConfigRobotServices::SetIntrinsicParameters(kortex_driver::SetIntrinsicParameters::Request &req, kortex_driver::SetIntrinsicParameters::Response &res) { Kinova::Api::VisionConfig::IntrinsicParameters input; @@ -434,7 +434,7 @@ bool VisionConfigServices::SetIntrinsicParameters(kortex_driver::SetIntrinsicPar return true; } -bool VisionConfigServices::GetExtrinsicParameters(kortex_driver::GetExtrinsicParameters::Request &req, kortex_driver::GetExtrinsicParameters::Response &res) +bool VisionConfigRobotServices::GetExtrinsicParameters(kortex_driver::GetExtrinsicParameters::Request &req, kortex_driver::GetExtrinsicParameters::Response &res) { Kinova::Api::VisionConfig::ExtrinsicParameters output; @@ -467,7 +467,7 @@ bool VisionConfigServices::GetExtrinsicParameters(kortex_driver::GetExtrinsicPar return true; } -bool VisionConfigServices::SetExtrinsicParameters(kortex_driver::SetExtrinsicParameters::Request &req, kortex_driver::SetExtrinsicParameters::Response &res) +bool VisionConfigRobotServices::SetExtrinsicParameters(kortex_driver::SetExtrinsicParameters::Request &req, kortex_driver::SetExtrinsicParameters::Response &res) { Kinova::Api::VisionConfig::ExtrinsicParameters input; diff --git a/kortex_driver/src/generated/simulation/actuatorconfig_services.cpp b/kortex_driver/src/generated/simulation/actuatorconfig_services.cpp new file mode 100644 index 00000000..224c14b5 --- /dev/null +++ b/kortex_driver/src/generated/simulation/actuatorconfig_services.cpp @@ -0,0 +1,386 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" +#include "kortex_driver/generated/simulation/actuatorconfig_services.h" + +ActuatorConfigSimulationServices::ActuatorConfigSimulationServices(ros::NodeHandle& node_handle): + IActuatorConfigServices(node_handle) +{ + m_pub_Error = m_node_handle.advertise("kortex_error", 1000); + + m_serviceSetDeviceID = m_node_handle.advertiseService("actuator_config/set_device_id", &ActuatorConfigSimulationServices::SetDeviceID, this); + m_serviceSetApiOptions = m_node_handle.advertiseService("actuator_config/set_api_options", &ActuatorConfigSimulationServices::SetApiOptions, this); + + m_serviceGetAxisOffsets = m_node_handle.advertiseService("actuator_config/get_axis_offsets", &ActuatorConfigSimulationServices::GetAxisOffsets, this); + m_serviceSetAxisOffsets = m_node_handle.advertiseService("actuator_config/set_axis_offsets", &ActuatorConfigSimulationServices::SetAxisOffsets, this); + m_serviceSetTorqueOffset = m_node_handle.advertiseService("actuator_config/set_torque_offset", &ActuatorConfigSimulationServices::SetTorqueOffset, this); + m_serviceActuatorConfig_GetControlMode = m_node_handle.advertiseService("actuator_config/get_control_mode", &ActuatorConfigSimulationServices::ActuatorConfig_GetControlMode, this); + m_serviceSetControlMode = m_node_handle.advertiseService("actuator_config/set_control_mode", &ActuatorConfigSimulationServices::SetControlMode, this); + m_serviceGetActivatedControlLoop = m_node_handle.advertiseService("actuator_config/get_activated_control_loop", &ActuatorConfigSimulationServices::GetActivatedControlLoop, this); + m_serviceSetActivatedControlLoop = m_node_handle.advertiseService("actuator_config/set_activated_control_loop", &ActuatorConfigSimulationServices::SetActivatedControlLoop, this); + m_serviceGetControlLoopParameters = m_node_handle.advertiseService("actuator_config/get_control_loop_parameters", &ActuatorConfigSimulationServices::GetControlLoopParameters, this); + m_serviceSetControlLoopParameters = m_node_handle.advertiseService("actuator_config/set_control_loop_parameters", &ActuatorConfigSimulationServices::SetControlLoopParameters, this); + m_serviceSelectCustomData = m_node_handle.advertiseService("actuator_config/select_custom_data", &ActuatorConfigSimulationServices::SelectCustomData, this); + m_serviceGetSelectedCustomData = m_node_handle.advertiseService("actuator_config/get_selected_custom_data", &ActuatorConfigSimulationServices::GetSelectedCustomData, this); + m_serviceSetCommandMode = m_node_handle.advertiseService("actuator_config/set_command_mode", &ActuatorConfigSimulationServices::SetCommandMode, this); + m_serviceActuatorConfig_ClearFaults = m_node_handle.advertiseService("actuator_config/clear_faults", &ActuatorConfigSimulationServices::ActuatorConfig_ClearFaults, this); + m_serviceSetServoing = m_node_handle.advertiseService("actuator_config/set_servoing", &ActuatorConfigSimulationServices::SetServoing, this); + m_serviceMoveToPosition = m_node_handle.advertiseService("actuator_config/move_to_position", &ActuatorConfigSimulationServices::MoveToPosition, this); + m_serviceGetCommandMode = m_node_handle.advertiseService("actuator_config/get_command_mode", &ActuatorConfigSimulationServices::GetCommandMode, this); + m_serviceGetServoing = m_node_handle.advertiseService("actuator_config/get_servoing", &ActuatorConfigSimulationServices::GetServoing, this); + m_serviceGetTorqueOffset = m_node_handle.advertiseService("actuator_config/get_torque_offset", &ActuatorConfigSimulationServices::GetTorqueOffset, this); + m_serviceSetCoggingFeedforwardMode = m_node_handle.advertiseService("actuator_config/set_cogging_feedforward_mode", &ActuatorConfigSimulationServices::SetCoggingFeedforwardMode, this); + m_serviceGetCoggingFeedforwardMode = m_node_handle.advertiseService("actuator_config/get_cogging_feedforward_mode", &ActuatorConfigSimulationServices::GetCoggingFeedforwardMode, this); +} + +bool ActuatorConfigSimulationServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) +{ + ROS_WARN_ONCE("The SetDeviceID service is not implemented in simulation, and has no effect."); + return true; +} + +bool ActuatorConfigSimulationServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) +{ + ROS_WARN_ONCE("The SetDeviceID service is not implemented in simulation, and has no effect."); + return true; +} + + +bool ActuatorConfigSimulationServices::GetAxisOffsets(kortex_driver::GetAxisOffsets::Request &req, kortex_driver::GetAxisOffsets::Response &res) +{ + + + if (GetAxisOffsetsHandler) + { + res = GetAxisOffsetsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for actuator_config/get_axis_offsets is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ActuatorConfigSimulationServices::SetAxisOffsets(kortex_driver::SetAxisOffsets::Request &req, kortex_driver::SetAxisOffsets::Response &res) +{ + + + if (SetAxisOffsetsHandler) + { + res = SetAxisOffsetsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for actuator_config/set_axis_offsets is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ActuatorConfigSimulationServices::SetTorqueOffset(kortex_driver::SetTorqueOffset::Request &req, kortex_driver::SetTorqueOffset::Response &res) +{ + + + if (SetTorqueOffsetHandler) + { + res = SetTorqueOffsetHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for actuator_config/set_torque_offset is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ActuatorConfigSimulationServices::ActuatorConfig_GetControlMode(kortex_driver::ActuatorConfig_GetControlMode::Request &req, kortex_driver::ActuatorConfig_GetControlMode::Response &res) +{ + + + if (ActuatorConfig_GetControlModeHandler) + { + res = ActuatorConfig_GetControlModeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for actuator_config/get_control_mode is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ActuatorConfigSimulationServices::SetControlMode(kortex_driver::SetControlMode::Request &req, kortex_driver::SetControlMode::Response &res) +{ + + + if (SetControlModeHandler) + { + res = SetControlModeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for actuator_config/set_control_mode is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ActuatorConfigSimulationServices::GetActivatedControlLoop(kortex_driver::GetActivatedControlLoop::Request &req, kortex_driver::GetActivatedControlLoop::Response &res) +{ + + + if (GetActivatedControlLoopHandler) + { + res = GetActivatedControlLoopHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for actuator_config/get_activated_control_loop is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ActuatorConfigSimulationServices::SetActivatedControlLoop(kortex_driver::SetActivatedControlLoop::Request &req, kortex_driver::SetActivatedControlLoop::Response &res) +{ + + + if (SetActivatedControlLoopHandler) + { + res = SetActivatedControlLoopHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for actuator_config/set_activated_control_loop is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ActuatorConfigSimulationServices::GetControlLoopParameters(kortex_driver::GetControlLoopParameters::Request &req, kortex_driver::GetControlLoopParameters::Response &res) +{ + + + if (GetControlLoopParametersHandler) + { + res = GetControlLoopParametersHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for actuator_config/get_control_loop_parameters is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ActuatorConfigSimulationServices::SetControlLoopParameters(kortex_driver::SetControlLoopParameters::Request &req, kortex_driver::SetControlLoopParameters::Response &res) +{ + + + if (SetControlLoopParametersHandler) + { + res = SetControlLoopParametersHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for actuator_config/set_control_loop_parameters is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ActuatorConfigSimulationServices::SelectCustomData(kortex_driver::SelectCustomData::Request &req, kortex_driver::SelectCustomData::Response &res) +{ + + + if (SelectCustomDataHandler) + { + res = SelectCustomDataHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for actuator_config/select_custom_data is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ActuatorConfigSimulationServices::GetSelectedCustomData(kortex_driver::GetSelectedCustomData::Request &req, kortex_driver::GetSelectedCustomData::Response &res) +{ + + + if (GetSelectedCustomDataHandler) + { + res = GetSelectedCustomDataHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for actuator_config/get_selected_custom_data is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ActuatorConfigSimulationServices::SetCommandMode(kortex_driver::SetCommandMode::Request &req, kortex_driver::SetCommandMode::Response &res) +{ + + + if (SetCommandModeHandler) + { + res = SetCommandModeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for actuator_config/set_command_mode is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ActuatorConfigSimulationServices::ActuatorConfig_ClearFaults(kortex_driver::ActuatorConfig_ClearFaults::Request &req, kortex_driver::ActuatorConfig_ClearFaults::Response &res) +{ + + + if (ActuatorConfig_ClearFaultsHandler) + { + res = ActuatorConfig_ClearFaultsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for actuator_config/clear_faults is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ActuatorConfigSimulationServices::SetServoing(kortex_driver::SetServoing::Request &req, kortex_driver::SetServoing::Response &res) +{ + + + if (SetServoingHandler) + { + res = SetServoingHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for actuator_config/set_servoing is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ActuatorConfigSimulationServices::MoveToPosition(kortex_driver::MoveToPosition::Request &req, kortex_driver::MoveToPosition::Response &res) +{ + + + if (MoveToPositionHandler) + { + res = MoveToPositionHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for actuator_config/move_to_position is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ActuatorConfigSimulationServices::GetCommandMode(kortex_driver::GetCommandMode::Request &req, kortex_driver::GetCommandMode::Response &res) +{ + + + if (GetCommandModeHandler) + { + res = GetCommandModeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for actuator_config/get_command_mode is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ActuatorConfigSimulationServices::GetServoing(kortex_driver::GetServoing::Request &req, kortex_driver::GetServoing::Response &res) +{ + + + if (GetServoingHandler) + { + res = GetServoingHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for actuator_config/get_servoing is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ActuatorConfigSimulationServices::GetTorqueOffset(kortex_driver::GetTorqueOffset::Request &req, kortex_driver::GetTorqueOffset::Response &res) +{ + + + if (GetTorqueOffsetHandler) + { + res = GetTorqueOffsetHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for actuator_config/get_torque_offset is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ActuatorConfigSimulationServices::SetCoggingFeedforwardMode(kortex_driver::SetCoggingFeedforwardMode::Request &req, kortex_driver::SetCoggingFeedforwardMode::Response &res) +{ + + + if (SetCoggingFeedforwardModeHandler) + { + res = SetCoggingFeedforwardModeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for actuator_config/set_cogging_feedforward_mode is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ActuatorConfigSimulationServices::GetCoggingFeedforwardMode(kortex_driver::GetCoggingFeedforwardMode::Request &req, kortex_driver::GetCoggingFeedforwardMode::Response &res) +{ + + + if (GetCoggingFeedforwardModeHandler) + { + res = GetCoggingFeedforwardModeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for actuator_config/get_cogging_feedforward_mode is not implemented, so the service calls will return the default response."); + } + return true; +} diff --git a/kortex_driver/src/generated/simulation/base_services.cpp b/kortex_driver/src/generated/simulation/base_services.cpp new file mode 100644 index 00000000..a8e3589f --- /dev/null +++ b/kortex_driver/src/generated/simulation/base_services.cpp @@ -0,0 +1,2504 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" +#include "kortex_driver/generated/simulation/base_services.h" + +BaseSimulationServices::BaseSimulationServices(ros::NodeHandle& node_handle): + IBaseServices(node_handle) +{ + m_pub_Error = m_node_handle.advertise("kortex_error", 1000); + m_pub_ConfigurationChangeTopic = m_node_handle.advertise("configuration_change_topic", 1000); + m_is_activated_ConfigurationChangeTopic = false; + m_pub_MappingInfoTopic = m_node_handle.advertise("mapping_info_topic", 1000); + m_is_activated_MappingInfoTopic = false; + m_pub_ControlModeTopic = m_node_handle.advertise("control_mode_topic", 1000); + m_is_activated_ControlModeTopic = false; + m_pub_OperatingModeTopic = m_node_handle.advertise("operating_mode_topic", 1000); + m_is_activated_OperatingModeTopic = false; + m_pub_SequenceInfoTopic = m_node_handle.advertise("sequence_info_topic", 1000); + m_is_activated_SequenceInfoTopic = false; + m_pub_ProtectionZoneTopic = m_node_handle.advertise("protection_zone_topic", 1000); + m_is_activated_ProtectionZoneTopic = false; + m_pub_UserTopic = m_node_handle.advertise("user_topic", 1000); + m_is_activated_UserTopic = false; + m_pub_ControllerTopic = m_node_handle.advertise("controller_topic", 1000); + m_is_activated_ControllerTopic = false; + m_pub_ActionTopic = m_node_handle.advertise("action_topic", 1000); + m_is_activated_ActionTopic = false; + m_pub_RobotEventTopic = m_node_handle.advertise("robot_event_topic", 1000); + m_is_activated_RobotEventTopic = false; + m_pub_ServoingModeTopic = m_node_handle.advertise("servoing_mode_topic", 1000); + m_is_activated_ServoingModeTopic = false; + m_pub_FactoryTopic = m_node_handle.advertise("factory_topic", 1000); + m_is_activated_FactoryTopic = false; + m_pub_NetworkTopic = m_node_handle.advertise("network_topic", 1000); + m_is_activated_NetworkTopic = false; + m_pub_ArmStateTopic = m_node_handle.advertise("arm_state_topic", 1000); + m_is_activated_ArmStateTopic = false; + + m_serviceSetDeviceID = m_node_handle.advertiseService("base/set_device_id", &BaseSimulationServices::SetDeviceID, this); + m_serviceSetApiOptions = m_node_handle.advertiseService("base/set_api_options", &BaseSimulationServices::SetApiOptions, this); + + m_serviceCreateUserProfile = m_node_handle.advertiseService("base/create_user_profile", &BaseSimulationServices::CreateUserProfile, this); + m_serviceUpdateUserProfile = m_node_handle.advertiseService("base/update_user_profile", &BaseSimulationServices::UpdateUserProfile, this); + m_serviceReadUserProfile = m_node_handle.advertiseService("base/read_user_profile", &BaseSimulationServices::ReadUserProfile, this); + m_serviceDeleteUserProfile = m_node_handle.advertiseService("base/delete_user_profile", &BaseSimulationServices::DeleteUserProfile, this); + m_serviceReadAllUserProfiles = m_node_handle.advertiseService("base/read_all_user_profiles", &BaseSimulationServices::ReadAllUserProfiles, this); + m_serviceReadAllUsers = m_node_handle.advertiseService("base/read_all_users", &BaseSimulationServices::ReadAllUsers, this); + m_serviceChangePassword = m_node_handle.advertiseService("base/change_password", &BaseSimulationServices::ChangePassword, this); + m_serviceCreateSequence = m_node_handle.advertiseService("base/create_sequence", &BaseSimulationServices::CreateSequence, this); + m_serviceUpdateSequence = m_node_handle.advertiseService("base/update_sequence", &BaseSimulationServices::UpdateSequence, this); + m_serviceReadSequence = m_node_handle.advertiseService("base/read_sequence", &BaseSimulationServices::ReadSequence, this); + m_serviceDeleteSequence = m_node_handle.advertiseService("base/delete_sequence", &BaseSimulationServices::DeleteSequence, this); + m_serviceReadAllSequences = m_node_handle.advertiseService("base/read_all_sequences", &BaseSimulationServices::ReadAllSequences, this); + m_servicePlaySequence = m_node_handle.advertiseService("base/play_sequence", &BaseSimulationServices::PlaySequence, this); + m_servicePlayAdvancedSequence = m_node_handle.advertiseService("base/play_advanced_sequence", &BaseSimulationServices::PlayAdvancedSequence, this); + m_serviceStopSequence = m_node_handle.advertiseService("base/stop_sequence", &BaseSimulationServices::StopSequence, this); + m_servicePauseSequence = m_node_handle.advertiseService("base/pause_sequence", &BaseSimulationServices::PauseSequence, this); + m_serviceResumeSequence = m_node_handle.advertiseService("base/resume_sequence", &BaseSimulationServices::ResumeSequence, this); + m_serviceCreateProtectionZone = m_node_handle.advertiseService("base/create_protection_zone", &BaseSimulationServices::CreateProtectionZone, this); + m_serviceUpdateProtectionZone = m_node_handle.advertiseService("base/update_protection_zone", &BaseSimulationServices::UpdateProtectionZone, this); + m_serviceReadProtectionZone = m_node_handle.advertiseService("base/read_protection_zone", &BaseSimulationServices::ReadProtectionZone, this); + m_serviceDeleteProtectionZone = m_node_handle.advertiseService("base/delete_protection_zone", &BaseSimulationServices::DeleteProtectionZone, this); + m_serviceReadAllProtectionZones = m_node_handle.advertiseService("base/read_all_protection_zones", &BaseSimulationServices::ReadAllProtectionZones, this); + m_serviceCreateMapping = m_node_handle.advertiseService("base/create_mapping", &BaseSimulationServices::CreateMapping, this); + m_serviceReadMapping = m_node_handle.advertiseService("base/read_mapping", &BaseSimulationServices::ReadMapping, this); + m_serviceUpdateMapping = m_node_handle.advertiseService("base/update_mapping", &BaseSimulationServices::UpdateMapping, this); + m_serviceDeleteMapping = m_node_handle.advertiseService("base/delete_mapping", &BaseSimulationServices::DeleteMapping, this); + m_serviceReadAllMappings = m_node_handle.advertiseService("base/read_all_mappings", &BaseSimulationServices::ReadAllMappings, this); + m_serviceCreateMap = m_node_handle.advertiseService("base/create_map", &BaseSimulationServices::CreateMap, this); + m_serviceReadMap = m_node_handle.advertiseService("base/read_map", &BaseSimulationServices::ReadMap, this); + m_serviceUpdateMap = m_node_handle.advertiseService("base/update_map", &BaseSimulationServices::UpdateMap, this); + m_serviceDeleteMap = m_node_handle.advertiseService("base/delete_map", &BaseSimulationServices::DeleteMap, this); + m_serviceReadAllMaps = m_node_handle.advertiseService("base/read_all_maps", &BaseSimulationServices::ReadAllMaps, this); + m_serviceActivateMap = m_node_handle.advertiseService("base/activate_map", &BaseSimulationServices::ActivateMap, this); + m_serviceCreateAction = m_node_handle.advertiseService("base/create_action", &BaseSimulationServices::CreateAction, this); + m_serviceReadAction = m_node_handle.advertiseService("base/read_action", &BaseSimulationServices::ReadAction, this); + m_serviceReadAllActions = m_node_handle.advertiseService("base/read_all_actions", &BaseSimulationServices::ReadAllActions, this); + m_serviceDeleteAction = m_node_handle.advertiseService("base/delete_action", &BaseSimulationServices::DeleteAction, this); + m_serviceUpdateAction = m_node_handle.advertiseService("base/update_action", &BaseSimulationServices::UpdateAction, this); + m_serviceExecuteActionFromReference = m_node_handle.advertiseService("base/execute_action_from_reference", &BaseSimulationServices::ExecuteActionFromReference, this); + m_serviceExecuteAction = m_node_handle.advertiseService("base/execute_action", &BaseSimulationServices::ExecuteAction, this); + m_servicePauseAction = m_node_handle.advertiseService("base/pause_action", &BaseSimulationServices::PauseAction, this); + m_serviceStopAction = m_node_handle.advertiseService("base/stop_action", &BaseSimulationServices::StopAction, this); + m_serviceResumeAction = m_node_handle.advertiseService("base/resume_action", &BaseSimulationServices::ResumeAction, this); + m_serviceGetIPv4Configuration = m_node_handle.advertiseService("base/get_i_pv4_configuration", &BaseSimulationServices::GetIPv4Configuration, this); + m_serviceSetIPv4Configuration = m_node_handle.advertiseService("base/set_i_pv4_configuration", &BaseSimulationServices::SetIPv4Configuration, this); + m_serviceSetCommunicationInterfaceEnable = m_node_handle.advertiseService("base/set_communication_interface_enable", &BaseSimulationServices::SetCommunicationInterfaceEnable, this); + m_serviceIsCommunicationInterfaceEnable = m_node_handle.advertiseService("base/is_communication_interface_enable", &BaseSimulationServices::IsCommunicationInterfaceEnable, this); + m_serviceGetAvailableWifi = m_node_handle.advertiseService("base/get_available_wifi", &BaseSimulationServices::GetAvailableWifi, this); + m_serviceGetWifiInformation = m_node_handle.advertiseService("base/get_wifi_information", &BaseSimulationServices::GetWifiInformation, this); + m_serviceAddWifiConfiguration = m_node_handle.advertiseService("base/add_wifi_configuration", &BaseSimulationServices::AddWifiConfiguration, this); + m_serviceDeleteWifiConfiguration = m_node_handle.advertiseService("base/delete_wifi_configuration", &BaseSimulationServices::DeleteWifiConfiguration, this); + m_serviceGetAllConfiguredWifis = m_node_handle.advertiseService("base/get_all_configured_wifis", &BaseSimulationServices::GetAllConfiguredWifis, this); + m_serviceConnectWifi = m_node_handle.advertiseService("base/connect_wifi", &BaseSimulationServices::ConnectWifi, this); + m_serviceDisconnectWifi = m_node_handle.advertiseService("base/disconnect_wifi", &BaseSimulationServices::DisconnectWifi, this); + m_serviceGetConnectedWifiInformation = m_node_handle.advertiseService("base/get_connected_wifi_information", &BaseSimulationServices::GetConnectedWifiInformation, this); + m_serviceBase_Unsubscribe = m_node_handle.advertiseService("base/unsubscribe", &BaseSimulationServices::Base_Unsubscribe, this); + m_serviceOnNotificationConfigurationChangeTopic = m_node_handle.advertiseService("base/activate_publishing_of_configuration_change_topic", &BaseSimulationServices::OnNotificationConfigurationChangeTopic, this); + m_serviceOnNotificationMappingInfoTopic = m_node_handle.advertiseService("base/activate_publishing_of_mapping_info_topic", &BaseSimulationServices::OnNotificationMappingInfoTopic, this); + m_serviceOnNotificationControlModeTopic = m_node_handle.advertiseService("base/activate_publishing_of_control_mode_topic", &BaseSimulationServices::OnNotificationControlModeTopic, this); + m_serviceOnNotificationOperatingModeTopic = m_node_handle.advertiseService("base/activate_publishing_of_operating_mode_topic", &BaseSimulationServices::OnNotificationOperatingModeTopic, this); + m_serviceOnNotificationSequenceInfoTopic = m_node_handle.advertiseService("base/activate_publishing_of_sequence_info_topic", &BaseSimulationServices::OnNotificationSequenceInfoTopic, this); + m_serviceOnNotificationProtectionZoneTopic = m_node_handle.advertiseService("base/activate_publishing_of_protection_zone_topic", &BaseSimulationServices::OnNotificationProtectionZoneTopic, this); + m_serviceOnNotificationUserTopic = m_node_handle.advertiseService("base/activate_publishing_of_user_topic", &BaseSimulationServices::OnNotificationUserTopic, this); + m_serviceOnNotificationControllerTopic = m_node_handle.advertiseService("base/activate_publishing_of_controller_topic", &BaseSimulationServices::OnNotificationControllerTopic, this); + m_serviceOnNotificationActionTopic = m_node_handle.advertiseService("base/activate_publishing_of_action_topic", &BaseSimulationServices::OnNotificationActionTopic, this); + m_serviceOnNotificationRobotEventTopic = m_node_handle.advertiseService("base/activate_publishing_of_robot_event_topic", &BaseSimulationServices::OnNotificationRobotEventTopic, this); + m_servicePlayCartesianTrajectory = m_node_handle.advertiseService("base/play_cartesian_trajectory", &BaseSimulationServices::PlayCartesianTrajectory, this); + m_servicePlayCartesianTrajectoryPosition = m_node_handle.advertiseService("base/play_cartesian_trajectory_position", &BaseSimulationServices::PlayCartesianTrajectoryPosition, this); + m_servicePlayCartesianTrajectoryOrientation = m_node_handle.advertiseService("base/play_cartesian_trajectory_orientation", &BaseSimulationServices::PlayCartesianTrajectoryOrientation, this); + m_serviceStop = m_node_handle.advertiseService("base/stop", &BaseSimulationServices::Stop, this); + m_serviceGetMeasuredCartesianPose = m_node_handle.advertiseService("base/get_measured_cartesian_pose", &BaseSimulationServices::GetMeasuredCartesianPose, this); + m_serviceSendWrenchCommand = m_node_handle.advertiseService("base/send_wrench_command", &BaseSimulationServices::SendWrenchCommand, this); + m_serviceSendWrenchJoystickCommand = m_node_handle.advertiseService("base/send_wrench_joystick_command", &BaseSimulationServices::SendWrenchJoystickCommand, this); + m_serviceSendTwistJoystickCommand = m_node_handle.advertiseService("base/send_twist_joystick_command", &BaseSimulationServices::SendTwistJoystickCommand, this); + m_serviceSendTwistCommand = m_node_handle.advertiseService("base/send_twist_command", &BaseSimulationServices::SendTwistCommand, this); + m_servicePlayJointTrajectory = m_node_handle.advertiseService("base/play_joint_trajectory", &BaseSimulationServices::PlayJointTrajectory, this); + m_servicePlaySelectedJointTrajectory = m_node_handle.advertiseService("base/play_selected_joint_trajectory", &BaseSimulationServices::PlaySelectedJointTrajectory, this); + m_serviceGetMeasuredJointAngles = m_node_handle.advertiseService("base/get_measured_joint_angles", &BaseSimulationServices::GetMeasuredJointAngles, this); + m_serviceSendJointSpeedsCommand = m_node_handle.advertiseService("base/send_joint_speeds_command", &BaseSimulationServices::SendJointSpeedsCommand, this); + m_serviceSendSelectedJointSpeedCommand = m_node_handle.advertiseService("base/send_selected_joint_speed_command", &BaseSimulationServices::SendSelectedJointSpeedCommand, this); + m_serviceSendGripperCommand = m_node_handle.advertiseService("base/send_gripper_command", &BaseSimulationServices::SendGripperCommand, this); + m_serviceGetMeasuredGripperMovement = m_node_handle.advertiseService("base/get_measured_gripper_movement", &BaseSimulationServices::GetMeasuredGripperMovement, this); + m_serviceSetAdmittance = m_node_handle.advertiseService("base/set_admittance", &BaseSimulationServices::SetAdmittance, this); + m_serviceSetOperatingMode = m_node_handle.advertiseService("base/set_operating_mode", &BaseSimulationServices::SetOperatingMode, this); + m_serviceApplyEmergencyStop = m_node_handle.advertiseService("base/apply_emergency_stop", &BaseSimulationServices::ApplyEmergencyStop, this); + m_serviceBase_ClearFaults = m_node_handle.advertiseService("base/clear_faults", &BaseSimulationServices::Base_ClearFaults, this); + m_serviceBase_GetControlMode = m_node_handle.advertiseService("base/get_control_mode", &BaseSimulationServices::Base_GetControlMode, this); + m_serviceGetOperatingMode = m_node_handle.advertiseService("base/get_operating_mode", &BaseSimulationServices::GetOperatingMode, this); + m_serviceSetServoingMode = m_node_handle.advertiseService("base/set_servoing_mode", &BaseSimulationServices::SetServoingMode, this); + m_serviceGetServoingMode = m_node_handle.advertiseService("base/get_servoing_mode", &BaseSimulationServices::GetServoingMode, this); + m_serviceOnNotificationServoingModeTopic = m_node_handle.advertiseService("base/activate_publishing_of_servoing_mode_topic", &BaseSimulationServices::OnNotificationServoingModeTopic, this); + m_serviceRestoreFactorySettings = m_node_handle.advertiseService("base/restore_factory_settings", &BaseSimulationServices::RestoreFactorySettings, this); + m_serviceOnNotificationFactoryTopic = m_node_handle.advertiseService("base/activate_publishing_of_factory_topic", &BaseSimulationServices::OnNotificationFactoryTopic, this); + m_serviceGetAllConnectedControllers = m_node_handle.advertiseService("base/get_all_connected_controllers", &BaseSimulationServices::GetAllConnectedControllers, this); + m_serviceGetControllerState = m_node_handle.advertiseService("base/get_controller_state", &BaseSimulationServices::GetControllerState, this); + m_serviceGetActuatorCount = m_node_handle.advertiseService("base/get_actuator_count", &BaseSimulationServices::GetActuatorCount, this); + m_serviceStartWifiScan = m_node_handle.advertiseService("base/start_wifi_scan", &BaseSimulationServices::StartWifiScan, this); + m_serviceGetConfiguredWifi = m_node_handle.advertiseService("base/get_configured_wifi", &BaseSimulationServices::GetConfiguredWifi, this); + m_serviceOnNotificationNetworkTopic = m_node_handle.advertiseService("base/activate_publishing_of_network_topic", &BaseSimulationServices::OnNotificationNetworkTopic, this); + m_serviceGetArmState = m_node_handle.advertiseService("base/get_arm_state", &BaseSimulationServices::GetArmState, this); + m_serviceOnNotificationArmStateTopic = m_node_handle.advertiseService("base/activate_publishing_of_arm_state_topic", &BaseSimulationServices::OnNotificationArmStateTopic, this); + m_serviceGetIPv4Information = m_node_handle.advertiseService("base/get_i_pv4_information", &BaseSimulationServices::GetIPv4Information, this); + m_serviceSetWifiCountryCode = m_node_handle.advertiseService("base/set_wifi_country_code", &BaseSimulationServices::SetWifiCountryCode, this); + m_serviceGetWifiCountryCode = m_node_handle.advertiseService("base/get_wifi_country_code", &BaseSimulationServices::GetWifiCountryCode, this); + m_serviceBase_SetCapSenseConfig = m_node_handle.advertiseService("base/set_cap_sense_config", &BaseSimulationServices::Base_SetCapSenseConfig, this); + m_serviceBase_GetCapSenseConfig = m_node_handle.advertiseService("base/get_cap_sense_config", &BaseSimulationServices::Base_GetCapSenseConfig, this); + m_serviceGetAllJointsSpeedHardLimitation = m_node_handle.advertiseService("base/get_all_joints_speed_hard_limitation", &BaseSimulationServices::GetAllJointsSpeedHardLimitation, this); + m_serviceGetAllJointsTorqueHardLimitation = m_node_handle.advertiseService("base/get_all_joints_torque_hard_limitation", &BaseSimulationServices::GetAllJointsTorqueHardLimitation, this); + m_serviceGetTwistHardLimitation = m_node_handle.advertiseService("base/get_twist_hard_limitation", &BaseSimulationServices::GetTwistHardLimitation, this); + m_serviceGetWrenchHardLimitation = m_node_handle.advertiseService("base/get_wrench_hard_limitation", &BaseSimulationServices::GetWrenchHardLimitation, this); + m_serviceSendJointSpeedsJoystickCommand = m_node_handle.advertiseService("base/send_joint_speeds_joystick_command", &BaseSimulationServices::SendJointSpeedsJoystickCommand, this); + m_serviceSendSelectedJointSpeedJoystickCommand = m_node_handle.advertiseService("base/send_selected_joint_speed_joystick_command", &BaseSimulationServices::SendSelectedJointSpeedJoystickCommand, this); + m_serviceEnableBridge = m_node_handle.advertiseService("base/enable_bridge", &BaseSimulationServices::EnableBridge, this); + m_serviceDisableBridge = m_node_handle.advertiseService("base/disable_bridge", &BaseSimulationServices::DisableBridge, this); + m_serviceGetBridgeList = m_node_handle.advertiseService("base/get_bridge_list", &BaseSimulationServices::GetBridgeList, this); + m_serviceGetBridgeConfig = m_node_handle.advertiseService("base/get_bridge_config", &BaseSimulationServices::GetBridgeConfig, this); + m_servicePlayPreComputedJointTrajectory = m_node_handle.advertiseService("base/play_pre_computed_joint_trajectory", &BaseSimulationServices::PlayPreComputedJointTrajectory, this); + m_serviceGetProductConfiguration = m_node_handle.advertiseService("base/get_product_configuration", &BaseSimulationServices::GetProductConfiguration, this); + m_serviceUpdateEndEffectorTypeConfiguration = m_node_handle.advertiseService("base/update_end_effector_type_configuration", &BaseSimulationServices::UpdateEndEffectorTypeConfiguration, this); + m_serviceRestoreFactoryProductConfiguration = m_node_handle.advertiseService("base/restore_factory_product_configuration", &BaseSimulationServices::RestoreFactoryProductConfiguration, this); + m_serviceGetTrajectoryErrorReport = m_node_handle.advertiseService("base/get_trajectory_error_report", &BaseSimulationServices::GetTrajectoryErrorReport, this); + m_serviceGetAllJointsSpeedSoftLimitation = m_node_handle.advertiseService("base/get_all_joints_speed_soft_limitation", &BaseSimulationServices::GetAllJointsSpeedSoftLimitation, this); + m_serviceGetAllJointsTorqueSoftLimitation = m_node_handle.advertiseService("base/get_all_joints_torque_soft_limitation", &BaseSimulationServices::GetAllJointsTorqueSoftLimitation, this); + m_serviceGetTwistSoftLimitation = m_node_handle.advertiseService("base/get_twist_soft_limitation", &BaseSimulationServices::GetTwistSoftLimitation, this); + m_serviceGetWrenchSoftLimitation = m_node_handle.advertiseService("base/get_wrench_soft_limitation", &BaseSimulationServices::GetWrenchSoftLimitation, this); + m_serviceSetControllerConfigurationMode = m_node_handle.advertiseService("base/set_controller_configuration_mode", &BaseSimulationServices::SetControllerConfigurationMode, this); + m_serviceGetControllerConfigurationMode = m_node_handle.advertiseService("base/get_controller_configuration_mode", &BaseSimulationServices::GetControllerConfigurationMode, this); + m_serviceStartTeaching = m_node_handle.advertiseService("base/start_teaching", &BaseSimulationServices::StartTeaching, this); + m_serviceStopTeaching = m_node_handle.advertiseService("base/stop_teaching", &BaseSimulationServices::StopTeaching, this); + m_serviceAddSequenceTasks = m_node_handle.advertiseService("base/add_sequence_tasks", &BaseSimulationServices::AddSequenceTasks, this); + m_serviceUpdateSequenceTask = m_node_handle.advertiseService("base/update_sequence_task", &BaseSimulationServices::UpdateSequenceTask, this); + m_serviceSwapSequenceTasks = m_node_handle.advertiseService("base/swap_sequence_tasks", &BaseSimulationServices::SwapSequenceTasks, this); + m_serviceReadSequenceTask = m_node_handle.advertiseService("base/read_sequence_task", &BaseSimulationServices::ReadSequenceTask, this); + m_serviceReadAllSequenceTasks = m_node_handle.advertiseService("base/read_all_sequence_tasks", &BaseSimulationServices::ReadAllSequenceTasks, this); + m_serviceDeleteSequenceTask = m_node_handle.advertiseService("base/delete_sequence_task", &BaseSimulationServices::DeleteSequenceTask, this); + m_serviceDeleteAllSequenceTasks = m_node_handle.advertiseService("base/delete_all_sequence_tasks", &BaseSimulationServices::DeleteAllSequenceTasks, this); + m_serviceTakeSnapshot = m_node_handle.advertiseService("base/take_snapshot", &BaseSimulationServices::TakeSnapshot, this); + m_serviceGetFirmwareBundleVersions = m_node_handle.advertiseService("base/get_firmware_bundle_versions", &BaseSimulationServices::GetFirmwareBundleVersions, this); + m_serviceMoveSequenceTask = m_node_handle.advertiseService("base/move_sequence_task", &BaseSimulationServices::MoveSequenceTask, this); + m_serviceDuplicateMapping = m_node_handle.advertiseService("base/duplicate_mapping", &BaseSimulationServices::DuplicateMapping, this); + m_serviceDuplicateMap = m_node_handle.advertiseService("base/duplicate_map", &BaseSimulationServices::DuplicateMap, this); + m_serviceSetControllerConfiguration = m_node_handle.advertiseService("base/set_controller_configuration", &BaseSimulationServices::SetControllerConfiguration, this); + m_serviceGetControllerConfiguration = m_node_handle.advertiseService("base/get_controller_configuration", &BaseSimulationServices::GetControllerConfiguration, this); + m_serviceGetAllControllerConfigurations = m_node_handle.advertiseService("base/get_all_controller_configurations", &BaseSimulationServices::GetAllControllerConfigurations, this); +} + +bool BaseSimulationServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) +{ + ROS_WARN_ONCE("The SetDeviceID service is not implemented in simulation, and has no effect."); + return true; +} + +bool BaseSimulationServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) +{ + ROS_WARN_ONCE("The SetDeviceID service is not implemented in simulation, and has no effect."); + return true; +} + + +bool BaseSimulationServices::CreateUserProfile(kortex_driver::CreateUserProfile::Request &req, kortex_driver::CreateUserProfile::Response &res) +{ + + + if (CreateUserProfileHandler) + { + res = CreateUserProfileHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/create_user_profile is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::UpdateUserProfile(kortex_driver::UpdateUserProfile::Request &req, kortex_driver::UpdateUserProfile::Response &res) +{ + + + if (UpdateUserProfileHandler) + { + res = UpdateUserProfileHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/update_user_profile is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ReadUserProfile(kortex_driver::ReadUserProfile::Request &req, kortex_driver::ReadUserProfile::Response &res) +{ + + + if (ReadUserProfileHandler) + { + res = ReadUserProfileHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/read_user_profile is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::DeleteUserProfile(kortex_driver::DeleteUserProfile::Request &req, kortex_driver::DeleteUserProfile::Response &res) +{ + + + if (DeleteUserProfileHandler) + { + res = DeleteUserProfileHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/delete_user_profile is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ReadAllUserProfiles(kortex_driver::ReadAllUserProfiles::Request &req, kortex_driver::ReadAllUserProfiles::Response &res) +{ + + + if (ReadAllUserProfilesHandler) + { + res = ReadAllUserProfilesHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/read_all_user_profiles is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ReadAllUsers(kortex_driver::ReadAllUsers::Request &req, kortex_driver::ReadAllUsers::Response &res) +{ + + + if (ReadAllUsersHandler) + { + res = ReadAllUsersHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/read_all_users is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ChangePassword(kortex_driver::ChangePassword::Request &req, kortex_driver::ChangePassword::Response &res) +{ + + + if (ChangePasswordHandler) + { + res = ChangePasswordHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/change_password is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::CreateSequence(kortex_driver::CreateSequence::Request &req, kortex_driver::CreateSequence::Response &res) +{ + + + if (CreateSequenceHandler) + { + res = CreateSequenceHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/create_sequence is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::UpdateSequence(kortex_driver::UpdateSequence::Request &req, kortex_driver::UpdateSequence::Response &res) +{ + + + if (UpdateSequenceHandler) + { + res = UpdateSequenceHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/update_sequence is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ReadSequence(kortex_driver::ReadSequence::Request &req, kortex_driver::ReadSequence::Response &res) +{ + + + if (ReadSequenceHandler) + { + res = ReadSequenceHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/read_sequence is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::DeleteSequence(kortex_driver::DeleteSequence::Request &req, kortex_driver::DeleteSequence::Response &res) +{ + + + if (DeleteSequenceHandler) + { + res = DeleteSequenceHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/delete_sequence is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ReadAllSequences(kortex_driver::ReadAllSequences::Request &req, kortex_driver::ReadAllSequences::Response &res) +{ + + + if (ReadAllSequencesHandler) + { + res = ReadAllSequencesHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/read_all_sequences is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::PlaySequence(kortex_driver::PlaySequence::Request &req, kortex_driver::PlaySequence::Response &res) +{ + + + if (PlaySequenceHandler) + { + res = PlaySequenceHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/play_sequence is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::PlayAdvancedSequence(kortex_driver::PlayAdvancedSequence::Request &req, kortex_driver::PlayAdvancedSequence::Response &res) +{ + + + if (PlayAdvancedSequenceHandler) + { + res = PlayAdvancedSequenceHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/play_advanced_sequence is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::StopSequence(kortex_driver::StopSequence::Request &req, kortex_driver::StopSequence::Response &res) +{ + + + if (StopSequenceHandler) + { + res = StopSequenceHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/stop_sequence is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::PauseSequence(kortex_driver::PauseSequence::Request &req, kortex_driver::PauseSequence::Response &res) +{ + + + if (PauseSequenceHandler) + { + res = PauseSequenceHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/pause_sequence is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ResumeSequence(kortex_driver::ResumeSequence::Request &req, kortex_driver::ResumeSequence::Response &res) +{ + + + if (ResumeSequenceHandler) + { + res = ResumeSequenceHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/resume_sequence is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::CreateProtectionZone(kortex_driver::CreateProtectionZone::Request &req, kortex_driver::CreateProtectionZone::Response &res) +{ + + + if (CreateProtectionZoneHandler) + { + res = CreateProtectionZoneHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/create_protection_zone is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::UpdateProtectionZone(kortex_driver::UpdateProtectionZone::Request &req, kortex_driver::UpdateProtectionZone::Response &res) +{ + + + if (UpdateProtectionZoneHandler) + { + res = UpdateProtectionZoneHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/update_protection_zone is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ReadProtectionZone(kortex_driver::ReadProtectionZone::Request &req, kortex_driver::ReadProtectionZone::Response &res) +{ + + + if (ReadProtectionZoneHandler) + { + res = ReadProtectionZoneHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/read_protection_zone is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::DeleteProtectionZone(kortex_driver::DeleteProtectionZone::Request &req, kortex_driver::DeleteProtectionZone::Response &res) +{ + + + if (DeleteProtectionZoneHandler) + { + res = DeleteProtectionZoneHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/delete_protection_zone is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ReadAllProtectionZones(kortex_driver::ReadAllProtectionZones::Request &req, kortex_driver::ReadAllProtectionZones::Response &res) +{ + + + if (ReadAllProtectionZonesHandler) + { + res = ReadAllProtectionZonesHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/read_all_protection_zones is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::CreateMapping(kortex_driver::CreateMapping::Request &req, kortex_driver::CreateMapping::Response &res) +{ + + + if (CreateMappingHandler) + { + res = CreateMappingHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/create_mapping is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ReadMapping(kortex_driver::ReadMapping::Request &req, kortex_driver::ReadMapping::Response &res) +{ + + + if (ReadMappingHandler) + { + res = ReadMappingHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/read_mapping is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::UpdateMapping(kortex_driver::UpdateMapping::Request &req, kortex_driver::UpdateMapping::Response &res) +{ + + + if (UpdateMappingHandler) + { + res = UpdateMappingHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/update_mapping is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::DeleteMapping(kortex_driver::DeleteMapping::Request &req, kortex_driver::DeleteMapping::Response &res) +{ + + + if (DeleteMappingHandler) + { + res = DeleteMappingHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/delete_mapping is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ReadAllMappings(kortex_driver::ReadAllMappings::Request &req, kortex_driver::ReadAllMappings::Response &res) +{ + + + if (ReadAllMappingsHandler) + { + res = ReadAllMappingsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/read_all_mappings is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::CreateMap(kortex_driver::CreateMap::Request &req, kortex_driver::CreateMap::Response &res) +{ + + + if (CreateMapHandler) + { + res = CreateMapHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/create_map is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ReadMap(kortex_driver::ReadMap::Request &req, kortex_driver::ReadMap::Response &res) +{ + + + if (ReadMapHandler) + { + res = ReadMapHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/read_map is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::UpdateMap(kortex_driver::UpdateMap::Request &req, kortex_driver::UpdateMap::Response &res) +{ + + + if (UpdateMapHandler) + { + res = UpdateMapHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/update_map is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::DeleteMap(kortex_driver::DeleteMap::Request &req, kortex_driver::DeleteMap::Response &res) +{ + + + if (DeleteMapHandler) + { + res = DeleteMapHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/delete_map is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ReadAllMaps(kortex_driver::ReadAllMaps::Request &req, kortex_driver::ReadAllMaps::Response &res) +{ + + + if (ReadAllMapsHandler) + { + res = ReadAllMapsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/read_all_maps is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ActivateMap(kortex_driver::ActivateMap::Request &req, kortex_driver::ActivateMap::Response &res) +{ + + + if (ActivateMapHandler) + { + res = ActivateMapHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/activate_map is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::CreateAction(kortex_driver::CreateAction::Request &req, kortex_driver::CreateAction::Response &res) +{ + + + if (CreateActionHandler) + { + res = CreateActionHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/create_action is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ReadAction(kortex_driver::ReadAction::Request &req, kortex_driver::ReadAction::Response &res) +{ + + + if (ReadActionHandler) + { + res = ReadActionHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/read_action is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ReadAllActions(kortex_driver::ReadAllActions::Request &req, kortex_driver::ReadAllActions::Response &res) +{ + + + if (ReadAllActionsHandler) + { + res = ReadAllActionsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/read_all_actions is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::DeleteAction(kortex_driver::DeleteAction::Request &req, kortex_driver::DeleteAction::Response &res) +{ + + + if (DeleteActionHandler) + { + res = DeleteActionHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/delete_action is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::UpdateAction(kortex_driver::UpdateAction::Request &req, kortex_driver::UpdateAction::Response &res) +{ + + + if (UpdateActionHandler) + { + res = UpdateActionHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/update_action is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ExecuteActionFromReference(kortex_driver::ExecuteActionFromReference::Request &req, kortex_driver::ExecuteActionFromReference::Response &res) +{ + + + if (ExecuteActionFromReferenceHandler) + { + res = ExecuteActionFromReferenceHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/execute_action_from_reference is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ExecuteAction(kortex_driver::ExecuteAction::Request &req, kortex_driver::ExecuteAction::Response &res) +{ + + + if (ExecuteActionHandler) + { + res = ExecuteActionHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/execute_action is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::PauseAction(kortex_driver::PauseAction::Request &req, kortex_driver::PauseAction::Response &res) +{ + + + if (PauseActionHandler) + { + res = PauseActionHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/pause_action is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::StopAction(kortex_driver::StopAction::Request &req, kortex_driver::StopAction::Response &res) +{ + + + if (StopActionHandler) + { + res = StopActionHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/stop_action is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ResumeAction(kortex_driver::ResumeAction::Request &req, kortex_driver::ResumeAction::Response &res) +{ + + + if (ResumeActionHandler) + { + res = ResumeActionHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/resume_action is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetIPv4Configuration(kortex_driver::GetIPv4Configuration::Request &req, kortex_driver::GetIPv4Configuration::Response &res) +{ + + + if (GetIPv4ConfigurationHandler) + { + res = GetIPv4ConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_i_pv4_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::SetIPv4Configuration(kortex_driver::SetIPv4Configuration::Request &req, kortex_driver::SetIPv4Configuration::Response &res) +{ + + + if (SetIPv4ConfigurationHandler) + { + res = SetIPv4ConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/set_i_pv4_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::SetCommunicationInterfaceEnable(kortex_driver::SetCommunicationInterfaceEnable::Request &req, kortex_driver::SetCommunicationInterfaceEnable::Response &res) +{ + + + if (SetCommunicationInterfaceEnableHandler) + { + res = SetCommunicationInterfaceEnableHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/set_communication_interface_enable is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::IsCommunicationInterfaceEnable(kortex_driver::IsCommunicationInterfaceEnable::Request &req, kortex_driver::IsCommunicationInterfaceEnable::Response &res) +{ + + + if (IsCommunicationInterfaceEnableHandler) + { + res = IsCommunicationInterfaceEnableHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/is_communication_interface_enable is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetAvailableWifi(kortex_driver::GetAvailableWifi::Request &req, kortex_driver::GetAvailableWifi::Response &res) +{ + + + if (GetAvailableWifiHandler) + { + res = GetAvailableWifiHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_available_wifi is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetWifiInformation(kortex_driver::GetWifiInformation::Request &req, kortex_driver::GetWifiInformation::Response &res) +{ + + + if (GetWifiInformationHandler) + { + res = GetWifiInformationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_wifi_information is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::AddWifiConfiguration(kortex_driver::AddWifiConfiguration::Request &req, kortex_driver::AddWifiConfiguration::Response &res) +{ + + + if (AddWifiConfigurationHandler) + { + res = AddWifiConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/add_wifi_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::DeleteWifiConfiguration(kortex_driver::DeleteWifiConfiguration::Request &req, kortex_driver::DeleteWifiConfiguration::Response &res) +{ + + + if (DeleteWifiConfigurationHandler) + { + res = DeleteWifiConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/delete_wifi_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetAllConfiguredWifis(kortex_driver::GetAllConfiguredWifis::Request &req, kortex_driver::GetAllConfiguredWifis::Response &res) +{ + + + if (GetAllConfiguredWifisHandler) + { + res = GetAllConfiguredWifisHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_all_configured_wifis is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ConnectWifi(kortex_driver::ConnectWifi::Request &req, kortex_driver::ConnectWifi::Response &res) +{ + + + if (ConnectWifiHandler) + { + res = ConnectWifiHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/connect_wifi is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::DisconnectWifi(kortex_driver::DisconnectWifi::Request &req, kortex_driver::DisconnectWifi::Response &res) +{ + + + if (DisconnectWifiHandler) + { + res = DisconnectWifiHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/disconnect_wifi is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetConnectedWifiInformation(kortex_driver::GetConnectedWifiInformation::Request &req, kortex_driver::GetConnectedWifiInformation::Response &res) +{ + + + if (GetConnectedWifiInformationHandler) + { + res = GetConnectedWifiInformationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_connected_wifi_information is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::Base_Unsubscribe(kortex_driver::Base_Unsubscribe::Request &req, kortex_driver::Base_Unsubscribe::Response &res) +{ + + + if (Base_UnsubscribeHandler) + { + res = Base_UnsubscribeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/unsubscribe is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::OnNotificationConfigurationChangeTopic(kortex_driver::OnNotificationConfigurationChangeTopic::Request &req, kortex_driver::OnNotificationConfigurationChangeTopic::Response &res) +{ + + m_is_activated_ConfigurationChangeTopic = true; + + if (OnNotificationConfigurationChangeTopicHandler) + { + res = OnNotificationConfigurationChangeTopicHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/activate_publishing_of_configuration_change_topic is not implemented, so the service calls will return the default response."); + } + return true; +} +void BaseSimulationServices::cb_ConfigurationChangeTopic(Kinova::Api::Base::ConfigurationChangeNotification notif) +{ + kortex_driver::ConfigurationChangeNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_ConfigurationChangeTopic.publish(ros_msg); +} + +bool BaseSimulationServices::OnNotificationMappingInfoTopic(kortex_driver::OnNotificationMappingInfoTopic::Request &req, kortex_driver::OnNotificationMappingInfoTopic::Response &res) +{ + + m_is_activated_MappingInfoTopic = true; + + if (OnNotificationMappingInfoTopicHandler) + { + res = OnNotificationMappingInfoTopicHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/activate_publishing_of_mapping_info_topic is not implemented, so the service calls will return the default response."); + } + return true; +} +void BaseSimulationServices::cb_MappingInfoTopic(Kinova::Api::Base::MappingInfoNotification notif) +{ + kortex_driver::MappingInfoNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_MappingInfoTopic.publish(ros_msg); +} + +bool BaseSimulationServices::OnNotificationControlModeTopic(kortex_driver::OnNotificationControlModeTopic::Request &req, kortex_driver::OnNotificationControlModeTopic::Response &res) +{ + + m_is_activated_ControlModeTopic = true; + + if (OnNotificationControlModeTopicHandler) + { + res = OnNotificationControlModeTopicHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/activate_publishing_of_control_mode_topic is not implemented, so the service calls will return the default response."); + } + return true; +} +void BaseSimulationServices::cb_ControlModeTopic(Kinova::Api::Base::ControlModeNotification notif) +{ + kortex_driver::ControlModeNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_ControlModeTopic.publish(ros_msg); +} + +bool BaseSimulationServices::OnNotificationOperatingModeTopic(kortex_driver::OnNotificationOperatingModeTopic::Request &req, kortex_driver::OnNotificationOperatingModeTopic::Response &res) +{ + + m_is_activated_OperatingModeTopic = true; + + if (OnNotificationOperatingModeTopicHandler) + { + res = OnNotificationOperatingModeTopicHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/activate_publishing_of_operating_mode_topic is not implemented, so the service calls will return the default response."); + } + return true; +} +void BaseSimulationServices::cb_OperatingModeTopic(Kinova::Api::Base::OperatingModeNotification notif) +{ + kortex_driver::OperatingModeNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_OperatingModeTopic.publish(ros_msg); +} + +bool BaseSimulationServices::OnNotificationSequenceInfoTopic(kortex_driver::OnNotificationSequenceInfoTopic::Request &req, kortex_driver::OnNotificationSequenceInfoTopic::Response &res) +{ + + m_is_activated_SequenceInfoTopic = true; + + if (OnNotificationSequenceInfoTopicHandler) + { + res = OnNotificationSequenceInfoTopicHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/activate_publishing_of_sequence_info_topic is not implemented, so the service calls will return the default response."); + } + return true; +} +void BaseSimulationServices::cb_SequenceInfoTopic(Kinova::Api::Base::SequenceInfoNotification notif) +{ + kortex_driver::SequenceInfoNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_SequenceInfoTopic.publish(ros_msg); +} + +bool BaseSimulationServices::OnNotificationProtectionZoneTopic(kortex_driver::OnNotificationProtectionZoneTopic::Request &req, kortex_driver::OnNotificationProtectionZoneTopic::Response &res) +{ + + m_is_activated_ProtectionZoneTopic = true; + + if (OnNotificationProtectionZoneTopicHandler) + { + res = OnNotificationProtectionZoneTopicHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/activate_publishing_of_protection_zone_topic is not implemented, so the service calls will return the default response."); + } + return true; +} +void BaseSimulationServices::cb_ProtectionZoneTopic(Kinova::Api::Base::ProtectionZoneNotification notif) +{ + kortex_driver::ProtectionZoneNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_ProtectionZoneTopic.publish(ros_msg); +} + +bool BaseSimulationServices::OnNotificationUserTopic(kortex_driver::OnNotificationUserTopic::Request &req, kortex_driver::OnNotificationUserTopic::Response &res) +{ + + m_is_activated_UserTopic = true; + + if (OnNotificationUserTopicHandler) + { + res = OnNotificationUserTopicHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/activate_publishing_of_user_topic is not implemented, so the service calls will return the default response."); + } + return true; +} +void BaseSimulationServices::cb_UserTopic(Kinova::Api::Base::UserNotification notif) +{ + kortex_driver::UserNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_UserTopic.publish(ros_msg); +} + +bool BaseSimulationServices::OnNotificationControllerTopic(kortex_driver::OnNotificationControllerTopic::Request &req, kortex_driver::OnNotificationControllerTopic::Response &res) +{ + + m_is_activated_ControllerTopic = true; + + if (OnNotificationControllerTopicHandler) + { + res = OnNotificationControllerTopicHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/activate_publishing_of_controller_topic is not implemented, so the service calls will return the default response."); + } + return true; +} +void BaseSimulationServices::cb_ControllerTopic(Kinova::Api::Base::ControllerNotification notif) +{ + kortex_driver::ControllerNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_ControllerTopic.publish(ros_msg); +} + +bool BaseSimulationServices::OnNotificationActionTopic(kortex_driver::OnNotificationActionTopic::Request &req, kortex_driver::OnNotificationActionTopic::Response &res) +{ + + m_is_activated_ActionTopic = true; + + if (OnNotificationActionTopicHandler) + { + res = OnNotificationActionTopicHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/activate_publishing_of_action_topic is not implemented, so the service calls will return the default response."); + } + return true; +} +void BaseSimulationServices::cb_ActionTopic(Kinova::Api::Base::ActionNotification notif) +{ + kortex_driver::ActionNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_ActionTopic.publish(ros_msg); +} + +bool BaseSimulationServices::OnNotificationRobotEventTopic(kortex_driver::OnNotificationRobotEventTopic::Request &req, kortex_driver::OnNotificationRobotEventTopic::Response &res) +{ + + m_is_activated_RobotEventTopic = true; + + if (OnNotificationRobotEventTopicHandler) + { + res = OnNotificationRobotEventTopicHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/activate_publishing_of_robot_event_topic is not implemented, so the service calls will return the default response."); + } + return true; +} +void BaseSimulationServices::cb_RobotEventTopic(Kinova::Api::Base::RobotEventNotification notif) +{ + kortex_driver::RobotEventNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_RobotEventTopic.publish(ros_msg); +} + +bool BaseSimulationServices::PlayCartesianTrajectory(kortex_driver::PlayCartesianTrajectory::Request &req, kortex_driver::PlayCartesianTrajectory::Response &res) +{ + + + if (PlayCartesianTrajectoryHandler) + { + res = PlayCartesianTrajectoryHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/play_cartesian_trajectory is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::PlayCartesianTrajectoryPosition(kortex_driver::PlayCartesianTrajectoryPosition::Request &req, kortex_driver::PlayCartesianTrajectoryPosition::Response &res) +{ + + + if (PlayCartesianTrajectoryPositionHandler) + { + res = PlayCartesianTrajectoryPositionHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/play_cartesian_trajectory_position is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::PlayCartesianTrajectoryOrientation(kortex_driver::PlayCartesianTrajectoryOrientation::Request &req, kortex_driver::PlayCartesianTrajectoryOrientation::Response &res) +{ + + + if (PlayCartesianTrajectoryOrientationHandler) + { + res = PlayCartesianTrajectoryOrientationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/play_cartesian_trajectory_orientation is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::Stop(kortex_driver::Stop::Request &req, kortex_driver::Stop::Response &res) +{ + + + if (StopHandler) + { + res = StopHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/stop is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetMeasuredCartesianPose(kortex_driver::GetMeasuredCartesianPose::Request &req, kortex_driver::GetMeasuredCartesianPose::Response &res) +{ + + + if (GetMeasuredCartesianPoseHandler) + { + res = GetMeasuredCartesianPoseHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_measured_cartesian_pose is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::SendWrenchCommand(kortex_driver::SendWrenchCommand::Request &req, kortex_driver::SendWrenchCommand::Response &res) +{ + + + if (SendWrenchCommandHandler) + { + res = SendWrenchCommandHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/send_wrench_command is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::SendWrenchJoystickCommand(kortex_driver::SendWrenchJoystickCommand::Request &req, kortex_driver::SendWrenchJoystickCommand::Response &res) +{ + + + if (SendWrenchJoystickCommandHandler) + { + res = SendWrenchJoystickCommandHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/send_wrench_joystick_command is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::SendTwistJoystickCommand(kortex_driver::SendTwistJoystickCommand::Request &req, kortex_driver::SendTwistJoystickCommand::Response &res) +{ + + + if (SendTwistJoystickCommandHandler) + { + res = SendTwistJoystickCommandHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/send_twist_joystick_command is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::SendTwistCommand(kortex_driver::SendTwistCommand::Request &req, kortex_driver::SendTwistCommand::Response &res) +{ + + + if (SendTwistCommandHandler) + { + res = SendTwistCommandHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/send_twist_command is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::PlayJointTrajectory(kortex_driver::PlayJointTrajectory::Request &req, kortex_driver::PlayJointTrajectory::Response &res) +{ + + + if (PlayJointTrajectoryHandler) + { + res = PlayJointTrajectoryHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/play_joint_trajectory is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::PlaySelectedJointTrajectory(kortex_driver::PlaySelectedJointTrajectory::Request &req, kortex_driver::PlaySelectedJointTrajectory::Response &res) +{ + + + if (PlaySelectedJointTrajectoryHandler) + { + res = PlaySelectedJointTrajectoryHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/play_selected_joint_trajectory is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetMeasuredJointAngles(kortex_driver::GetMeasuredJointAngles::Request &req, kortex_driver::GetMeasuredJointAngles::Response &res) +{ + + + if (GetMeasuredJointAnglesHandler) + { + res = GetMeasuredJointAnglesHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_measured_joint_angles is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::SendJointSpeedsCommand(kortex_driver::SendJointSpeedsCommand::Request &req, kortex_driver::SendJointSpeedsCommand::Response &res) +{ + + + if (SendJointSpeedsCommandHandler) + { + res = SendJointSpeedsCommandHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/send_joint_speeds_command is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::SendSelectedJointSpeedCommand(kortex_driver::SendSelectedJointSpeedCommand::Request &req, kortex_driver::SendSelectedJointSpeedCommand::Response &res) +{ + + + if (SendSelectedJointSpeedCommandHandler) + { + res = SendSelectedJointSpeedCommandHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/send_selected_joint_speed_command is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::SendGripperCommand(kortex_driver::SendGripperCommand::Request &req, kortex_driver::SendGripperCommand::Response &res) +{ + + + if (SendGripperCommandHandler) + { + res = SendGripperCommandHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/send_gripper_command is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetMeasuredGripperMovement(kortex_driver::GetMeasuredGripperMovement::Request &req, kortex_driver::GetMeasuredGripperMovement::Response &res) +{ + + + if (GetMeasuredGripperMovementHandler) + { + res = GetMeasuredGripperMovementHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_measured_gripper_movement is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::SetAdmittance(kortex_driver::SetAdmittance::Request &req, kortex_driver::SetAdmittance::Response &res) +{ + + + if (SetAdmittanceHandler) + { + res = SetAdmittanceHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/set_admittance is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::SetOperatingMode(kortex_driver::SetOperatingMode::Request &req, kortex_driver::SetOperatingMode::Response &res) +{ + + + if (SetOperatingModeHandler) + { + res = SetOperatingModeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/set_operating_mode is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ApplyEmergencyStop(kortex_driver::ApplyEmergencyStop::Request &req, kortex_driver::ApplyEmergencyStop::Response &res) +{ + + + if (ApplyEmergencyStopHandler) + { + res = ApplyEmergencyStopHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/apply_emergency_stop is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::Base_ClearFaults(kortex_driver::Base_ClearFaults::Request &req, kortex_driver::Base_ClearFaults::Response &res) +{ + + + if (Base_ClearFaultsHandler) + { + res = Base_ClearFaultsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/clear_faults is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::Base_GetControlMode(kortex_driver::Base_GetControlMode::Request &req, kortex_driver::Base_GetControlMode::Response &res) +{ + + + if (Base_GetControlModeHandler) + { + res = Base_GetControlModeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_control_mode is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetOperatingMode(kortex_driver::GetOperatingMode::Request &req, kortex_driver::GetOperatingMode::Response &res) +{ + + + if (GetOperatingModeHandler) + { + res = GetOperatingModeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_operating_mode is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::SetServoingMode(kortex_driver::SetServoingMode::Request &req, kortex_driver::SetServoingMode::Response &res) +{ + + + if (SetServoingModeHandler) + { + res = SetServoingModeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/set_servoing_mode is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetServoingMode(kortex_driver::GetServoingMode::Request &req, kortex_driver::GetServoingMode::Response &res) +{ + + + if (GetServoingModeHandler) + { + res = GetServoingModeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_servoing_mode is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::OnNotificationServoingModeTopic(kortex_driver::OnNotificationServoingModeTopic::Request &req, kortex_driver::OnNotificationServoingModeTopic::Response &res) +{ + + m_is_activated_ServoingModeTopic = true; + + if (OnNotificationServoingModeTopicHandler) + { + res = OnNotificationServoingModeTopicHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/activate_publishing_of_servoing_mode_topic is not implemented, so the service calls will return the default response."); + } + return true; +} +void BaseSimulationServices::cb_ServoingModeTopic(Kinova::Api::Base::ServoingModeNotification notif) +{ + kortex_driver::ServoingModeNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_ServoingModeTopic.publish(ros_msg); +} + +bool BaseSimulationServices::RestoreFactorySettings(kortex_driver::RestoreFactorySettings::Request &req, kortex_driver::RestoreFactorySettings::Response &res) +{ + + + if (RestoreFactorySettingsHandler) + { + res = RestoreFactorySettingsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/restore_factory_settings is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::OnNotificationFactoryTopic(kortex_driver::OnNotificationFactoryTopic::Request &req, kortex_driver::OnNotificationFactoryTopic::Response &res) +{ + + m_is_activated_FactoryTopic = true; + + if (OnNotificationFactoryTopicHandler) + { + res = OnNotificationFactoryTopicHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/activate_publishing_of_factory_topic is not implemented, so the service calls will return the default response."); + } + return true; +} +void BaseSimulationServices::cb_FactoryTopic(Kinova::Api::Base::FactoryNotification notif) +{ + kortex_driver::FactoryNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_FactoryTopic.publish(ros_msg); +} + +bool BaseSimulationServices::GetAllConnectedControllers(kortex_driver::GetAllConnectedControllers::Request &req, kortex_driver::GetAllConnectedControllers::Response &res) +{ + + + if (GetAllConnectedControllersHandler) + { + res = GetAllConnectedControllersHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_all_connected_controllers is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetControllerState(kortex_driver::GetControllerState::Request &req, kortex_driver::GetControllerState::Response &res) +{ + + + if (GetControllerStateHandler) + { + res = GetControllerStateHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_controller_state is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetActuatorCount(kortex_driver::GetActuatorCount::Request &req, kortex_driver::GetActuatorCount::Response &res) +{ + + + if (GetActuatorCountHandler) + { + res = GetActuatorCountHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_actuator_count is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::StartWifiScan(kortex_driver::StartWifiScan::Request &req, kortex_driver::StartWifiScan::Response &res) +{ + + + if (StartWifiScanHandler) + { + res = StartWifiScanHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/start_wifi_scan is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetConfiguredWifi(kortex_driver::GetConfiguredWifi::Request &req, kortex_driver::GetConfiguredWifi::Response &res) +{ + + + if (GetConfiguredWifiHandler) + { + res = GetConfiguredWifiHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_configured_wifi is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::OnNotificationNetworkTopic(kortex_driver::OnNotificationNetworkTopic::Request &req, kortex_driver::OnNotificationNetworkTopic::Response &res) +{ + + m_is_activated_NetworkTopic = true; + + if (OnNotificationNetworkTopicHandler) + { + res = OnNotificationNetworkTopicHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/activate_publishing_of_network_topic is not implemented, so the service calls will return the default response."); + } + return true; +} +void BaseSimulationServices::cb_NetworkTopic(Kinova::Api::Base::NetworkNotification notif) +{ + kortex_driver::NetworkNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_NetworkTopic.publish(ros_msg); +} + +bool BaseSimulationServices::GetArmState(kortex_driver::GetArmState::Request &req, kortex_driver::GetArmState::Response &res) +{ + + + if (GetArmStateHandler) + { + res = GetArmStateHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_arm_state is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::OnNotificationArmStateTopic(kortex_driver::OnNotificationArmStateTopic::Request &req, kortex_driver::OnNotificationArmStateTopic::Response &res) +{ + + m_is_activated_ArmStateTopic = true; + + if (OnNotificationArmStateTopicHandler) + { + res = OnNotificationArmStateTopicHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/activate_publishing_of_arm_state_topic is not implemented, so the service calls will return the default response."); + } + return true; +} +void BaseSimulationServices::cb_ArmStateTopic(Kinova::Api::Base::ArmStateNotification notif) +{ + kortex_driver::ArmStateNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_ArmStateTopic.publish(ros_msg); +} + +bool BaseSimulationServices::GetIPv4Information(kortex_driver::GetIPv4Information::Request &req, kortex_driver::GetIPv4Information::Response &res) +{ + + + if (GetIPv4InformationHandler) + { + res = GetIPv4InformationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_i_pv4_information is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::SetWifiCountryCode(kortex_driver::SetWifiCountryCode::Request &req, kortex_driver::SetWifiCountryCode::Response &res) +{ + + + if (SetWifiCountryCodeHandler) + { + res = SetWifiCountryCodeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/set_wifi_country_code is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetWifiCountryCode(kortex_driver::GetWifiCountryCode::Request &req, kortex_driver::GetWifiCountryCode::Response &res) +{ + + + if (GetWifiCountryCodeHandler) + { + res = GetWifiCountryCodeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_wifi_country_code is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::Base_SetCapSenseConfig(kortex_driver::Base_SetCapSenseConfig::Request &req, kortex_driver::Base_SetCapSenseConfig::Response &res) +{ + + + if (Base_SetCapSenseConfigHandler) + { + res = Base_SetCapSenseConfigHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/set_cap_sense_config is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::Base_GetCapSenseConfig(kortex_driver::Base_GetCapSenseConfig::Request &req, kortex_driver::Base_GetCapSenseConfig::Response &res) +{ + + + if (Base_GetCapSenseConfigHandler) + { + res = Base_GetCapSenseConfigHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_cap_sense_config is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetAllJointsSpeedHardLimitation(kortex_driver::GetAllJointsSpeedHardLimitation::Request &req, kortex_driver::GetAllJointsSpeedHardLimitation::Response &res) +{ + ROS_WARN("The base/get_all_joints_speed_hard_limitation service is now deprecated and will be removed in a future release."); + + + if (GetAllJointsSpeedHardLimitationHandler) + { + res = GetAllJointsSpeedHardLimitationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_all_joints_speed_hard_limitation is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetAllJointsTorqueHardLimitation(kortex_driver::GetAllJointsTorqueHardLimitation::Request &req, kortex_driver::GetAllJointsTorqueHardLimitation::Response &res) +{ + ROS_WARN("The base/get_all_joints_torque_hard_limitation service is now deprecated and will be removed in a future release."); + + + if (GetAllJointsTorqueHardLimitationHandler) + { + res = GetAllJointsTorqueHardLimitationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_all_joints_torque_hard_limitation is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetTwistHardLimitation(kortex_driver::GetTwistHardLimitation::Request &req, kortex_driver::GetTwistHardLimitation::Response &res) +{ + ROS_WARN("The base/get_twist_hard_limitation service is now deprecated and will be removed in a future release."); + + + if (GetTwistHardLimitationHandler) + { + res = GetTwistHardLimitationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_twist_hard_limitation is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetWrenchHardLimitation(kortex_driver::GetWrenchHardLimitation::Request &req, kortex_driver::GetWrenchHardLimitation::Response &res) +{ + ROS_WARN("The base/get_wrench_hard_limitation service is now deprecated and will be removed in a future release."); + + + if (GetWrenchHardLimitationHandler) + { + res = GetWrenchHardLimitationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_wrench_hard_limitation is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::SendJointSpeedsJoystickCommand(kortex_driver::SendJointSpeedsJoystickCommand::Request &req, kortex_driver::SendJointSpeedsJoystickCommand::Response &res) +{ + + + if (SendJointSpeedsJoystickCommandHandler) + { + res = SendJointSpeedsJoystickCommandHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/send_joint_speeds_joystick_command is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::SendSelectedJointSpeedJoystickCommand(kortex_driver::SendSelectedJointSpeedJoystickCommand::Request &req, kortex_driver::SendSelectedJointSpeedJoystickCommand::Response &res) +{ + + + if (SendSelectedJointSpeedJoystickCommandHandler) + { + res = SendSelectedJointSpeedJoystickCommandHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/send_selected_joint_speed_joystick_command is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::EnableBridge(kortex_driver::EnableBridge::Request &req, kortex_driver::EnableBridge::Response &res) +{ + + + if (EnableBridgeHandler) + { + res = EnableBridgeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/enable_bridge is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::DisableBridge(kortex_driver::DisableBridge::Request &req, kortex_driver::DisableBridge::Response &res) +{ + + + if (DisableBridgeHandler) + { + res = DisableBridgeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/disable_bridge is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetBridgeList(kortex_driver::GetBridgeList::Request &req, kortex_driver::GetBridgeList::Response &res) +{ + + + if (GetBridgeListHandler) + { + res = GetBridgeListHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_bridge_list is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetBridgeConfig(kortex_driver::GetBridgeConfig::Request &req, kortex_driver::GetBridgeConfig::Response &res) +{ + + + if (GetBridgeConfigHandler) + { + res = GetBridgeConfigHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_bridge_config is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::PlayPreComputedJointTrajectory(kortex_driver::PlayPreComputedJointTrajectory::Request &req, kortex_driver::PlayPreComputedJointTrajectory::Response &res) +{ + + + if (PlayPreComputedJointTrajectoryHandler) + { + res = PlayPreComputedJointTrajectoryHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/play_pre_computed_joint_trajectory is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetProductConfiguration(kortex_driver::GetProductConfiguration::Request &req, kortex_driver::GetProductConfiguration::Response &res) +{ + + + if (GetProductConfigurationHandler) + { + res = GetProductConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_product_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::UpdateEndEffectorTypeConfiguration(kortex_driver::UpdateEndEffectorTypeConfiguration::Request &req, kortex_driver::UpdateEndEffectorTypeConfiguration::Response &res) +{ + + + if (UpdateEndEffectorTypeConfigurationHandler) + { + res = UpdateEndEffectorTypeConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/update_end_effector_type_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::RestoreFactoryProductConfiguration(kortex_driver::RestoreFactoryProductConfiguration::Request &req, kortex_driver::RestoreFactoryProductConfiguration::Response &res) +{ + + + if (RestoreFactoryProductConfigurationHandler) + { + res = RestoreFactoryProductConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/restore_factory_product_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetTrajectoryErrorReport(kortex_driver::GetTrajectoryErrorReport::Request &req, kortex_driver::GetTrajectoryErrorReport::Response &res) +{ + + + if (GetTrajectoryErrorReportHandler) + { + res = GetTrajectoryErrorReportHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_trajectory_error_report is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetAllJointsSpeedSoftLimitation(kortex_driver::GetAllJointsSpeedSoftLimitation::Request &req, kortex_driver::GetAllJointsSpeedSoftLimitation::Response &res) +{ + ROS_WARN("The base/get_all_joints_speed_soft_limitation service is now deprecated and will be removed in a future release."); + + + if (GetAllJointsSpeedSoftLimitationHandler) + { + res = GetAllJointsSpeedSoftLimitationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_all_joints_speed_soft_limitation is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetAllJointsTorqueSoftLimitation(kortex_driver::GetAllJointsTorqueSoftLimitation::Request &req, kortex_driver::GetAllJointsTorqueSoftLimitation::Response &res) +{ + ROS_WARN("The base/get_all_joints_torque_soft_limitation service is now deprecated and will be removed in a future release."); + + + if (GetAllJointsTorqueSoftLimitationHandler) + { + res = GetAllJointsTorqueSoftLimitationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_all_joints_torque_soft_limitation is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetTwistSoftLimitation(kortex_driver::GetTwistSoftLimitation::Request &req, kortex_driver::GetTwistSoftLimitation::Response &res) +{ + ROS_WARN("The base/get_twist_soft_limitation service is now deprecated and will be removed in a future release."); + + + if (GetTwistSoftLimitationHandler) + { + res = GetTwistSoftLimitationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_twist_soft_limitation is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetWrenchSoftLimitation(kortex_driver::GetWrenchSoftLimitation::Request &req, kortex_driver::GetWrenchSoftLimitation::Response &res) +{ + ROS_WARN("The base/get_wrench_soft_limitation service is now deprecated and will be removed in a future release."); + + + if (GetWrenchSoftLimitationHandler) + { + res = GetWrenchSoftLimitationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_wrench_soft_limitation is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::SetControllerConfigurationMode(kortex_driver::SetControllerConfigurationMode::Request &req, kortex_driver::SetControllerConfigurationMode::Response &res) +{ + + + if (SetControllerConfigurationModeHandler) + { + res = SetControllerConfigurationModeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/set_controller_configuration_mode is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetControllerConfigurationMode(kortex_driver::GetControllerConfigurationMode::Request &req, kortex_driver::GetControllerConfigurationMode::Response &res) +{ + + + if (GetControllerConfigurationModeHandler) + { + res = GetControllerConfigurationModeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_controller_configuration_mode is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::StartTeaching(kortex_driver::StartTeaching::Request &req, kortex_driver::StartTeaching::Response &res) +{ + + + if (StartTeachingHandler) + { + res = StartTeachingHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/start_teaching is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::StopTeaching(kortex_driver::StopTeaching::Request &req, kortex_driver::StopTeaching::Response &res) +{ + + + if (StopTeachingHandler) + { + res = StopTeachingHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/stop_teaching is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::AddSequenceTasks(kortex_driver::AddSequenceTasks::Request &req, kortex_driver::AddSequenceTasks::Response &res) +{ + + + if (AddSequenceTasksHandler) + { + res = AddSequenceTasksHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/add_sequence_tasks is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::UpdateSequenceTask(kortex_driver::UpdateSequenceTask::Request &req, kortex_driver::UpdateSequenceTask::Response &res) +{ + + + if (UpdateSequenceTaskHandler) + { + res = UpdateSequenceTaskHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/update_sequence_task is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::SwapSequenceTasks(kortex_driver::SwapSequenceTasks::Request &req, kortex_driver::SwapSequenceTasks::Response &res) +{ + + + if (SwapSequenceTasksHandler) + { + res = SwapSequenceTasksHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/swap_sequence_tasks is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ReadSequenceTask(kortex_driver::ReadSequenceTask::Request &req, kortex_driver::ReadSequenceTask::Response &res) +{ + + + if (ReadSequenceTaskHandler) + { + res = ReadSequenceTaskHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/read_sequence_task is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ReadAllSequenceTasks(kortex_driver::ReadAllSequenceTasks::Request &req, kortex_driver::ReadAllSequenceTasks::Response &res) +{ + + + if (ReadAllSequenceTasksHandler) + { + res = ReadAllSequenceTasksHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/read_all_sequence_tasks is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::DeleteSequenceTask(kortex_driver::DeleteSequenceTask::Request &req, kortex_driver::DeleteSequenceTask::Response &res) +{ + + + if (DeleteSequenceTaskHandler) + { + res = DeleteSequenceTaskHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/delete_sequence_task is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::DeleteAllSequenceTasks(kortex_driver::DeleteAllSequenceTasks::Request &req, kortex_driver::DeleteAllSequenceTasks::Response &res) +{ + + + if (DeleteAllSequenceTasksHandler) + { + res = DeleteAllSequenceTasksHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/delete_all_sequence_tasks is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::TakeSnapshot(kortex_driver::TakeSnapshot::Request &req, kortex_driver::TakeSnapshot::Response &res) +{ + + + if (TakeSnapshotHandler) + { + res = TakeSnapshotHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/take_snapshot is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetFirmwareBundleVersions(kortex_driver::GetFirmwareBundleVersions::Request &req, kortex_driver::GetFirmwareBundleVersions::Response &res) +{ + + + if (GetFirmwareBundleVersionsHandler) + { + res = GetFirmwareBundleVersionsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_firmware_bundle_versions is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::MoveSequenceTask(kortex_driver::MoveSequenceTask::Request &req, kortex_driver::MoveSequenceTask::Response &res) +{ + + + if (MoveSequenceTaskHandler) + { + res = MoveSequenceTaskHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/move_sequence_task is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::DuplicateMapping(kortex_driver::DuplicateMapping::Request &req, kortex_driver::DuplicateMapping::Response &res) +{ + + + if (DuplicateMappingHandler) + { + res = DuplicateMappingHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/duplicate_mapping is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::DuplicateMap(kortex_driver::DuplicateMap::Request &req, kortex_driver::DuplicateMap::Response &res) +{ + + + if (DuplicateMapHandler) + { + res = DuplicateMapHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/duplicate_map is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::SetControllerConfiguration(kortex_driver::SetControllerConfiguration::Request &req, kortex_driver::SetControllerConfiguration::Response &res) +{ + + + if (SetControllerConfigurationHandler) + { + res = SetControllerConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/set_controller_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetControllerConfiguration(kortex_driver::GetControllerConfiguration::Request &req, kortex_driver::GetControllerConfiguration::Response &res) +{ + + + if (GetControllerConfigurationHandler) + { + res = GetControllerConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_controller_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::GetAllControllerConfigurations(kortex_driver::GetAllControllerConfigurations::Request &req, kortex_driver::GetAllControllerConfigurations::Response &res) +{ + + + if (GetAllControllerConfigurationsHandler) + { + res = GetAllControllerConfigurationsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/get_all_controller_configurations is not implemented, so the service calls will return the default response."); + } + return true; +} diff --git a/kortex_driver/src/generated/simulation/controlconfig_services.cpp b/kortex_driver/src/generated/simulation/controlconfig_services.cpp new file mode 100644 index 00000000..1dfd2669 --- /dev/null +++ b/kortex_driver/src/generated/simulation/controlconfig_services.cpp @@ -0,0 +1,539 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" +#include "kortex_driver/generated/simulation/controlconfig_services.h" + +ControlConfigSimulationServices::ControlConfigSimulationServices(ros::NodeHandle& node_handle): + IControlConfigServices(node_handle) +{ + m_pub_Error = m_node_handle.advertise("kortex_error", 1000); + m_pub_ControlConfigurationTopic = m_node_handle.advertise("control_configuration_topic", 1000); + m_is_activated_ControlConfigurationTopic = false; + + m_serviceSetDeviceID = m_node_handle.advertiseService("control_config/set_device_id", &ControlConfigSimulationServices::SetDeviceID, this); + m_serviceSetApiOptions = m_node_handle.advertiseService("control_config/set_api_options", &ControlConfigSimulationServices::SetApiOptions, this); + + m_serviceSetGravityVector = m_node_handle.advertiseService("control_config/set_gravity_vector", &ControlConfigSimulationServices::SetGravityVector, this); + m_serviceGetGravityVector = m_node_handle.advertiseService("control_config/get_gravity_vector", &ControlConfigSimulationServices::GetGravityVector, this); + m_serviceSetPayloadInformation = m_node_handle.advertiseService("control_config/set_payload_information", &ControlConfigSimulationServices::SetPayloadInformation, this); + m_serviceGetPayloadInformation = m_node_handle.advertiseService("control_config/get_payload_information", &ControlConfigSimulationServices::GetPayloadInformation, this); + m_serviceSetToolConfiguration = m_node_handle.advertiseService("control_config/set_tool_configuration", &ControlConfigSimulationServices::SetToolConfiguration, this); + m_serviceGetToolConfiguration = m_node_handle.advertiseService("control_config/get_tool_configuration", &ControlConfigSimulationServices::GetToolConfiguration, this); + m_serviceOnNotificationControlConfigurationTopic = m_node_handle.advertiseService("control_config/activate_publishing_of_control_configuration_topic", &ControlConfigSimulationServices::OnNotificationControlConfigurationTopic, this); + m_serviceControlConfig_Unsubscribe = m_node_handle.advertiseService("control_config/unsubscribe", &ControlConfigSimulationServices::ControlConfig_Unsubscribe, this); + m_serviceSetCartesianReferenceFrame = m_node_handle.advertiseService("control_config/set_cartesian_reference_frame", &ControlConfigSimulationServices::SetCartesianReferenceFrame, this); + m_serviceGetCartesianReferenceFrame = m_node_handle.advertiseService("control_config/get_cartesian_reference_frame", &ControlConfigSimulationServices::GetCartesianReferenceFrame, this); + m_serviceControlConfig_GetControlMode = m_node_handle.advertiseService("control_config/get_control_mode", &ControlConfigSimulationServices::ControlConfig_GetControlMode, this); + m_serviceSetJointSpeedSoftLimits = m_node_handle.advertiseService("control_config/set_joint_speed_soft_limits", &ControlConfigSimulationServices::SetJointSpeedSoftLimits, this); + m_serviceSetTwistLinearSoftLimit = m_node_handle.advertiseService("control_config/set_twist_linear_soft_limit", &ControlConfigSimulationServices::SetTwistLinearSoftLimit, this); + m_serviceSetTwistAngularSoftLimit = m_node_handle.advertiseService("control_config/set_twist_angular_soft_limit", &ControlConfigSimulationServices::SetTwistAngularSoftLimit, this); + m_serviceSetJointAccelerationSoftLimits = m_node_handle.advertiseService("control_config/set_joint_acceleration_soft_limits", &ControlConfigSimulationServices::SetJointAccelerationSoftLimits, this); + m_serviceGetKinematicHardLimits = m_node_handle.advertiseService("control_config/get_kinematic_hard_limits", &ControlConfigSimulationServices::GetKinematicHardLimits, this); + m_serviceGetKinematicSoftLimits = m_node_handle.advertiseService("control_config/get_kinematic_soft_limits", &ControlConfigSimulationServices::GetKinematicSoftLimits, this); + m_serviceGetAllKinematicSoftLimits = m_node_handle.advertiseService("control_config/get_all_kinematic_soft_limits", &ControlConfigSimulationServices::GetAllKinematicSoftLimits, this); + m_serviceSetDesiredLinearTwist = m_node_handle.advertiseService("control_config/set_desired_linear_twist", &ControlConfigSimulationServices::SetDesiredLinearTwist, this); + m_serviceSetDesiredAngularTwist = m_node_handle.advertiseService("control_config/set_desired_angular_twist", &ControlConfigSimulationServices::SetDesiredAngularTwist, this); + m_serviceSetDesiredJointSpeeds = m_node_handle.advertiseService("control_config/set_desired_joint_speeds", &ControlConfigSimulationServices::SetDesiredJointSpeeds, this); + m_serviceGetDesiredSpeeds = m_node_handle.advertiseService("control_config/get_desired_speeds", &ControlConfigSimulationServices::GetDesiredSpeeds, this); + m_serviceResetGravityVector = m_node_handle.advertiseService("control_config/reset_gravity_vector", &ControlConfigSimulationServices::ResetGravityVector, this); + m_serviceResetPayloadInformation = m_node_handle.advertiseService("control_config/reset_payload_information", &ControlConfigSimulationServices::ResetPayloadInformation, this); + m_serviceResetToolConfiguration = m_node_handle.advertiseService("control_config/reset_tool_configuration", &ControlConfigSimulationServices::ResetToolConfiguration, this); + m_serviceResetJointSpeedSoftLimits = m_node_handle.advertiseService("control_config/reset_joint_speed_soft_limits", &ControlConfigSimulationServices::ResetJointSpeedSoftLimits, this); + m_serviceResetTwistLinearSoftLimit = m_node_handle.advertiseService("control_config/reset_twist_linear_soft_limit", &ControlConfigSimulationServices::ResetTwistLinearSoftLimit, this); + m_serviceResetTwistAngularSoftLimit = m_node_handle.advertiseService("control_config/reset_twist_angular_soft_limit", &ControlConfigSimulationServices::ResetTwistAngularSoftLimit, this); + m_serviceResetJointAccelerationSoftLimits = m_node_handle.advertiseService("control_config/reset_joint_acceleration_soft_limits", &ControlConfigSimulationServices::ResetJointAccelerationSoftLimits, this); +} + +bool ControlConfigSimulationServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) +{ + ROS_WARN_ONCE("The SetDeviceID service is not implemented in simulation, and has no effect."); + return true; +} + +bool ControlConfigSimulationServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) +{ + ROS_WARN_ONCE("The SetDeviceID service is not implemented in simulation, and has no effect."); + return true; +} + + +bool ControlConfigSimulationServices::SetGravityVector(kortex_driver::SetGravityVector::Request &req, kortex_driver::SetGravityVector::Response &res) +{ + + + if (SetGravityVectorHandler) + { + res = SetGravityVectorHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/set_gravity_vector is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::GetGravityVector(kortex_driver::GetGravityVector::Request &req, kortex_driver::GetGravityVector::Response &res) +{ + + + if (GetGravityVectorHandler) + { + res = GetGravityVectorHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/get_gravity_vector is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::SetPayloadInformation(kortex_driver::SetPayloadInformation::Request &req, kortex_driver::SetPayloadInformation::Response &res) +{ + + + if (SetPayloadInformationHandler) + { + res = SetPayloadInformationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/set_payload_information is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::GetPayloadInformation(kortex_driver::GetPayloadInformation::Request &req, kortex_driver::GetPayloadInformation::Response &res) +{ + + + if (GetPayloadInformationHandler) + { + res = GetPayloadInformationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/get_payload_information is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::SetToolConfiguration(kortex_driver::SetToolConfiguration::Request &req, kortex_driver::SetToolConfiguration::Response &res) +{ + + + if (SetToolConfigurationHandler) + { + res = SetToolConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/set_tool_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::GetToolConfiguration(kortex_driver::GetToolConfiguration::Request &req, kortex_driver::GetToolConfiguration::Response &res) +{ + + + if (GetToolConfigurationHandler) + { + res = GetToolConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/get_tool_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::OnNotificationControlConfigurationTopic(kortex_driver::OnNotificationControlConfigurationTopic::Request &req, kortex_driver::OnNotificationControlConfigurationTopic::Response &res) +{ + + m_is_activated_ControlConfigurationTopic = true; + + if (OnNotificationControlConfigurationTopicHandler) + { + res = OnNotificationControlConfigurationTopicHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/activate_publishing_of_control_configuration_topic is not implemented, so the service calls will return the default response."); + } + return true; +} +void ControlConfigSimulationServices::cb_ControlConfigurationTopic(Kinova::Api::ControlConfig::ControlConfigurationNotification notif) +{ + kortex_driver::ControlConfigurationNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_ControlConfigurationTopic.publish(ros_msg); +} + +bool ControlConfigSimulationServices::ControlConfig_Unsubscribe(kortex_driver::ControlConfig_Unsubscribe::Request &req, kortex_driver::ControlConfig_Unsubscribe::Response &res) +{ + + + if (ControlConfig_UnsubscribeHandler) + { + res = ControlConfig_UnsubscribeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/unsubscribe is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::SetCartesianReferenceFrame(kortex_driver::SetCartesianReferenceFrame::Request &req, kortex_driver::SetCartesianReferenceFrame::Response &res) +{ + + + if (SetCartesianReferenceFrameHandler) + { + res = SetCartesianReferenceFrameHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/set_cartesian_reference_frame is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::GetCartesianReferenceFrame(kortex_driver::GetCartesianReferenceFrame::Request &req, kortex_driver::GetCartesianReferenceFrame::Response &res) +{ + + + if (GetCartesianReferenceFrameHandler) + { + res = GetCartesianReferenceFrameHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/get_cartesian_reference_frame is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::ControlConfig_GetControlMode(kortex_driver::ControlConfig_GetControlMode::Request &req, kortex_driver::ControlConfig_GetControlMode::Response &res) +{ + + + if (ControlConfig_GetControlModeHandler) + { + res = ControlConfig_GetControlModeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/get_control_mode is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::SetJointSpeedSoftLimits(kortex_driver::SetJointSpeedSoftLimits::Request &req, kortex_driver::SetJointSpeedSoftLimits::Response &res) +{ + + + if (SetJointSpeedSoftLimitsHandler) + { + res = SetJointSpeedSoftLimitsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/set_joint_speed_soft_limits is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::SetTwistLinearSoftLimit(kortex_driver::SetTwistLinearSoftLimit::Request &req, kortex_driver::SetTwistLinearSoftLimit::Response &res) +{ + + + if (SetTwistLinearSoftLimitHandler) + { + res = SetTwistLinearSoftLimitHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/set_twist_linear_soft_limit is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::SetTwistAngularSoftLimit(kortex_driver::SetTwistAngularSoftLimit::Request &req, kortex_driver::SetTwistAngularSoftLimit::Response &res) +{ + + + if (SetTwistAngularSoftLimitHandler) + { + res = SetTwistAngularSoftLimitHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/set_twist_angular_soft_limit is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::SetJointAccelerationSoftLimits(kortex_driver::SetJointAccelerationSoftLimits::Request &req, kortex_driver::SetJointAccelerationSoftLimits::Response &res) +{ + + + if (SetJointAccelerationSoftLimitsHandler) + { + res = SetJointAccelerationSoftLimitsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/set_joint_acceleration_soft_limits is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::GetKinematicHardLimits(kortex_driver::GetKinematicHardLimits::Request &req, kortex_driver::GetKinematicHardLimits::Response &res) +{ + + + if (GetKinematicHardLimitsHandler) + { + res = GetKinematicHardLimitsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/get_kinematic_hard_limits is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::GetKinematicSoftLimits(kortex_driver::GetKinematicSoftLimits::Request &req, kortex_driver::GetKinematicSoftLimits::Response &res) +{ + + + if (GetKinematicSoftLimitsHandler) + { + res = GetKinematicSoftLimitsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/get_kinematic_soft_limits is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::GetAllKinematicSoftLimits(kortex_driver::GetAllKinematicSoftLimits::Request &req, kortex_driver::GetAllKinematicSoftLimits::Response &res) +{ + + + if (GetAllKinematicSoftLimitsHandler) + { + res = GetAllKinematicSoftLimitsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/get_all_kinematic_soft_limits is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::SetDesiredLinearTwist(kortex_driver::SetDesiredLinearTwist::Request &req, kortex_driver::SetDesiredLinearTwist::Response &res) +{ + + + if (SetDesiredLinearTwistHandler) + { + res = SetDesiredLinearTwistHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/set_desired_linear_twist is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::SetDesiredAngularTwist(kortex_driver::SetDesiredAngularTwist::Request &req, kortex_driver::SetDesiredAngularTwist::Response &res) +{ + + + if (SetDesiredAngularTwistHandler) + { + res = SetDesiredAngularTwistHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/set_desired_angular_twist is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::SetDesiredJointSpeeds(kortex_driver::SetDesiredJointSpeeds::Request &req, kortex_driver::SetDesiredJointSpeeds::Response &res) +{ + + + if (SetDesiredJointSpeedsHandler) + { + res = SetDesiredJointSpeedsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/set_desired_joint_speeds is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::GetDesiredSpeeds(kortex_driver::GetDesiredSpeeds::Request &req, kortex_driver::GetDesiredSpeeds::Response &res) +{ + + + if (GetDesiredSpeedsHandler) + { + res = GetDesiredSpeedsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/get_desired_speeds is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::ResetGravityVector(kortex_driver::ResetGravityVector::Request &req, kortex_driver::ResetGravityVector::Response &res) +{ + + + if (ResetGravityVectorHandler) + { + res = ResetGravityVectorHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/reset_gravity_vector is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::ResetPayloadInformation(kortex_driver::ResetPayloadInformation::Request &req, kortex_driver::ResetPayloadInformation::Response &res) +{ + + + if (ResetPayloadInformationHandler) + { + res = ResetPayloadInformationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/reset_payload_information is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::ResetToolConfiguration(kortex_driver::ResetToolConfiguration::Request &req, kortex_driver::ResetToolConfiguration::Response &res) +{ + + + if (ResetToolConfigurationHandler) + { + res = ResetToolConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/reset_tool_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::ResetJointSpeedSoftLimits(kortex_driver::ResetJointSpeedSoftLimits::Request &req, kortex_driver::ResetJointSpeedSoftLimits::Response &res) +{ + + + if (ResetJointSpeedSoftLimitsHandler) + { + res = ResetJointSpeedSoftLimitsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/reset_joint_speed_soft_limits is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::ResetTwistLinearSoftLimit(kortex_driver::ResetTwistLinearSoftLimit::Request &req, kortex_driver::ResetTwistLinearSoftLimit::Response &res) +{ + + + if (ResetTwistLinearSoftLimitHandler) + { + res = ResetTwistLinearSoftLimitHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/reset_twist_linear_soft_limit is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::ResetTwistAngularSoftLimit(kortex_driver::ResetTwistAngularSoftLimit::Request &req, kortex_driver::ResetTwistAngularSoftLimit::Response &res) +{ + + + if (ResetTwistAngularSoftLimitHandler) + { + res = ResetTwistAngularSoftLimitHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/reset_twist_angular_soft_limit is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool ControlConfigSimulationServices::ResetJointAccelerationSoftLimits(kortex_driver::ResetJointAccelerationSoftLimits::Request &req, kortex_driver::ResetJointAccelerationSoftLimits::Response &res) +{ + + + if (ResetJointAccelerationSoftLimitsHandler) + { + res = ResetJointAccelerationSoftLimitsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/reset_joint_acceleration_soft_limits is not implemented, so the service calls will return the default response."); + } + return true; +} diff --git a/kortex_driver/src/generated/simulation/deviceconfig_services.cpp b/kortex_driver/src/generated/simulation/deviceconfig_services.cpp new file mode 100644 index 00000000..0f17821f --- /dev/null +++ b/kortex_driver/src/generated/simulation/deviceconfig_services.cpp @@ -0,0 +1,587 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" +#include "kortex_driver/generated/simulation/deviceconfig_services.h" + +DeviceConfigSimulationServices::DeviceConfigSimulationServices(ros::NodeHandle& node_handle): + IDeviceConfigServices(node_handle) +{ + m_pub_Error = m_node_handle.advertise("kortex_error", 1000); + m_pub_SafetyTopic = m_node_handle.advertise("safety_topic", 1000); + m_is_activated_SafetyTopic = false; + + m_serviceSetDeviceID = m_node_handle.advertiseService("device_config/set_device_id", &DeviceConfigSimulationServices::SetDeviceID, this); + m_serviceSetApiOptions = m_node_handle.advertiseService("device_config/set_api_options", &DeviceConfigSimulationServices::SetApiOptions, this); + + m_serviceGetRunMode = m_node_handle.advertiseService("device_config/get_run_mode", &DeviceConfigSimulationServices::GetRunMode, this); + m_serviceSetRunMode = m_node_handle.advertiseService("device_config/set_run_mode", &DeviceConfigSimulationServices::SetRunMode, this); + m_serviceGetDeviceType = m_node_handle.advertiseService("device_config/get_device_type", &DeviceConfigSimulationServices::GetDeviceType, this); + m_serviceGetFirmwareVersion = m_node_handle.advertiseService("device_config/get_firmware_version", &DeviceConfigSimulationServices::GetFirmwareVersion, this); + m_serviceGetBootloaderVersion = m_node_handle.advertiseService("device_config/get_bootloader_version", &DeviceConfigSimulationServices::GetBootloaderVersion, this); + m_serviceGetModelNumber = m_node_handle.advertiseService("device_config/get_model_number", &DeviceConfigSimulationServices::GetModelNumber, this); + m_serviceGetPartNumber = m_node_handle.advertiseService("device_config/get_part_number", &DeviceConfigSimulationServices::GetPartNumber, this); + m_serviceGetSerialNumber = m_node_handle.advertiseService("device_config/get_serial_number", &DeviceConfigSimulationServices::GetSerialNumber, this); + m_serviceGetMACAddress = m_node_handle.advertiseService("device_config/get_m_a_c_address", &DeviceConfigSimulationServices::GetMACAddress, this); + m_serviceGetIPv4Settings = m_node_handle.advertiseService("device_config/get_i_pv4_settings", &DeviceConfigSimulationServices::GetIPv4Settings, this); + m_serviceSetIPv4Settings = m_node_handle.advertiseService("device_config/set_i_pv4_settings", &DeviceConfigSimulationServices::SetIPv4Settings, this); + m_serviceGetPartNumberRevision = m_node_handle.advertiseService("device_config/get_part_number_revision", &DeviceConfigSimulationServices::GetPartNumberRevision, this); + m_serviceRebootRequest = m_node_handle.advertiseService("device_config/reboot_request", &DeviceConfigSimulationServices::RebootRequest, this); + m_serviceSetSafetyEnable = m_node_handle.advertiseService("device_config/set_safety_enable", &DeviceConfigSimulationServices::SetSafetyEnable, this); + m_serviceSetSafetyErrorThreshold = m_node_handle.advertiseService("device_config/set_safety_error_threshold", &DeviceConfigSimulationServices::SetSafetyErrorThreshold, this); + m_serviceSetSafetyWarningThreshold = m_node_handle.advertiseService("device_config/set_safety_warning_threshold", &DeviceConfigSimulationServices::SetSafetyWarningThreshold, this); + m_serviceSetSafetyConfiguration = m_node_handle.advertiseService("device_config/set_safety_configuration", &DeviceConfigSimulationServices::SetSafetyConfiguration, this); + m_serviceGetSafetyConfiguration = m_node_handle.advertiseService("device_config/get_safety_configuration", &DeviceConfigSimulationServices::GetSafetyConfiguration, this); + m_serviceGetSafetyInformation = m_node_handle.advertiseService("device_config/get_safety_information", &DeviceConfigSimulationServices::GetSafetyInformation, this); + m_serviceGetSafetyEnable = m_node_handle.advertiseService("device_config/get_safety_enable", &DeviceConfigSimulationServices::GetSafetyEnable, this); + m_serviceGetSafetyStatus = m_node_handle.advertiseService("device_config/get_safety_status", &DeviceConfigSimulationServices::GetSafetyStatus, this); + m_serviceClearAllSafetyStatus = m_node_handle.advertiseService("device_config/clear_all_safety_status", &DeviceConfigSimulationServices::ClearAllSafetyStatus, this); + m_serviceClearSafetyStatus = m_node_handle.advertiseService("device_config/clear_safety_status", &DeviceConfigSimulationServices::ClearSafetyStatus, this); + m_serviceGetAllSafetyConfiguration = m_node_handle.advertiseService("device_config/get_all_safety_configuration", &DeviceConfigSimulationServices::GetAllSafetyConfiguration, this); + m_serviceGetAllSafetyInformation = m_node_handle.advertiseService("device_config/get_all_safety_information", &DeviceConfigSimulationServices::GetAllSafetyInformation, this); + m_serviceResetSafetyDefaults = m_node_handle.advertiseService("device_config/reset_safety_defaults", &DeviceConfigSimulationServices::ResetSafetyDefaults, this); + m_serviceOnNotificationSafetyTopic = m_node_handle.advertiseService("device_config/activate_publishing_of_safety_topic", &DeviceConfigSimulationServices::OnNotificationSafetyTopic, this); + m_serviceExecuteCalibration = m_node_handle.advertiseService("device_config/execute_calibration", &DeviceConfigSimulationServices::ExecuteCalibration, this); + m_serviceGetCalibrationResult = m_node_handle.advertiseService("device_config/get_calibration_result", &DeviceConfigSimulationServices::GetCalibrationResult, this); + m_serviceStopCalibration = m_node_handle.advertiseService("device_config/stop_calibration", &DeviceConfigSimulationServices::StopCalibration, this); + m_serviceDeviceConfig_SetCapSenseConfig = m_node_handle.advertiseService("device_config/set_cap_sense_config", &DeviceConfigSimulationServices::DeviceConfig_SetCapSenseConfig, this); + m_serviceDeviceConfig_GetCapSenseConfig = m_node_handle.advertiseService("device_config/get_cap_sense_config", &DeviceConfigSimulationServices::DeviceConfig_GetCapSenseConfig, this); +} + +bool DeviceConfigSimulationServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) +{ + ROS_WARN_ONCE("The SetDeviceID service is not implemented in simulation, and has no effect."); + return true; +} + +bool DeviceConfigSimulationServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) +{ + ROS_WARN_ONCE("The SetDeviceID service is not implemented in simulation, and has no effect."); + return true; +} + + +bool DeviceConfigSimulationServices::GetRunMode(kortex_driver::GetRunMode::Request &req, kortex_driver::GetRunMode::Response &res) +{ + + + if (GetRunModeHandler) + { + res = GetRunModeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/get_run_mode is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::SetRunMode(kortex_driver::SetRunMode::Request &req, kortex_driver::SetRunMode::Response &res) +{ + + + if (SetRunModeHandler) + { + res = SetRunModeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/set_run_mode is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::GetDeviceType(kortex_driver::GetDeviceType::Request &req, kortex_driver::GetDeviceType::Response &res) +{ + + + if (GetDeviceTypeHandler) + { + res = GetDeviceTypeHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/get_device_type is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::GetFirmwareVersion(kortex_driver::GetFirmwareVersion::Request &req, kortex_driver::GetFirmwareVersion::Response &res) +{ + + + if (GetFirmwareVersionHandler) + { + res = GetFirmwareVersionHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/get_firmware_version is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::GetBootloaderVersion(kortex_driver::GetBootloaderVersion::Request &req, kortex_driver::GetBootloaderVersion::Response &res) +{ + + + if (GetBootloaderVersionHandler) + { + res = GetBootloaderVersionHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/get_bootloader_version is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::GetModelNumber(kortex_driver::GetModelNumber::Request &req, kortex_driver::GetModelNumber::Response &res) +{ + + + if (GetModelNumberHandler) + { + res = GetModelNumberHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/get_model_number is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::GetPartNumber(kortex_driver::GetPartNumber::Request &req, kortex_driver::GetPartNumber::Response &res) +{ + + + if (GetPartNumberHandler) + { + res = GetPartNumberHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/get_part_number is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::GetSerialNumber(kortex_driver::GetSerialNumber::Request &req, kortex_driver::GetSerialNumber::Response &res) +{ + + + if (GetSerialNumberHandler) + { + res = GetSerialNumberHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/get_serial_number is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::GetMACAddress(kortex_driver::GetMACAddress::Request &req, kortex_driver::GetMACAddress::Response &res) +{ + + + if (GetMACAddressHandler) + { + res = GetMACAddressHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/get_m_a_c_address is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::GetIPv4Settings(kortex_driver::GetIPv4Settings::Request &req, kortex_driver::GetIPv4Settings::Response &res) +{ + + + if (GetIPv4SettingsHandler) + { + res = GetIPv4SettingsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/get_i_pv4_settings is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::SetIPv4Settings(kortex_driver::SetIPv4Settings::Request &req, kortex_driver::SetIPv4Settings::Response &res) +{ + + + if (SetIPv4SettingsHandler) + { + res = SetIPv4SettingsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/set_i_pv4_settings is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::GetPartNumberRevision(kortex_driver::GetPartNumberRevision::Request &req, kortex_driver::GetPartNumberRevision::Response &res) +{ + + + if (GetPartNumberRevisionHandler) + { + res = GetPartNumberRevisionHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/get_part_number_revision is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::RebootRequest(kortex_driver::RebootRequest::Request &req, kortex_driver::RebootRequest::Response &res) +{ + + + if (RebootRequestHandler) + { + res = RebootRequestHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/reboot_request is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::SetSafetyEnable(kortex_driver::SetSafetyEnable::Request &req, kortex_driver::SetSafetyEnable::Response &res) +{ + + + if (SetSafetyEnableHandler) + { + res = SetSafetyEnableHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/set_safety_enable is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::SetSafetyErrorThreshold(kortex_driver::SetSafetyErrorThreshold::Request &req, kortex_driver::SetSafetyErrorThreshold::Response &res) +{ + + + if (SetSafetyErrorThresholdHandler) + { + res = SetSafetyErrorThresholdHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/set_safety_error_threshold is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::SetSafetyWarningThreshold(kortex_driver::SetSafetyWarningThreshold::Request &req, kortex_driver::SetSafetyWarningThreshold::Response &res) +{ + + + if (SetSafetyWarningThresholdHandler) + { + res = SetSafetyWarningThresholdHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/set_safety_warning_threshold is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::SetSafetyConfiguration(kortex_driver::SetSafetyConfiguration::Request &req, kortex_driver::SetSafetyConfiguration::Response &res) +{ + + + if (SetSafetyConfigurationHandler) + { + res = SetSafetyConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/set_safety_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::GetSafetyConfiguration(kortex_driver::GetSafetyConfiguration::Request &req, kortex_driver::GetSafetyConfiguration::Response &res) +{ + + + if (GetSafetyConfigurationHandler) + { + res = GetSafetyConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/get_safety_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::GetSafetyInformation(kortex_driver::GetSafetyInformation::Request &req, kortex_driver::GetSafetyInformation::Response &res) +{ + + + if (GetSafetyInformationHandler) + { + res = GetSafetyInformationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/get_safety_information is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::GetSafetyEnable(kortex_driver::GetSafetyEnable::Request &req, kortex_driver::GetSafetyEnable::Response &res) +{ + + + if (GetSafetyEnableHandler) + { + res = GetSafetyEnableHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/get_safety_enable is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::GetSafetyStatus(kortex_driver::GetSafetyStatus::Request &req, kortex_driver::GetSafetyStatus::Response &res) +{ + + + if (GetSafetyStatusHandler) + { + res = GetSafetyStatusHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/get_safety_status is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::ClearAllSafetyStatus(kortex_driver::ClearAllSafetyStatus::Request &req, kortex_driver::ClearAllSafetyStatus::Response &res) +{ + + + if (ClearAllSafetyStatusHandler) + { + res = ClearAllSafetyStatusHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/clear_all_safety_status is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::ClearSafetyStatus(kortex_driver::ClearSafetyStatus::Request &req, kortex_driver::ClearSafetyStatus::Response &res) +{ + + + if (ClearSafetyStatusHandler) + { + res = ClearSafetyStatusHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/clear_safety_status is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::GetAllSafetyConfiguration(kortex_driver::GetAllSafetyConfiguration::Request &req, kortex_driver::GetAllSafetyConfiguration::Response &res) +{ + + + if (GetAllSafetyConfigurationHandler) + { + res = GetAllSafetyConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/get_all_safety_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::GetAllSafetyInformation(kortex_driver::GetAllSafetyInformation::Request &req, kortex_driver::GetAllSafetyInformation::Response &res) +{ + + + if (GetAllSafetyInformationHandler) + { + res = GetAllSafetyInformationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/get_all_safety_information is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::ResetSafetyDefaults(kortex_driver::ResetSafetyDefaults::Request &req, kortex_driver::ResetSafetyDefaults::Response &res) +{ + + + if (ResetSafetyDefaultsHandler) + { + res = ResetSafetyDefaultsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/reset_safety_defaults is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::OnNotificationSafetyTopic(kortex_driver::OnNotificationSafetyTopic::Request &req, kortex_driver::OnNotificationSafetyTopic::Response &res) +{ + + m_is_activated_SafetyTopic = true; + + if (OnNotificationSafetyTopicHandler) + { + res = OnNotificationSafetyTopicHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/activate_publishing_of_safety_topic is not implemented, so the service calls will return the default response."); + } + return true; +} +void DeviceConfigSimulationServices::cb_SafetyTopic(Kinova::Api::Common::SafetyNotification notif) +{ + kortex_driver::SafetyNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_SafetyTopic.publish(ros_msg); +} + +bool DeviceConfigSimulationServices::ExecuteCalibration(kortex_driver::ExecuteCalibration::Request &req, kortex_driver::ExecuteCalibration::Response &res) +{ + + + if (ExecuteCalibrationHandler) + { + res = ExecuteCalibrationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/execute_calibration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::GetCalibrationResult(kortex_driver::GetCalibrationResult::Request &req, kortex_driver::GetCalibrationResult::Response &res) +{ + + + if (GetCalibrationResultHandler) + { + res = GetCalibrationResultHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/get_calibration_result is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::StopCalibration(kortex_driver::StopCalibration::Request &req, kortex_driver::StopCalibration::Response &res) +{ + + + if (StopCalibrationHandler) + { + res = StopCalibrationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/stop_calibration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::DeviceConfig_SetCapSenseConfig(kortex_driver::DeviceConfig_SetCapSenseConfig::Request &req, kortex_driver::DeviceConfig_SetCapSenseConfig::Response &res) +{ + + + if (DeviceConfig_SetCapSenseConfigHandler) + { + res = DeviceConfig_SetCapSenseConfigHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/set_cap_sense_config is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool DeviceConfigSimulationServices::DeviceConfig_GetCapSenseConfig(kortex_driver::DeviceConfig_GetCapSenseConfig::Request &req, kortex_driver::DeviceConfig_GetCapSenseConfig::Response &res) +{ + + + if (DeviceConfig_GetCapSenseConfigHandler) + { + res = DeviceConfig_GetCapSenseConfigHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_config/get_cap_sense_config is not implemented, so the service calls will return the default response."); + } + return true; +} diff --git a/kortex_driver/src/generated/simulation/devicemanager_services.cpp b/kortex_driver/src/generated/simulation/devicemanager_services.cpp new file mode 100644 index 00000000..f0be4c07 --- /dev/null +++ b/kortex_driver/src/generated/simulation/devicemanager_services.cpp @@ -0,0 +1,82 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" +#include "kortex_driver/generated/simulation/devicemanager_services.h" + +DeviceManagerSimulationServices::DeviceManagerSimulationServices(ros::NodeHandle& node_handle): + IDeviceManagerServices(node_handle) +{ + m_pub_Error = m_node_handle.advertise("kortex_error", 1000); + + m_serviceSetDeviceID = m_node_handle.advertiseService("device_manager/set_device_id", &DeviceManagerSimulationServices::SetDeviceID, this); + m_serviceSetApiOptions = m_node_handle.advertiseService("device_manager/set_api_options", &DeviceManagerSimulationServices::SetApiOptions, this); + + m_serviceReadAllDevices = m_node_handle.advertiseService("device_manager/read_all_devices", &DeviceManagerSimulationServices::ReadAllDevices, this); +} + +bool DeviceManagerSimulationServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) +{ + ROS_WARN_ONCE("The SetDeviceID service is not implemented in simulation, and has no effect."); + return true; +} + +bool DeviceManagerSimulationServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) +{ + ROS_WARN_ONCE("The SetDeviceID service is not implemented in simulation, and has no effect."); + return true; +} + + +bool DeviceManagerSimulationServices::ReadAllDevices(kortex_driver::ReadAllDevices::Request &req, kortex_driver::ReadAllDevices::Response &res) +{ + + + if (ReadAllDevicesHandler) + { + res = ReadAllDevicesHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for device_manager/read_all_devices is not implemented, so the service calls will return the default response."); + } + return true; +} diff --git a/kortex_driver/src/generated/simulation/interconnectconfig_services.cpp b/kortex_driver/src/generated/simulation/interconnectconfig_services.cpp new file mode 100644 index 00000000..32287a2b --- /dev/null +++ b/kortex_driver/src/generated/simulation/interconnectconfig_services.cpp @@ -0,0 +1,290 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" +#include "kortex_driver/generated/simulation/interconnectconfig_services.h" + +InterconnectConfigSimulationServices::InterconnectConfigSimulationServices(ros::NodeHandle& node_handle): + IInterconnectConfigServices(node_handle) +{ + m_pub_Error = m_node_handle.advertise("kortex_error", 1000); + + m_serviceSetDeviceID = m_node_handle.advertiseService("interconnect_config/set_device_id", &InterconnectConfigSimulationServices::SetDeviceID, this); + m_serviceSetApiOptions = m_node_handle.advertiseService("interconnect_config/set_api_options", &InterconnectConfigSimulationServices::SetApiOptions, this); + + m_serviceGetUARTConfiguration = m_node_handle.advertiseService("interconnect_config/get_u_a_r_t_configuration", &InterconnectConfigSimulationServices::GetUARTConfiguration, this); + m_serviceSetUARTConfiguration = m_node_handle.advertiseService("interconnect_config/set_u_a_r_t_configuration", &InterconnectConfigSimulationServices::SetUARTConfiguration, this); + m_serviceGetEthernetConfiguration = m_node_handle.advertiseService("interconnect_config/get_ethernet_configuration", &InterconnectConfigSimulationServices::GetEthernetConfiguration, this); + m_serviceSetEthernetConfiguration = m_node_handle.advertiseService("interconnect_config/set_ethernet_configuration", &InterconnectConfigSimulationServices::SetEthernetConfiguration, this); + m_serviceGetGPIOConfiguration = m_node_handle.advertiseService("interconnect_config/get_g_p_i_o_configuration", &InterconnectConfigSimulationServices::GetGPIOConfiguration, this); + m_serviceSetGPIOConfiguration = m_node_handle.advertiseService("interconnect_config/set_g_p_i_o_configuration", &InterconnectConfigSimulationServices::SetGPIOConfiguration, this); + m_serviceGetGPIOState = m_node_handle.advertiseService("interconnect_config/get_g_p_i_o_state", &InterconnectConfigSimulationServices::GetGPIOState, this); + m_serviceSetGPIOState = m_node_handle.advertiseService("interconnect_config/set_g_p_i_o_state", &InterconnectConfigSimulationServices::SetGPIOState, this); + m_serviceGetI2CConfiguration = m_node_handle.advertiseService("interconnect_config/get_i2_c_configuration", &InterconnectConfigSimulationServices::GetI2CConfiguration, this); + m_serviceSetI2CConfiguration = m_node_handle.advertiseService("interconnect_config/set_i2_c_configuration", &InterconnectConfigSimulationServices::SetI2CConfiguration, this); + m_serviceI2CRead = m_node_handle.advertiseService("interconnect_config/i2_c_read", &InterconnectConfigSimulationServices::I2CRead, this); + m_serviceI2CReadRegister = m_node_handle.advertiseService("interconnect_config/i2_c_read_register", &InterconnectConfigSimulationServices::I2CReadRegister, this); + m_serviceI2CWrite = m_node_handle.advertiseService("interconnect_config/i2_c_write", &InterconnectConfigSimulationServices::I2CWrite, this); + m_serviceI2CWriteRegister = m_node_handle.advertiseService("interconnect_config/i2_c_write_register", &InterconnectConfigSimulationServices::I2CWriteRegister, this); +} + +bool InterconnectConfigSimulationServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) +{ + ROS_WARN_ONCE("The SetDeviceID service is not implemented in simulation, and has no effect."); + return true; +} + +bool InterconnectConfigSimulationServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) +{ + ROS_WARN_ONCE("The SetDeviceID service is not implemented in simulation, and has no effect."); + return true; +} + + +bool InterconnectConfigSimulationServices::GetUARTConfiguration(kortex_driver::GetUARTConfiguration::Request &req, kortex_driver::GetUARTConfiguration::Response &res) +{ + + + if (GetUARTConfigurationHandler) + { + res = GetUARTConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for interconnect_config/get_u_a_r_t_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool InterconnectConfigSimulationServices::SetUARTConfiguration(kortex_driver::SetUARTConfiguration::Request &req, kortex_driver::SetUARTConfiguration::Response &res) +{ + + + if (SetUARTConfigurationHandler) + { + res = SetUARTConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for interconnect_config/set_u_a_r_t_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool InterconnectConfigSimulationServices::GetEthernetConfiguration(kortex_driver::GetEthernetConfiguration::Request &req, kortex_driver::GetEthernetConfiguration::Response &res) +{ + + + if (GetEthernetConfigurationHandler) + { + res = GetEthernetConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for interconnect_config/get_ethernet_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool InterconnectConfigSimulationServices::SetEthernetConfiguration(kortex_driver::SetEthernetConfiguration::Request &req, kortex_driver::SetEthernetConfiguration::Response &res) +{ + + + if (SetEthernetConfigurationHandler) + { + res = SetEthernetConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for interconnect_config/set_ethernet_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool InterconnectConfigSimulationServices::GetGPIOConfiguration(kortex_driver::GetGPIOConfiguration::Request &req, kortex_driver::GetGPIOConfiguration::Response &res) +{ + + + if (GetGPIOConfigurationHandler) + { + res = GetGPIOConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for interconnect_config/get_g_p_i_o_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool InterconnectConfigSimulationServices::SetGPIOConfiguration(kortex_driver::SetGPIOConfiguration::Request &req, kortex_driver::SetGPIOConfiguration::Response &res) +{ + + + if (SetGPIOConfigurationHandler) + { + res = SetGPIOConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for interconnect_config/set_g_p_i_o_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool InterconnectConfigSimulationServices::GetGPIOState(kortex_driver::GetGPIOState::Request &req, kortex_driver::GetGPIOState::Response &res) +{ + + + if (GetGPIOStateHandler) + { + res = GetGPIOStateHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for interconnect_config/get_g_p_i_o_state is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool InterconnectConfigSimulationServices::SetGPIOState(kortex_driver::SetGPIOState::Request &req, kortex_driver::SetGPIOState::Response &res) +{ + + + if (SetGPIOStateHandler) + { + res = SetGPIOStateHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for interconnect_config/set_g_p_i_o_state is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool InterconnectConfigSimulationServices::GetI2CConfiguration(kortex_driver::GetI2CConfiguration::Request &req, kortex_driver::GetI2CConfiguration::Response &res) +{ + + + if (GetI2CConfigurationHandler) + { + res = GetI2CConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for interconnect_config/get_i2_c_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool InterconnectConfigSimulationServices::SetI2CConfiguration(kortex_driver::SetI2CConfiguration::Request &req, kortex_driver::SetI2CConfiguration::Response &res) +{ + + + if (SetI2CConfigurationHandler) + { + res = SetI2CConfigurationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for interconnect_config/set_i2_c_configuration is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool InterconnectConfigSimulationServices::I2CRead(kortex_driver::I2CRead::Request &req, kortex_driver::I2CRead::Response &res) +{ + + + if (I2CReadHandler) + { + res = I2CReadHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for interconnect_config/i2_c_read is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool InterconnectConfigSimulationServices::I2CReadRegister(kortex_driver::I2CReadRegister::Request &req, kortex_driver::I2CReadRegister::Response &res) +{ + + + if (I2CReadRegisterHandler) + { + res = I2CReadRegisterHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for interconnect_config/i2_c_read_register is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool InterconnectConfigSimulationServices::I2CWrite(kortex_driver::I2CWrite::Request &req, kortex_driver::I2CWrite::Response &res) +{ + + + if (I2CWriteHandler) + { + res = I2CWriteHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for interconnect_config/i2_c_write is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool InterconnectConfigSimulationServices::I2CWriteRegister(kortex_driver::I2CWriteRegister::Request &req, kortex_driver::I2CWriteRegister::Response &res) +{ + + + if (I2CWriteRegisterHandler) + { + res = I2CWriteRegisterHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for interconnect_config/i2_c_write_register is not implemented, so the service calls will return the default response."); + } + return true; +} diff --git a/kortex_driver/src/generated/simulation/visionconfig_services.cpp b/kortex_driver/src/generated/simulation/visionconfig_services.cpp new file mode 100644 index 00000000..b50c05c5 --- /dev/null +++ b/kortex_driver/src/generated/simulation/visionconfig_services.cpp @@ -0,0 +1,267 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2019 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "kortex_driver/generated/robot/common_proto_converter.h" +#include "kortex_driver/generated/robot/common_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorconfig_ros_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/actuatorcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_proto_converter.h" +#include "kortex_driver/generated/robot/productconfiguration_ros_converter.h" +#include "kortex_driver/generated/robot/base_proto_converter.h" +#include "kortex_driver/generated/robot/base_ros_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_proto_converter.h" +#include "kortex_driver/generated/robot/grippercyclic_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectcyclic_ros_converter.h" +#include "kortex_driver/generated/robot/basecyclic_proto_converter.h" +#include "kortex_driver/generated/robot/basecyclic_ros_converter.h" +#include "kortex_driver/generated/robot/controlconfig_proto_converter.h" +#include "kortex_driver/generated/robot/controlconfig_ros_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_proto_converter.h" +#include "kortex_driver/generated/robot/deviceconfig_ros_converter.h" +#include "kortex_driver/generated/robot/devicemanager_proto_converter.h" +#include "kortex_driver/generated/robot/devicemanager_ros_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_proto_converter.h" +#include "kortex_driver/generated/robot/interconnectconfig_ros_converter.h" +#include "kortex_driver/generated/robot/visionconfig_proto_converter.h" +#include "kortex_driver/generated/robot/visionconfig_ros_converter.h" +#include "kortex_driver/generated/simulation/visionconfig_services.h" + +VisionConfigSimulationServices::VisionConfigSimulationServices(ros::NodeHandle& node_handle): + IVisionConfigServices(node_handle) +{ + m_pub_Error = m_node_handle.advertise("kortex_error", 1000); + m_pub_VisionTopic = m_node_handle.advertise("vision_topic", 1000); + m_is_activated_VisionTopic = false; + + m_serviceSetDeviceID = m_node_handle.advertiseService("vision_config/set_device_id", &VisionConfigSimulationServices::SetDeviceID, this); + m_serviceSetApiOptions = m_node_handle.advertiseService("vision_config/set_api_options", &VisionConfigSimulationServices::SetApiOptions, this); + + m_serviceSetSensorSettings = m_node_handle.advertiseService("vision_config/set_sensor_settings", &VisionConfigSimulationServices::SetSensorSettings, this); + m_serviceGetSensorSettings = m_node_handle.advertiseService("vision_config/get_sensor_settings", &VisionConfigSimulationServices::GetSensorSettings, this); + m_serviceGetOptionValue = m_node_handle.advertiseService("vision_config/get_option_value", &VisionConfigSimulationServices::GetOptionValue, this); + m_serviceSetOptionValue = m_node_handle.advertiseService("vision_config/set_option_value", &VisionConfigSimulationServices::SetOptionValue, this); + m_serviceGetOptionInformation = m_node_handle.advertiseService("vision_config/get_option_information", &VisionConfigSimulationServices::GetOptionInformation, this); + m_serviceOnNotificationVisionTopic = m_node_handle.advertiseService("vision_config/activate_publishing_of_vision_topic", &VisionConfigSimulationServices::OnNotificationVisionTopic, this); + m_serviceDoSensorFocusAction = m_node_handle.advertiseService("vision_config/do_sensor_focus_action", &VisionConfigSimulationServices::DoSensorFocusAction, this); + m_serviceGetIntrinsicParameters = m_node_handle.advertiseService("vision_config/get_intrinsic_parameters", &VisionConfigSimulationServices::GetIntrinsicParameters, this); + m_serviceGetIntrinsicParametersProfile = m_node_handle.advertiseService("vision_config/get_intrinsic_parameters_profile", &VisionConfigSimulationServices::GetIntrinsicParametersProfile, this); + m_serviceSetIntrinsicParameters = m_node_handle.advertiseService("vision_config/set_intrinsic_parameters", &VisionConfigSimulationServices::SetIntrinsicParameters, this); + m_serviceGetExtrinsicParameters = m_node_handle.advertiseService("vision_config/get_extrinsic_parameters", &VisionConfigSimulationServices::GetExtrinsicParameters, this); + m_serviceSetExtrinsicParameters = m_node_handle.advertiseService("vision_config/set_extrinsic_parameters", &VisionConfigSimulationServices::SetExtrinsicParameters, this); +} + +bool VisionConfigSimulationServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) +{ + ROS_WARN_ONCE("The SetDeviceID service is not implemented in simulation, and has no effect."); + return true; +} + +bool VisionConfigSimulationServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) +{ + ROS_WARN_ONCE("The SetDeviceID service is not implemented in simulation, and has no effect."); + return true; +} + + +bool VisionConfigSimulationServices::SetSensorSettings(kortex_driver::SetSensorSettings::Request &req, kortex_driver::SetSensorSettings::Response &res) +{ + + + if (SetSensorSettingsHandler) + { + res = SetSensorSettingsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for vision_config/set_sensor_settings is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool VisionConfigSimulationServices::GetSensorSettings(kortex_driver::GetSensorSettings::Request &req, kortex_driver::GetSensorSettings::Response &res) +{ + + + if (GetSensorSettingsHandler) + { + res = GetSensorSettingsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for vision_config/get_sensor_settings is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool VisionConfigSimulationServices::GetOptionValue(kortex_driver::GetOptionValue::Request &req, kortex_driver::GetOptionValue::Response &res) +{ + + + if (GetOptionValueHandler) + { + res = GetOptionValueHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for vision_config/get_option_value is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool VisionConfigSimulationServices::SetOptionValue(kortex_driver::SetOptionValue::Request &req, kortex_driver::SetOptionValue::Response &res) +{ + + + if (SetOptionValueHandler) + { + res = SetOptionValueHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for vision_config/set_option_value is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool VisionConfigSimulationServices::GetOptionInformation(kortex_driver::GetOptionInformation::Request &req, kortex_driver::GetOptionInformation::Response &res) +{ + + + if (GetOptionInformationHandler) + { + res = GetOptionInformationHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for vision_config/get_option_information is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool VisionConfigSimulationServices::OnNotificationVisionTopic(kortex_driver::OnNotificationVisionTopic::Request &req, kortex_driver::OnNotificationVisionTopic::Response &res) +{ + + m_is_activated_VisionTopic = true; + + if (OnNotificationVisionTopicHandler) + { + res = OnNotificationVisionTopicHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for vision_config/activate_publishing_of_vision_topic is not implemented, so the service calls will return the default response."); + } + return true; +} +void VisionConfigSimulationServices::cb_VisionTopic(Kinova::Api::VisionConfig::VisionNotification notif) +{ + kortex_driver::VisionNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_VisionTopic.publish(ros_msg); +} + +bool VisionConfigSimulationServices::DoSensorFocusAction(kortex_driver::DoSensorFocusAction::Request &req, kortex_driver::DoSensorFocusAction::Response &res) +{ + + + if (DoSensorFocusActionHandler) + { + res = DoSensorFocusActionHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for vision_config/do_sensor_focus_action is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool VisionConfigSimulationServices::GetIntrinsicParameters(kortex_driver::GetIntrinsicParameters::Request &req, kortex_driver::GetIntrinsicParameters::Response &res) +{ + + + if (GetIntrinsicParametersHandler) + { + res = GetIntrinsicParametersHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for vision_config/get_intrinsic_parameters is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool VisionConfigSimulationServices::GetIntrinsicParametersProfile(kortex_driver::GetIntrinsicParametersProfile::Request &req, kortex_driver::GetIntrinsicParametersProfile::Response &res) +{ + + + if (GetIntrinsicParametersProfileHandler) + { + res = GetIntrinsicParametersProfileHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for vision_config/get_intrinsic_parameters_profile is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool VisionConfigSimulationServices::SetIntrinsicParameters(kortex_driver::SetIntrinsicParameters::Request &req, kortex_driver::SetIntrinsicParameters::Response &res) +{ + + + if (SetIntrinsicParametersHandler) + { + res = SetIntrinsicParametersHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for vision_config/set_intrinsic_parameters is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool VisionConfigSimulationServices::GetExtrinsicParameters(kortex_driver::GetExtrinsicParameters::Request &req, kortex_driver::GetExtrinsicParameters::Response &res) +{ + + + if (GetExtrinsicParametersHandler) + { + res = GetExtrinsicParametersHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for vision_config/get_extrinsic_parameters is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool VisionConfigSimulationServices::SetExtrinsicParameters(kortex_driver::SetExtrinsicParameters::Request &req, kortex_driver::SetExtrinsicParameters::Response &res) +{ + + + if (SetExtrinsicParametersHandler) + { + res = SetExtrinsicParametersHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for vision_config/set_extrinsic_parameters is not implemented, so the service calls will return the default response."); + } + return true; +} From 382909f8ef277f7f2f4fc7c81bc22c2aa4263362 Mon Sep 17 00:00:00 2001 From: Alexandre Vannobel Date: Wed, 10 Jun 2020 11:38:31 -0400 Subject: [PATCH 06/33] Added simulation option to the driver so the gazebo package can launch the kortex_driver too, in a different mode --- .../non-generated/kortex_arm_driver.h | 5 +- kortex_driver/launch/kortex_driver.launch | 1 + .../driver/kortex_arm_driver.cpp | 272 +++++++++++------- .../launch/spawn_kortex_robot.launch | 15 + kortex_gazebo/launch/test.launch | 53 ++++ kortex_gazebo/package.xml | 1 + 6 files changed, 237 insertions(+), 110 deletions(-) create mode 100644 kortex_gazebo/launch/test.launch diff --git a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h index 335a8cb0..5249fe33 100644 --- a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h +++ b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h @@ -83,6 +83,9 @@ class KortexArmDriver ros::NodeHandle m_node_handle; + // False if in simulation + bool m_is_real_robot; + // Api options std::string m_ip_address; std::string m_username; @@ -157,7 +160,7 @@ class KortexArmDriver // Private methods bool isGripperPresent(); void setAngularTrajectorySoftLimitsToMax(); - void publishFeedback(); + void publishRobotFeedback(); }; #endif diff --git a/kortex_driver/launch/kortex_driver.launch b/kortex_driver/launch/kortex_driver.launch index 3d6fe61d..113eaea3 100644 --- a/kortex_driver/launch/kortex_driver.launch +++ b/kortex_driver/launch/kortex_driver.launch @@ -56,6 +56,7 @@ + diff --git a/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp b/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp index 5f57e83e..1ead99b9 100644 --- a/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp +++ b/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp @@ -12,23 +12,36 @@ #include "kortex_driver/non-generated/kortex_arm_driver.h" -KortexArmDriver::KortexArmDriver(ros::NodeHandle nh): m_node_handle(nh), m_node_is_running(true), m_consecutive_base_cyclic_timeouts(0) +KortexArmDriver::KortexArmDriver(ros::NodeHandle nh): m_node_handle(nh), + m_node_is_running(true), + m_consecutive_base_cyclic_timeouts(0), + m_is_interconnect_module_present(false), + m_is_vision_module_present(false) { // Parameter to let the other nodes know this node is up ros::param::set("is_initialized", false); - // Start the node in the different initialization functions parseRosArguments(); - initApi(); - verifyProductConfiguration(); - initSubscribers(); + + // If with a real robot, start the robot-specific stuff + if (m_is_real_robot) + { + initApi(); + verifyProductConfiguration(); + initSubscribers(); + startActionServers(); + } + + // ROS Services are always started initRosServices(); - startActionServers(); // Start the thread to publish the feedback and joint states m_pub_base_feedback = m_node_handle.advertise("base_feedback", 1000); m_pub_joint_state = m_node_handle.advertise("base_feedback/joint_state", 1000); - m_publish_feedback_thread = std::thread(&KortexArmDriver::publishFeedback, this); + if (m_is_real_robot) + { + m_publish_feedback_thread = std::thread(&KortexArmDriver::publishRobotFeedback, this); + } // If we get here and no error was thrown we started the node correctly ROS_INFO("%sThe Kortex driver has been initialized correctly!%s", GREEN_COLOR_CONSOLE, RESET_COLOR_CONSOLE); @@ -44,14 +57,6 @@ KortexArmDriver::~KortexArmDriver() m_publish_feedback_thread.join(); } - delete m_action_server_follow_joint_trajectory; - if (m_action_server_gripper_command) - { - delete m_action_server_gripper_command; - } - - delete m_topic_subscribers; - delete m_actuator_config_ros_services; delete m_base_ros_services; delete m_control_config_ros_services; @@ -65,100 +70,126 @@ KortexArmDriver::~KortexArmDriver() { delete m_vision_config_ros_services; } - - m_tcp_session_manager->CloseSession(); - m_udp_session_manager->CloseSession(); - m_tcp_router->SetActivationStatus(false); - m_udp_router->SetActivationStatus(false); - m_tcp_transport->disconnect(); - m_udp_transport->disconnect(); - - delete m_actuator_config; - delete m_base; - delete m_control_config; - delete m_device_config; - delete m_device_manager; - delete m_interconnect_config; - delete m_vision_config; - delete m_base_cyclic; - - delete m_tcp_session_manager; - delete m_udp_session_manager; - - delete m_tcp_router; - delete m_udp_router; - delete m_tcp_transport; - delete m_udp_transport; -} -void KortexArmDriver::parseRosArguments() -{ - bool use_sim_time = false; - if (ros::param::get("/use_sim_time", use_sim_time)) + if (!m_is_real_robot) { - if (use_sim_time) + + delete m_action_server_follow_joint_trajectory; + if (m_action_server_gripper_command) { - std::string error_string = "ROS parameter /use_sim_time is true : you may experience problems and you should set it to false and restart the driver."; - ROS_WARN("%s", error_string.c_str()); + delete m_action_server_gripper_command; } - } - if (!ros::param::get("~ip_address", m_ip_address)) - { - std::string error_string = "IP address of the robot was not specified in the launch file, shutting down the node..."; - ROS_ERROR("%s", error_string.c_str()); - throw new std::runtime_error(error_string); - } + delete m_topic_subscribers; - if (!ros::param::get("~username", m_username)) - { - std::string error_string = "Username for the robot session was not specified in the launch file, shutting down the node..."; - ROS_ERROR("%s", error_string.c_str()); - throw new std::runtime_error(error_string); + m_tcp_session_manager->CloseSession(); + m_udp_session_manager->CloseSession(); + m_tcp_router->SetActivationStatus(false); + m_udp_router->SetActivationStatus(false); + m_tcp_transport->disconnect(); + m_udp_transport->disconnect(); + + delete m_actuator_config; + delete m_base; + delete m_control_config; + delete m_device_config; + delete m_device_manager; + delete m_interconnect_config; + delete m_vision_config; + delete m_base_cyclic; + + delete m_tcp_session_manager; + delete m_udp_session_manager; + + delete m_tcp_router; + delete m_udp_router; + delete m_tcp_transport; + delete m_udp_transport; } +} - if (!ros::param::get("~password", m_password)) +void KortexArmDriver::parseRosArguments() +{ + bool sim; + if (!ros::param::get("~sim", sim)) { - std::string error_string = "Password for the robot session was not specified in the launch file, shutting down the node..."; + std::string error_string = "Simulation was not specified in the launch file, shutting down the node..."; ROS_ERROR("%s", error_string.c_str()); throw new std::runtime_error(error_string); } + m_is_real_robot = !sim; - if (!ros::param::get("~use_hard_limits", m_use_hard_limits)) + // Some parameters are only necessary with a real robot + if (m_is_real_robot) { - std::string error_string = "Usage of hard limits as soft was not specified in the launch file, shutting down the node..."; - ROS_ERROR("%s", error_string.c_str()); - throw new std::runtime_error(error_string); - } + bool use_sim_time = false; + if (ros::param::get("/use_sim_time", use_sim_time)) + { + if (use_sim_time) + { + std::string error_string = "ROS parameter /use_sim_time is true : you may experience problems and you should set it to false and restart the driver."; + ROS_WARN("%s", error_string.c_str()); + } + } - if (!ros::param::get("~cyclic_data_publish_rate", m_cyclic_data_publish_rate)) - { - std::string error_string = "Publish rate of the cyclic data was not specified in the launch file, shutting down the node..."; - ROS_ERROR("%s", error_string.c_str()); - throw new std::runtime_error(error_string); - } + if (!ros::param::get("~ip_address", m_ip_address)) + { + std::string error_string = "IP address of the robot was not specified in the launch file, shutting down the node..."; + ROS_ERROR("%s", error_string.c_str()); + throw new std::runtime_error(error_string); + } - if (!ros::param::get("~api_rpc_timeout_ms", m_api_rpc_timeout_ms)) - { - std::string error_string = "API RPC timeout duration was not specified in the launch file, shutting down the node..."; - ROS_ERROR("%s", error_string.c_str()); - throw new std::runtime_error(error_string); - } + if (!ros::param::get("~username", m_username)) + { + std::string error_string = "Username for the robot session was not specified in the launch file, shutting down the node..."; + ROS_ERROR("%s", error_string.c_str()); + throw new std::runtime_error(error_string); + } - if (!ros::param::get("~api_session_inactivity_timeout_ms", m_api_session_inactivity_timeout_ms)) - { - std::string error_string = "API session inactivity timeout duration was not specified in the launch file, shutting down the node..."; - ROS_ERROR("%s", error_string.c_str()); - throw new std::runtime_error(error_string); + if (!ros::param::get("~password", m_password)) + { + std::string error_string = "Password for the robot session was not specified in the launch file, shutting down the node..."; + ROS_ERROR("%s", error_string.c_str()); + throw new std::runtime_error(error_string); + } + + if (!ros::param::get("~use_hard_limits", m_use_hard_limits)) + { + std::string error_string = "Usage of hard limits as soft was not specified in the launch file, shutting down the node..."; + ROS_ERROR("%s", error_string.c_str()); + throw new std::runtime_error(error_string); + } + + if (!ros::param::get("~api_rpc_timeout_ms", m_api_rpc_timeout_ms)) + { + std::string error_string = "API RPC timeout duration was not specified in the launch file, shutting down the node..."; + ROS_ERROR("%s", error_string.c_str()); + throw new std::runtime_error(error_string); + } + + if (!ros::param::get("~api_session_inactivity_timeout_ms", m_api_session_inactivity_timeout_ms)) + { + std::string error_string = "API session inactivity timeout duration was not specified in the launch file, shutting down the node..."; + ROS_ERROR("%s", error_string.c_str()); + throw new std::runtime_error(error_string); + } + + if (!ros::param::get("~api_connection_inactivity_timeout_ms", m_api_connection_inactivity_timeout_ms)) + { + std::string error_string = "API connection inactivity timeout duration was not specified in the launch file, shutting down the node..."; + ROS_ERROR("%s", error_string.c_str()); + throw new std::runtime_error(error_string); + } } - if (!ros::param::get("~api_connection_inactivity_timeout_ms", m_api_connection_inactivity_timeout_ms)) + + if (!ros::param::get("~cyclic_data_publish_rate", m_cyclic_data_publish_rate)) { - std::string error_string = "API connection inactivity timeout duration was not specified in the launch file, shutting down the node..."; + std::string error_string = "Publish rate of the cyclic data was not specified in the launch file, shutting down the node..."; ROS_ERROR("%s", error_string.c_str()); throw new std::runtime_error(error_string); } - + if (!ros::param::get("~arm", m_arm_name)) { std::string error_string = "Arm name was not specified in the launch file, shutting down the node..."; @@ -364,11 +395,7 @@ void KortexArmDriver::verifyProductConfiguration() ROS_INFO("-------------------------------------------------"); ROS_INFO("Scanning all devices in robot... "); - // We suppose we don't have an interconnect or a vision module until we find it - m_is_interconnect_module_present = false; - m_is_vision_module_present = false; - - // Loop through the devices to find the device ID's +// Loop through the devices to find the device ID's auto devices = m_device_manager->ReadAllDevices(); for (auto device : devices.device_handle()) { @@ -415,26 +442,53 @@ void KortexArmDriver::initRosServices() { ROS_INFO("-------------------------------------------------"); ROS_INFO("Initializing Kortex Driver's services..."); - m_actuator_config_ros_services = new ActuatorConfigRobotServices(m_node_handle, m_actuator_config, 0, m_api_rpc_timeout_ms); - m_base_ros_services = new BaseRobotServices(m_node_handle, m_base, 0, m_api_rpc_timeout_ms); - m_control_config_ros_services = new ControlConfigRobotServices(m_node_handle, m_control_config, 0, m_api_rpc_timeout_ms); - m_device_config_ros_services = new DeviceConfigRobotServices(m_node_handle, m_device_config, 0, m_api_rpc_timeout_ms); - m_device_manager_ros_services = new DeviceManagerRobotServices(m_node_handle, m_device_manager, 0, m_api_rpc_timeout_ms); - if (m_is_interconnect_module_present) - { - m_interconnect_config_ros_services = new InterconnectConfigRobotServices(m_node_handle, m_interconnect_config, m_interconnect_device_id, m_api_rpc_timeout_ms); - } - else - { - m_interconnect_config_ros_services = nullptr; - } - if (m_is_vision_module_present) - { - m_vision_config_ros_services = new VisionConfigRobotServices(m_node_handle, m_vision_config, m_vision_device_id, m_api_rpc_timeout_ms); + if (m_is_real_robot) + { + m_actuator_config_ros_services = new ActuatorConfigRobotServices(m_node_handle, m_actuator_config, 0, m_api_rpc_timeout_ms); + m_base_ros_services = new BaseRobotServices(m_node_handle, m_base, 0, m_api_rpc_timeout_ms); + m_control_config_ros_services = new ControlConfigRobotServices(m_node_handle, m_control_config, 0, m_api_rpc_timeout_ms); + m_device_config_ros_services = new DeviceConfigRobotServices(m_node_handle, m_device_config, 0, m_api_rpc_timeout_ms); + m_device_manager_ros_services = new DeviceManagerRobotServices(m_node_handle, m_device_manager, 0, m_api_rpc_timeout_ms); + if (m_is_interconnect_module_present) + { + m_interconnect_config_ros_services = new InterconnectConfigRobotServices(m_node_handle, m_interconnect_config, m_interconnect_device_id, m_api_rpc_timeout_ms); + } + else + { + m_interconnect_config_ros_services = nullptr; + } + if (m_is_vision_module_present) + { + m_vision_config_ros_services = new VisionConfigRobotServices(m_node_handle, m_vision_config, m_vision_device_id, m_api_rpc_timeout_ms); + } + else + { + m_vision_config_ros_services = nullptr; + } } else { - m_vision_config_ros_services = nullptr; + m_actuator_config_ros_services = new ActuatorConfigSimulationServices(m_node_handle); + m_base_ros_services = new BaseSimulationServices(m_node_handle); + m_control_config_ros_services = new ControlConfigSimulationServices(m_node_handle); + m_device_config_ros_services = new DeviceConfigSimulationServices(m_node_handle); + m_device_manager_ros_services = new DeviceManagerSimulationServices(m_node_handle); + if (m_is_interconnect_module_present) + { + m_interconnect_config_ros_services = new InterconnectConfigSimulationServices(m_node_handle); + } + else + { + m_interconnect_config_ros_services = nullptr; + } + if (m_is_vision_module_present) + { + m_vision_config_ros_services = new VisionConfigSimulationServices(m_node_handle); + } + else + { + m_vision_config_ros_services = nullptr; + } } std::this_thread::sleep_for(std::chrono::milliseconds(2000)); ROS_INFO("Kortex Driver's services initialized correctly."); @@ -492,7 +546,7 @@ void KortexArmDriver::setAngularTrajectorySoftLimitsToMax() } } -void KortexArmDriver::publishFeedback() +void KortexArmDriver::publishRobotFeedback() { Kinova::Api::BaseCyclic::Feedback feedback_from_api; diff --git a/kortex_gazebo/launch/spawn_kortex_robot.launch b/kortex_gazebo/launch/spawn_kortex_robot.launch index 646f42c8..57dc1cf3 100644 --- a/kortex_gazebo/launch/spawn_kortex_robot.launch +++ b/kortex_gazebo/launch/spawn_kortex_robot.launch @@ -18,6 +18,8 @@ + + @@ -119,6 +121,19 @@ + + + + + + + + + + + + + diff --git a/kortex_gazebo/launch/test.launch b/kortex_gazebo/launch/test.launch new file mode 100644 index 00000000..26999bcd --- /dev/null +++ b/kortex_gazebo/launch/test.launch @@ -0,0 +1,53 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/kortex_gazebo/package.xml b/kortex_gazebo/package.xml index 100ca5da..060b1d94 100644 --- a/kortex_gazebo/package.xml +++ b/kortex_gazebo/package.xml @@ -10,6 +10,7 @@ BSD catkin roslaunch + kortex_driver robot_state_publisher rviz joint_state_publisher From 4e448241c866d10c9c86aaa9ad07fcfaf2659bc7 Mon Sep 17 00:00:00 2001 From: Alexandre Vannobel Date: Wed, 10 Jun 2020 15:24:05 -0400 Subject: [PATCH 07/33] Adapt joint_limits files to correctly take prefix option --- .../gazebo_initial_joint_positions.yaml | 12 ++--- .../arms/gen3/6dof/config/joint_limits.yaml | 12 ++--- .../arms/gen3/6dof/urdf/gen3_macro.xacro | 2 +- .../gazebo_initial_joint_positions.yaml | 14 ++--- .../arms/gen3/7dof/config/joint_limits.yaml | 14 ++--- .../gen3_lite/6dof/config/joint_limits.yaml | 12 ++--- .../gen3_lite_2f/config/joint_limits.yaml | 2 +- .../robotiq_2f_140/config/joint_limits.yaml | 2 +- .../robotiq_2f_85/config/joint_limits.yaml | 2 +- kortex_driver/launch/kortex_driver.launch | 10 ++-- .../launch/kortex_dual_driver.launch | 8 +-- .../launch/spawn_kortex_robot.launch | 4 +- kortex_gazebo/launch/test.launch | 53 ------------------- 13 files changed, 47 insertions(+), 100 deletions(-) delete mode 100644 kortex_gazebo/launch/test.launch diff --git a/kortex_description/arms/gen3/6dof/config/gazebo_initial_joint_positions.yaml b/kortex_description/arms/gen3/6dof/config/gazebo_initial_joint_positions.yaml index 62f19ff8..729f82b9 100644 --- a/kortex_description/arms/gen3/6dof/config/gazebo_initial_joint_positions.yaml +++ b/kortex_description/arms/gen3/6dof/config/gazebo_initial_joint_positions.yaml @@ -1,6 +1,6 @@ -initial_joint_positions: "-J joint_1 1.57 - -J joint_2 -0.35 - -J joint_3 -2.00 - -J joint_4 0 - -J joint_5 -1.00 - -J joint_6 1.57 " \ No newline at end of file +initial_joint_positions: "-J $(arg prefix)joint_1 1.57 + -J $(arg prefix)joint_2 -0.35 + -J $(arg prefix)joint_3 -2.00 + -J $(arg prefix)joint_4 0 + -J $(arg prefix)joint_5 -1.00 + -J $(arg prefix)joint_6 1.57 " \ No newline at end of file diff --git a/kortex_description/arms/gen3/6dof/config/joint_limits.yaml b/kortex_description/arms/gen3/6dof/config/joint_limits.yaml index 8692c090..0f20e6bc 100644 --- a/kortex_description/arms/gen3/6dof/config/joint_limits.yaml +++ b/kortex_description/arms/gen3/6dof/config/joint_limits.yaml @@ -1,7 +1,7 @@ joint_names: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 diff --git a/kortex_description/arms/gen3/6dof/urdf/gen3_macro.xacro b/kortex_description/arms/gen3/6dof/urdf/gen3_macro.xacro index db152ab5..34dfa702 100644 --- a/kortex_description/arms/gen3/6dof/urdf/gen3_macro.xacro +++ b/kortex_description/arms/gen3/6dof/urdf/gen3_macro.xacro @@ -228,7 +228,7 @@ - + diff --git a/kortex_description/arms/gen3/7dof/config/gazebo_initial_joint_positions.yaml b/kortex_description/arms/gen3/7dof/config/gazebo_initial_joint_positions.yaml index 7cef4ab6..2ab9226f 100644 --- a/kortex_description/arms/gen3/7dof/config/gazebo_initial_joint_positions.yaml +++ b/kortex_description/arms/gen3/7dof/config/gazebo_initial_joint_positions.yaml @@ -1,7 +1,7 @@ -initial_joint_positions: "-J joint_1 1.57 - -J joint_2 -0.35 - -J joint_3 3.14 - -J joint_4 -2.00 - -J joint_5 0 - -J joint_6 -1.00 - -J joint_7 1.57" \ No newline at end of file +initial_joint_positions: "-J $(arg prefix)joint_1 1.57 + -J $(arg prefix)joint_2 -0.35 + -J $(arg prefix)joint_3 3.14 + -J $(arg prefix)joint_4 -2.00 + -J $(arg prefix)joint_5 0 + -J $(arg prefix)joint_6 -1.00 + -J $(arg prefix)joint_7 1.57" \ No newline at end of file diff --git a/kortex_description/arms/gen3/7dof/config/joint_limits.yaml b/kortex_description/arms/gen3/7dof/config/joint_limits.yaml index 37f3ee5b..28d1ff07 100644 --- a/kortex_description/arms/gen3/7dof/config/joint_limits.yaml +++ b/kortex_description/arms/gen3/7dof/config/joint_limits.yaml @@ -1,8 +1,8 @@ joint_names: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 - - joint_7 \ No newline at end of file + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 + - $(arg prefix)joint_7 \ No newline at end of file diff --git a/kortex_description/arms/gen3_lite/6dof/config/joint_limits.yaml b/kortex_description/arms/gen3_lite/6dof/config/joint_limits.yaml index 8692c090..0f20e6bc 100644 --- a/kortex_description/arms/gen3_lite/6dof/config/joint_limits.yaml +++ b/kortex_description/arms/gen3_lite/6dof/config/joint_limits.yaml @@ -1,7 +1,7 @@ joint_names: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 diff --git a/kortex_description/grippers/gen3_lite_2f/config/joint_limits.yaml b/kortex_description/grippers/gen3_lite_2f/config/joint_limits.yaml index fb76b03e..b0f21f26 100644 --- a/kortex_description/grippers/gen3_lite_2f/config/joint_limits.yaml +++ b/kortex_description/grippers/gen3_lite_2f/config/joint_limits.yaml @@ -1,5 +1,5 @@ gripper_joint_names: - - right_finger_bottom_joint + - $(arg prefix)right_finger_bottom_joint gripper_joint_limits_min: - 0.96 diff --git a/kortex_description/grippers/robotiq_2f_140/config/joint_limits.yaml b/kortex_description/grippers/robotiq_2f_140/config/joint_limits.yaml index 3c16e30a..dd1fd9e8 100644 --- a/kortex_description/grippers/robotiq_2f_140/config/joint_limits.yaml +++ b/kortex_description/grippers/robotiq_2f_140/config/joint_limits.yaml @@ -1,5 +1,5 @@ gripper_joint_names: - - finger_joint + - $(arg prefix)finger_joint gripper_joint_limits_min: - 0.000 diff --git a/kortex_description/grippers/robotiq_2f_85/config/joint_limits.yaml b/kortex_description/grippers/robotiq_2f_85/config/joint_limits.yaml index 3c64028a..d67e31fe 100644 --- a/kortex_description/grippers/robotiq_2f_85/config/joint_limits.yaml +++ b/kortex_description/grippers/robotiq_2f_85/config/joint_limits.yaml @@ -1,5 +1,5 @@ gripper_joint_names: - - finger_joint + - $(arg prefix)finger_joint gripper_joint_limits_min: - 0.000 diff --git a/kortex_driver/launch/kortex_driver.launch b/kortex_driver/launch/kortex_driver.launch index 113eaea3..febffd7d 100644 --- a/kortex_driver/launch/kortex_driver.launch +++ b/kortex_driver/launch/kortex_driver.launch @@ -71,9 +71,9 @@ - + - + @@ -99,7 +99,7 @@ - + @@ -117,9 +117,9 @@ - + - + diff --git a/kortex_driver/launch/kortex_dual_driver.launch b/kortex_driver/launch/kortex_dual_driver.launch index 0939e816..436a91f9 100644 --- a/kortex_driver/launch/kortex_dual_driver.launch +++ b/kortex_driver/launch/kortex_dual_driver.launch @@ -90,9 +90,9 @@ - + - + launch-prefix="gdb -ex run args" @@ -109,9 +109,9 @@ - + - + diff --git a/kortex_gazebo/launch/spawn_kortex_robot.launch b/kortex_gazebo/launch/spawn_kortex_robot.launch index 57dc1cf3..5197b2cf 100644 --- a/kortex_gazebo/launch/spawn_kortex_robot.launch +++ b/kortex_gazebo/launch/spawn_kortex_robot.launch @@ -129,9 +129,9 @@ - + - + diff --git a/kortex_gazebo/launch/test.launch b/kortex_gazebo/launch/test.launch deleted file mode 100644 index 26999bcd..00000000 --- a/kortex_gazebo/launch/test.launch +++ /dev/null @@ -1,53 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file From 0e6cfe6b5593885e961992372d6cb282409f30a3 Mon Sep 17 00:00:00 2001 From: Alexandre Vannobel Date: Wed, 10 Jun 2020 16:19:04 -0400 Subject: [PATCH 08/33] Change description and control files to support prefix option --- .../config/joint_position_controllers.yaml | 52 ++++++------- .../config/joint_position_controllers.yaml | 60 +++++++-------- .../config/joint_position_controllers.yaml | 52 ++++++------- .../gripper_action_controller_parameters.yaml | 12 +-- .../gripper_action_controller_parameters.yaml | 16 ++-- .../gripper_action_controller_parameters.yaml | 16 ++-- .../gen3_lite_2f_transmission_macro.xacro | 24 +++--- .../robotiq_2f_140_transmission_macro.xacro | 32 ++++---- .../robotiq_2f_85_transmission_macro.xacro | 32 ++++---- .../launch/spawn_kortex_robot.launch | 76 ++++++++++--------- 10 files changed, 189 insertions(+), 183 deletions(-) diff --git a/kortex_control/arms/gen3/6dof/config/joint_position_controllers.yaml b/kortex_control/arms/gen3/6dof/config/joint_position_controllers.yaml index 05e23931..5c2f22bf 100644 --- a/kortex_control/arms/gen3/6dof/config/joint_position_controllers.yaml +++ b/kortex_control/arms/gen3/6dof/config/joint_position_controllers.yaml @@ -1,17 +1,17 @@ # Publish all joint states ----------------------------------- -joint_state_controller: +$(arg prefix)joint_state_controller: type: joint_state_controller/JointStateController publish_rate: 50 -gen3_joint_trajectory_controller: +$(arg prefix)gen3_joint_trajectory_controller: type: effort_controllers/JointTrajectoryController joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 constraints: goal_time: 1.0 stopped_velocity_tolerance: 0.5 @@ -19,55 +19,55 @@ gen3_joint_trajectory_controller: state_publish_rate: 25 action_monitor_rate: 25 gains: - joint_1: {p: 3000.0, i: 0.0, d: 2.0, i_clamp_min: -100.0, i_clamp_max: 100.0} - joint_2: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -5.0, i_clamp_max: 5.0} - joint_3: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -1.0, i_clamp_max: 1.0} - joint_4: {p: 750.0, i: 0.0, d: 0.2, i_clamp_min: -1.0, i_clamp_max: 1.0} - joint_5: {p: 5000.0, i: 0.0, d: 1.0, i_clamp_min: -1.0, i_clamp_max: 1.0} - joint_6: {p: 100.0, i: 0.0, d: 0.0, i_clamp_min: -0.1, i_clamp_max: 0.1} + $(arg prefix)joint_1: {p: 3000.0, i: 0.0, d: 2.0, i_clamp_min: -100.0, i_clamp_max: 100.0} + $(arg prefix)joint_2: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -5.0, i_clamp_max: 5.0} + $(arg prefix)joint_3: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -1.0, i_clamp_max: 1.0} + $(arg prefix)joint_4: {p: 750.0, i: 0.0, d: 0.2, i_clamp_min: -1.0, i_clamp_max: 1.0} + $(arg prefix)joint_5: {p: 5000.0, i: 0.0, d: 1.0, i_clamp_min: -1.0, i_clamp_max: 1.0} + $(arg prefix)joint_6: {p: 100.0, i: 0.0, d: 0.0, i_clamp_min: -0.1, i_clamp_max: 0.1} -joint_1_position_controller: - joint: joint_1 +$(arg prefix)joint_1_position_controller: + joint: $(arg prefix)joint_1 pid: p: 3000.0 i: 0.0 d: 2.0 type: effort_controllers/JointPositionController -joint_2_position_controller: - joint: joint_2 +$(arg prefix)joint_2_position_controller: + joint: $(arg prefix)joint_2 pid: p: 50000.0 i: 0.0 d: 0.0 type: effort_controllers/JointPositionController -joint_3_position_controller: - joint: joint_3 +$(arg prefix)joint_3_position_controller: + joint: $(arg prefix)joint_3 pid: p: 50000.0 i: 0.0 d: 0.0 type: effort_controllers/JointPositionController -joint_4_position_controller: - joint: joint_4 +$(arg prefix)joint_4_position_controller: + joint: $(arg prefix)joint_4 pid: p: 750.0 i: 0.0 d: 0.2 type: effort_controllers/JointPositionController -joint_5_position_controller: - joint: joint_5 +$(arg prefix)joint_5_position_controller: + joint: $(arg prefix)joint_5 pid: p: 5000.0 i: 0.0 d: 1.0 type: effort_controllers/JointPositionController -joint_6_position_controller: - joint: joint_6 +$(arg prefix)joint_6_position_controller: + joint: $(arg prefix)joint_6 pid: p: 100.0 i: 0.0 diff --git a/kortex_control/arms/gen3/7dof/config/joint_position_controllers.yaml b/kortex_control/arms/gen3/7dof/config/joint_position_controllers.yaml index bb6fa834..621a0b30 100644 --- a/kortex_control/arms/gen3/7dof/config/joint_position_controllers.yaml +++ b/kortex_control/arms/gen3/7dof/config/joint_position_controllers.yaml @@ -1,18 +1,18 @@ # Publish all joint states ----------------------------------- -joint_state_controller: +$(arg prefix)joint_state_controller: type: joint_state_controller/JointStateController publish_rate: 50 -gen3_joint_trajectory_controller: +$(arg prefix)gen3_joint_trajectory_controller: type: effort_controllers/JointTrajectoryController joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 - - joint_7 + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 + - $(arg prefix)joint_7 constraints: goal_time: 1.0 stopped_velocity_tolerance: 0.5 @@ -20,64 +20,64 @@ gen3_joint_trajectory_controller: state_publish_rate: 25 action_monitor_rate: 25 gains: - joint_1: {p: 3000.0, i: 0.0, d: 2.0, i_clamp_min: -100.0, i_clamp_max: 100.0} - joint_2: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -5.0, i_clamp_max: 5.0} - joint_3: {p: 3000.0, i: 0.0, d: 0.0, i_clamp_min: -1.0, i_clamp_max: 1.0} - joint_4: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -1.0, i_clamp_max: 1.0} - joint_5: {p: 750.0, i: 0.0, d: 0.2, i_clamp_min: -1.0, i_clamp_max: 1.0} - joint_6: {p: 5000.0, i: 0.0, d: 1.0, i_clamp_min: -1.0, i_clamp_max: 1.0} - joint_7: {p: 100.0, i: 0.0, d: 0.0, i_clamp_min: -0.1, i_clamp_max: 0.1} + $(arg prefix)joint_1: {p: 3000.0, i: 0.0, d: 2.0, i_clamp_min: -100.0, i_clamp_max: 100.0} + $(arg prefix)joint_2: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -5.0, i_clamp_max: 5.0} + $(arg prefix)joint_3: {p: 3000.0, i: 0.0, d: 0.0, i_clamp_min: -1.0, i_clamp_max: 1.0} + $(arg prefix)joint_4: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -1.0, i_clamp_max: 1.0} + $(arg prefix)joint_5: {p: 750.0, i: 0.0, d: 0.2, i_clamp_min: -1.0, i_clamp_max: 1.0} + $(arg prefix)joint_6: {p: 5000.0, i: 0.0, d: 1.0, i_clamp_min: -1.0, i_clamp_max: 1.0} + $(arg prefix)joint_7: {p: 100.0, i: 0.0, d: 0.0, i_clamp_min: -0.1, i_clamp_max: 0.1} -joint_1_position_controller: - joint: joint_1 +$(arg prefix)joint_1_position_controller: + joint: $(arg prefix)joint_1 pid: p: 3000.0 i: 0.0 d: 2.0 type: effort_controllers/JointPositionController -joint_2_position_controller: - joint: joint_2 +$(arg prefix)joint_2_position_controller: + joint: $(arg prefix)joint_2 pid: p: 50000.0 i: 0.0 d: 0.0 type: effort_controllers/JointPositionController -joint_3_position_controller: - joint: joint_3 +$(arg prefix)joint_3_position_controller: + joint: $(arg prefix)joint_3 pid: p: 3000.0 i: 0.0 d: 0.0 type: effort_controllers/JointPositionController -joint_4_position_controller: - joint: joint_4 +$(arg prefix)joint_4_position_controller: + joint: $(arg prefix)joint_4 pid: p: 50000.0 i: 0.0 d: 0.0 type: effort_controllers/JointPositionController -joint_5_position_controller: - joint: joint_5 +$(arg prefix)joint_5_position_controller: + joint: $(arg prefix)joint_5 pid: p: 750.0 i: 0.0 d: 0.2 type: effort_controllers/JointPositionController -joint_6_position_controller: - joint: joint_6 +$(arg prefix)joint_6_position_controller: + joint: $(arg prefix)joint_6 pid: p: 5000.0 i: 0.0 d: 1.0 type: effort_controllers/JointPositionController -joint_7_position_controller: - joint: joint_7 +$(arg prefix)joint_7_position_controller: + joint: $(arg prefix)joint_7 pid: p: 100.0 i: 0.0 diff --git a/kortex_control/arms/gen3_lite/6dof/config/joint_position_controllers.yaml b/kortex_control/arms/gen3_lite/6dof/config/joint_position_controllers.yaml index 05c1c470..c869f63a 100644 --- a/kortex_control/arms/gen3_lite/6dof/config/joint_position_controllers.yaml +++ b/kortex_control/arms/gen3_lite/6dof/config/joint_position_controllers.yaml @@ -1,17 +1,17 @@ # Publish all joint states ----------------------------------- -joint_state_controller: +$(arg prefix)joint_state_controller: type: joint_state_controller/JointStateController publish_rate: 50 -gen3_lite_joint_trajectory_controller: +$(arg prefix)gen3_lite_joint_trajectory_controller: type: effort_controllers/JointTrajectoryController joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 constraints: goal_time: 1.0 stopped_velocity_tolerance: 0.5 @@ -19,55 +19,55 @@ gen3_lite_joint_trajectory_controller: state_publish_rate: 25 action_monitor_rate: 25 gains: - joint_1: {p: 3000.0, i: 0.0, d: 2.0, i_clamp_min: -100.0, i_clamp_max: 100.0} - joint_2: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -5.0, i_clamp_max: 5.0} - joint_3: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -1.0, i_clamp_max: 1.0} - joint_4: {p: 750.0, i: 0.0, d: 0.2, i_clamp_min: -1.0, i_clamp_max: 1.0} - joint_5: {p: 5000.0, i: 0.0, d: 1.0, i_clamp_min: -1.0, i_clamp_max: 1.0} - joint_6: {p: 100.0, i: 0.0, d: 0.0, i_clamp_min: -0.1, i_clamp_max: 0.1} + $(arg prefix)joint_1: {p: 3000.0, i: 0.0, d: 2.0, i_clamp_min: -100.0, i_clamp_max: 100.0} + $(arg prefix)joint_2: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -5.0, i_clamp_max: 5.0} + $(arg prefix)joint_3: {p: 50000.0, i: 0.0, d: 0.0, i_clamp_min: -1.0, i_clamp_max: 1.0} + $(arg prefix)joint_4: {p: 750.0, i: 0.0, d: 0.2, i_clamp_min: -1.0, i_clamp_max: 1.0} + $(arg prefix)joint_5: {p: 5000.0, i: 0.0, d: 1.0, i_clamp_min: -1.0, i_clamp_max: 1.0} + $(arg prefix)joint_6: {p: 100.0, i: 0.0, d: 0.0, i_clamp_min: -0.1, i_clamp_max: 0.1} -joint_1_position_controller: - joint: joint_1 +$(arg prefix)joint_1_position_controller: + joint: $(arg prefix)joint_1 pid: p: 3000.0 i: 0.0 d: 2.0 type: effort_controllers/JointPositionController -joint_2_position_controller: - joint: joint_2 +$(arg prefix)joint_2_position_controller: + joint: $(arg prefix)joint_2 pid: p: 50000.0 i: 0.0 d: 0.0 type: effort_controllers/JointPositionController -joint_3_position_controller: - joint: joint_3 +$(arg prefix)joint_3_position_controller: + joint: $(arg prefix)joint_3 pid: p: 50000.0 i: 0.0 d: 0.0 type: effort_controllers/JointPositionController -joint_4_position_controller: - joint: joint_4 +$(arg prefix)joint_4_position_controller: + joint: $(arg prefix)joint_4 pid: p: 750.0 i: 0.0 d: 0.2 type: effort_controllers/JointPositionController -joint_5_position_controller: - joint: joint_5 +$(arg prefix)joint_5_position_controller: + joint: $(arg prefix)joint_5 pid: p: 5000.0 i: 0.0 d: 1.0 type: effort_controllers/JointPositionController -joint_6_position_controller: - joint: joint_6 +$(arg prefix)joint_6_position_controller: + joint: $(arg prefix)joint_6 pid: p: 100.0 i: 0.0 diff --git a/kortex_control/grippers/gen3_lite_2f/config/gripper_action_controller_parameters.yaml b/kortex_control/grippers/gen3_lite_2f/config/gripper_action_controller_parameters.yaml index 7d8c81d0..652cfca3 100644 --- a/kortex_control/grippers/gen3_lite_2f/config/gripper_action_controller_parameters.yaml +++ b/kortex_control/grippers/gen3_lite_2f/config/gripper_action_controller_parameters.yaml @@ -1,11 +1,11 @@ -gen3_lite_2f_gripper_controller: +$(arg prefix)gen3_lite_2f_gripper_controller: type: position_controllers/GripperActionController - joint: right_finger_bottom_joint + joint: $(arg prefix)right_finger_bottom_joint action_monitor_rate: 100 gazebo_ros_control: pid_gains: - right_finger_bottom_joint: {p: 0.1, i: 0.0, d: 0.0} - right_finger_tip_joint: {p: 1.0, i: 0.0, d: 0.0} - left_finger_bottom_joint: {p: 10.0, i: 0.0, d: 0.0} - left_finger_tip_joint: {p: 1.0, i: 0.0, d: 0.0} \ No newline at end of file + $(arg prefix)right_finger_bottom_joint: {p: 0.1, i: 0.0, d: 0.0} + $(arg prefix)right_finger_tip_joint: {p: 1.0, i: 0.0, d: 0.0} + $(arg prefix)left_finger_bottom_joint: {p: 10.0, i: 0.0, d: 0.0} + $(arg prefix)left_finger_tip_joint: {p: 1.0, i: 0.0, d: 0.0} \ No newline at end of file diff --git a/kortex_control/grippers/robotiq_2f_140/config/gripper_action_controller_parameters.yaml b/kortex_control/grippers/robotiq_2f_140/config/gripper_action_controller_parameters.yaml index bc61292d..869f515a 100644 --- a/kortex_control/grippers/robotiq_2f_140/config/gripper_action_controller_parameters.yaml +++ b/kortex_control/grippers/robotiq_2f_140/config/gripper_action_controller_parameters.yaml @@ -1,13 +1,13 @@ -robotiq_2f_140_gripper_controller: +$(arg prefix)robotiq_2f_140_gripper_controller: type: position_controllers/GripperActionController - joint: finger_joint + joint: $(arg prefix)finger_joint action_monitor_rate: 100 gazebo_ros_control: pid_gains: - finger_joint: {p: 10.0, i: 0.0, d: 0.01} - right_outer_knuckle_joint: {p: 10.0, i: 0.0, d: 0.00} - right_inner_knuckle_joint: {p: 10.0, i: 0.0, d: 0.01} - left_inner_knuckle_joint: {p: 10.0, i: 0.0, d: 0.01} - left_inner_finger_joint: {p: 0.5, i: 0.0, d: 0.001} - right_inner_finger_joint: {p: 0.5, i: 0.0, d: 0.001} \ No newline at end of file + $(arg prefix)finger_joint: {p: 10.0, i: 0.0, d: 0.01} + $(arg prefix)right_outer_knuckle_joint: {p: 10.0, i: 0.0, d: 0.00} + $(arg prefix)right_inner_knuckle_joint: {p: 10.0, i: 0.0, d: 0.01} + $(arg prefix)left_inner_knuckle_joint: {p: 10.0, i: 0.0, d: 0.01} + $(arg prefix)left_inner_finger_joint: {p: 0.5, i: 0.0, d: 0.001} + $(arg prefix)right_inner_finger_joint: {p: 0.5, i: 0.0, d: 0.001} \ No newline at end of file diff --git a/kortex_control/grippers/robotiq_2f_85/config/gripper_action_controller_parameters.yaml b/kortex_control/grippers/robotiq_2f_85/config/gripper_action_controller_parameters.yaml index dc06e4f8..4e40277e 100644 --- a/kortex_control/grippers/robotiq_2f_85/config/gripper_action_controller_parameters.yaml +++ b/kortex_control/grippers/robotiq_2f_85/config/gripper_action_controller_parameters.yaml @@ -1,13 +1,13 @@ -robotiq_2f_85_gripper_controller: +$(arg prefix)robotiq_2f_85_gripper_controller: type: position_controllers/GripperActionController - joint: finger_joint + joint: $(arg prefix)finger_joint action_monitor_rate: 100 gazebo_ros_control: pid_gains: - finger_joint: {p: 10.0, i: 0.0, d: 0.01} - right_outer_knuckle_joint: {p: 10.0, i: 0.0, d: 0.00} - right_inner_knuckle_joint: {p: 10.0, i: 0.0, d: 0.01} - left_inner_knuckle_joint: {p: 10.0, i: 0.0, d: 0.01} - left_inner_finger_joint: {p: 0.5, i: 0.0, d: 0.001} - right_inner_finger_joint: {p: 0.5, i: 0.0, d: 0.001} \ No newline at end of file + $(arg prefix)finger_joint: {p: 10.0, i: 0.0, d: 0.01} + $(arg prefix)right_outer_knuckle_joint: {p: 10.0, i: 0.0, d: 0.00} + $(arg prefix)right_inner_knuckle_joint: {p: 10.0, i: 0.0, d: 0.01} + $(arg prefix)left_inner_knuckle_joint: {p: 10.0, i: 0.0, d: 0.01} + $(arg prefix)left_inner_finger_joint: {p: 0.5, i: 0.0, d: 0.001} + $(arg prefix)right_inner_finger_joint: {p: 0.5, i: 0.0, d: 0.001} \ No newline at end of file diff --git a/kortex_description/grippers/gen3_lite_2f/urdf/gen3_lite_2f_transmission_macro.xacro b/kortex_description/grippers/gen3_lite_2f/urdf/gen3_lite_2f_transmission_macro.xacro index d6e40348..e04ae667 100644 --- a/kortex_description/grippers/gen3_lite_2f/urdf/gen3_lite_2f_transmission_macro.xacro +++ b/kortex_description/grippers/gen3_lite_2f/urdf/gen3_lite_2f_transmission_macro.xacro @@ -3,12 +3,12 @@ - + transmission_interface/SimpleTransmission - + hardware_interface/PositionJointInterface - + 1 hardware_interface/PositionJointInterface @@ -16,24 +16,24 @@ - right_finger_bottom_joint - right_finger_tip_joint + ${prefix}right_finger_bottom_joint + ${prefix}right_finger_tip_joint -0.676 0.149 5.0 - right_finger_bottom_joint - left_finger_bottom_joint + ${prefix}right_finger_bottom_joint + ${prefix}left_finger_bottom_joint -1.0 0.0 5.0 - right_finger_bottom_joint - left_finger_tip_joint + ${prefix}right_finger_bottom_joint + ${prefix}left_finger_tip_joint -0.676 0.149 5.0 @@ -43,9 +43,9 @@ my_gen3_lite_gripper - end_effector_link - right_finger_dist_link - left_finger_dist_link + ${prefix}end_effector_link + ${prefix}right_finger_dist_link + ${prefix}left_finger_dist_link 100 10 diff --git a/kortex_description/grippers/robotiq_2f_140/urdf/robotiq_2f_140_transmission_macro.xacro b/kortex_description/grippers/robotiq_2f_140/urdf/robotiq_2f_140_transmission_macro.xacro index 11adb98f..f1a85b8c 100644 --- a/kortex_description/grippers/robotiq_2f_140/urdf/robotiq_2f_140_transmission_macro.xacro +++ b/kortex_description/grippers/robotiq_2f_140/urdf/robotiq_2f_140_transmission_macro.xacro @@ -3,12 +3,12 @@ - + transmission_interface/SimpleTransmission - + hardware_interface/PositionJointInterface - + 1 hardware_interface/PositionJointInterface @@ -16,40 +16,40 @@ - finger_joint - right_outer_knuckle_joint + ${prefix}finger_joint + ${prefix}right_outer_knuckle_joint -1.0 0.0 5.0 - finger_joint - right_inner_knuckle_joint + ${prefix}finger_joint + ${prefix}right_inner_knuckle_joint -1.0 0.0 5.0 - finger_joint - left_inner_knuckle_joint + ${prefix}finger_joint + ${prefix}left_inner_knuckle_joint -1.0 0.0 5.0 - finger_joint - left_inner_finger_joint + ${prefix}finger_joint + ${prefix}left_inner_finger_joint 1.0 0.0 5.0 - finger_joint - right_inner_finger_joint + ${prefix}finger_joint + ${prefix}right_inner_finger_joint 1.0 0.0 5.0 @@ -59,9 +59,9 @@ my_robotiq_gripper - bracelet_link - left_inner_finger - right_inner_finger + ${prefix}bracelet_link + ${prefix}left_inner_finger + ${prefix}right_inner_finger 100 10 diff --git a/kortex_description/grippers/robotiq_2f_85/urdf/robotiq_2f_85_transmission_macro.xacro b/kortex_description/grippers/robotiq_2f_85/urdf/robotiq_2f_85_transmission_macro.xacro index fe287f37..4ed28bab 100644 --- a/kortex_description/grippers/robotiq_2f_85/urdf/robotiq_2f_85_transmission_macro.xacro +++ b/kortex_description/grippers/robotiq_2f_85/urdf/robotiq_2f_85_transmission_macro.xacro @@ -3,12 +3,12 @@ - + transmission_interface/SimpleTransmission - + hardware_interface/PositionJointInterface - + 1 hardware_interface/PositionJointInterface @@ -16,40 +16,40 @@ - finger_joint - right_outer_knuckle_joint + ${prefix}finger_joint + ${prefix}right_outer_knuckle_joint 1.0 0.0 5.0 - finger_joint - right_inner_knuckle_joint + ${prefix}finger_joint + ${prefix}right_inner_knuckle_joint 1.0 0.0 5.0 - finger_joint - left_inner_knuckle_joint + ${prefix}finger_joint + ${prefix}left_inner_knuckle_joint 1.0 0.0 5.0 - finger_joint - left_inner_finger_joint + ${prefix}finger_joint + ${prefix}left_inner_finger_joint -1.0 0.0 5.0 - finger_joint - right_inner_finger_joint + ${prefix}finger_joint + ${prefix}right_inner_finger_joint -1.0 0.0 5.0 @@ -59,9 +59,9 @@ my_robotiq_gripper - bracelet_link - left_inner_finger - right_inner_finger + ${prefix}bracelet_link + ${prefix}left_inner_finger + ${prefix}right_inner_finger 100 10 diff --git a/kortex_gazebo/launch/spawn_kortex_robot.launch b/kortex_gazebo/launch/spawn_kortex_robot.launch index 5197b2cf..07c39613 100644 --- a/kortex_gazebo/launch/spawn_kortex_robot.launch +++ b/kortex_gazebo/launch/spawn_kortex_robot.launch @@ -44,10 +44,10 @@ - - @@ -57,30 +57,30 @@ - - + @@ -89,13 +89,13 @@ - - @@ -105,20 +105,26 @@ - - - + output="screen" args="$(arg prefix)joint_1_position_controller + $(arg prefix)joint_2_position_controller + $(arg prefix)joint_3_position_controller + $(arg prefix)joint_4_position_controller + $(arg prefix)joint_5_position_controller + $(arg prefix)joint_6_position_controller + $(arg prefix)joint_7_position_controller + $(arg prefix)joint_state_controller" /> + + From 9de4f6ca03c785914d87e0a3085158ac1737f187 Mon Sep 17 00:00:00 2001 From: Alexandre Vannobel Date: Wed, 10 Jun 2020 17:08:08 -0400 Subject: [PATCH 09/33] Change MoveIt config launch and config files to enable prefix option - only SRDF modifications remain --- kortex_driver/launch/kortex_driver.launch | 2 ++ .../launch/spawn_kortex_robot.launch | 2 ++ .../config/fake_controllers.yaml | 18 ++++++------- .../config/joint_limits.yaml | 12 ++++----- .../config/ros_controllers.yaml | 18 ++++++------- ...te_2f_moveit_controller_manager.launch.xml | 4 ++- .../launch/move_group.launch | 8 +++++- .../launch/planning_context.launch | 5 +++- .../launch/trajectory_execution.launch.xml | 6 +++-- .../config/6dof/default_joint_limits.yaml | 12 ++++----- .../config/6dof/fake_controllers.yaml | 14 +++++----- .../config/6dof/hard_joint_limits.yaml | 12 ++++----- .../config/6dof/ros_controllers.yaml | 14 +++++----- .../config/7dof/default_joint_limits.yaml | 14 +++++----- .../config/7dof/fake_controllers.yaml | 16 ++++++------ .../config/7dof/hard_joint_limits.yaml | 14 +++++----- .../config/7dof/ros_controllers.yaml | 16 ++++++------ .../gen3_moveit_controller_manager.launch.xml | 3 ++- .../launch/move_group.launch | 5 ++++ .../launch/planning_context.launch | 7 +++-- .../launch/trajectory_execution.launch.xml | 2 ++ .../config/6dof/default_joint_limits.yaml | 24 ++++++++--------- .../config/6dof/fake_controllers.yaml | 18 ++++++------- .../config/6dof/hard_joint_limits.yaml | 24 ++++++++--------- .../config/6dof/ros_controllers.yaml | 18 ++++++------- .../config/7dof/default_joint_limits.yaml | 26 +++++++++---------- .../config/7dof/fake_controllers.yaml | 20 +++++++------- .../config/7dof/hard_joint_limits.yaml | 26 +++++++++---------- .../config/7dof/ros_controllers.yaml | 20 +++++++------- ...f_140_moveit_controller_manager.launch.xml | 3 ++- .../launch/move_group.launch | 5 ++++ .../launch/planning_context.launch | 7 +++-- .../launch/trajectory_execution.launch.xml | 2 ++ .../config/6dof/default_joint_limits.yaml | 24 ++++++++--------- .../config/6dof/fake_controllers.yaml | 18 ++++++------- .../config/6dof/hard_joint_limits.yaml | 24 ++++++++--------- .../config/6dof/ros_controllers.yaml | 18 ++++++------- .../config/7dof/default_joint_limits.yaml | 26 +++++++++---------- .../config/7dof/fake_controllers.yaml | 20 +++++++------- .../config/7dof/hard_joint_limits.yaml | 26 +++++++++---------- .../config/7dof/ros_controllers.yaml | 20 +++++++------- ...2f_85_moveit_controller_manager.launch.xml | 3 ++- .../launch/move_group.launch | 5 ++++ .../launch/planning_context.launch | 7 +++-- .../launch/trajectory_execution.launch.xml | 2 ++ 45 files changed, 320 insertions(+), 270 deletions(-) diff --git a/kortex_driver/launch/kortex_driver.launch b/kortex_driver/launch/kortex_driver.launch index febffd7d..a489325b 100644 --- a/kortex_driver/launch/kortex_driver.launch +++ b/kortex_driver/launch/kortex_driver.launch @@ -84,11 +84,13 @@ + + diff --git a/kortex_gazebo/launch/spawn_kortex_robot.launch b/kortex_gazebo/launch/spawn_kortex_robot.launch index 07c39613..d7dc3422 100644 --- a/kortex_gazebo/launch/spawn_kortex_robot.launch +++ b/kortex_gazebo/launch/spawn_kortex_robot.launch @@ -151,10 +151,12 @@ + + diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/fake_controllers.yaml b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/fake_controllers.yaml index 8eda2826..60730a04 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/fake_controllers.yaml +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/fake_controllers.yaml @@ -1,12 +1,12 @@ controller_list: - - name: fake_arm_controller + - name: $(arg prefix)fake_arm_controller joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 - - name: fake_gripper_controller + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 + - name: $(arg prefix)fake_gripper_controller joints: - - right_finger_bottom_joint \ No newline at end of file + - $(arg prefix)right_finger_bottom_joint \ No newline at end of file diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/joint_limits.yaml b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/joint_limits.yaml index 6bc8c8b7..66e05b12 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/joint_limits.yaml +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/joint_limits.yaml @@ -2,32 +2,32 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - joint_1: + $(arg prefix)joint_1: has_velocity_limits: true max_velocity: 0.99 has_acceleration_limits: true max_acceleration: 0.35 - joint_2: + $(arg prefix)joint_2: has_velocity_limits: true max_velocity: 0.99 has_acceleration_limits: true max_acceleration: 0.17 - joint_3: + $(arg prefix)joint_3: has_velocity_limits: true max_velocity: 0.99 has_acceleration_limits: true max_acceleration: 0.14 - joint_4: + $(arg prefix)joint_4: has_velocity_limits: true max_velocity: 0.99 has_acceleration_limits: true max_acceleration: 0.35 - joint_5: + $(arg prefix)joint_5: has_velocity_limits: true max_velocity: 0.99 has_acceleration_limits: true max_acceleration: 3.5 - joint_6: + $(arg prefix)joint_6: has_velocity_limits: true max_velocity: 1.55 has_acceleration_limits: true diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/ros_controllers.yaml b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/ros_controllers.yaml index a71f1cae..2934c6dc 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/ros_controllers.yaml +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/ros_controllers.yaml @@ -2,21 +2,21 @@ joint_state_controller: type: joint_state_controller/JointStateController publish_rate: 50 controller_list: - - name: "gen3_lite_joint_trajectory_controller" + - name: "$(arg prefix)gen3_lite_joint_trajectory_controller" action_ns: follow_joint_trajectory default: True type: FollowJointTrajectory joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 - - name: "gen3_lite_2f_gripper_controller" + - name: "$(arg prefix)gen3_lite_2f_gripper_controller" action_ns: gripper_cmd default: True type: GripperCommand joints: - - right_finger_bottom_joint \ No newline at end of file + - $(arg prefix)right_finger_bottom_joint \ No newline at end of file diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/gen3_lite_gen3_lite_2f_moveit_controller_manager.launch.xml b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/gen3_lite_gen3_lite_2f_moveit_controller_manager.launch.xml index ec664d10..afb73408 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/gen3_lite_gen3_lite_2f_moveit_controller_manager.launch.xml +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/gen3_lite_gen3_lite_2f_moveit_controller_manager.launch.xml @@ -1,9 +1,11 @@ + + - + diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/move_group.launch b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/move_group.launch index 5dfab46d..f4d48513 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/move_group.launch +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/move_group.launch @@ -5,7 +5,12 @@ - + + + + + + @@ -53,6 +58,7 @@ + diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_context.launch b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_context.launch index f501959f..3836dc11 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_context.launch +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_context.launch @@ -5,6 +5,9 @@ + + + @@ -13,7 +16,7 @@ - + diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/trajectory_execution.launch.xml b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/trajectory_execution.launch.xml index 9a8e008c..7a09c04f 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/trajectory_execution.launch.xml +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/trajectory_execution.launch.xml @@ -1,7 +1,7 @@ - + @@ -15,6 +15,8 @@ - + + + diff --git a/kortex_move_it_config/gen3_move_it_config/config/6dof/default_joint_limits.yaml b/kortex_move_it_config/gen3_move_it_config/config/6dof/default_joint_limits.yaml index 6a93c689..7a51e2c1 100644 --- a/kortex_move_it_config/gen3_move_it_config/config/6dof/default_joint_limits.yaml +++ b/kortex_move_it_config/gen3_move_it_config/config/6dof/default_joint_limits.yaml @@ -2,32 +2,32 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - joint_1: + $(arg prefix)joint_1: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_2: + $(arg prefix)joint_2: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_3: + $(arg prefix)joint_3: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_4: + $(arg prefix)joint_4: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_5: + $(arg prefix)joint_5: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_6: + $(arg prefix)joint_6: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true diff --git a/kortex_move_it_config/gen3_move_it_config/config/6dof/fake_controllers.yaml b/kortex_move_it_config/gen3_move_it_config/config/6dof/fake_controllers.yaml index c693bcb7..84de5e8a 100644 --- a/kortex_move_it_config/gen3_move_it_config/config/6dof/fake_controllers.yaml +++ b/kortex_move_it_config/gen3_move_it_config/config/6dof/fake_controllers.yaml @@ -1,9 +1,9 @@ controller_list: - - name: fake_arm_controller + - name: $(arg prefix)fake_arm_controller joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 \ No newline at end of file + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 \ No newline at end of file diff --git a/kortex_move_it_config/gen3_move_it_config/config/6dof/hard_joint_limits.yaml b/kortex_move_it_config/gen3_move_it_config/config/6dof/hard_joint_limits.yaml index 6783c6e2..f43b69a2 100644 --- a/kortex_move_it_config/gen3_move_it_config/config/6dof/hard_joint_limits.yaml +++ b/kortex_move_it_config/gen3_move_it_config/config/6dof/hard_joint_limits.yaml @@ -2,32 +2,32 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - joint_1: + $(arg prefix)joint_1: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_2: + $(arg prefix)joint_2: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_3: + $(arg prefix)joint_3: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_4: + $(arg prefix)joint_4: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - joint_5: + $(arg prefix)joint_5: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - joint_6: + $(arg prefix)joint_6: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true diff --git a/kortex_move_it_config/gen3_move_it_config/config/6dof/ros_controllers.yaml b/kortex_move_it_config/gen3_move_it_config/config/6dof/ros_controllers.yaml index 9048503d..221f9604 100644 --- a/kortex_move_it_config/gen3_move_it_config/config/6dof/ros_controllers.yaml +++ b/kortex_move_it_config/gen3_move_it_config/config/6dof/ros_controllers.yaml @@ -2,14 +2,14 @@ joint_state_controller: type: joint_state_controller/JointStateController publish_rate: 50 controller_list: - - name: "gen3_joint_trajectory_controller" + - name: "$(arg prefix)gen3_joint_trajectory_controller" action_ns: follow_joint_trajectory default: True type: FollowJointTrajectory joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 diff --git a/kortex_move_it_config/gen3_move_it_config/config/7dof/default_joint_limits.yaml b/kortex_move_it_config/gen3_move_it_config/config/7dof/default_joint_limits.yaml index 68e9894d..056e61f7 100644 --- a/kortex_move_it_config/gen3_move_it_config/config/7dof/default_joint_limits.yaml +++ b/kortex_move_it_config/gen3_move_it_config/config/7dof/default_joint_limits.yaml @@ -2,37 +2,37 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - joint_1: + $(arg prefix)joint_1: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_2: + $(arg prefix)joint_2: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_3: + $(arg prefix)joint_3: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_4: + $(arg prefix)joint_4: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_5: + $(arg prefix)joint_5: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_6: + $(arg prefix)joint_6: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_7: + $(arg prefix)joint_7: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true diff --git a/kortex_move_it_config/gen3_move_it_config/config/7dof/fake_controllers.yaml b/kortex_move_it_config/gen3_move_it_config/config/7dof/fake_controllers.yaml index 159da410..9bf3b0da 100644 --- a/kortex_move_it_config/gen3_move_it_config/config/7dof/fake_controllers.yaml +++ b/kortex_move_it_config/gen3_move_it_config/config/7dof/fake_controllers.yaml @@ -1,10 +1,10 @@ controller_list: - - name: fake_arm_controller + - name: $(arg prefix)fake_arm_controller joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 - - joint_7 \ No newline at end of file + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 + - $(arg prefix)joint_7 \ No newline at end of file diff --git a/kortex_move_it_config/gen3_move_it_config/config/7dof/hard_joint_limits.yaml b/kortex_move_it_config/gen3_move_it_config/config/7dof/hard_joint_limits.yaml index 642c2528..fdd996b5 100644 --- a/kortex_move_it_config/gen3_move_it_config/config/7dof/hard_joint_limits.yaml +++ b/kortex_move_it_config/gen3_move_it_config/config/7dof/hard_joint_limits.yaml @@ -2,37 +2,37 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - joint_1: + $(arg prefix)joint_1: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_2: + $(arg prefix)joint_2: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_3: + $(arg prefix)joint_3: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_4: + $(arg prefix)joint_4: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_5: + $(arg prefix)joint_5: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - joint_6: + $(arg prefix)joint_6: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - joint_7: + $(arg prefix)joint_7: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true diff --git a/kortex_move_it_config/gen3_move_it_config/config/7dof/ros_controllers.yaml b/kortex_move_it_config/gen3_move_it_config/config/7dof/ros_controllers.yaml index 8a0e3f50..cfaa6205 100644 --- a/kortex_move_it_config/gen3_move_it_config/config/7dof/ros_controllers.yaml +++ b/kortex_move_it_config/gen3_move_it_config/config/7dof/ros_controllers.yaml @@ -2,15 +2,15 @@ joint_state_controller: type: joint_state_controller/JointStateController publish_rate: 50 controller_list: - - name: "gen3_joint_trajectory_controller" + - name: "$(arg prefix)gen3_joint_trajectory_controller" action_ns: follow_joint_trajectory default: True type: FollowJointTrajectory joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 - - joint_7 \ No newline at end of file + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 + - $(arg prefix)joint_7 \ No newline at end of file diff --git a/kortex_move_it_config/gen3_move_it_config/launch/gen3_moveit_controller_manager.launch.xml b/kortex_move_it_config/gen3_move_it_config/launch/gen3_moveit_controller_manager.launch.xml index e0b4c42f..5b91ac16 100644 --- a/kortex_move_it_config/gen3_move_it_config/launch/gen3_moveit_controller_manager.launch.xml +++ b/kortex_move_it_config/gen3_move_it_config/launch/gen3_moveit_controller_manager.launch.xml @@ -1,11 +1,12 @@ + - + diff --git a/kortex_move_it_config/gen3_move_it_config/launch/move_group.launch b/kortex_move_it_config/gen3_move_it_config/launch/move_group.launch index 20422937..42ce6287 100644 --- a/kortex_move_it_config/gen3_move_it_config/launch/move_group.launch +++ b/kortex_move_it_config/gen3_move_it_config/launch/move_group.launch @@ -3,10 +3,14 @@ + + + + @@ -56,6 +60,7 @@ + diff --git a/kortex_move_it_config/gen3_move_it_config/launch/planning_context.launch b/kortex_move_it_config/gen3_move_it_config/launch/planning_context.launch index 0ef0c93b..c7c9be6f 100644 --- a/kortex_move_it_config/gen3_move_it_config/launch/planning_context.launch +++ b/kortex_move_it_config/gen3_move_it_config/launch/planning_context.launch @@ -8,6 +8,9 @@ + + + @@ -19,8 +22,8 @@ - - + + diff --git a/kortex_move_it_config/gen3_move_it_config/launch/trajectory_execution.launch.xml b/kortex_move_it_config/gen3_move_it_config/launch/trajectory_execution.launch.xml index 7f071c1a..ebb523b9 100644 --- a/kortex_move_it_config/gen3_move_it_config/launch/trajectory_execution.launch.xml +++ b/kortex_move_it_config/gen3_move_it_config/launch/trajectory_execution.launch.xml @@ -2,6 +2,7 @@ + @@ -18,6 +19,7 @@ + diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/default_joint_limits.yaml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/default_joint_limits.yaml index 426d9e04..83fdb6bc 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/default_joint_limits.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/default_joint_limits.yaml @@ -2,62 +2,62 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - joint_1: + $(arg prefix)joint_1: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_2: + $(arg prefix)joint_2: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_3: + $(arg prefix)joint_3: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_4: + $(arg prefix)joint_4: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_5: + $(arg prefix)joint_5: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_6: + $(arg prefix)joint_6: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - left_inner_finger_joint: + $(arg prefix)left_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_knuckle_joint: + $(arg prefix)right_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - finger_joint: + $(arg prefix)finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_finger_joint: + $(arg prefix)right_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - left_inner_knuckle_joint: + $(arg prefix)left_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_outer_knuckle_joint: + $(arg prefix)right_outer_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/fake_controllers.yaml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/fake_controllers.yaml index 5b130b75..b9ec3932 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/fake_controllers.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/fake_controllers.yaml @@ -1,11 +1,11 @@ - - name: fake_arm_controller + - name: $(arg prefix)fake_arm_controller joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 - - name: fake_gripper_controller + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 + - name: $(arg prefix)fake_gripper_controller joints: - - finger_joint \ No newline at end of file + - $(arg prefix)finger_joint \ No newline at end of file diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/hard_joint_limits.yaml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/hard_joint_limits.yaml index eb4a100f..18bcded9 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/hard_joint_limits.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/hard_joint_limits.yaml @@ -2,62 +2,62 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - joint_1: + $(arg prefix)joint_1: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_2: + $(arg prefix)joint_2: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_3: + $(arg prefix)joint_3: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_4: + $(arg prefix)joint_4: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - joint_5: + $(arg prefix)joint_5: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - joint_6: + $(arg prefix)joint_6: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - left_inner_finger_joint: + $(arg prefix)left_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_knuckle_joint: + $(arg prefix)right_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - finger_joint: + $(arg prefix)finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_finger_joint: + $(arg prefix)right_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - left_inner_knuckle_joint: + $(arg prefix)left_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_outer_knuckle_joint: + $(arg prefix)right_outer_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/ros_controllers.yaml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/ros_controllers.yaml index b741c94d..f5d5e610 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/ros_controllers.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/ros_controllers.yaml @@ -2,21 +2,21 @@ joint_state_controller: type: joint_state_controller/JointStateController publish_rate: 50 controller_list: - - name: "gen3_joint_trajectory_controller" + - name: "$(arg prefix)gen3_joint_trajectory_controller" action_ns: follow_joint_trajectory default: True type: FollowJointTrajectory joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 - - name: "robotiq_2f_140_gripper_controller" + - name: "$(arg prefix)robotiq_2f_140_gripper_controller" action_ns: gripper_cmd default: True type: GripperCommand joints: - - finger_joint \ No newline at end of file + - $(arg prefix)finger_joint \ No newline at end of file diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/default_joint_limits.yaml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/default_joint_limits.yaml index f943baec..b172331f 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/default_joint_limits.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/default_joint_limits.yaml @@ -2,67 +2,67 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - joint_1: + $(arg prefix)joint_1: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_2: + $(arg prefix)joint_2: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_3: + $(arg prefix)joint_3: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_4: + $(arg prefix)joint_4: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_5: + $(arg prefix)joint_5: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_6: + $(arg prefix)joint_6: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_7: + $(arg prefix)joint_7: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - left_inner_finger_joint: + $(arg prefix)left_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_knuckle_joint: + $(arg prefix)right_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - finger_joint: + $(arg prefix)finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_finger_joint: + $(arg prefix)right_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - left_inner_knuckle_joint: + $(arg prefix)left_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_outer_knuckle_joint: + $(arg prefix)right_outer_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/fake_controllers.yaml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/fake_controllers.yaml index ca87393a..57fb469b 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/fake_controllers.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/fake_controllers.yaml @@ -1,13 +1,13 @@ controller_list: - - name: fake_arm_controller + - name: $(arg prefix)fake_arm_controller joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 - - joint_7 - - name: fake_gripper_controller + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 + - $(arg prefix)joint_7 + - name: $(arg prefix)fake_gripper_controller joints: - - finger_joint \ No newline at end of file + - $(arg prefix)finger_joint \ No newline at end of file diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/hard_joint_limits.yaml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/hard_joint_limits.yaml index 583d6682..7c291587 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/hard_joint_limits.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/hard_joint_limits.yaml @@ -2,67 +2,67 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - joint_1: + $(arg prefix)joint_1: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_2: + $(arg prefix)joint_2: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_3: + $(arg prefix)joint_3: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_4: + $(arg prefix)joint_4: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_5: + $(arg prefix)joint_5: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - joint_6: + $(arg prefix)joint_6: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - joint_7: + $(arg prefix)joint_7: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - left_inner_finger_joint: + $(arg prefix)left_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_knuckle_joint: + $(arg prefix)right_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - finger_joint: + $(arg prefix)finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_finger_joint: + $(arg prefix)right_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - left_inner_knuckle_joint: + $(arg prefix)left_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_outer_knuckle_joint: + $(arg prefix)right_outer_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/ros_controllers.yaml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/ros_controllers.yaml index 53436240..f1b8067b 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/ros_controllers.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/ros_controllers.yaml @@ -2,22 +2,22 @@ joint_state_controller: type: joint_state_controller/JointStateController publish_rate: 50 controller_list: - - name: "gen3_joint_trajectory_controller" + - name: "$(arg prefix)gen3_joint_trajectory_controller" action_ns: follow_joint_trajectory default: True type: FollowJointTrajectory joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 - - joint_7 + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 + - $(arg prefix)joint_7 - - name: "robotiq_2f_140_gripper_controller" + - name: "$(arg prefix)robotiq_2f_140_gripper_controller" action_ns: gripper_cmd default: True type: GripperCommand joints: - - finger_joint \ No newline at end of file + - $(arg prefix)finger_joint \ No newline at end of file diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/gen3_robotiq_2f_140_moveit_controller_manager.launch.xml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/gen3_robotiq_2f_140_moveit_controller_manager.launch.xml index 0d27267f..6007253d 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/gen3_robotiq_2f_140_moveit_controller_manager.launch.xml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/gen3_robotiq_2f_140_moveit_controller_manager.launch.xml @@ -1,11 +1,12 @@ + - + diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/move_group.launch b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/move_group.launch index 2a927a10..f9b9e6a0 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/move_group.launch +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/move_group.launch @@ -4,9 +4,13 @@ + + + + @@ -56,6 +60,7 @@ + diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_context.launch b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_context.launch index a9676df5..822ea32f 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_context.launch +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_context.launch @@ -8,6 +8,9 @@ + + + @@ -19,8 +22,8 @@ - - + + diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/trajectory_execution.launch.xml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/trajectory_execution.launch.xml index 92cfbd85..3eb4a38c 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/trajectory_execution.launch.xml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/trajectory_execution.launch.xml @@ -2,6 +2,7 @@ + @@ -18,6 +19,7 @@ + diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/default_joint_limits.yaml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/default_joint_limits.yaml index 426d9e04..83fdb6bc 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/default_joint_limits.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/default_joint_limits.yaml @@ -2,62 +2,62 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - joint_1: + $(arg prefix)joint_1: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_2: + $(arg prefix)joint_2: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_3: + $(arg prefix)joint_3: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_4: + $(arg prefix)joint_4: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_5: + $(arg prefix)joint_5: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_6: + $(arg prefix)joint_6: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - left_inner_finger_joint: + $(arg prefix)left_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_knuckle_joint: + $(arg prefix)right_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - finger_joint: + $(arg prefix)finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_finger_joint: + $(arg prefix)right_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - left_inner_knuckle_joint: + $(arg prefix)left_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_outer_knuckle_joint: + $(arg prefix)right_outer_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/fake_controllers.yaml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/fake_controllers.yaml index 5b130b75..b9ec3932 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/fake_controllers.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/fake_controllers.yaml @@ -1,11 +1,11 @@ - - name: fake_arm_controller + - name: $(arg prefix)fake_arm_controller joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 - - name: fake_gripper_controller + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 + - name: $(arg prefix)fake_gripper_controller joints: - - finger_joint \ No newline at end of file + - $(arg prefix)finger_joint \ No newline at end of file diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/hard_joint_limits.yaml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/hard_joint_limits.yaml index eb4a100f..18bcded9 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/hard_joint_limits.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/hard_joint_limits.yaml @@ -2,62 +2,62 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - joint_1: + $(arg prefix)joint_1: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_2: + $(arg prefix)joint_2: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_3: + $(arg prefix)joint_3: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_4: + $(arg prefix)joint_4: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - joint_5: + $(arg prefix)joint_5: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - joint_6: + $(arg prefix)joint_6: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - left_inner_finger_joint: + $(arg prefix)left_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_knuckle_joint: + $(arg prefix)right_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - finger_joint: + $(arg prefix)finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_finger_joint: + $(arg prefix)right_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - left_inner_knuckle_joint: + $(arg prefix)left_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_outer_knuckle_joint: + $(arg prefix)right_outer_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/ros_controllers.yaml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/ros_controllers.yaml index 792f1fc1..f13b3e57 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/ros_controllers.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/ros_controllers.yaml @@ -2,21 +2,21 @@ joint_state_controller: type: joint_state_controller/JointStateController publish_rate: 50 controller_list: - - name: "gen3_joint_trajectory_controller" + - name: "$(arg prefix)gen3_joint_trajectory_controller" action_ns: follow_joint_trajectory default: True type: FollowJointTrajectory joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 - - name: "robotiq_2f_85_gripper_controller" + - name: "$(arg prefix)robotiq_2f_85_gripper_controller" action_ns: gripper_cmd default: True type: GripperCommand joints: - - finger_joint \ No newline at end of file + - $(arg prefix)finger_joint \ No newline at end of file diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/default_joint_limits.yaml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/default_joint_limits.yaml index f943baec..b172331f 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/default_joint_limits.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/default_joint_limits.yaml @@ -2,67 +2,67 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - joint_1: + $(arg prefix)joint_1: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_2: + $(arg prefix)joint_2: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_3: + $(arg prefix)joint_3: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_4: + $(arg prefix)joint_4: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_5: + $(arg prefix)joint_5: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_6: + $(arg prefix)joint_6: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - joint_7: + $(arg prefix)joint_7: has_velocity_limits: true max_velocity: 0.86 has_acceleration_limits: true max_acceleration: 0.3 - left_inner_finger_joint: + $(arg prefix)left_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_knuckle_joint: + $(arg prefix)right_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - finger_joint: + $(arg prefix)finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_finger_joint: + $(arg prefix)right_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - left_inner_knuckle_joint: + $(arg prefix)left_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_outer_knuckle_joint: + $(arg prefix)right_outer_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/fake_controllers.yaml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/fake_controllers.yaml index ca87393a..57fb469b 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/fake_controllers.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/fake_controllers.yaml @@ -1,13 +1,13 @@ controller_list: - - name: fake_arm_controller + - name: $(arg prefix)fake_arm_controller joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 - - joint_7 - - name: fake_gripper_controller + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 + - $(arg prefix)joint_7 + - name: $(arg prefix)fake_gripper_controller joints: - - finger_joint \ No newline at end of file + - $(arg prefix)finger_joint \ No newline at end of file diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/hard_joint_limits.yaml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/hard_joint_limits.yaml index 583d6682..7c291587 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/hard_joint_limits.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/hard_joint_limits.yaml @@ -2,67 +2,67 @@ # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - joint_1: + $(arg prefix)joint_1: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_2: + $(arg prefix)joint_2: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_3: + $(arg prefix)joint_3: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_4: + $(arg prefix)joint_4: has_velocity_limits: true max_velocity: 1.37 has_acceleration_limits: true max_acceleration: 1.56 - joint_5: + $(arg prefix)joint_5: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - joint_6: + $(arg prefix)joint_6: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - joint_7: + $(arg prefix)joint_7: has_velocity_limits: true max_velocity: 1.15 has_acceleration_limits: true max_acceleration: 3.0 - left_inner_finger_joint: + $(arg prefix)left_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_knuckle_joint: + $(arg prefix)right_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - finger_joint: + $(arg prefix)finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_inner_finger_joint: + $(arg prefix)right_inner_finger_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - left_inner_knuckle_joint: + $(arg prefix)left_inner_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false max_acceleration: 0 - right_outer_knuckle_joint: + $(arg prefix)right_outer_knuckle_joint: has_velocity_limits: true max_velocity: 2 has_acceleration_limits: false diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/ros_controllers.yaml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/ros_controllers.yaml index 5b62d88e..f401d726 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/ros_controllers.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/ros_controllers.yaml @@ -2,22 +2,22 @@ joint_state_controller: type: joint_state_controller/JointStateController publish_rate: 50 controller_list: - - name: "gen3_joint_trajectory_controller" + - name: "$(arg prefix)gen3_joint_trajectory_controller" action_ns: follow_joint_trajectory default: True type: FollowJointTrajectory joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 - - joint_7 + - $(arg prefix)joint_1 + - $(arg prefix)joint_2 + - $(arg prefix)joint_3 + - $(arg prefix)joint_4 + - $(arg prefix)joint_5 + - $(arg prefix)joint_6 + - $(arg prefix)joint_7 - - name: "robotiq_2f_85_gripper_controller" + - name: "$(arg prefix)robotiq_2f_85_gripper_controller" action_ns: gripper_cmd default: True type: GripperCommand joints: - - finger_joint \ No newline at end of file + - $(arg prefix)finger_joint \ No newline at end of file diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/gen3_robotiq_2f_85_moveit_controller_manager.launch.xml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/gen3_robotiq_2f_85_moveit_controller_manager.launch.xml index 8ba0c8f6..1d654026 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/gen3_robotiq_2f_85_moveit_controller_manager.launch.xml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/gen3_robotiq_2f_85_moveit_controller_manager.launch.xml @@ -1,11 +1,12 @@ + - + diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/move_group.launch b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/move_group.launch index efaa685e..8864a277 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/move_group.launch +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/move_group.launch @@ -4,9 +4,13 @@ + + + + @@ -56,6 +60,7 @@ + diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_context.launch b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_context.launch index 583764e0..05905a4d 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_context.launch +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_context.launch @@ -7,6 +7,9 @@ + + + @@ -19,8 +22,8 @@ - - + + diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/trajectory_execution.launch.xml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/trajectory_execution.launch.xml index d8509a8f..55e216ca 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/trajectory_execution.launch.xml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/trajectory_execution.launch.xml @@ -2,6 +2,7 @@ + @@ -18,6 +19,7 @@ + From ab82fde4bd0f3e56ccf6ee2e9240dd8340e8e8a1 Mon Sep 17 00:00:00 2001 From: Alexandre Vannobel Date: Wed, 10 Jun 2020 19:08:54 -0400 Subject: [PATCH 10/33] Added xacro usage in SRDF files to be able to prefix joint names --- .../config/gen3_lite_gen3_lite_2f.srdf | 107 --------- .../config/gen3_lite_gen3_lite_2f.srdf.xacro | 108 +++++++++ .../launch/move_group.launch | 2 +- .../launch/planning_context.launch | 3 +- .../gen3_move_it_config/config/6dof/gen3.srdf | 62 ----- .../config/6dof/gen3.srdf.xacro | 63 +++++ .../gen3_move_it_config/config/7dof/gen3.srdf | 74 ------ .../config/7dof/gen3.srdf.xacro | 74 ++++++ .../launch/planning_context.launch | 2 +- .../config/6dof/gen3_robotiq_2f_140.srdf | 197 ---------------- .../6dof/gen3_robotiq_2f_140.srdf.xacro | 198 ++++++++++++++++ .../config/7dof/gen3_robotiq_2f_140.srdf | 217 ----------------- .../7dof/gen3_robotiq_2f_140.srdf.xacro | 218 ++++++++++++++++++ .../launch/planning_context.launch | 2 +- .../config/6dof/gen3_robotiq_2f_85.srdf | 198 ---------------- .../config/6dof/gen3_robotiq_2f_85.srdf.xacro | 199 ++++++++++++++++ .../config/7dof/gen3_robotiq_2f_85.srdf | 217 ----------------- .../config/7dof/gen3_robotiq_2f_85.srdf.xacro | 218 ++++++++++++++++++ .../launch/planning_context.launch | 2 +- 19 files changed, 1083 insertions(+), 1078 deletions(-) delete mode 100644 kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/gen3_lite_gen3_lite_2f.srdf create mode 100644 kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/gen3_lite_gen3_lite_2f.srdf.xacro delete mode 100644 kortex_move_it_config/gen3_move_it_config/config/6dof/gen3.srdf create mode 100644 kortex_move_it_config/gen3_move_it_config/config/6dof/gen3.srdf.xacro delete mode 100644 kortex_move_it_config/gen3_move_it_config/config/7dof/gen3.srdf create mode 100644 kortex_move_it_config/gen3_move_it_config/config/7dof/gen3.srdf.xacro delete mode 100644 kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/gen3_robotiq_2f_140.srdf create mode 100644 kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/gen3_robotiq_2f_140.srdf.xacro delete mode 100644 kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/gen3_robotiq_2f_140.srdf create mode 100644 kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/gen3_robotiq_2f_140.srdf.xacro delete mode 100644 kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/gen3_robotiq_2f_85.srdf create mode 100644 kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/gen3_robotiq_2f_85.srdf.xacro delete mode 100644 kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/gen3_robotiq_2f_85.srdf create mode 100644 kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/gen3_robotiq_2f_85.srdf.xacro diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/gen3_lite_gen3_lite_2f.srdf b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/gen3_lite_gen3_lite_2f.srdf deleted file mode 100644 index 178ab9bc..00000000 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/gen3_lite_gen3_lite_2f.srdf +++ /dev/null @@ -1,107 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/gen3_lite_gen3_lite_2f.srdf.xacro b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/gen3_lite_gen3_lite_2f.srdf.xacro new file mode 100644 index 00000000..4bbad952 --- /dev/null +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/gen3_lite_gen3_lite_2f.srdf.xacro @@ -0,0 +1,108 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/move_group.launch b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/move_group.launch index f4d48513..4c5fa0d0 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/move_group.launch +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/move_group.launch @@ -10,7 +10,7 @@ - diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_context.launch b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_context.launch index 3836dc11..693eb14f 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_context.launch +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_context.launch @@ -12,8 +12,7 @@ - - + diff --git a/kortex_move_it_config/gen3_move_it_config/config/6dof/gen3.srdf b/kortex_move_it_config/gen3_move_it_config/config/6dof/gen3.srdf deleted file mode 100644 index 586f19ba..00000000 --- a/kortex_move_it_config/gen3_move_it_config/config/6dof/gen3.srdf +++ /dev/null @@ -1,62 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/kortex_move_it_config/gen3_move_it_config/config/6dof/gen3.srdf.xacro b/kortex_move_it_config/gen3_move_it_config/config/6dof/gen3.srdf.xacro new file mode 100644 index 00000000..1257563e --- /dev/null +++ b/kortex_move_it_config/gen3_move_it_config/config/6dof/gen3.srdf.xacro @@ -0,0 +1,63 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/kortex_move_it_config/gen3_move_it_config/config/7dof/gen3.srdf b/kortex_move_it_config/gen3_move_it_config/config/7dof/gen3.srdf deleted file mode 100644 index f7b254af..00000000 --- a/kortex_move_it_config/gen3_move_it_config/config/7dof/gen3.srdf +++ /dev/null @@ -1,74 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/kortex_move_it_config/gen3_move_it_config/config/7dof/gen3.srdf.xacro b/kortex_move_it_config/gen3_move_it_config/config/7dof/gen3.srdf.xacro new file mode 100644 index 00000000..5930cc1e --- /dev/null +++ b/kortex_move_it_config/gen3_move_it_config/config/7dof/gen3.srdf.xacro @@ -0,0 +1,74 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/kortex_move_it_config/gen3_move_it_config/launch/planning_context.launch b/kortex_move_it_config/gen3_move_it_config/launch/planning_context.launch index c7c9be6f..2954400e 100644 --- a/kortex_move_it_config/gen3_move_it_config/launch/planning_context.launch +++ b/kortex_move_it_config/gen3_move_it_config/launch/planning_context.launch @@ -18,7 +18,7 @@ - + diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/gen3_robotiq_2f_140.srdf b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/gen3_robotiq_2f_140.srdf deleted file mode 100644 index e2cd635b..00000000 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/gen3_robotiq_2f_140.srdf +++ /dev/null @@ -1,197 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/gen3_robotiq_2f_140.srdf.xacro b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/gen3_robotiq_2f_140.srdf.xacro new file mode 100644 index 00000000..8521c61e --- /dev/null +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/gen3_robotiq_2f_140.srdf.xacro @@ -0,0 +1,198 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/gen3_robotiq_2f_140.srdf b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/gen3_robotiq_2f_140.srdf deleted file mode 100644 index fd4a5cfd..00000000 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/gen3_robotiq_2f_140.srdf +++ /dev/null @@ -1,217 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/gen3_robotiq_2f_140.srdf.xacro b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/gen3_robotiq_2f_140.srdf.xacro new file mode 100644 index 00000000..cd5bb0e8 --- /dev/null +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/gen3_robotiq_2f_140.srdf.xacro @@ -0,0 +1,218 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_context.launch b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_context.launch index 822ea32f..0d801377 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_context.launch +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_context.launch @@ -18,7 +18,7 @@ - + diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/gen3_robotiq_2f_85.srdf b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/gen3_robotiq_2f_85.srdf deleted file mode 100644 index 8ec5af66..00000000 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/gen3_robotiq_2f_85.srdf +++ /dev/null @@ -1,198 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/gen3_robotiq_2f_85.srdf.xacro b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/gen3_robotiq_2f_85.srdf.xacro new file mode 100644 index 00000000..0ff251e0 --- /dev/null +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/gen3_robotiq_2f_85.srdf.xacro @@ -0,0 +1,199 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/gen3_robotiq_2f_85.srdf b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/gen3_robotiq_2f_85.srdf deleted file mode 100644 index a15f8f84..00000000 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/gen3_robotiq_2f_85.srdf +++ /dev/null @@ -1,217 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/gen3_robotiq_2f_85.srdf.xacro b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/gen3_robotiq_2f_85.srdf.xacro new file mode 100644 index 00000000..0463d2de --- /dev/null +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/gen3_robotiq_2f_85.srdf.xacro @@ -0,0 +1,218 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_context.launch b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_context.launch index 05905a4d..5c58ddd4 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_context.launch +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_context.launch @@ -18,7 +18,7 @@ - + From 3c019007e73e036c8b37ebf7da4bf55a670a03ec Mon Sep 17 00:00:00 2001 From: Alexandre Vannobel Date: Wed, 10 Jun 2020 20:16:15 -0400 Subject: [PATCH 11/33] Adjust ompl planning launch and config files to support prefixing joint names --- .../config/ompl_planning.yaml | 2 +- .../launch/move_group.launch | 1 + .../launch/ompl_planning_pipeline.launch.xml | 3 ++- .../launch/planning_pipeline.launch.xml | 5 ++++- .../gen3_move_it_config/config/ompl_planning.yaml | 2 +- .../gen3_move_it_config/launch/move_group.launch | 1 + .../launch/ompl_planning_pipeline.launch.xml | 3 ++- .../gen3_move_it_config/launch/planning_pipeline.launch.xml | 5 ++++- .../config/ompl_planning.yaml | 2 +- .../launch/move_group.launch | 1 + .../launch/ompl_planning_pipeline.launch.xml | 3 ++- .../launch/planning_pipeline.launch.xml | 5 ++++- .../config/ompl_planning.yaml | 2 +- .../launch/move_group.launch | 1 + .../launch/ompl_planning_pipeline.launch.xml | 3 ++- .../launch/planning_pipeline.launch.xml | 5 ++++- 16 files changed, 32 insertions(+), 12 deletions(-) diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/ompl_planning.yaml b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/ompl_planning.yaml index a49c255b..2ee2dbe8 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/ompl_planning.yaml +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/ompl_planning.yaml @@ -146,7 +146,7 @@ arm: - LazyPRMstar - SPARS - SPARStwo - projection_evaluator: joints(joint_1,joint_2) + projection_evaluator: joints($(arg prefix)joint_1,$(arg prefix)joint_2) longest_valid_segment_fraction: 0.005 gripper: default_planner_config: RRTConnect diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/move_group.launch b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/move_group.launch index 4c5fa0d0..f26a35d8 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/move_group.launch +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/move_group.launch @@ -51,6 +51,7 @@ + diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/ompl_planning_pipeline.launch.xml b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/ompl_planning_pipeline.launch.xml index 7dc43ada..b8cd5b6a 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/ompl_planning_pipeline.launch.xml +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/ompl_planning_pipeline.launch.xml @@ -2,6 +2,7 @@ + @@ -21,6 +22,6 @@ - + diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_pipeline.launch.xml b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_pipeline.launch.xml index b941658f..6f172429 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_pipeline.launch.xml +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_pipeline.launch.xml @@ -4,7 +4,10 @@ It is assumed that all planning pipelines are named XXX_planning_pipeline.launch --> + - + + + diff --git a/kortex_move_it_config/gen3_move_it_config/config/ompl_planning.yaml b/kortex_move_it_config/gen3_move_it_config/config/ompl_planning.yaml index 17829f90..1667cab7 100644 --- a/kortex_move_it_config/gen3_move_it_config/config/ompl_planning.yaml +++ b/kortex_move_it_config/gen3_move_it_config/config/ompl_planning.yaml @@ -146,5 +146,5 @@ arm: - LazyPRMstar - SPARS - SPARStwo - projection_evaluator: joints(joint_1,joint_2) + projection_evaluator: joints($(arg prefix)joint_1,$(arg prefix)joint_2) longest_valid_segment_fraction: 0.005 \ No newline at end of file diff --git a/kortex_move_it_config/gen3_move_it_config/launch/move_group.launch b/kortex_move_it_config/gen3_move_it_config/launch/move_group.launch index 42ce6287..56ffd49c 100644 --- a/kortex_move_it_config/gen3_move_it_config/launch/move_group.launch +++ b/kortex_move_it_config/gen3_move_it_config/launch/move_group.launch @@ -52,6 +52,7 @@ + diff --git a/kortex_move_it_config/gen3_move_it_config/launch/ompl_planning_pipeline.launch.xml b/kortex_move_it_config/gen3_move_it_config/launch/ompl_planning_pipeline.launch.xml index 2ab04a16..bfe3d6c6 100644 --- a/kortex_move_it_config/gen3_move_it_config/launch/ompl_planning_pipeline.launch.xml +++ b/kortex_move_it_config/gen3_move_it_config/launch/ompl_planning_pipeline.launch.xml @@ -2,6 +2,7 @@ + @@ -21,6 +22,6 @@ - + diff --git a/kortex_move_it_config/gen3_move_it_config/launch/planning_pipeline.launch.xml b/kortex_move_it_config/gen3_move_it_config/launch/planning_pipeline.launch.xml index 02c62e47..607842b2 100644 --- a/kortex_move_it_config/gen3_move_it_config/launch/planning_pipeline.launch.xml +++ b/kortex_move_it_config/gen3_move_it_config/launch/planning_pipeline.launch.xml @@ -4,7 +4,10 @@ It is assumed that all planning pipelines are named XXX_planning_pipeline.launch --> + - + + + diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/ompl_planning.yaml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/ompl_planning.yaml index a49c255b..2ee2dbe8 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/ompl_planning.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/ompl_planning.yaml @@ -146,7 +146,7 @@ arm: - LazyPRMstar - SPARS - SPARStwo - projection_evaluator: joints(joint_1,joint_2) + projection_evaluator: joints($(arg prefix)joint_1,$(arg prefix)joint_2) longest_valid_segment_fraction: 0.005 gripper: default_planner_config: RRTConnect diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/move_group.launch b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/move_group.launch index f9b9e6a0..765335b7 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/move_group.launch +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/move_group.launch @@ -52,6 +52,7 @@ + diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/ompl_planning_pipeline.launch.xml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/ompl_planning_pipeline.launch.xml index 56d02bf4..99ec2a50 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/ompl_planning_pipeline.launch.xml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/ompl_planning_pipeline.launch.xml @@ -2,6 +2,7 @@ + @@ -21,6 +22,6 @@ - + diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_pipeline.launch.xml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_pipeline.launch.xml index c2c6d8e2..4e99c9c6 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_pipeline.launch.xml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_pipeline.launch.xml @@ -4,7 +4,10 @@ It is assumed that all planning pipelines are named XXX_planning_pipeline.launch --> + - + + + diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/ompl_planning.yaml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/ompl_planning.yaml index a49c255b..2ee2dbe8 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/ompl_planning.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/ompl_planning.yaml @@ -146,7 +146,7 @@ arm: - LazyPRMstar - SPARS - SPARStwo - projection_evaluator: joints(joint_1,joint_2) + projection_evaluator: joints($(arg prefix)joint_1,$(arg prefix)joint_2) longest_valid_segment_fraction: 0.005 gripper: default_planner_config: RRTConnect diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/move_group.launch b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/move_group.launch index 8864a277..806b0a22 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/move_group.launch +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/move_group.launch @@ -52,6 +52,7 @@ + diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/ompl_planning_pipeline.launch.xml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/ompl_planning_pipeline.launch.xml index 6f683627..5ef1a126 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/ompl_planning_pipeline.launch.xml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/ompl_planning_pipeline.launch.xml @@ -2,6 +2,7 @@ + @@ -21,6 +22,6 @@ - + diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_pipeline.launch.xml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_pipeline.launch.xml index ab340f3d..438586b2 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_pipeline.launch.xml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_pipeline.launch.xml @@ -4,7 +4,10 @@ It is assumed that all planning pipelines are named XXX_planning_pipeline.launch --> + - + + + From f22a40d2326009bca67affa13516d6b62885f71d Mon Sep 17 00:00:00 2001 From: Alexandre Vannobel Date: Wed, 10 Jun 2020 20:19:23 -0400 Subject: [PATCH 12/33] Remove --inorder from xacro calls since it's default now --- kortex_description/launch/visualize.launch | 4 ++-- kortex_description/readme.md | 4 ++-- kortex_driver/launch/kortex_driver.launch | 8 ++++---- kortex_driver/launch/kortex_dual_driver.launch | 2 +- kortex_gazebo/launch/spawn_kortex_robot.launch | 4 ++-- .../.setup_assistant | 2 +- .../launch/planning_context.launch | 4 ++-- .../gen3_move_it_config/.setup_assistant | 2 +- .../gen3_move_it_config/launch/planning_context.launch | 4 ++-- .../gen3_robotiq_2f_140_move_it_config/.setup_assistant | 2 +- .../launch/planning_context.launch | 4 ++-- .../gen3_robotiq_2f_85_move_it_config/.setup_assistant | 2 +- .../launch/planning_context.launch | 4 ++-- 13 files changed, 23 insertions(+), 23 deletions(-) diff --git a/kortex_description/launch/visualize.launch b/kortex_description/launch/visualize.launch index 30914192..6fe5e26f 100644 --- a/kortex_description/launch/visualize.launch +++ b/kortex_description/launch/visualize.launch @@ -14,10 +14,10 @@ - - diff --git a/kortex_description/readme.md b/kortex_description/readme.md index 2ecf830b..1811c64e 100644 --- a/kortex_description/readme.md +++ b/kortex_description/readme.md @@ -23,10 +23,10 @@ To load the description of a robot, you simply have to load the **ARM.xacro** or For example: - To load the Gen3 description with a Robotiq 2-F 85 gripper for simulation, you would put in your launch file : - + - To load the Gen3 lite description, you would put in your launch file : - + ## Tool frame diff --git a/kortex_driver/launch/kortex_driver.launch b/kortex_driver/launch/kortex_driver.launch index a489325b..79123220 100644 --- a/kortex_driver/launch/kortex_driver.launch +++ b/kortex_driver/launch/kortex_driver.launch @@ -36,19 +36,19 @@ - - - - diff --git a/kortex_driver/launch/kortex_dual_driver.launch b/kortex_driver/launch/kortex_dual_driver.launch index 436a91f9..4cee2ba6 100644 --- a/kortex_driver/launch/kortex_dual_driver.launch +++ b/kortex_driver/launch/kortex_dual_driver.launch @@ -60,7 +60,7 @@ - - diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/.setup_assistant b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/.setup_assistant index af9086aa..8619322a 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/.setup_assistant +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/.setup_assistant @@ -2,7 +2,7 @@ moveit_setup_assistant_config: URDF: package: kortex_description relative_path: robots/gen3_lite_gen3_lite_2f.xacro - xacro_args: "--inorder " + xacro_args: "" SRDF: relative_path: config/gen3_lite_gen3_lite_2f.srdf CONFIG: diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_context.launch b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_context.launch index 693eb14f..990954d1 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_context.launch +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_context.launch @@ -9,10 +9,10 @@ - + - + diff --git a/kortex_move_it_config/gen3_move_it_config/.setup_assistant b/kortex_move_it_config/gen3_move_it_config/.setup_assistant index f94315d9..94c2fcba 100644 --- a/kortex_move_it_config/gen3_move_it_config/.setup_assistant +++ b/kortex_move_it_config/gen3_move_it_config/.setup_assistant @@ -2,7 +2,7 @@ moveit_setup_assistant_config: URDF: package: kortex_description relative_path: robots/gen3.xacro - xacro_args: --inorder arm:=gen3 + xacro_args: arm:=gen3 SRDF: relative_path: config/gen3.srdf CONFIG: diff --git a/kortex_move_it_config/gen3_move_it_config/launch/planning_context.launch b/kortex_move_it_config/gen3_move_it_config/launch/planning_context.launch index 2954400e..4548bcdc 100644 --- a/kortex_move_it_config/gen3_move_it_config/launch/planning_context.launch +++ b/kortex_move_it_config/gen3_move_it_config/launch/planning_context.launch @@ -15,10 +15,10 @@ - + - + diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/.setup_assistant b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/.setup_assistant index e8017e1b..05b9cb92 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/.setup_assistant +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/.setup_assistant @@ -2,7 +2,7 @@ moveit_setup_assistant_config: URDF: package: kortex_description relative_path: robots/gen3_robotiq_2f_85.xacro - xacro_args: --inorder arm:=gen3 gripper:=robotiq_2f_85 + xacro_args: arm:=gen3 gripper:=robotiq_2f_85 SRDF: relative_path: config/gen3_robotiq_2f_85.srdf CONFIG: diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_context.launch b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_context.launch index 0d801377..73d7424e 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_context.launch +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_context.launch @@ -15,10 +15,10 @@ - + - + diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/.setup_assistant b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/.setup_assistant index e8017e1b..05b9cb92 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/.setup_assistant +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/.setup_assistant @@ -2,7 +2,7 @@ moveit_setup_assistant_config: URDF: package: kortex_description relative_path: robots/gen3_robotiq_2f_85.xacro - xacro_args: --inorder arm:=gen3 gripper:=robotiq_2f_85 + xacro_args: arm:=gen3 gripper:=robotiq_2f_85 SRDF: relative_path: config/gen3_robotiq_2f_85.srdf CONFIG: diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_context.launch b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_context.launch index 5c58ddd4..72420090 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_context.launch +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_context.launch @@ -15,10 +15,10 @@ - + - + From 5587eeaa2361d80187c1b0c8f03af2331422d3f1 Mon Sep 17 00:00:00 2001 From: Alexandre Vannobel Date: Thu, 11 Jun 2020 10:38:19 -0400 Subject: [PATCH 13/33] Set default gripper in launch files --- kortex_driver/launch/kortex_driver.launch | 4 +++- kortex_gazebo/launch/spawn_kortex_robot.launch | 8 ++++++-- 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/kortex_driver/launch/kortex_driver.launch b/kortex_driver/launch/kortex_driver.launch index 79123220..a8bb1160 100644 --- a/kortex_driver/launch/kortex_driver.launch +++ b/kortex_driver/launch/kortex_driver.launch @@ -9,7 +9,9 @@ - + + + diff --git a/kortex_gazebo/launch/spawn_kortex_robot.launch b/kortex_gazebo/launch/spawn_kortex_robot.launch index 836674bc..47272b40 100644 --- a/kortex_gazebo/launch/spawn_kortex_robot.launch +++ b/kortex_gazebo/launch/spawn_kortex_robot.launch @@ -11,12 +11,16 @@ - + - + + + + + From d1990b56717223227d8630ae940ce7de6941d8d9 Mon Sep 17 00:00:00 2001 From: Alexandre Vannobel Date: Thu, 11 Jun 2020 11:49:03 -0400 Subject: [PATCH 14/33] Add namespace to launch file parameters passed to driver --- kortex_driver/launch/kortex_driver.launch | 1 + kortex_driver/launch/kortex_dual_driver.launch | 2 ++ kortex_gazebo/launch/spawn_kortex_robot.launch | 1 + 3 files changed, 4 insertions(+) diff --git a/kortex_driver/launch/kortex_driver.launch b/kortex_driver/launch/kortex_driver.launch index a8bb1160..ce95d368 100644 --- a/kortex_driver/launch/kortex_driver.launch +++ b/kortex_driver/launch/kortex_driver.launch @@ -72,6 +72,7 @@ + diff --git a/kortex_driver/launch/kortex_dual_driver.launch b/kortex_driver/launch/kortex_dual_driver.launch index 4cee2ba6..57b7260c 100644 --- a/kortex_driver/launch/kortex_dual_driver.launch +++ b/kortex_driver/launch/kortex_dual_driver.launch @@ -90,6 +90,7 @@ + @@ -109,6 +110,7 @@ + diff --git a/kortex_gazebo/launch/spawn_kortex_robot.launch b/kortex_gazebo/launch/spawn_kortex_robot.launch index 47272b40..44f4bf5b 100644 --- a/kortex_gazebo/launch/spawn_kortex_robot.launch +++ b/kortex_gazebo/launch/spawn_kortex_robot.launch @@ -138,6 +138,7 @@ + From 6b86feb50f383b2da066d39aa9c97d06200e027a Mon Sep 17 00:00:00 2001 From: Alexandre Vannobel Date: Thu, 11 Jun 2020 11:49:37 -0400 Subject: [PATCH 15/33] Added class and implementation for simulator --- .../non-generated/kortex_arm_driver.h | 2 + .../non-generated/kortex_arm_simulation.h | 54 +++++++++++++++++++ .../driver/kortex_arm_driver.cpp | 15 +++++- .../driver/kortex_arm_simulation.cpp | 51 ++++++++++++++++++ 4 files changed, 121 insertions(+), 1 deletion(-) create mode 100644 kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h create mode 100644 kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp diff --git a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h index 5249fe33..a32c7fdd 100644 --- a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h +++ b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h @@ -56,6 +56,7 @@ #include "kortex_driver/non-generated/pre_computed_joint_trajectory_action_server.h" #include "kortex_driver/non-generated/robotiq_gripper_command_action_server.h" #include "kortex_driver/non-generated/kortex_subscribers.h" +#include "kortex_driver/non-generated/kortex_arm_simulation.h" #define TCP_PORT 10000 #define UDP_PORT 10001 @@ -85,6 +86,7 @@ class KortexArmDriver // False if in simulation bool m_is_real_robot; + std::unique_ptr m_simulator; // Api options std::string m_ip_address; diff --git a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h new file mode 100644 index 00000000..3e559dfe --- /dev/null +++ b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h @@ -0,0 +1,54 @@ +#ifndef _KORTEX_ARM_SIMULATION_H_ +#define _KORTEX_ARM_SIMULATION_H_ + +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2020 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +#include + +#include "kortex_driver/non-generated/kortex_math_util.h" + +class KortexArmSimulation +{ + public: + KortexArmSimulation() = delete; + KortexArmSimulation(ros::NodeHandle& nh); + ~KortexArmSimulation(); + + // Handlers for simulated Kortex API functions + // TODO Add handlers + + private: + + ros::NodeHandle m_node_handle; + + // Namespacing and prefixing information + std::string m_prefix; + std::string m_robot_name; + + // Arm and gripper information + std::string m_arm_name; + std::vector m_arm_joint_names; + std::string m_gripper_name; + std::vector m_gripper_joint_names; + std::vector m_gripper_joint_limits_max; + std::vector m_gripper_joint_limits_min; + int m_degrees_of_freedom; + + // ROS and thread objects to publish the feedback from the robot + KortexMathUtil m_math_util; + + // Helper functions + bool IsGripperPresent() const {return !m_gripper_name.empty();} +}; + +#endif //_KORTEX_ARM_SIMULATION_H_ diff --git a/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp b/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp index 1ead99b9..caa12317 100644 --- a/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp +++ b/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp @@ -16,7 +16,8 @@ KortexArmDriver::KortexArmDriver(ros::NodeHandle nh): m_node_handle(nh), m_node_is_running(true), m_consecutive_base_cyclic_timeouts(0), m_is_interconnect_module_present(false), - m_is_vision_module_present(false) + m_is_vision_module_present(false), + m_simulator{} { // Parameter to let the other nodes know this node is up ros::param::set("is_initialized", false); @@ -31,6 +32,11 @@ KortexArmDriver::KortexArmDriver(ros::NodeHandle nh): m_node_handle(nh), initSubscribers(); startActionServers(); } + else + { + m_simulator.reset(new KortexArmSimulation(nh)); + } + // ROS Services are always started initRosServices(); @@ -217,6 +223,13 @@ void KortexArmDriver::parseRosArguments() ROS_ERROR("%s", error_string.c_str()); throw new std::runtime_error(error_string); } + std::string robot_name; + if (!ros::param::get("~robot_name", robot_name)) + { + std::string error_string = "Robot name was not specified in the launch file, shutting down the node..."; + ROS_ERROR("%s", error_string.c_str()); + throw new std::runtime_error(error_string); + } if (!ros::param::get("~prefix", m_prefix)) { std::string error_string = "Prefix name was not specified in the launch file, shutting down the node..."; diff --git a/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp b/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp new file mode 100644 index 00000000..749df03d --- /dev/null +++ b/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp @@ -0,0 +1,51 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2020 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +#include "kortex_driver/non-generated/kortex_arm_simulation.h" + +KortexArmSimulation::KortexArmSimulation(ros::NodeHandle& node_handle): m_node_handle(node_handle) +{ + // Namespacing and prefixing information + ros::param::get("~robot_name", m_robot_name); + ros::param::get("~prefix", m_prefix); + + // Arm and gripper information + ros::param::get("~dof", m_degrees_of_freedom); + ros::param::get("~arm", m_arm_name); + ros::param::get("~joint_names", m_arm_joint_names); + for (auto s : m_arm_joint_names) + { + s.insert(0, m_prefix); + } + ros::param::get("~gripper", m_gripper_name); + if (IsGripperPresent()) + { + ros::param::get("~gripper_joint_names", m_gripper_joint_names); + for (auto s : m_gripper_joint_names) + { + s.insert(0, m_prefix); + } + ros::param::get("~gripper_joint_limits_max", m_gripper_joint_limits_max); + ros::param::get("~gripper_joint_limits_min", m_gripper_joint_limits_min); + } + + // Print out simulation configuration + ROS_INFO("Simulating arm with following characteristics:"); + ROS_INFO("Arm type : %s", m_arm_name.c_str()); + ROS_INFO("Gripper type : %s", m_gripper_name.empty() ? "None" : m_gripper_name.c_str()); + ROS_INFO("Arm namespace : %s", m_robot_name.c_str()); + ROS_INFO("URDF prefix : %s", m_prefix.c_str()); +} + +KortexArmSimulation::~KortexArmSimulation() +{ +} From fd2c4f31606091a62da2da785b680208fe68c4e5 Mon Sep 17 00:00:00 2001 From: Alexandre Vannobel Date: Thu, 11 Jun 2020 14:29:23 -0400 Subject: [PATCH 16/33] Add stubs and register handlers for Action API --- .../simulation/actuatorconfig_services.h | 40 +-- .../generated/simulation/base_services.h | 288 +++++++++--------- .../simulation/controlconfig_services.h | 58 ++-- .../simulation/deviceconfig_services.h | 64 ++-- .../simulation/devicemanager_services.h | 2 +- .../simulation/interconnectconfig_services.h | 28 +- .../simulation/visionconfig_services.h | 24 +- .../non-generated/kortex_arm_driver.h | 1 + .../non-generated/kortex_arm_simulation.h | 24 +- .../driver/kortex_arm_driver.cpp | 29 +- .../driver/kortex_arm_simulation.cpp | 54 ++++ .../templates/services_simulation.h.jinja2 | 2 +- 12 files changed, 354 insertions(+), 260 deletions(-) diff --git a/kortex_driver/include/kortex_driver/generated/simulation/actuatorconfig_services.h b/kortex_driver/include/kortex_driver/generated/simulation/actuatorconfig_services.h index e5a38be3..0a04e424 100644 --- a/kortex_driver/include/kortex_driver/generated/simulation/actuatorconfig_services.h +++ b/kortex_driver/include/kortex_driver/generated/simulation/actuatorconfig_services.h @@ -28,45 +28,45 @@ class ActuatorConfigSimulationServices : public IActuatorConfigServices virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; - std::function GetAxisOffsetsHandler = nullptr; + std::function GetAxisOffsetsHandler = nullptr; virtual bool GetAxisOffsets(kortex_driver::GetAxisOffsets::Request &req, kortex_driver::GetAxisOffsets::Response &res) override; - std::function SetAxisOffsetsHandler = nullptr; + std::function SetAxisOffsetsHandler = nullptr; virtual bool SetAxisOffsets(kortex_driver::SetAxisOffsets::Request &req, kortex_driver::SetAxisOffsets::Response &res) override; - std::function SetTorqueOffsetHandler = nullptr; + std::function SetTorqueOffsetHandler = nullptr; virtual bool SetTorqueOffset(kortex_driver::SetTorqueOffset::Request &req, kortex_driver::SetTorqueOffset::Response &res) override; - std::function ActuatorConfig_GetControlModeHandler = nullptr; + std::function ActuatorConfig_GetControlModeHandler = nullptr; virtual bool ActuatorConfig_GetControlMode(kortex_driver::ActuatorConfig_GetControlMode::Request &req, kortex_driver::ActuatorConfig_GetControlMode::Response &res) override; - std::function SetControlModeHandler = nullptr; + std::function SetControlModeHandler = nullptr; virtual bool SetControlMode(kortex_driver::SetControlMode::Request &req, kortex_driver::SetControlMode::Response &res) override; - std::function GetActivatedControlLoopHandler = nullptr; + std::function GetActivatedControlLoopHandler = nullptr; virtual bool GetActivatedControlLoop(kortex_driver::GetActivatedControlLoop::Request &req, kortex_driver::GetActivatedControlLoop::Response &res) override; - std::function SetActivatedControlLoopHandler = nullptr; + std::function SetActivatedControlLoopHandler = nullptr; virtual bool SetActivatedControlLoop(kortex_driver::SetActivatedControlLoop::Request &req, kortex_driver::SetActivatedControlLoop::Response &res) override; - std::function GetControlLoopParametersHandler = nullptr; + std::function GetControlLoopParametersHandler = nullptr; virtual bool GetControlLoopParameters(kortex_driver::GetControlLoopParameters::Request &req, kortex_driver::GetControlLoopParameters::Response &res) override; - std::function SetControlLoopParametersHandler = nullptr; + std::function SetControlLoopParametersHandler = nullptr; virtual bool SetControlLoopParameters(kortex_driver::SetControlLoopParameters::Request &req, kortex_driver::SetControlLoopParameters::Response &res) override; - std::function SelectCustomDataHandler = nullptr; + std::function SelectCustomDataHandler = nullptr; virtual bool SelectCustomData(kortex_driver::SelectCustomData::Request &req, kortex_driver::SelectCustomData::Response &res) override; - std::function GetSelectedCustomDataHandler = nullptr; + std::function GetSelectedCustomDataHandler = nullptr; virtual bool GetSelectedCustomData(kortex_driver::GetSelectedCustomData::Request &req, kortex_driver::GetSelectedCustomData::Response &res) override; - std::function SetCommandModeHandler = nullptr; + std::function SetCommandModeHandler = nullptr; virtual bool SetCommandMode(kortex_driver::SetCommandMode::Request &req, kortex_driver::SetCommandMode::Response &res) override; - std::function ActuatorConfig_ClearFaultsHandler = nullptr; + std::function ActuatorConfig_ClearFaultsHandler = nullptr; virtual bool ActuatorConfig_ClearFaults(kortex_driver::ActuatorConfig_ClearFaults::Request &req, kortex_driver::ActuatorConfig_ClearFaults::Response &res) override; - std::function SetServoingHandler = nullptr; + std::function SetServoingHandler = nullptr; virtual bool SetServoing(kortex_driver::SetServoing::Request &req, kortex_driver::SetServoing::Response &res) override; - std::function MoveToPositionHandler = nullptr; + std::function MoveToPositionHandler = nullptr; virtual bool MoveToPosition(kortex_driver::MoveToPosition::Request &req, kortex_driver::MoveToPosition::Response &res) override; - std::function GetCommandModeHandler = nullptr; + std::function GetCommandModeHandler = nullptr; virtual bool GetCommandMode(kortex_driver::GetCommandMode::Request &req, kortex_driver::GetCommandMode::Response &res) override; - std::function GetServoingHandler = nullptr; + std::function GetServoingHandler = nullptr; virtual bool GetServoing(kortex_driver::GetServoing::Request &req, kortex_driver::GetServoing::Response &res) override; - std::function GetTorqueOffsetHandler = nullptr; + std::function GetTorqueOffsetHandler = nullptr; virtual bool GetTorqueOffset(kortex_driver::GetTorqueOffset::Request &req, kortex_driver::GetTorqueOffset::Response &res) override; - std::function SetCoggingFeedforwardModeHandler = nullptr; + std::function SetCoggingFeedforwardModeHandler = nullptr; virtual bool SetCoggingFeedforwardMode(kortex_driver::SetCoggingFeedforwardMode::Request &req, kortex_driver::SetCoggingFeedforwardMode::Response &res) override; - std::function GetCoggingFeedforwardModeHandler = nullptr; + std::function GetCoggingFeedforwardModeHandler = nullptr; virtual bool GetCoggingFeedforwardMode(kortex_driver::GetCoggingFeedforwardMode::Request &req, kortex_driver::GetCoggingFeedforwardMode::Response &res) override; }; diff --git a/kortex_driver/include/kortex_driver/generated/simulation/base_services.h b/kortex_driver/include/kortex_driver/generated/simulation/base_services.h index d38aa0b2..65566751 100644 --- a/kortex_driver/include/kortex_driver/generated/simulation/base_services.h +++ b/kortex_driver/include/kortex_driver/generated/simulation/base_services.h @@ -28,307 +28,307 @@ class BaseSimulationServices : public IBaseServices virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; - std::function CreateUserProfileHandler = nullptr; + std::function CreateUserProfileHandler = nullptr; virtual bool CreateUserProfile(kortex_driver::CreateUserProfile::Request &req, kortex_driver::CreateUserProfile::Response &res) override; - std::function UpdateUserProfileHandler = nullptr; + std::function UpdateUserProfileHandler = nullptr; virtual bool UpdateUserProfile(kortex_driver::UpdateUserProfile::Request &req, kortex_driver::UpdateUserProfile::Response &res) override; - std::function ReadUserProfileHandler = nullptr; + std::function ReadUserProfileHandler = nullptr; virtual bool ReadUserProfile(kortex_driver::ReadUserProfile::Request &req, kortex_driver::ReadUserProfile::Response &res) override; - std::function DeleteUserProfileHandler = nullptr; + std::function DeleteUserProfileHandler = nullptr; virtual bool DeleteUserProfile(kortex_driver::DeleteUserProfile::Request &req, kortex_driver::DeleteUserProfile::Response &res) override; - std::function ReadAllUserProfilesHandler = nullptr; + std::function ReadAllUserProfilesHandler = nullptr; virtual bool ReadAllUserProfiles(kortex_driver::ReadAllUserProfiles::Request &req, kortex_driver::ReadAllUserProfiles::Response &res) override; - std::function ReadAllUsersHandler = nullptr; + std::function ReadAllUsersHandler = nullptr; virtual bool ReadAllUsers(kortex_driver::ReadAllUsers::Request &req, kortex_driver::ReadAllUsers::Response &res) override; - std::function ChangePasswordHandler = nullptr; + std::function ChangePasswordHandler = nullptr; virtual bool ChangePassword(kortex_driver::ChangePassword::Request &req, kortex_driver::ChangePassword::Response &res) override; - std::function CreateSequenceHandler = nullptr; + std::function CreateSequenceHandler = nullptr; virtual bool CreateSequence(kortex_driver::CreateSequence::Request &req, kortex_driver::CreateSequence::Response &res) override; - std::function UpdateSequenceHandler = nullptr; + std::function UpdateSequenceHandler = nullptr; virtual bool UpdateSequence(kortex_driver::UpdateSequence::Request &req, kortex_driver::UpdateSequence::Response &res) override; - std::function ReadSequenceHandler = nullptr; + std::function ReadSequenceHandler = nullptr; virtual bool ReadSequence(kortex_driver::ReadSequence::Request &req, kortex_driver::ReadSequence::Response &res) override; - std::function DeleteSequenceHandler = nullptr; + std::function DeleteSequenceHandler = nullptr; virtual bool DeleteSequence(kortex_driver::DeleteSequence::Request &req, kortex_driver::DeleteSequence::Response &res) override; - std::function ReadAllSequencesHandler = nullptr; + std::function ReadAllSequencesHandler = nullptr; virtual bool ReadAllSequences(kortex_driver::ReadAllSequences::Request &req, kortex_driver::ReadAllSequences::Response &res) override; - std::function PlaySequenceHandler = nullptr; + std::function PlaySequenceHandler = nullptr; virtual bool PlaySequence(kortex_driver::PlaySequence::Request &req, kortex_driver::PlaySequence::Response &res) override; - std::function PlayAdvancedSequenceHandler = nullptr; + std::function PlayAdvancedSequenceHandler = nullptr; virtual bool PlayAdvancedSequence(kortex_driver::PlayAdvancedSequence::Request &req, kortex_driver::PlayAdvancedSequence::Response &res) override; - std::function StopSequenceHandler = nullptr; + std::function StopSequenceHandler = nullptr; virtual bool StopSequence(kortex_driver::StopSequence::Request &req, kortex_driver::StopSequence::Response &res) override; - std::function PauseSequenceHandler = nullptr; + std::function PauseSequenceHandler = nullptr; virtual bool PauseSequence(kortex_driver::PauseSequence::Request &req, kortex_driver::PauseSequence::Response &res) override; - std::function ResumeSequenceHandler = nullptr; + std::function ResumeSequenceHandler = nullptr; virtual bool ResumeSequence(kortex_driver::ResumeSequence::Request &req, kortex_driver::ResumeSequence::Response &res) override; - std::function CreateProtectionZoneHandler = nullptr; + std::function CreateProtectionZoneHandler = nullptr; virtual bool CreateProtectionZone(kortex_driver::CreateProtectionZone::Request &req, kortex_driver::CreateProtectionZone::Response &res) override; - std::function UpdateProtectionZoneHandler = nullptr; + std::function UpdateProtectionZoneHandler = nullptr; virtual bool UpdateProtectionZone(kortex_driver::UpdateProtectionZone::Request &req, kortex_driver::UpdateProtectionZone::Response &res) override; - std::function ReadProtectionZoneHandler = nullptr; + std::function ReadProtectionZoneHandler = nullptr; virtual bool ReadProtectionZone(kortex_driver::ReadProtectionZone::Request &req, kortex_driver::ReadProtectionZone::Response &res) override; - std::function DeleteProtectionZoneHandler = nullptr; + std::function DeleteProtectionZoneHandler = nullptr; virtual bool DeleteProtectionZone(kortex_driver::DeleteProtectionZone::Request &req, kortex_driver::DeleteProtectionZone::Response &res) override; - std::function ReadAllProtectionZonesHandler = nullptr; + std::function ReadAllProtectionZonesHandler = nullptr; virtual bool ReadAllProtectionZones(kortex_driver::ReadAllProtectionZones::Request &req, kortex_driver::ReadAllProtectionZones::Response &res) override; - std::function CreateMappingHandler = nullptr; + std::function CreateMappingHandler = nullptr; virtual bool CreateMapping(kortex_driver::CreateMapping::Request &req, kortex_driver::CreateMapping::Response &res) override; - std::function ReadMappingHandler = nullptr; + std::function ReadMappingHandler = nullptr; virtual bool ReadMapping(kortex_driver::ReadMapping::Request &req, kortex_driver::ReadMapping::Response &res) override; - std::function UpdateMappingHandler = nullptr; + std::function UpdateMappingHandler = nullptr; virtual bool UpdateMapping(kortex_driver::UpdateMapping::Request &req, kortex_driver::UpdateMapping::Response &res) override; - std::function DeleteMappingHandler = nullptr; + std::function DeleteMappingHandler = nullptr; virtual bool DeleteMapping(kortex_driver::DeleteMapping::Request &req, kortex_driver::DeleteMapping::Response &res) override; - std::function ReadAllMappingsHandler = nullptr; + std::function ReadAllMappingsHandler = nullptr; virtual bool ReadAllMappings(kortex_driver::ReadAllMappings::Request &req, kortex_driver::ReadAllMappings::Response &res) override; - std::function CreateMapHandler = nullptr; + std::function CreateMapHandler = nullptr; virtual bool CreateMap(kortex_driver::CreateMap::Request &req, kortex_driver::CreateMap::Response &res) override; - std::function ReadMapHandler = nullptr; + std::function ReadMapHandler = nullptr; virtual bool ReadMap(kortex_driver::ReadMap::Request &req, kortex_driver::ReadMap::Response &res) override; - std::function UpdateMapHandler = nullptr; + std::function UpdateMapHandler = nullptr; virtual bool UpdateMap(kortex_driver::UpdateMap::Request &req, kortex_driver::UpdateMap::Response &res) override; - std::function DeleteMapHandler = nullptr; + std::function DeleteMapHandler = nullptr; virtual bool DeleteMap(kortex_driver::DeleteMap::Request &req, kortex_driver::DeleteMap::Response &res) override; - std::function ReadAllMapsHandler = nullptr; + std::function ReadAllMapsHandler = nullptr; virtual bool ReadAllMaps(kortex_driver::ReadAllMaps::Request &req, kortex_driver::ReadAllMaps::Response &res) override; - std::function ActivateMapHandler = nullptr; + std::function ActivateMapHandler = nullptr; virtual bool ActivateMap(kortex_driver::ActivateMap::Request &req, kortex_driver::ActivateMap::Response &res) override; - std::function CreateActionHandler = nullptr; + std::function CreateActionHandler = nullptr; virtual bool CreateAction(kortex_driver::CreateAction::Request &req, kortex_driver::CreateAction::Response &res) override; - std::function ReadActionHandler = nullptr; + std::function ReadActionHandler = nullptr; virtual bool ReadAction(kortex_driver::ReadAction::Request &req, kortex_driver::ReadAction::Response &res) override; - std::function ReadAllActionsHandler = nullptr; + std::function ReadAllActionsHandler = nullptr; virtual bool ReadAllActions(kortex_driver::ReadAllActions::Request &req, kortex_driver::ReadAllActions::Response &res) override; - std::function DeleteActionHandler = nullptr; + std::function DeleteActionHandler = nullptr; virtual bool DeleteAction(kortex_driver::DeleteAction::Request &req, kortex_driver::DeleteAction::Response &res) override; - std::function UpdateActionHandler = nullptr; + std::function UpdateActionHandler = nullptr; virtual bool UpdateAction(kortex_driver::UpdateAction::Request &req, kortex_driver::UpdateAction::Response &res) override; - std::function ExecuteActionFromReferenceHandler = nullptr; + std::function ExecuteActionFromReferenceHandler = nullptr; virtual bool ExecuteActionFromReference(kortex_driver::ExecuteActionFromReference::Request &req, kortex_driver::ExecuteActionFromReference::Response &res) override; - std::function ExecuteActionHandler = nullptr; + std::function ExecuteActionHandler = nullptr; virtual bool ExecuteAction(kortex_driver::ExecuteAction::Request &req, kortex_driver::ExecuteAction::Response &res) override; - std::function PauseActionHandler = nullptr; + std::function PauseActionHandler = nullptr; virtual bool PauseAction(kortex_driver::PauseAction::Request &req, kortex_driver::PauseAction::Response &res) override; - std::function StopActionHandler = nullptr; + std::function StopActionHandler = nullptr; virtual bool StopAction(kortex_driver::StopAction::Request &req, kortex_driver::StopAction::Response &res) override; - std::function ResumeActionHandler = nullptr; + std::function ResumeActionHandler = nullptr; virtual bool ResumeAction(kortex_driver::ResumeAction::Request &req, kortex_driver::ResumeAction::Response &res) override; - std::function GetIPv4ConfigurationHandler = nullptr; + std::function GetIPv4ConfigurationHandler = nullptr; virtual bool GetIPv4Configuration(kortex_driver::GetIPv4Configuration::Request &req, kortex_driver::GetIPv4Configuration::Response &res) override; - std::function SetIPv4ConfigurationHandler = nullptr; + std::function SetIPv4ConfigurationHandler = nullptr; virtual bool SetIPv4Configuration(kortex_driver::SetIPv4Configuration::Request &req, kortex_driver::SetIPv4Configuration::Response &res) override; - std::function SetCommunicationInterfaceEnableHandler = nullptr; + std::function SetCommunicationInterfaceEnableHandler = nullptr; virtual bool SetCommunicationInterfaceEnable(kortex_driver::SetCommunicationInterfaceEnable::Request &req, kortex_driver::SetCommunicationInterfaceEnable::Response &res) override; - std::function IsCommunicationInterfaceEnableHandler = nullptr; + std::function IsCommunicationInterfaceEnableHandler = nullptr; virtual bool IsCommunicationInterfaceEnable(kortex_driver::IsCommunicationInterfaceEnable::Request &req, kortex_driver::IsCommunicationInterfaceEnable::Response &res) override; - std::function GetAvailableWifiHandler = nullptr; + std::function GetAvailableWifiHandler = nullptr; virtual bool GetAvailableWifi(kortex_driver::GetAvailableWifi::Request &req, kortex_driver::GetAvailableWifi::Response &res) override; - std::function GetWifiInformationHandler = nullptr; + std::function GetWifiInformationHandler = nullptr; virtual bool GetWifiInformation(kortex_driver::GetWifiInformation::Request &req, kortex_driver::GetWifiInformation::Response &res) override; - std::function AddWifiConfigurationHandler = nullptr; + std::function AddWifiConfigurationHandler = nullptr; virtual bool AddWifiConfiguration(kortex_driver::AddWifiConfiguration::Request &req, kortex_driver::AddWifiConfiguration::Response &res) override; - std::function DeleteWifiConfigurationHandler = nullptr; + std::function DeleteWifiConfigurationHandler = nullptr; virtual bool DeleteWifiConfiguration(kortex_driver::DeleteWifiConfiguration::Request &req, kortex_driver::DeleteWifiConfiguration::Response &res) override; - std::function GetAllConfiguredWifisHandler = nullptr; + std::function GetAllConfiguredWifisHandler = nullptr; virtual bool GetAllConfiguredWifis(kortex_driver::GetAllConfiguredWifis::Request &req, kortex_driver::GetAllConfiguredWifis::Response &res) override; - std::function ConnectWifiHandler = nullptr; + std::function ConnectWifiHandler = nullptr; virtual bool ConnectWifi(kortex_driver::ConnectWifi::Request &req, kortex_driver::ConnectWifi::Response &res) override; - std::function DisconnectWifiHandler = nullptr; + std::function DisconnectWifiHandler = nullptr; virtual bool DisconnectWifi(kortex_driver::DisconnectWifi::Request &req, kortex_driver::DisconnectWifi::Response &res) override; - std::function GetConnectedWifiInformationHandler = nullptr; + std::function GetConnectedWifiInformationHandler = nullptr; virtual bool GetConnectedWifiInformation(kortex_driver::GetConnectedWifiInformation::Request &req, kortex_driver::GetConnectedWifiInformation::Response &res) override; - std::function Base_UnsubscribeHandler = nullptr; + std::function Base_UnsubscribeHandler = nullptr; virtual bool Base_Unsubscribe(kortex_driver::Base_Unsubscribe::Request &req, kortex_driver::Base_Unsubscribe::Response &res) override; - std::function OnNotificationConfigurationChangeTopicHandler = nullptr; + std::function OnNotificationConfigurationChangeTopicHandler = nullptr; virtual bool OnNotificationConfigurationChangeTopic(kortex_driver::OnNotificationConfigurationChangeTopic::Request &req, kortex_driver::OnNotificationConfigurationChangeTopic::Response &res) override; virtual void cb_ConfigurationChangeTopic(Kinova::Api::Base::ConfigurationChangeNotification notif) override; - std::function OnNotificationMappingInfoTopicHandler = nullptr; + std::function OnNotificationMappingInfoTopicHandler = nullptr; virtual bool OnNotificationMappingInfoTopic(kortex_driver::OnNotificationMappingInfoTopic::Request &req, kortex_driver::OnNotificationMappingInfoTopic::Response &res) override; virtual void cb_MappingInfoTopic(Kinova::Api::Base::MappingInfoNotification notif) override; - std::function OnNotificationControlModeTopicHandler = nullptr; + std::function OnNotificationControlModeTopicHandler = nullptr; virtual bool OnNotificationControlModeTopic(kortex_driver::OnNotificationControlModeTopic::Request &req, kortex_driver::OnNotificationControlModeTopic::Response &res) override; virtual void cb_ControlModeTopic(Kinova::Api::Base::ControlModeNotification notif) override; - std::function OnNotificationOperatingModeTopicHandler = nullptr; + std::function OnNotificationOperatingModeTopicHandler = nullptr; virtual bool OnNotificationOperatingModeTopic(kortex_driver::OnNotificationOperatingModeTopic::Request &req, kortex_driver::OnNotificationOperatingModeTopic::Response &res) override; virtual void cb_OperatingModeTopic(Kinova::Api::Base::OperatingModeNotification notif) override; - std::function OnNotificationSequenceInfoTopicHandler = nullptr; + std::function OnNotificationSequenceInfoTopicHandler = nullptr; virtual bool OnNotificationSequenceInfoTopic(kortex_driver::OnNotificationSequenceInfoTopic::Request &req, kortex_driver::OnNotificationSequenceInfoTopic::Response &res) override; virtual void cb_SequenceInfoTopic(Kinova::Api::Base::SequenceInfoNotification notif) override; - std::function OnNotificationProtectionZoneTopicHandler = nullptr; + std::function OnNotificationProtectionZoneTopicHandler = nullptr; virtual bool OnNotificationProtectionZoneTopic(kortex_driver::OnNotificationProtectionZoneTopic::Request &req, kortex_driver::OnNotificationProtectionZoneTopic::Response &res) override; virtual void cb_ProtectionZoneTopic(Kinova::Api::Base::ProtectionZoneNotification notif) override; - std::function OnNotificationUserTopicHandler = nullptr; + std::function OnNotificationUserTopicHandler = nullptr; virtual bool OnNotificationUserTopic(kortex_driver::OnNotificationUserTopic::Request &req, kortex_driver::OnNotificationUserTopic::Response &res) override; virtual void cb_UserTopic(Kinova::Api::Base::UserNotification notif) override; - std::function OnNotificationControllerTopicHandler = nullptr; + std::function OnNotificationControllerTopicHandler = nullptr; virtual bool OnNotificationControllerTopic(kortex_driver::OnNotificationControllerTopic::Request &req, kortex_driver::OnNotificationControllerTopic::Response &res) override; virtual void cb_ControllerTopic(Kinova::Api::Base::ControllerNotification notif) override; - std::function OnNotificationActionTopicHandler = nullptr; + std::function OnNotificationActionTopicHandler = nullptr; virtual bool OnNotificationActionTopic(kortex_driver::OnNotificationActionTopic::Request &req, kortex_driver::OnNotificationActionTopic::Response &res) override; virtual void cb_ActionTopic(Kinova::Api::Base::ActionNotification notif) override; - std::function OnNotificationRobotEventTopicHandler = nullptr; + std::function OnNotificationRobotEventTopicHandler = nullptr; virtual bool OnNotificationRobotEventTopic(kortex_driver::OnNotificationRobotEventTopic::Request &req, kortex_driver::OnNotificationRobotEventTopic::Response &res) override; virtual void cb_RobotEventTopic(Kinova::Api::Base::RobotEventNotification notif) override; - std::function PlayCartesianTrajectoryHandler = nullptr; + std::function PlayCartesianTrajectoryHandler = nullptr; virtual bool PlayCartesianTrajectory(kortex_driver::PlayCartesianTrajectory::Request &req, kortex_driver::PlayCartesianTrajectory::Response &res) override; - std::function PlayCartesianTrajectoryPositionHandler = nullptr; + std::function PlayCartesianTrajectoryPositionHandler = nullptr; virtual bool PlayCartesianTrajectoryPosition(kortex_driver::PlayCartesianTrajectoryPosition::Request &req, kortex_driver::PlayCartesianTrajectoryPosition::Response &res) override; - std::function PlayCartesianTrajectoryOrientationHandler = nullptr; + std::function PlayCartesianTrajectoryOrientationHandler = nullptr; virtual bool PlayCartesianTrajectoryOrientation(kortex_driver::PlayCartesianTrajectoryOrientation::Request &req, kortex_driver::PlayCartesianTrajectoryOrientation::Response &res) override; - std::function StopHandler = nullptr; + std::function StopHandler = nullptr; virtual bool Stop(kortex_driver::Stop::Request &req, kortex_driver::Stop::Response &res) override; - std::function GetMeasuredCartesianPoseHandler = nullptr; + std::function GetMeasuredCartesianPoseHandler = nullptr; virtual bool GetMeasuredCartesianPose(kortex_driver::GetMeasuredCartesianPose::Request &req, kortex_driver::GetMeasuredCartesianPose::Response &res) override; - std::function SendWrenchCommandHandler = nullptr; + std::function SendWrenchCommandHandler = nullptr; virtual bool SendWrenchCommand(kortex_driver::SendWrenchCommand::Request &req, kortex_driver::SendWrenchCommand::Response &res) override; - std::function SendWrenchJoystickCommandHandler = nullptr; + std::function SendWrenchJoystickCommandHandler = nullptr; virtual bool SendWrenchJoystickCommand(kortex_driver::SendWrenchJoystickCommand::Request &req, kortex_driver::SendWrenchJoystickCommand::Response &res) override; - std::function SendTwistJoystickCommandHandler = nullptr; + std::function SendTwistJoystickCommandHandler = nullptr; virtual bool SendTwistJoystickCommand(kortex_driver::SendTwistJoystickCommand::Request &req, kortex_driver::SendTwistJoystickCommand::Response &res) override; - std::function SendTwistCommandHandler = nullptr; + std::function SendTwistCommandHandler = nullptr; virtual bool SendTwistCommand(kortex_driver::SendTwistCommand::Request &req, kortex_driver::SendTwistCommand::Response &res) override; - std::function PlayJointTrajectoryHandler = nullptr; + std::function PlayJointTrajectoryHandler = nullptr; virtual bool PlayJointTrajectory(kortex_driver::PlayJointTrajectory::Request &req, kortex_driver::PlayJointTrajectory::Response &res) override; - std::function PlaySelectedJointTrajectoryHandler = nullptr; + std::function PlaySelectedJointTrajectoryHandler = nullptr; virtual bool PlaySelectedJointTrajectory(kortex_driver::PlaySelectedJointTrajectory::Request &req, kortex_driver::PlaySelectedJointTrajectory::Response &res) override; - std::function GetMeasuredJointAnglesHandler = nullptr; + std::function GetMeasuredJointAnglesHandler = nullptr; virtual bool GetMeasuredJointAngles(kortex_driver::GetMeasuredJointAngles::Request &req, kortex_driver::GetMeasuredJointAngles::Response &res) override; - std::function SendJointSpeedsCommandHandler = nullptr; + std::function SendJointSpeedsCommandHandler = nullptr; virtual bool SendJointSpeedsCommand(kortex_driver::SendJointSpeedsCommand::Request &req, kortex_driver::SendJointSpeedsCommand::Response &res) override; - std::function SendSelectedJointSpeedCommandHandler = nullptr; + std::function SendSelectedJointSpeedCommandHandler = nullptr; virtual bool SendSelectedJointSpeedCommand(kortex_driver::SendSelectedJointSpeedCommand::Request &req, kortex_driver::SendSelectedJointSpeedCommand::Response &res) override; - std::function SendGripperCommandHandler = nullptr; + std::function SendGripperCommandHandler = nullptr; virtual bool SendGripperCommand(kortex_driver::SendGripperCommand::Request &req, kortex_driver::SendGripperCommand::Response &res) override; - std::function GetMeasuredGripperMovementHandler = nullptr; + std::function GetMeasuredGripperMovementHandler = nullptr; virtual bool GetMeasuredGripperMovement(kortex_driver::GetMeasuredGripperMovement::Request &req, kortex_driver::GetMeasuredGripperMovement::Response &res) override; - std::function SetAdmittanceHandler = nullptr; + std::function SetAdmittanceHandler = nullptr; virtual bool SetAdmittance(kortex_driver::SetAdmittance::Request &req, kortex_driver::SetAdmittance::Response &res) override; - std::function SetOperatingModeHandler = nullptr; + std::function SetOperatingModeHandler = nullptr; virtual bool SetOperatingMode(kortex_driver::SetOperatingMode::Request &req, kortex_driver::SetOperatingMode::Response &res) override; - std::function ApplyEmergencyStopHandler = nullptr; + std::function ApplyEmergencyStopHandler = nullptr; virtual bool ApplyEmergencyStop(kortex_driver::ApplyEmergencyStop::Request &req, kortex_driver::ApplyEmergencyStop::Response &res) override; - std::function Base_ClearFaultsHandler = nullptr; + std::function Base_ClearFaultsHandler = nullptr; virtual bool Base_ClearFaults(kortex_driver::Base_ClearFaults::Request &req, kortex_driver::Base_ClearFaults::Response &res) override; - std::function Base_GetControlModeHandler = nullptr; + std::function Base_GetControlModeHandler = nullptr; virtual bool Base_GetControlMode(kortex_driver::Base_GetControlMode::Request &req, kortex_driver::Base_GetControlMode::Response &res) override; - std::function GetOperatingModeHandler = nullptr; + std::function GetOperatingModeHandler = nullptr; virtual bool GetOperatingMode(kortex_driver::GetOperatingMode::Request &req, kortex_driver::GetOperatingMode::Response &res) override; - std::function SetServoingModeHandler = nullptr; + std::function SetServoingModeHandler = nullptr; virtual bool SetServoingMode(kortex_driver::SetServoingMode::Request &req, kortex_driver::SetServoingMode::Response &res) override; - std::function GetServoingModeHandler = nullptr; + std::function GetServoingModeHandler = nullptr; virtual bool GetServoingMode(kortex_driver::GetServoingMode::Request &req, kortex_driver::GetServoingMode::Response &res) override; - std::function OnNotificationServoingModeTopicHandler = nullptr; + std::function OnNotificationServoingModeTopicHandler = nullptr; virtual bool OnNotificationServoingModeTopic(kortex_driver::OnNotificationServoingModeTopic::Request &req, kortex_driver::OnNotificationServoingModeTopic::Response &res) override; virtual void cb_ServoingModeTopic(Kinova::Api::Base::ServoingModeNotification notif) override; - std::function RestoreFactorySettingsHandler = nullptr; + std::function RestoreFactorySettingsHandler = nullptr; virtual bool RestoreFactorySettings(kortex_driver::RestoreFactorySettings::Request &req, kortex_driver::RestoreFactorySettings::Response &res) override; - std::function OnNotificationFactoryTopicHandler = nullptr; + std::function OnNotificationFactoryTopicHandler = nullptr; virtual bool OnNotificationFactoryTopic(kortex_driver::OnNotificationFactoryTopic::Request &req, kortex_driver::OnNotificationFactoryTopic::Response &res) override; virtual void cb_FactoryTopic(Kinova::Api::Base::FactoryNotification notif) override; - std::function GetAllConnectedControllersHandler = nullptr; + std::function GetAllConnectedControllersHandler = nullptr; virtual bool GetAllConnectedControllers(kortex_driver::GetAllConnectedControllers::Request &req, kortex_driver::GetAllConnectedControllers::Response &res) override; - std::function GetControllerStateHandler = nullptr; + std::function GetControllerStateHandler = nullptr; virtual bool GetControllerState(kortex_driver::GetControllerState::Request &req, kortex_driver::GetControllerState::Response &res) override; - std::function GetActuatorCountHandler = nullptr; + std::function GetActuatorCountHandler = nullptr; virtual bool GetActuatorCount(kortex_driver::GetActuatorCount::Request &req, kortex_driver::GetActuatorCount::Response &res) override; - std::function StartWifiScanHandler = nullptr; + std::function StartWifiScanHandler = nullptr; virtual bool StartWifiScan(kortex_driver::StartWifiScan::Request &req, kortex_driver::StartWifiScan::Response &res) override; - std::function GetConfiguredWifiHandler = nullptr; + std::function GetConfiguredWifiHandler = nullptr; virtual bool GetConfiguredWifi(kortex_driver::GetConfiguredWifi::Request &req, kortex_driver::GetConfiguredWifi::Response &res) override; - std::function OnNotificationNetworkTopicHandler = nullptr; + std::function OnNotificationNetworkTopicHandler = nullptr; virtual bool OnNotificationNetworkTopic(kortex_driver::OnNotificationNetworkTopic::Request &req, kortex_driver::OnNotificationNetworkTopic::Response &res) override; virtual void cb_NetworkTopic(Kinova::Api::Base::NetworkNotification notif) override; - std::function GetArmStateHandler = nullptr; + std::function GetArmStateHandler = nullptr; virtual bool GetArmState(kortex_driver::GetArmState::Request &req, kortex_driver::GetArmState::Response &res) override; - std::function OnNotificationArmStateTopicHandler = nullptr; + std::function OnNotificationArmStateTopicHandler = nullptr; virtual bool OnNotificationArmStateTopic(kortex_driver::OnNotificationArmStateTopic::Request &req, kortex_driver::OnNotificationArmStateTopic::Response &res) override; virtual void cb_ArmStateTopic(Kinova::Api::Base::ArmStateNotification notif) override; - std::function GetIPv4InformationHandler = nullptr; + std::function GetIPv4InformationHandler = nullptr; virtual bool GetIPv4Information(kortex_driver::GetIPv4Information::Request &req, kortex_driver::GetIPv4Information::Response &res) override; - std::function SetWifiCountryCodeHandler = nullptr; + std::function SetWifiCountryCodeHandler = nullptr; virtual bool SetWifiCountryCode(kortex_driver::SetWifiCountryCode::Request &req, kortex_driver::SetWifiCountryCode::Response &res) override; - std::function GetWifiCountryCodeHandler = nullptr; + std::function GetWifiCountryCodeHandler = nullptr; virtual bool GetWifiCountryCode(kortex_driver::GetWifiCountryCode::Request &req, kortex_driver::GetWifiCountryCode::Response &res) override; - std::function Base_SetCapSenseConfigHandler = nullptr; + std::function Base_SetCapSenseConfigHandler = nullptr; virtual bool Base_SetCapSenseConfig(kortex_driver::Base_SetCapSenseConfig::Request &req, kortex_driver::Base_SetCapSenseConfig::Response &res) override; - std::function Base_GetCapSenseConfigHandler = nullptr; + std::function Base_GetCapSenseConfigHandler = nullptr; virtual bool Base_GetCapSenseConfig(kortex_driver::Base_GetCapSenseConfig::Request &req, kortex_driver::Base_GetCapSenseConfig::Response &res) override; - std::function GetAllJointsSpeedHardLimitationHandler = nullptr; + std::function GetAllJointsSpeedHardLimitationHandler = nullptr; virtual bool GetAllJointsSpeedHardLimitation(kortex_driver::GetAllJointsSpeedHardLimitation::Request &req, kortex_driver::GetAllJointsSpeedHardLimitation::Response &res) override; - std::function GetAllJointsTorqueHardLimitationHandler = nullptr; + std::function GetAllJointsTorqueHardLimitationHandler = nullptr; virtual bool GetAllJointsTorqueHardLimitation(kortex_driver::GetAllJointsTorqueHardLimitation::Request &req, kortex_driver::GetAllJointsTorqueHardLimitation::Response &res) override; - std::function GetTwistHardLimitationHandler = nullptr; + std::function GetTwistHardLimitationHandler = nullptr; virtual bool GetTwistHardLimitation(kortex_driver::GetTwistHardLimitation::Request &req, kortex_driver::GetTwistHardLimitation::Response &res) override; - std::function GetWrenchHardLimitationHandler = nullptr; + std::function GetWrenchHardLimitationHandler = nullptr; virtual bool GetWrenchHardLimitation(kortex_driver::GetWrenchHardLimitation::Request &req, kortex_driver::GetWrenchHardLimitation::Response &res) override; - std::function SendJointSpeedsJoystickCommandHandler = nullptr; + std::function SendJointSpeedsJoystickCommandHandler = nullptr; virtual bool SendJointSpeedsJoystickCommand(kortex_driver::SendJointSpeedsJoystickCommand::Request &req, kortex_driver::SendJointSpeedsJoystickCommand::Response &res) override; - std::function SendSelectedJointSpeedJoystickCommandHandler = nullptr; + std::function SendSelectedJointSpeedJoystickCommandHandler = nullptr; virtual bool SendSelectedJointSpeedJoystickCommand(kortex_driver::SendSelectedJointSpeedJoystickCommand::Request &req, kortex_driver::SendSelectedJointSpeedJoystickCommand::Response &res) override; - std::function EnableBridgeHandler = nullptr; + std::function EnableBridgeHandler = nullptr; virtual bool EnableBridge(kortex_driver::EnableBridge::Request &req, kortex_driver::EnableBridge::Response &res) override; - std::function DisableBridgeHandler = nullptr; + std::function DisableBridgeHandler = nullptr; virtual bool DisableBridge(kortex_driver::DisableBridge::Request &req, kortex_driver::DisableBridge::Response &res) override; - std::function GetBridgeListHandler = nullptr; + std::function GetBridgeListHandler = nullptr; virtual bool GetBridgeList(kortex_driver::GetBridgeList::Request &req, kortex_driver::GetBridgeList::Response &res) override; - std::function GetBridgeConfigHandler = nullptr; + std::function GetBridgeConfigHandler = nullptr; virtual bool GetBridgeConfig(kortex_driver::GetBridgeConfig::Request &req, kortex_driver::GetBridgeConfig::Response &res) override; - std::function PlayPreComputedJointTrajectoryHandler = nullptr; + std::function PlayPreComputedJointTrajectoryHandler = nullptr; virtual bool PlayPreComputedJointTrajectory(kortex_driver::PlayPreComputedJointTrajectory::Request &req, kortex_driver::PlayPreComputedJointTrajectory::Response &res) override; - std::function GetProductConfigurationHandler = nullptr; + std::function GetProductConfigurationHandler = nullptr; virtual bool GetProductConfiguration(kortex_driver::GetProductConfiguration::Request &req, kortex_driver::GetProductConfiguration::Response &res) override; - std::function UpdateEndEffectorTypeConfigurationHandler = nullptr; + std::function UpdateEndEffectorTypeConfigurationHandler = nullptr; virtual bool UpdateEndEffectorTypeConfiguration(kortex_driver::UpdateEndEffectorTypeConfiguration::Request &req, kortex_driver::UpdateEndEffectorTypeConfiguration::Response &res) override; - std::function RestoreFactoryProductConfigurationHandler = nullptr; + std::function RestoreFactoryProductConfigurationHandler = nullptr; virtual bool RestoreFactoryProductConfiguration(kortex_driver::RestoreFactoryProductConfiguration::Request &req, kortex_driver::RestoreFactoryProductConfiguration::Response &res) override; - std::function GetTrajectoryErrorReportHandler = nullptr; + std::function GetTrajectoryErrorReportHandler = nullptr; virtual bool GetTrajectoryErrorReport(kortex_driver::GetTrajectoryErrorReport::Request &req, kortex_driver::GetTrajectoryErrorReport::Response &res) override; - std::function GetAllJointsSpeedSoftLimitationHandler = nullptr; + std::function GetAllJointsSpeedSoftLimitationHandler = nullptr; virtual bool GetAllJointsSpeedSoftLimitation(kortex_driver::GetAllJointsSpeedSoftLimitation::Request &req, kortex_driver::GetAllJointsSpeedSoftLimitation::Response &res) override; - std::function GetAllJointsTorqueSoftLimitationHandler = nullptr; + std::function GetAllJointsTorqueSoftLimitationHandler = nullptr; virtual bool GetAllJointsTorqueSoftLimitation(kortex_driver::GetAllJointsTorqueSoftLimitation::Request &req, kortex_driver::GetAllJointsTorqueSoftLimitation::Response &res) override; - std::function GetTwistSoftLimitationHandler = nullptr; + std::function GetTwistSoftLimitationHandler = nullptr; virtual bool GetTwistSoftLimitation(kortex_driver::GetTwistSoftLimitation::Request &req, kortex_driver::GetTwistSoftLimitation::Response &res) override; - std::function GetWrenchSoftLimitationHandler = nullptr; + std::function GetWrenchSoftLimitationHandler = nullptr; virtual bool GetWrenchSoftLimitation(kortex_driver::GetWrenchSoftLimitation::Request &req, kortex_driver::GetWrenchSoftLimitation::Response &res) override; - std::function SetControllerConfigurationModeHandler = nullptr; + std::function SetControllerConfigurationModeHandler = nullptr; virtual bool SetControllerConfigurationMode(kortex_driver::SetControllerConfigurationMode::Request &req, kortex_driver::SetControllerConfigurationMode::Response &res) override; - std::function GetControllerConfigurationModeHandler = nullptr; + std::function GetControllerConfigurationModeHandler = nullptr; virtual bool GetControllerConfigurationMode(kortex_driver::GetControllerConfigurationMode::Request &req, kortex_driver::GetControllerConfigurationMode::Response &res) override; - std::function StartTeachingHandler = nullptr; + std::function StartTeachingHandler = nullptr; virtual bool StartTeaching(kortex_driver::StartTeaching::Request &req, kortex_driver::StartTeaching::Response &res) override; - std::function StopTeachingHandler = nullptr; + std::function StopTeachingHandler = nullptr; virtual bool StopTeaching(kortex_driver::StopTeaching::Request &req, kortex_driver::StopTeaching::Response &res) override; - std::function AddSequenceTasksHandler = nullptr; + std::function AddSequenceTasksHandler = nullptr; virtual bool AddSequenceTasks(kortex_driver::AddSequenceTasks::Request &req, kortex_driver::AddSequenceTasks::Response &res) override; - std::function UpdateSequenceTaskHandler = nullptr; + std::function UpdateSequenceTaskHandler = nullptr; virtual bool UpdateSequenceTask(kortex_driver::UpdateSequenceTask::Request &req, kortex_driver::UpdateSequenceTask::Response &res) override; - std::function SwapSequenceTasksHandler = nullptr; + std::function SwapSequenceTasksHandler = nullptr; virtual bool SwapSequenceTasks(kortex_driver::SwapSequenceTasks::Request &req, kortex_driver::SwapSequenceTasks::Response &res) override; - std::function ReadSequenceTaskHandler = nullptr; + std::function ReadSequenceTaskHandler = nullptr; virtual bool ReadSequenceTask(kortex_driver::ReadSequenceTask::Request &req, kortex_driver::ReadSequenceTask::Response &res) override; - std::function ReadAllSequenceTasksHandler = nullptr; + std::function ReadAllSequenceTasksHandler = nullptr; virtual bool ReadAllSequenceTasks(kortex_driver::ReadAllSequenceTasks::Request &req, kortex_driver::ReadAllSequenceTasks::Response &res) override; - std::function DeleteSequenceTaskHandler = nullptr; + std::function DeleteSequenceTaskHandler = nullptr; virtual bool DeleteSequenceTask(kortex_driver::DeleteSequenceTask::Request &req, kortex_driver::DeleteSequenceTask::Response &res) override; - std::function DeleteAllSequenceTasksHandler = nullptr; + std::function DeleteAllSequenceTasksHandler = nullptr; virtual bool DeleteAllSequenceTasks(kortex_driver::DeleteAllSequenceTasks::Request &req, kortex_driver::DeleteAllSequenceTasks::Response &res) override; - std::function TakeSnapshotHandler = nullptr; + std::function TakeSnapshotHandler = nullptr; virtual bool TakeSnapshot(kortex_driver::TakeSnapshot::Request &req, kortex_driver::TakeSnapshot::Response &res) override; - std::function GetFirmwareBundleVersionsHandler = nullptr; + std::function GetFirmwareBundleVersionsHandler = nullptr; virtual bool GetFirmwareBundleVersions(kortex_driver::GetFirmwareBundleVersions::Request &req, kortex_driver::GetFirmwareBundleVersions::Response &res) override; - std::function MoveSequenceTaskHandler = nullptr; + std::function MoveSequenceTaskHandler = nullptr; virtual bool MoveSequenceTask(kortex_driver::MoveSequenceTask::Request &req, kortex_driver::MoveSequenceTask::Response &res) override; - std::function DuplicateMappingHandler = nullptr; + std::function DuplicateMappingHandler = nullptr; virtual bool DuplicateMapping(kortex_driver::DuplicateMapping::Request &req, kortex_driver::DuplicateMapping::Response &res) override; - std::function DuplicateMapHandler = nullptr; + std::function DuplicateMapHandler = nullptr; virtual bool DuplicateMap(kortex_driver::DuplicateMap::Request &req, kortex_driver::DuplicateMap::Response &res) override; - std::function SetControllerConfigurationHandler = nullptr; + std::function SetControllerConfigurationHandler = nullptr; virtual bool SetControllerConfiguration(kortex_driver::SetControllerConfiguration::Request &req, kortex_driver::SetControllerConfiguration::Response &res) override; - std::function GetControllerConfigurationHandler = nullptr; + std::function GetControllerConfigurationHandler = nullptr; virtual bool GetControllerConfiguration(kortex_driver::GetControllerConfiguration::Request &req, kortex_driver::GetControllerConfiguration::Response &res) override; - std::function GetAllControllerConfigurationsHandler = nullptr; + std::function GetAllControllerConfigurationsHandler = nullptr; virtual bool GetAllControllerConfigurations(kortex_driver::GetAllControllerConfigurations::Request &req, kortex_driver::GetAllControllerConfigurations::Response &res) override; }; diff --git a/kortex_driver/include/kortex_driver/generated/simulation/controlconfig_services.h b/kortex_driver/include/kortex_driver/generated/simulation/controlconfig_services.h index 8e6ce705..30a8738a 100644 --- a/kortex_driver/include/kortex_driver/generated/simulation/controlconfig_services.h +++ b/kortex_driver/include/kortex_driver/generated/simulation/controlconfig_services.h @@ -28,64 +28,64 @@ class ControlConfigSimulationServices : public IControlConfigServices virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; - std::function SetGravityVectorHandler = nullptr; + std::function SetGravityVectorHandler = nullptr; virtual bool SetGravityVector(kortex_driver::SetGravityVector::Request &req, kortex_driver::SetGravityVector::Response &res) override; - std::function GetGravityVectorHandler = nullptr; + std::function GetGravityVectorHandler = nullptr; virtual bool GetGravityVector(kortex_driver::GetGravityVector::Request &req, kortex_driver::GetGravityVector::Response &res) override; - std::function SetPayloadInformationHandler = nullptr; + std::function SetPayloadInformationHandler = nullptr; virtual bool SetPayloadInformation(kortex_driver::SetPayloadInformation::Request &req, kortex_driver::SetPayloadInformation::Response &res) override; - std::function GetPayloadInformationHandler = nullptr; + std::function GetPayloadInformationHandler = nullptr; virtual bool GetPayloadInformation(kortex_driver::GetPayloadInformation::Request &req, kortex_driver::GetPayloadInformation::Response &res) override; - std::function SetToolConfigurationHandler = nullptr; + std::function SetToolConfigurationHandler = nullptr; virtual bool SetToolConfiguration(kortex_driver::SetToolConfiguration::Request &req, kortex_driver::SetToolConfiguration::Response &res) override; - std::function GetToolConfigurationHandler = nullptr; + std::function GetToolConfigurationHandler = nullptr; virtual bool GetToolConfiguration(kortex_driver::GetToolConfiguration::Request &req, kortex_driver::GetToolConfiguration::Response &res) override; - std::function OnNotificationControlConfigurationTopicHandler = nullptr; + std::function OnNotificationControlConfigurationTopicHandler = nullptr; virtual bool OnNotificationControlConfigurationTopic(kortex_driver::OnNotificationControlConfigurationTopic::Request &req, kortex_driver::OnNotificationControlConfigurationTopic::Response &res) override; virtual void cb_ControlConfigurationTopic(Kinova::Api::ControlConfig::ControlConfigurationNotification notif) override; - std::function ControlConfig_UnsubscribeHandler = nullptr; + std::function ControlConfig_UnsubscribeHandler = nullptr; virtual bool ControlConfig_Unsubscribe(kortex_driver::ControlConfig_Unsubscribe::Request &req, kortex_driver::ControlConfig_Unsubscribe::Response &res) override; - std::function SetCartesianReferenceFrameHandler = nullptr; + std::function SetCartesianReferenceFrameHandler = nullptr; virtual bool SetCartesianReferenceFrame(kortex_driver::SetCartesianReferenceFrame::Request &req, kortex_driver::SetCartesianReferenceFrame::Response &res) override; - std::function GetCartesianReferenceFrameHandler = nullptr; + std::function GetCartesianReferenceFrameHandler = nullptr; virtual bool GetCartesianReferenceFrame(kortex_driver::GetCartesianReferenceFrame::Request &req, kortex_driver::GetCartesianReferenceFrame::Response &res) override; - std::function ControlConfig_GetControlModeHandler = nullptr; + std::function ControlConfig_GetControlModeHandler = nullptr; virtual bool ControlConfig_GetControlMode(kortex_driver::ControlConfig_GetControlMode::Request &req, kortex_driver::ControlConfig_GetControlMode::Response &res) override; - std::function SetJointSpeedSoftLimitsHandler = nullptr; + std::function SetJointSpeedSoftLimitsHandler = nullptr; virtual bool SetJointSpeedSoftLimits(kortex_driver::SetJointSpeedSoftLimits::Request &req, kortex_driver::SetJointSpeedSoftLimits::Response &res) override; - std::function SetTwistLinearSoftLimitHandler = nullptr; + std::function SetTwistLinearSoftLimitHandler = nullptr; virtual bool SetTwistLinearSoftLimit(kortex_driver::SetTwistLinearSoftLimit::Request &req, kortex_driver::SetTwistLinearSoftLimit::Response &res) override; - std::function SetTwistAngularSoftLimitHandler = nullptr; + std::function SetTwistAngularSoftLimitHandler = nullptr; virtual bool SetTwistAngularSoftLimit(kortex_driver::SetTwistAngularSoftLimit::Request &req, kortex_driver::SetTwistAngularSoftLimit::Response &res) override; - std::function SetJointAccelerationSoftLimitsHandler = nullptr; + std::function SetJointAccelerationSoftLimitsHandler = nullptr; virtual bool SetJointAccelerationSoftLimits(kortex_driver::SetJointAccelerationSoftLimits::Request &req, kortex_driver::SetJointAccelerationSoftLimits::Response &res) override; - std::function GetKinematicHardLimitsHandler = nullptr; + std::function GetKinematicHardLimitsHandler = nullptr; virtual bool GetKinematicHardLimits(kortex_driver::GetKinematicHardLimits::Request &req, kortex_driver::GetKinematicHardLimits::Response &res) override; - std::function GetKinematicSoftLimitsHandler = nullptr; + std::function GetKinematicSoftLimitsHandler = nullptr; virtual bool GetKinematicSoftLimits(kortex_driver::GetKinematicSoftLimits::Request &req, kortex_driver::GetKinematicSoftLimits::Response &res) override; - std::function GetAllKinematicSoftLimitsHandler = nullptr; + std::function GetAllKinematicSoftLimitsHandler = nullptr; virtual bool GetAllKinematicSoftLimits(kortex_driver::GetAllKinematicSoftLimits::Request &req, kortex_driver::GetAllKinematicSoftLimits::Response &res) override; - std::function SetDesiredLinearTwistHandler = nullptr; + std::function SetDesiredLinearTwistHandler = nullptr; virtual bool SetDesiredLinearTwist(kortex_driver::SetDesiredLinearTwist::Request &req, kortex_driver::SetDesiredLinearTwist::Response &res) override; - std::function SetDesiredAngularTwistHandler = nullptr; + std::function SetDesiredAngularTwistHandler = nullptr; virtual bool SetDesiredAngularTwist(kortex_driver::SetDesiredAngularTwist::Request &req, kortex_driver::SetDesiredAngularTwist::Response &res) override; - std::function SetDesiredJointSpeedsHandler = nullptr; + std::function SetDesiredJointSpeedsHandler = nullptr; virtual bool SetDesiredJointSpeeds(kortex_driver::SetDesiredJointSpeeds::Request &req, kortex_driver::SetDesiredJointSpeeds::Response &res) override; - std::function GetDesiredSpeedsHandler = nullptr; + std::function GetDesiredSpeedsHandler = nullptr; virtual bool GetDesiredSpeeds(kortex_driver::GetDesiredSpeeds::Request &req, kortex_driver::GetDesiredSpeeds::Response &res) override; - std::function ResetGravityVectorHandler = nullptr; + std::function ResetGravityVectorHandler = nullptr; virtual bool ResetGravityVector(kortex_driver::ResetGravityVector::Request &req, kortex_driver::ResetGravityVector::Response &res) override; - std::function ResetPayloadInformationHandler = nullptr; + std::function ResetPayloadInformationHandler = nullptr; virtual bool ResetPayloadInformation(kortex_driver::ResetPayloadInformation::Request &req, kortex_driver::ResetPayloadInformation::Response &res) override; - std::function ResetToolConfigurationHandler = nullptr; + std::function ResetToolConfigurationHandler = nullptr; virtual bool ResetToolConfiguration(kortex_driver::ResetToolConfiguration::Request &req, kortex_driver::ResetToolConfiguration::Response &res) override; - std::function ResetJointSpeedSoftLimitsHandler = nullptr; + std::function ResetJointSpeedSoftLimitsHandler = nullptr; virtual bool ResetJointSpeedSoftLimits(kortex_driver::ResetJointSpeedSoftLimits::Request &req, kortex_driver::ResetJointSpeedSoftLimits::Response &res) override; - std::function ResetTwistLinearSoftLimitHandler = nullptr; + std::function ResetTwistLinearSoftLimitHandler = nullptr; virtual bool ResetTwistLinearSoftLimit(kortex_driver::ResetTwistLinearSoftLimit::Request &req, kortex_driver::ResetTwistLinearSoftLimit::Response &res) override; - std::function ResetTwistAngularSoftLimitHandler = nullptr; + std::function ResetTwistAngularSoftLimitHandler = nullptr; virtual bool ResetTwistAngularSoftLimit(kortex_driver::ResetTwistAngularSoftLimit::Request &req, kortex_driver::ResetTwistAngularSoftLimit::Response &res) override; - std::function ResetJointAccelerationSoftLimitsHandler = nullptr; + std::function ResetJointAccelerationSoftLimitsHandler = nullptr; virtual bool ResetJointAccelerationSoftLimits(kortex_driver::ResetJointAccelerationSoftLimits::Request &req, kortex_driver::ResetJointAccelerationSoftLimits::Response &res) override; }; diff --git a/kortex_driver/include/kortex_driver/generated/simulation/deviceconfig_services.h b/kortex_driver/include/kortex_driver/generated/simulation/deviceconfig_services.h index cd3429ec..d047b456 100644 --- a/kortex_driver/include/kortex_driver/generated/simulation/deviceconfig_services.h +++ b/kortex_driver/include/kortex_driver/generated/simulation/deviceconfig_services.h @@ -28,70 +28,70 @@ class DeviceConfigSimulationServices : public IDeviceConfigServices virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; - std::function GetRunModeHandler = nullptr; + std::function GetRunModeHandler = nullptr; virtual bool GetRunMode(kortex_driver::GetRunMode::Request &req, kortex_driver::GetRunMode::Response &res) override; - std::function SetRunModeHandler = nullptr; + std::function SetRunModeHandler = nullptr; virtual bool SetRunMode(kortex_driver::SetRunMode::Request &req, kortex_driver::SetRunMode::Response &res) override; - std::function GetDeviceTypeHandler = nullptr; + std::function GetDeviceTypeHandler = nullptr; virtual bool GetDeviceType(kortex_driver::GetDeviceType::Request &req, kortex_driver::GetDeviceType::Response &res) override; - std::function GetFirmwareVersionHandler = nullptr; + std::function GetFirmwareVersionHandler = nullptr; virtual bool GetFirmwareVersion(kortex_driver::GetFirmwareVersion::Request &req, kortex_driver::GetFirmwareVersion::Response &res) override; - std::function GetBootloaderVersionHandler = nullptr; + std::function GetBootloaderVersionHandler = nullptr; virtual bool GetBootloaderVersion(kortex_driver::GetBootloaderVersion::Request &req, kortex_driver::GetBootloaderVersion::Response &res) override; - std::function GetModelNumberHandler = nullptr; + std::function GetModelNumberHandler = nullptr; virtual bool GetModelNumber(kortex_driver::GetModelNumber::Request &req, kortex_driver::GetModelNumber::Response &res) override; - std::function GetPartNumberHandler = nullptr; + std::function GetPartNumberHandler = nullptr; virtual bool GetPartNumber(kortex_driver::GetPartNumber::Request &req, kortex_driver::GetPartNumber::Response &res) override; - std::function GetSerialNumberHandler = nullptr; + std::function GetSerialNumberHandler = nullptr; virtual bool GetSerialNumber(kortex_driver::GetSerialNumber::Request &req, kortex_driver::GetSerialNumber::Response &res) override; - std::function GetMACAddressHandler = nullptr; + std::function GetMACAddressHandler = nullptr; virtual bool GetMACAddress(kortex_driver::GetMACAddress::Request &req, kortex_driver::GetMACAddress::Response &res) override; - std::function GetIPv4SettingsHandler = nullptr; + std::function GetIPv4SettingsHandler = nullptr; virtual bool GetIPv4Settings(kortex_driver::GetIPv4Settings::Request &req, kortex_driver::GetIPv4Settings::Response &res) override; - std::function SetIPv4SettingsHandler = nullptr; + std::function SetIPv4SettingsHandler = nullptr; virtual bool SetIPv4Settings(kortex_driver::SetIPv4Settings::Request &req, kortex_driver::SetIPv4Settings::Response &res) override; - std::function GetPartNumberRevisionHandler = nullptr; + std::function GetPartNumberRevisionHandler = nullptr; virtual bool GetPartNumberRevision(kortex_driver::GetPartNumberRevision::Request &req, kortex_driver::GetPartNumberRevision::Response &res) override; - std::function RebootRequestHandler = nullptr; + std::function RebootRequestHandler = nullptr; virtual bool RebootRequest(kortex_driver::RebootRequest::Request &req, kortex_driver::RebootRequest::Response &res) override; - std::function SetSafetyEnableHandler = nullptr; + std::function SetSafetyEnableHandler = nullptr; virtual bool SetSafetyEnable(kortex_driver::SetSafetyEnable::Request &req, kortex_driver::SetSafetyEnable::Response &res) override; - std::function SetSafetyErrorThresholdHandler = nullptr; + std::function SetSafetyErrorThresholdHandler = nullptr; virtual bool SetSafetyErrorThreshold(kortex_driver::SetSafetyErrorThreshold::Request &req, kortex_driver::SetSafetyErrorThreshold::Response &res) override; - std::function SetSafetyWarningThresholdHandler = nullptr; + std::function SetSafetyWarningThresholdHandler = nullptr; virtual bool SetSafetyWarningThreshold(kortex_driver::SetSafetyWarningThreshold::Request &req, kortex_driver::SetSafetyWarningThreshold::Response &res) override; - std::function SetSafetyConfigurationHandler = nullptr; + std::function SetSafetyConfigurationHandler = nullptr; virtual bool SetSafetyConfiguration(kortex_driver::SetSafetyConfiguration::Request &req, kortex_driver::SetSafetyConfiguration::Response &res) override; - std::function GetSafetyConfigurationHandler = nullptr; + std::function GetSafetyConfigurationHandler = nullptr; virtual bool GetSafetyConfiguration(kortex_driver::GetSafetyConfiguration::Request &req, kortex_driver::GetSafetyConfiguration::Response &res) override; - std::function GetSafetyInformationHandler = nullptr; + std::function GetSafetyInformationHandler = nullptr; virtual bool GetSafetyInformation(kortex_driver::GetSafetyInformation::Request &req, kortex_driver::GetSafetyInformation::Response &res) override; - std::function GetSafetyEnableHandler = nullptr; + std::function GetSafetyEnableHandler = nullptr; virtual bool GetSafetyEnable(kortex_driver::GetSafetyEnable::Request &req, kortex_driver::GetSafetyEnable::Response &res) override; - std::function GetSafetyStatusHandler = nullptr; + std::function GetSafetyStatusHandler = nullptr; virtual bool GetSafetyStatus(kortex_driver::GetSafetyStatus::Request &req, kortex_driver::GetSafetyStatus::Response &res) override; - std::function ClearAllSafetyStatusHandler = nullptr; + std::function ClearAllSafetyStatusHandler = nullptr; virtual bool ClearAllSafetyStatus(kortex_driver::ClearAllSafetyStatus::Request &req, kortex_driver::ClearAllSafetyStatus::Response &res) override; - std::function ClearSafetyStatusHandler = nullptr; + std::function ClearSafetyStatusHandler = nullptr; virtual bool ClearSafetyStatus(kortex_driver::ClearSafetyStatus::Request &req, kortex_driver::ClearSafetyStatus::Response &res) override; - std::function GetAllSafetyConfigurationHandler = nullptr; + std::function GetAllSafetyConfigurationHandler = nullptr; virtual bool GetAllSafetyConfiguration(kortex_driver::GetAllSafetyConfiguration::Request &req, kortex_driver::GetAllSafetyConfiguration::Response &res) override; - std::function GetAllSafetyInformationHandler = nullptr; + std::function GetAllSafetyInformationHandler = nullptr; virtual bool GetAllSafetyInformation(kortex_driver::GetAllSafetyInformation::Request &req, kortex_driver::GetAllSafetyInformation::Response &res) override; - std::function ResetSafetyDefaultsHandler = nullptr; + std::function ResetSafetyDefaultsHandler = nullptr; virtual bool ResetSafetyDefaults(kortex_driver::ResetSafetyDefaults::Request &req, kortex_driver::ResetSafetyDefaults::Response &res) override; - std::function OnNotificationSafetyTopicHandler = nullptr; + std::function OnNotificationSafetyTopicHandler = nullptr; virtual bool OnNotificationSafetyTopic(kortex_driver::OnNotificationSafetyTopic::Request &req, kortex_driver::OnNotificationSafetyTopic::Response &res) override; virtual void cb_SafetyTopic(Kinova::Api::Common::SafetyNotification notif) override; - std::function ExecuteCalibrationHandler = nullptr; + std::function ExecuteCalibrationHandler = nullptr; virtual bool ExecuteCalibration(kortex_driver::ExecuteCalibration::Request &req, kortex_driver::ExecuteCalibration::Response &res) override; - std::function GetCalibrationResultHandler = nullptr; + std::function GetCalibrationResultHandler = nullptr; virtual bool GetCalibrationResult(kortex_driver::GetCalibrationResult::Request &req, kortex_driver::GetCalibrationResult::Response &res) override; - std::function StopCalibrationHandler = nullptr; + std::function StopCalibrationHandler = nullptr; virtual bool StopCalibration(kortex_driver::StopCalibration::Request &req, kortex_driver::StopCalibration::Response &res) override; - std::function DeviceConfig_SetCapSenseConfigHandler = nullptr; + std::function DeviceConfig_SetCapSenseConfigHandler = nullptr; virtual bool DeviceConfig_SetCapSenseConfig(kortex_driver::DeviceConfig_SetCapSenseConfig::Request &req, kortex_driver::DeviceConfig_SetCapSenseConfig::Response &res) override; - std::function DeviceConfig_GetCapSenseConfigHandler = nullptr; + std::function DeviceConfig_GetCapSenseConfigHandler = nullptr; virtual bool DeviceConfig_GetCapSenseConfig(kortex_driver::DeviceConfig_GetCapSenseConfig::Request &req, kortex_driver::DeviceConfig_GetCapSenseConfig::Response &res) override; }; diff --git a/kortex_driver/include/kortex_driver/generated/simulation/devicemanager_services.h b/kortex_driver/include/kortex_driver/generated/simulation/devicemanager_services.h index c3305cad..0c45ae75 100644 --- a/kortex_driver/include/kortex_driver/generated/simulation/devicemanager_services.h +++ b/kortex_driver/include/kortex_driver/generated/simulation/devicemanager_services.h @@ -28,7 +28,7 @@ class DeviceManagerSimulationServices : public IDeviceManagerServices virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; - std::function ReadAllDevicesHandler = nullptr; + std::function ReadAllDevicesHandler = nullptr; virtual bool ReadAllDevices(kortex_driver::ReadAllDevices::Request &req, kortex_driver::ReadAllDevices::Response &res) override; }; diff --git a/kortex_driver/include/kortex_driver/generated/simulation/interconnectconfig_services.h b/kortex_driver/include/kortex_driver/generated/simulation/interconnectconfig_services.h index a5444818..1053941f 100644 --- a/kortex_driver/include/kortex_driver/generated/simulation/interconnectconfig_services.h +++ b/kortex_driver/include/kortex_driver/generated/simulation/interconnectconfig_services.h @@ -28,33 +28,33 @@ class InterconnectConfigSimulationServices : public IInterconnectConfigServices virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; - std::function GetUARTConfigurationHandler = nullptr; + std::function GetUARTConfigurationHandler = nullptr; virtual bool GetUARTConfiguration(kortex_driver::GetUARTConfiguration::Request &req, kortex_driver::GetUARTConfiguration::Response &res) override; - std::function SetUARTConfigurationHandler = nullptr; + std::function SetUARTConfigurationHandler = nullptr; virtual bool SetUARTConfiguration(kortex_driver::SetUARTConfiguration::Request &req, kortex_driver::SetUARTConfiguration::Response &res) override; - std::function GetEthernetConfigurationHandler = nullptr; + std::function GetEthernetConfigurationHandler = nullptr; virtual bool GetEthernetConfiguration(kortex_driver::GetEthernetConfiguration::Request &req, kortex_driver::GetEthernetConfiguration::Response &res) override; - std::function SetEthernetConfigurationHandler = nullptr; + std::function SetEthernetConfigurationHandler = nullptr; virtual bool SetEthernetConfiguration(kortex_driver::SetEthernetConfiguration::Request &req, kortex_driver::SetEthernetConfiguration::Response &res) override; - std::function GetGPIOConfigurationHandler = nullptr; + std::function GetGPIOConfigurationHandler = nullptr; virtual bool GetGPIOConfiguration(kortex_driver::GetGPIOConfiguration::Request &req, kortex_driver::GetGPIOConfiguration::Response &res) override; - std::function SetGPIOConfigurationHandler = nullptr; + std::function SetGPIOConfigurationHandler = nullptr; virtual bool SetGPIOConfiguration(kortex_driver::SetGPIOConfiguration::Request &req, kortex_driver::SetGPIOConfiguration::Response &res) override; - std::function GetGPIOStateHandler = nullptr; + std::function GetGPIOStateHandler = nullptr; virtual bool GetGPIOState(kortex_driver::GetGPIOState::Request &req, kortex_driver::GetGPIOState::Response &res) override; - std::function SetGPIOStateHandler = nullptr; + std::function SetGPIOStateHandler = nullptr; virtual bool SetGPIOState(kortex_driver::SetGPIOState::Request &req, kortex_driver::SetGPIOState::Response &res) override; - std::function GetI2CConfigurationHandler = nullptr; + std::function GetI2CConfigurationHandler = nullptr; virtual bool GetI2CConfiguration(kortex_driver::GetI2CConfiguration::Request &req, kortex_driver::GetI2CConfiguration::Response &res) override; - std::function SetI2CConfigurationHandler = nullptr; + std::function SetI2CConfigurationHandler = nullptr; virtual bool SetI2CConfiguration(kortex_driver::SetI2CConfiguration::Request &req, kortex_driver::SetI2CConfiguration::Response &res) override; - std::function I2CReadHandler = nullptr; + std::function I2CReadHandler = nullptr; virtual bool I2CRead(kortex_driver::I2CRead::Request &req, kortex_driver::I2CRead::Response &res) override; - std::function I2CReadRegisterHandler = nullptr; + std::function I2CReadRegisterHandler = nullptr; virtual bool I2CReadRegister(kortex_driver::I2CReadRegister::Request &req, kortex_driver::I2CReadRegister::Response &res) override; - std::function I2CWriteHandler = nullptr; + std::function I2CWriteHandler = nullptr; virtual bool I2CWrite(kortex_driver::I2CWrite::Request &req, kortex_driver::I2CWrite::Response &res) override; - std::function I2CWriteRegisterHandler = nullptr; + std::function I2CWriteRegisterHandler = nullptr; virtual bool I2CWriteRegister(kortex_driver::I2CWriteRegister::Request &req, kortex_driver::I2CWriteRegister::Response &res) override; }; diff --git a/kortex_driver/include/kortex_driver/generated/simulation/visionconfig_services.h b/kortex_driver/include/kortex_driver/generated/simulation/visionconfig_services.h index 57ea508b..c98151ff 100644 --- a/kortex_driver/include/kortex_driver/generated/simulation/visionconfig_services.h +++ b/kortex_driver/include/kortex_driver/generated/simulation/visionconfig_services.h @@ -28,30 +28,30 @@ class VisionConfigSimulationServices : public IVisionConfigServices virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; - std::function SetSensorSettingsHandler = nullptr; + std::function SetSensorSettingsHandler = nullptr; virtual bool SetSensorSettings(kortex_driver::SetSensorSettings::Request &req, kortex_driver::SetSensorSettings::Response &res) override; - std::function GetSensorSettingsHandler = nullptr; + std::function GetSensorSettingsHandler = nullptr; virtual bool GetSensorSettings(kortex_driver::GetSensorSettings::Request &req, kortex_driver::GetSensorSettings::Response &res) override; - std::function GetOptionValueHandler = nullptr; + std::function GetOptionValueHandler = nullptr; virtual bool GetOptionValue(kortex_driver::GetOptionValue::Request &req, kortex_driver::GetOptionValue::Response &res) override; - std::function SetOptionValueHandler = nullptr; + std::function SetOptionValueHandler = nullptr; virtual bool SetOptionValue(kortex_driver::SetOptionValue::Request &req, kortex_driver::SetOptionValue::Response &res) override; - std::function GetOptionInformationHandler = nullptr; + std::function GetOptionInformationHandler = nullptr; virtual bool GetOptionInformation(kortex_driver::GetOptionInformation::Request &req, kortex_driver::GetOptionInformation::Response &res) override; - std::function OnNotificationVisionTopicHandler = nullptr; + std::function OnNotificationVisionTopicHandler = nullptr; virtual bool OnNotificationVisionTopic(kortex_driver::OnNotificationVisionTopic::Request &req, kortex_driver::OnNotificationVisionTopic::Response &res) override; virtual void cb_VisionTopic(Kinova::Api::VisionConfig::VisionNotification notif) override; - std::function DoSensorFocusActionHandler = nullptr; + std::function DoSensorFocusActionHandler = nullptr; virtual bool DoSensorFocusAction(kortex_driver::DoSensorFocusAction::Request &req, kortex_driver::DoSensorFocusAction::Response &res) override; - std::function GetIntrinsicParametersHandler = nullptr; + std::function GetIntrinsicParametersHandler = nullptr; virtual bool GetIntrinsicParameters(kortex_driver::GetIntrinsicParameters::Request &req, kortex_driver::GetIntrinsicParameters::Response &res) override; - std::function GetIntrinsicParametersProfileHandler = nullptr; + std::function GetIntrinsicParametersProfileHandler = nullptr; virtual bool GetIntrinsicParametersProfile(kortex_driver::GetIntrinsicParametersProfile::Request &req, kortex_driver::GetIntrinsicParametersProfile::Response &res) override; - std::function SetIntrinsicParametersHandler = nullptr; + std::function SetIntrinsicParametersHandler = nullptr; virtual bool SetIntrinsicParameters(kortex_driver::SetIntrinsicParameters::Request &req, kortex_driver::SetIntrinsicParameters::Response &res) override; - std::function GetExtrinsicParametersHandler = nullptr; + std::function GetExtrinsicParametersHandler = nullptr; virtual bool GetExtrinsicParameters(kortex_driver::GetExtrinsicParameters::Request &req, kortex_driver::GetExtrinsicParameters::Response &res) override; - std::function SetExtrinsicParametersHandler = nullptr; + std::function SetExtrinsicParametersHandler = nullptr; virtual bool SetExtrinsicParameters(kortex_driver::SetExtrinsicParameters::Request &req, kortex_driver::SetExtrinsicParameters::Response &res) override; }; diff --git a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h index a32c7fdd..63126621 100644 --- a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h +++ b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h @@ -163,6 +163,7 @@ class KortexArmDriver bool isGripperPresent(); void setAngularTrajectorySoftLimitsToMax(); void publishRobotFeedback(); + void registerSimulationHandlers(); }; #endif diff --git a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h index 3e559dfe..3ca2a2cb 100644 --- a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h +++ b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h @@ -17,6 +17,17 @@ #include "kortex_driver/non-generated/kortex_math_util.h" +#include "kortex_driver/CreateAction.h" +#include "kortex_driver/ReadAction.h" +#include "kortex_driver/ReadAllActions.h" +#include "kortex_driver/DeleteAction.h" +#include "kortex_driver/UpdateAction.h" +#include "kortex_driver/ExecuteActionFromReference.h" +#include "kortex_driver/ExecuteAction.h" +#include "kortex_driver/PauseAction.h" +#include "kortex_driver/StopAction.h" +#include "kortex_driver/ResumeAction.h" + class KortexArmSimulation { public: @@ -25,7 +36,18 @@ class KortexArmSimulation ~KortexArmSimulation(); // Handlers for simulated Kortex API functions - // TODO Add handlers + // Actions API + kortex_driver::CreateAction::Response CreateAction(const kortex_driver::CreateAction::Request& req); + kortex_driver::ReadAction::Response ReadAction(const kortex_driver::ReadAction::Request& req); + kortex_driver::ReadAllActions::Response ReadAllActions(const kortex_driver::ReadAllActions::Request& req); + kortex_driver::DeleteAction::Response DeleteAction(const kortex_driver::DeleteAction::Request& req); + kortex_driver::UpdateAction::Response UpdateAction(const kortex_driver::UpdateAction::Request& req); + kortex_driver::ExecuteActionFromReference::Response ExecuteActionFromReference(const kortex_driver::ExecuteActionFromReference::Request& req); + kortex_driver::ExecuteAction::Response ExecuteAction(const kortex_driver::ExecuteAction::Request& req); + kortex_driver::PauseAction::Response PauseAction(const kortex_driver::PauseAction::Request& req); + kortex_driver::StopAction::Response StopAction(const kortex_driver::StopAction::Request& req); + // Sequences API + // Velocity control RPCs private: diff --git a/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp b/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp index caa12317..2f312fd2 100644 --- a/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp +++ b/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp @@ -31,16 +31,18 @@ KortexArmDriver::KortexArmDriver(ros::NodeHandle nh): m_node_handle(nh), verifyProductConfiguration(); initSubscribers(); startActionServers(); - } - else - { - m_simulator.reset(new KortexArmSimulation(nh)); - } - + } // ROS Services are always started initRosServices(); + // Enable ROS Service simulation if not with a real robot + if (!m_is_real_robot) + { + m_simulator.reset(new KortexArmSimulation(nh)); + registerSimulationHandlers(); + } + // Start the thread to publish the feedback and joint states m_pub_base_feedback = m_node_handle.advertise("base_feedback", 1000); m_pub_joint_state = m_node_handle.advertise("base_feedback/joint_state", 1000); @@ -636,3 +638,18 @@ void KortexArmDriver::publishRobotFeedback() rate.sleep(); } } + +void KortexArmDriver::registerSimulationHandlers() +{ + BaseSimulationServices* base_services_simulation = dynamic_cast(m_base_ros_services); + // Link the m_simulator handlers to the ROS services callbacks + base_services_simulation->CreateActionHandler = std::bind(&KortexArmSimulation::CreateAction, m_simulator.get(), std::placeholders::_1); + base_services_simulation->ReadActionHandler = std::bind(&KortexArmSimulation::ReadAction, m_simulator.get(), std::placeholders::_1); + base_services_simulation->ReadAllActionsHandler = std::bind(&KortexArmSimulation::ReadAllActions, m_simulator.get(), std::placeholders::_1); + base_services_simulation->DeleteActionHandler = std::bind(&KortexArmSimulation::DeleteAction, m_simulator.get(), std::placeholders::_1); + base_services_simulation->UpdateActionHandler = std::bind(&KortexArmSimulation::UpdateAction, m_simulator.get(), std::placeholders::_1); + base_services_simulation->ExecuteActionFromReferenceHandler = std::bind(&KortexArmSimulation::ExecuteActionFromReference, m_simulator.get(), std::placeholders::_1); + base_services_simulation->ExecuteActionHandler = std::bind(&KortexArmSimulation::ExecuteAction, m_simulator.get(), std::placeholders::_1); + base_services_simulation->PauseActionHandler = std::bind(&KortexArmSimulation::PauseAction, m_simulator.get(), std::placeholders::_1); + base_services_simulation->StopActionHandler = std::bind(&KortexArmSimulation::StopAction, m_simulator.get(), std::placeholders::_1); +} \ No newline at end of file diff --git a/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp b/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp index 749df03d..d495104c 100644 --- a/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp +++ b/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp @@ -49,3 +49,57 @@ KortexArmSimulation::KortexArmSimulation(ros::NodeHandle& node_handle): m_node_h KortexArmSimulation::~KortexArmSimulation() { } + +kortex_driver::CreateAction::Response KortexArmSimulation::CreateAction(const kortex_driver::CreateAction::Request& req) +{ + kortex_driver::CreateAction::Response response; + return response; +} + +kortex_driver::ReadAction::Response KortexArmSimulation::ReadAction(const kortex_driver::ReadAction::Request& req) +{ + kortex_driver::ReadAction::Response response; + return response; +} + +kortex_driver::ReadAllActions::Response KortexArmSimulation::ReadAllActions(const kortex_driver::ReadAllActions::Request& req) +{ + kortex_driver::ReadAllActions::Response response; + return response; +} + +kortex_driver::DeleteAction::Response KortexArmSimulation::DeleteAction(const kortex_driver::DeleteAction::Request& req) +{ + kortex_driver::DeleteAction::Response response; + return response; +} + +kortex_driver::UpdateAction::Response KortexArmSimulation::UpdateAction(const kortex_driver::UpdateAction::Request& req) +{ + kortex_driver::UpdateAction::Response response; + return response; +} + +kortex_driver::ExecuteActionFromReference::Response KortexArmSimulation::ExecuteActionFromReference(const kortex_driver::ExecuteActionFromReference::Request& req) +{ + kortex_driver::ExecuteActionFromReference::Response response; + return response; +} + +kortex_driver::ExecuteAction::Response KortexArmSimulation::ExecuteAction(const kortex_driver::ExecuteAction::Request& req) +{ + kortex_driver::ExecuteAction::Response response; + return response; +} + +kortex_driver::PauseAction::Response KortexArmSimulation::PauseAction(const kortex_driver::PauseAction::Request& req) +{ + kortex_driver::PauseAction::Response response; + return response; +} + +kortex_driver::StopAction::Response KortexArmSimulation::StopAction(const kortex_driver::StopAction::Request& req) +{ + kortex_driver::StopAction::Response response; + return response; +} diff --git a/kortex_driver/templates/services_simulation.h.jinja2 b/kortex_driver/templates/services_simulation.h.jinja2 index 9a9e49a5..c2ccd970 100644 --- a/kortex_driver/templates/services_simulation.h.jinja2 +++ b/kortex_driver/templates/services_simulation.h.jinja2 @@ -29,7 +29,7 @@ class {{package.short_name}}SimulationServices : public I{{package.short_name}}S virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; {%- for method in package.methods %} - std::function {{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}Handler = nullptr; + std::function {{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}Handler = nullptr; virtual bool {{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}(kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Request &req, kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Response &res) override; {%- if method.is_notification_rpc %} virtual void cb_{{method.name}}({{method.notification_message_cpp_namespace}}::{{method.prepend_rpc_package_name}}{{method.name|replace("Topic", "")}}Notification notif) override; From 4e66bc9517e33c7fb0452338ad3e5b9ea922432d Mon Sep 17 00:00:00 2001 From: Alexandre Vannobel Date: Mon, 15 Jun 2020 13:22:01 -0400 Subject: [PATCH 17/33] Add implementation for default actions and ReacAction/ReadAllActions --- kortex_driver/CMakeLists.txt | 2 +- .../non-generated/kortex_arm_simulation.h | 14 +++ kortex_driver/package.xml | 1 + .../driver/kortex_arm_simulation.cpp | 96 ++++++++++++++++++- 4 files changed, 111 insertions(+), 2 deletions(-) diff --git a/kortex_driver/CMakeLists.txt b/kortex_driver/CMakeLists.txt index 58da43ad..f6f6b383 100644 --- a/kortex_driver/CMakeLists.txt +++ b/kortex_driver/CMakeLists.txt @@ -17,7 +17,7 @@ if(NOT CMAKE_BUILD_TYPE) endif() ## find catkin and any catkin packages -find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs message_generation actionlib control_msgs urdf) +find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs message_generation actionlib control_msgs urdf moveit_ros_planning_interface) find_package(Boost REQUIRED COMPONENTS system) file(GLOB_RECURSE generated_files RELATIVE ${PROJECT_SOURCE_DIR} "src/generated/robot/*.cpp" "src/generated/simulation/*.cpp") diff --git a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h index 3ca2a2cb..5b612beb 100644 --- a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h +++ b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h @@ -15,8 +15,12 @@ #include +#include + #include "kortex_driver/non-generated/kortex_math_util.h" +#include "kortex_driver/ActionType.h" + #include "kortex_driver/CreateAction.h" #include "kortex_driver/ReadAction.h" #include "kortex_driver/ReadAllActions.h" @@ -28,6 +32,8 @@ #include "kortex_driver/StopAction.h" #include "kortex_driver/ResumeAction.h" +#include + class KortexArmSimulation { public: @@ -66,11 +72,19 @@ class KortexArmSimulation std::vector m_gripper_joint_limits_min; int m_degrees_of_freedom; + // Action-related + std::unordered_map m_map_actions; + // ROS and thread objects to publish the feedback from the robot KortexMathUtil m_math_util; + // MoveIt-related + std::unique_ptr m_moveit_arm_interface; + std::unique_ptr m_moveit_gripper_interface; + // Helper functions bool IsGripperPresent() const {return !m_gripper_name.empty();} + void CreateDefaultActions(); }; #endif //_KORTEX_ARM_SIMULATION_H_ diff --git a/kortex_driver/package.xml b/kortex_driver/package.xml index 3654d29d..02f59c73 100644 --- a/kortex_driver/package.xml +++ b/kortex_driver/package.xml @@ -14,6 +14,7 @@ std_msgs control_msgs actionlib + moveit_ros_planning_interface roscpp rospy std_msgs diff --git a/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp b/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp index d495104c..711fdb51 100644 --- a/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp +++ b/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp @@ -12,7 +12,14 @@ #include "kortex_driver/non-generated/kortex_arm_simulation.h" -KortexArmSimulation::KortexArmSimulation(ros::NodeHandle& node_handle): m_node_handle(node_handle) +namespace +{ + static const std::string ARM_PLANNING_GROUP = "arm"; + static const std::string GRIPPER_PLANNING_GROUP = "gripper"; +} + +KortexArmSimulation::KortexArmSimulation(ros::NodeHandle& node_handle): m_node_handle(node_handle), + m_map_actions{} { // Namespacing and prefixing information ros::param::get("~robot_name", m_robot_name); @@ -44,6 +51,13 @@ KortexArmSimulation::KortexArmSimulation(ros::NodeHandle& node_handle): m_node_h ROS_INFO("Gripper type : %s", m_gripper_name.empty() ? "None" : m_gripper_name.c_str()); ROS_INFO("Arm namespace : %s", m_robot_name.c_str()); ROS_INFO("URDF prefix : %s", m_prefix.c_str()); + + // Start MoveIt client + m_moveit_arm_interface.reset(new moveit::planning_interface::MoveGroupInterface(ARM_PLANNING_GROUP)); + m_moveit_gripper_interface.reset(new moveit::planning_interface::MoveGroupInterface(GRIPPER_PLANNING_GROUP)); + + // Create default actions + CreateDefaultActions(); } KortexArmSimulation::~KortexArmSimulation() @@ -52,54 +66,134 @@ KortexArmSimulation::~KortexArmSimulation() kortex_driver::CreateAction::Response KortexArmSimulation::CreateAction(const kortex_driver::CreateAction::Request& req) { + auto input = req.input; kortex_driver::CreateAction::Response response; return response; } kortex_driver::ReadAction::Response KortexArmSimulation::ReadAction(const kortex_driver::ReadAction::Request& req) { + auto input = req.input; kortex_driver::ReadAction::Response response; + auto it = m_map_actions.find(input.identifier); + if (it != m_map_actions.end()) + { + response.output = it->second; + } return response; } kortex_driver::ReadAllActions::Response KortexArmSimulation::ReadAllActions(const kortex_driver::ReadAllActions::Request& req) { + auto input = req.input; kortex_driver::ReadAllActions::Response response; + kortex_driver::ActionList action_list; + for (auto a : m_map_actions) + { + // If requested action type is specified and matches iterated action's type, add it to the list + if (input.action_type == 0 || input.action_type == a.second.handle.action_type) + { + action_list.action_list.push_back(a.second); + } + + } + response.output = action_list; return response; } kortex_driver::DeleteAction::Response KortexArmSimulation::DeleteAction(const kortex_driver::DeleteAction::Request& req) { + auto input = req.input; kortex_driver::DeleteAction::Response response; return response; } kortex_driver::UpdateAction::Response KortexArmSimulation::UpdateAction(const kortex_driver::UpdateAction::Request& req) { + auto input = req.input; kortex_driver::UpdateAction::Response response; return response; } kortex_driver::ExecuteActionFromReference::Response KortexArmSimulation::ExecuteActionFromReference(const kortex_driver::ExecuteActionFromReference::Request& req) { + auto input = req.input; kortex_driver::ExecuteActionFromReference::Response response; return response; } kortex_driver::ExecuteAction::Response KortexArmSimulation::ExecuteAction(const kortex_driver::ExecuteAction::Request& req) { + auto input = req.input; kortex_driver::ExecuteAction::Response response; return response; } kortex_driver::PauseAction::Response KortexArmSimulation::PauseAction(const kortex_driver::PauseAction::Request& req) { + auto input = req.input; kortex_driver::PauseAction::Response response; return response; } kortex_driver::StopAction::Response KortexArmSimulation::StopAction(const kortex_driver::StopAction::Request& req) { + auto input = req.input; kortex_driver::StopAction::Response response; return response; } + +void KortexArmSimulation::CreateDefaultActions() +{ + kortex_driver::Action retract, home, zero; + kortex_driver::ConstrainedJointAngles retract_angles, home_angles, zero_angles; + // Retract + retract.handle.identifier = 1; + retract.handle.action_type = kortex_driver::ActionType::REACH_JOINT_ANGLES; + retract.handle.permission = 7; + retract.name = "Retract"; + for (int i = 0; i < m_degrees_of_freedom; i++) + { + kortex_driver::JointAngle a; + a.joint_identifier = i; + auto named_target = m_moveit_arm_interface->getNamedTargetValues("retract"); + double moveit_angle = named_target["joint_"+std::to_string(i+1)]; // rad + a.value = m_math_util.wrapDegreesFromZeroTo360(m_math_util.toDeg(moveit_angle)); + retract_angles.joint_angles.joint_angles.push_back(a); + } + retract.oneof_action_parameters.reach_joint_angles.push_back(retract_angles); + // Home + home.handle.identifier = 2; + home.handle.action_type = kortex_driver::ActionType::REACH_JOINT_ANGLES; + home.handle.permission = 7; + home.name = "Home"; + for (int i = 0; i < m_degrees_of_freedom; i++) + { + kortex_driver::JointAngle a; + a.joint_identifier = i; + auto named_target = m_moveit_arm_interface->getNamedTargetValues("home"); + double moveit_angle = named_target["joint_"+std::to_string(i+1)]; // rad + a.value = m_math_util.wrapDegreesFromZeroTo360(m_math_util.toDeg(moveit_angle)); + home_angles.joint_angles.joint_angles.push_back(a); + } + home.oneof_action_parameters.reach_joint_angles.push_back(home_angles); + // Zero + zero.handle.identifier = 3; + zero.handle.action_type = kortex_driver::ActionType::REACH_JOINT_ANGLES; + zero.handle.permission = 7; + zero.name = "Zero"; + for (int i = 0; i < m_degrees_of_freedom; i++) + { + kortex_driver::JointAngle a; + a.joint_identifier = i; + auto named_target = m_moveit_arm_interface->getNamedTargetValues("vertical"); + double moveit_angle = named_target["joint_"+std::to_string(i+1)]; // rad + a.value = m_math_util.wrapDegreesFromZeroTo360(m_math_util.toDeg(moveit_angle)); + zero_angles.joint_angles.joint_angles.push_back(a); + } + zero.oneof_action_parameters.reach_joint_angles.push_back(zero_angles); + // Add actions + m_map_actions.emplace(std::make_pair(retract.handle.identifier, retract)); + m_map_actions.emplace(std::make_pair(home.handle.identifier, home)); + m_map_actions.emplace(std::make_pair(zero.handle.identifier, zero)); +} \ No newline at end of file From 088919ef0d97a4880374d052f6020c9e5a344cb9 Mon Sep 17 00:00:00 2001 From: Alexandre Vannobel Date: Mon, 15 Jun 2020 15:24:50 -0400 Subject: [PATCH 18/33] Added some unit tests for simulator and CreateAction implentation --- .../non-generated/kortex_arm_simulation.h | 1 + .../driver/kortex_arm_simulation.cpp | 36 +++++- .../tests/kortex_arm_driver_func_tests.cc | 12 -- .../tests/kortex_simulator_unit_tests.cc | 118 ++++++++++++++++++ kortex_driver/src/non-generated/tests/main.cc | 15 +++ 5 files changed, 169 insertions(+), 13 deletions(-) create mode 100644 kortex_driver/src/non-generated/tests/kortex_simulator_unit_tests.cc create mode 100644 kortex_driver/src/non-generated/tests/main.cc diff --git a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h index 5b612beb..de50faa2 100644 --- a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h +++ b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h @@ -40,6 +40,7 @@ class KortexArmSimulation KortexArmSimulation() = delete; KortexArmSimulation(ros::NodeHandle& nh); ~KortexArmSimulation(); + std::unordered_map GetActionsMap() const; // Handlers for simulated Kortex API functions // Actions API diff --git a/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp b/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp index 711fdb51..93375ff7 100644 --- a/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp +++ b/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp @@ -16,6 +16,7 @@ namespace { static const std::string ARM_PLANNING_GROUP = "arm"; static const std::string GRIPPER_PLANNING_GROUP = "gripper"; + static constexpr unsigned int FIRST_CREATED_ACTION_ID = 10000; } KortexArmSimulation::KortexArmSimulation(ros::NodeHandle& node_handle): m_node_handle(node_handle), @@ -64,10 +65,43 @@ KortexArmSimulation::~KortexArmSimulation() { } +std::unordered_map KortexArmSimulation::GetActionsMap() const +{ + return m_map_actions; +} + kortex_driver::CreateAction::Response KortexArmSimulation::CreateAction(const kortex_driver::CreateAction::Request& req) { - auto input = req.input; + auto new_action = req.input; + unsigned int identifier = FIRST_CREATED_ACTION_ID; + bool identifier_taken = true; + // Find unique identifier for new action + while (identifier_taken) + { + identifier_taken = m_map_actions.count(identifier) == 1; + if (identifier_taken) + { + ++identifier; + } + } + // Add Action to map if type is supported + switch (new_action.handle.action_type) + { + case kortex_driver::ActionType::REACH_JOINT_ANGLES: + case kortex_driver::ActionType::REACH_POSE: + case kortex_driver::ActionType::SEND_GRIPPER_COMMAND: + case kortex_driver::ActionType::TIME_DELAY: + new_action.handle.identifier = identifier; + new_action.handle.permission = 7; + m_map_actions.emplace(std::make_pair(identifier, new_action)); + break; + default: + ROS_ERROR("Unsupported action type %d : could not create simulated action.", new_action.handle.action_type); + break; + } + // Return ActionHandle for added action kortex_driver::CreateAction::Response response; + response.output = new_action.handle; return response; } diff --git a/kortex_driver/src/non-generated/tests/kortex_arm_driver_func_tests.cc b/kortex_driver/src/non-generated/tests/kortex_arm_driver_func_tests.cc index 42ac5233..60ec3c50 100644 --- a/kortex_driver/src/non-generated/tests/kortex_arm_driver_func_tests.cc +++ b/kortex_driver/src/non-generated/tests/kortex_arm_driver_func_tests.cc @@ -54,15 +54,3 @@ TEST_F(KortexDriverTest, parseURDF) urdf::Model model; ASSERT_TRUE(model.initParam("robot_description")); } - -int main(int argc, char** argv){ - ros::init(argc, argv, "KortexArmDriverInitTestsNode"); - testing::InitGoogleTest(&argc, argv); - - std::thread t([]{while(ros::ok()) ros::spin();}); - - auto res = RUN_ALL_TESTS(); - - ros::shutdown(); - return res; -} \ No newline at end of file diff --git a/kortex_driver/src/non-generated/tests/kortex_simulator_unit_tests.cc b/kortex_driver/src/non-generated/tests/kortex_simulator_unit_tests.cc new file mode 100644 index 00000000..1b907e92 --- /dev/null +++ b/kortex_driver/src/non-generated/tests/kortex_simulator_unit_tests.cc @@ -0,0 +1,118 @@ +#include +#include +#include +#include +#include + +class KortexSimulatorTest : public ::testing::Test { + protected: + + void SetUp() override + { + // Create Simulator + m_simulator.reset(new KortexArmSimulation(n)); + } + + void TearDown() override + { + } + + ros::NodeHandle n; + std::unique_ptr m_simulator; + +}; + +// Make sure after initialisation the default actions are +// Tests ReadAllActions at the same time +TEST_F(KortexSimulatorTest, DefaultActions) +{ + auto actions = m_simulator->GetActionsMap(); + bool retract, home, zero, other = false; + for (auto a : actions) + { + if (a.second.name == "Retract") retract = true; + else if (a.second.name == "Home") home = true; + else if (a.second.name == "Zero") zero = true; + else other = true; + } + ASSERT_TRUE(retract); + ASSERT_TRUE(home); + ASSERT_TRUE(zero); + ASSERT_FALSE(other); +} + +// Tests ReadAllActions +TEST_F(KortexSimulatorTest, ReadAllActions) +{ + // Test for 3 known actions (default) of this type in the map + kortex_driver::ReadAllActions::Request req; + kortex_driver::RequestedActionType type; + type.action_type = kortex_driver::ActionType::REACH_JOINT_ANGLES; + req.input = type; + auto res = m_simulator->ReadAllActions(req); + auto action_list = res.output; + ASSERT_EQ(action_list.action_list.size(), 3); // Number of default actions + + // Test for 0 known actions of this type in the map + type.action_type = kortex_driver::ActionType::REACH_POSE; + req.input = type; + res = m_simulator->ReadAllActions(req); + action_list = res.output; + ASSERT_TRUE(action_list.action_list.empty()); +} + +// Tests CreateAction handler for a supported Action +TEST_F(KortexSimulatorTest, CreateSupportedAction) +{ + static const std::string name = "MyNewAction"; + + kortex_driver::CreateAction::Request req; + kortex_driver::Action action; + action.handle.action_type = kortex_driver::ActionType::REACH_JOINT_ANGLES; + action.name = name; + kortex_driver::ConstrainedJointAngles angles; + for (int i = 0; i < 7; i++) + { + kortex_driver::JointAngle angle; + angle.joint_identifier = i; + angle.value = 10.0f*i; + angles.joint_angles.joint_angles.push_back(angle); + } + action.oneof_action_parameters.reach_joint_angles.push_back(angles); + req.input = action; + auto res = m_simulator->CreateAction(req); + auto handle = res.output; + auto actions_map = m_simulator->GetActionsMap(); + + // Make sure the action was added to the map + ASSERT_EQ(actions_map.count(handle.identifier), 1); + ASSERT_EQ(actions_map[handle.identifier].name, name); + ASSERT_EQ(actions_map[handle.identifier].handle.action_type, kortex_driver::ActionType::REACH_JOINT_ANGLES); +} + +// Tests CreateAction handler for an unsupported Action +TEST_F(KortexSimulatorTest, CreateUnsupportedAction) +{ + static const std::string name = "MyNewAction"; + + kortex_driver::CreateAction::Request req; + kortex_driver::Action action; + action.handle.action_type = kortex_driver::ActionType::SEND_JOINT_SPEEDS; + action.name = name; + kortex_driver::Base_JointSpeeds speeds; + for (int i = 0; i < 7; i++) + { + kortex_driver::JointSpeed speed; + speed.joint_identifier = i; + speed.value = 10.0f*i; + speeds.joint_speeds.push_back(speed); + } + action.oneof_action_parameters.send_joint_speeds.push_back(speeds); + req.input = action; + auto res = m_simulator->CreateAction(req); + auto handle = res.output; + auto actions_map = m_simulator->GetActionsMap(); + + // Make sure the action was added to the map + ASSERT_EQ(actions_map.count(handle.identifier), 0); +} diff --git a/kortex_driver/src/non-generated/tests/main.cc b/kortex_driver/src/non-generated/tests/main.cc new file mode 100644 index 00000000..d330136e --- /dev/null +++ b/kortex_driver/src/non-generated/tests/main.cc @@ -0,0 +1,15 @@ +#include +#include +#include + +int main(int argc, char** argv){ + ros::init(argc, argv, "TestNode"); + testing::InitGoogleTest(&argc, argv); + + std::thread t([]{while(ros::ok()) ros::spin();}); + + auto res = RUN_ALL_TESTS(); + + ros::shutdown(); + return res; +} \ No newline at end of file From f9abad158e54472069489d43f91d895d9146e07d Mon Sep 17 00:00:00 2001 From: Alexandre Vannobel Date: Mon, 15 Jun 2020 16:08:14 -0400 Subject: [PATCH 19/33] Added the case where the prefix could be empty in XACRO --- .../launch/planning_context.launch | 7 ++++++- .../gen3_move_it_config/launch/planning_context.launch | 7 ++++++- .../launch/planning_context.launch | 7 ++++++- .../launch/planning_context.launch | 7 ++++++- 4 files changed, 24 insertions(+), 4 deletions(-) diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_context.launch b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_context.launch index 990954d1..b0962a7f 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_context.launch +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_context.launch @@ -12,7 +12,12 @@ - + + + + diff --git a/kortex_move_it_config/gen3_move_it_config/launch/planning_context.launch b/kortex_move_it_config/gen3_move_it_config/launch/planning_context.launch index 4548bcdc..fceb7d6a 100644 --- a/kortex_move_it_config/gen3_move_it_config/launch/planning_context.launch +++ b/kortex_move_it_config/gen3_move_it_config/launch/planning_context.launch @@ -18,7 +18,12 @@ - + + + + diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_context.launch b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_context.launch index 73d7424e..81bffeff 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_context.launch +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_context.launch @@ -18,7 +18,12 @@ - + + + + diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_context.launch b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_context.launch index 72420090..dd217119 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_context.launch +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_context.launch @@ -18,7 +18,12 @@ - + + + + From efcf923846f5bbc6f114c273c98b023173662c05 Mon Sep 17 00:00:00 2001 From: Alexandre Vannobel Date: Mon, 15 Jun 2020 16:11:51 -0400 Subject: [PATCH 20/33] Added prefix support for fake controllers in MoveIt configs --- .../launch/fake_moveit_controller_manager.launch.xml | 4 +++- .../launch/fake_moveit_controller_manager.launch.xml | 3 ++- .../launch/fake_moveit_controller_manager.launch.xml | 3 ++- .../launch/fake_moveit_controller_manager.launch.xml | 3 ++- 4 files changed, 9 insertions(+), 4 deletions(-) diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/fake_moveit_controller_manager.launch.xml b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/fake_moveit_controller_manager.launch.xml index ff37a3ce..3b0aed09 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/fake_moveit_controller_manager.launch.xml +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/fake_moveit_controller_manager.launch.xml @@ -1,9 +1,11 @@ + + - + diff --git a/kortex_move_it_config/gen3_move_it_config/launch/fake_moveit_controller_manager.launch.xml b/kortex_move_it_config/gen3_move_it_config/launch/fake_moveit_controller_manager.launch.xml index d2f4eaf8..482742e6 100644 --- a/kortex_move_it_config/gen3_move_it_config/launch/fake_moveit_controller_manager.launch.xml +++ b/kortex_move_it_config/gen3_move_it_config/launch/fake_moveit_controller_manager.launch.xml @@ -1,11 +1,12 @@ + - + diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/fake_moveit_controller_manager.launch.xml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/fake_moveit_controller_manager.launch.xml index f3a2be33..6ff1ed9f 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/fake_moveit_controller_manager.launch.xml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/fake_moveit_controller_manager.launch.xml @@ -2,11 +2,12 @@ + - + diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/fake_moveit_controller_manager.launch.xml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/fake_moveit_controller_manager.launch.xml index 65ad9d3f..d9a09845 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/fake_moveit_controller_manager.launch.xml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/fake_moveit_controller_manager.launch.xml @@ -2,11 +2,12 @@ + - + From 6f5b45b78d5e465407192454d89fe354c81b15a9 Mon Sep 17 00:00:00 2001 From: Alexandre Vannobel Date: Mon, 15 Jun 2020 16:32:35 -0400 Subject: [PATCH 21/33] Added a missing prefix value for Gen3 and fix the if and unless tags --- .../launch/planning_context.launch | 8 ++++---- .../gen3_move_it_config/config/7dof/gen3.srdf.xacro | 1 + .../gen3_move_it_config/launch/planning_context.launch | 6 +++--- .../launch/planning_context.launch | 6 +++--- .../launch/planning_context.launch | 8 ++++---- 5 files changed, 15 insertions(+), 14 deletions(-) diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_context.launch b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_context.launch index b0962a7f..222a7a55 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_context.launch +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_context.launch @@ -12,12 +12,12 @@ - - - + + diff --git a/kortex_move_it_config/gen3_move_it_config/config/7dof/gen3.srdf.xacro b/kortex_move_it_config/gen3_move_it_config/config/7dof/gen3.srdf.xacro index 5930cc1e..beb8c857 100644 --- a/kortex_move_it_config/gen3_move_it_config/config/7dof/gen3.srdf.xacro +++ b/kortex_move_it_config/gen3_move_it_config/config/7dof/gen3.srdf.xacro @@ -4,6 +4,7 @@ A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined --> + diff --git a/kortex_move_it_config/gen3_move_it_config/launch/planning_context.launch b/kortex_move_it_config/gen3_move_it_config/launch/planning_context.launch index fceb7d6a..21884a9f 100644 --- a/kortex_move_it_config/gen3_move_it_config/launch/planning_context.launch +++ b/kortex_move_it_config/gen3_move_it_config/launch/planning_context.launch @@ -18,12 +18,12 @@ - - + + diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_context.launch b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_context.launch index 81bffeff..7b26b18d 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_context.launch +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_context.launch @@ -18,12 +18,12 @@ - - + + diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_context.launch b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_context.launch index dd217119..5483e95a 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_context.launch +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_context.launch @@ -18,12 +18,12 @@ - - - + + From 19811b5b58d1d43e8ba68c926b8767fdf6bad218 Mon Sep 17 00:00:00 2001 From: Alexandre Vannobel Date: Mon, 15 Jun 2020 16:33:21 -0400 Subject: [PATCH 22/33] Add test launch file to launch simulator unit tests --- kortex_driver/launch/test_simulator.launch | 112 +++++++++++++++++++++ 1 file changed, 112 insertions(+) create mode 100644 kortex_driver/launch/test_simulator.launch diff --git a/kortex_driver/launch/test_simulator.launch b/kortex_driver/launch/test_simulator.launch new file mode 100644 index 00000000..fa61683d --- /dev/null +++ b/kortex_driver/launch/test_simulator.launch @@ -0,0 +1,112 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [base_feedback/joint_state] + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 1d24817f65aca8420b46b130bb0386b71ba0460d Mon Sep 17 00:00:00 2001 From: Alexandre Vannobel Date: Mon, 15 Jun 2020 16:34:27 -0400 Subject: [PATCH 23/33] Don't create the gripper Move Group if no gripper on the arm --- .../src/non-generated/driver/kortex_arm_simulation.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp b/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp index 93375ff7..39e71287 100644 --- a/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp +++ b/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp @@ -55,7 +55,10 @@ KortexArmSimulation::KortexArmSimulation(ros::NodeHandle& node_handle): m_node_h // Start MoveIt client m_moveit_arm_interface.reset(new moveit::planning_interface::MoveGroupInterface(ARM_PLANNING_GROUP)); - m_moveit_gripper_interface.reset(new moveit::planning_interface::MoveGroupInterface(GRIPPER_PLANNING_GROUP)); + if (IsGripperPresent()) + { + m_moveit_gripper_interface.reset(new moveit::planning_interface::MoveGroupInterface(GRIPPER_PLANNING_GROUP)); + } // Create default actions CreateDefaultActions(); From e2843c0f8e4fe8de78b0ff37fc333ded696a298b Mon Sep 17 00:00:00 2001 From: Alexandre Vannobel Date: Mon, 15 Jun 2020 17:05:15 -0400 Subject: [PATCH 24/33] Added DeleteAction implementation and tests --- .../driver/kortex_arm_simulation.cpp | 22 +++++++++++++++- .../tests/kortex_simulator_unit_tests.cc | 26 ++++++++++++++++++- 2 files changed, 46 insertions(+), 2 deletions(-) diff --git a/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp b/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp index 39e71287..072f0ccc 100644 --- a/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp +++ b/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp @@ -11,12 +11,14 @@ */ #include "kortex_driver/non-generated/kortex_arm_simulation.h" +#include namespace { static const std::string ARM_PLANNING_GROUP = "arm"; static const std::string GRIPPER_PLANNING_GROUP = "gripper"; static constexpr unsigned int FIRST_CREATED_ACTION_ID = 10000; + static const std::set DEFAULT_ACTIONS_IDENTIFIERS{1,2,3}; } KortexArmSimulation::KortexArmSimulation(ros::NodeHandle& node_handle): m_node_handle(node_handle), @@ -140,7 +142,25 @@ kortex_driver::ReadAllActions::Response KortexArmSimulation::ReadAllActions(cons kortex_driver::DeleteAction::Response KortexArmSimulation::DeleteAction(const kortex_driver::DeleteAction::Request& req) { - auto input = req.input; + auto handle = req.input; + // If the action is not a default action + if (DEFAULT_ACTIONS_IDENTIFIERS.find(handle.identifier) == DEFAULT_ACTIONS_IDENTIFIERS.end()) + { + auto it = m_map_actions.find(handle.identifier); + if (it != m_map_actions.end()) + { + m_map_actions.erase(it); + ROS_INFO("Simulated action #%u properly deleted.", handle.identifier); + } + else + { + ROS_WARN("Could not find simulated action #%u to delete in actions map.", handle.identifier); + } + } + else + { + ROS_ERROR("Cannot delete default simulated actions."); + } kortex_driver::DeleteAction::Response response; return response; } diff --git a/kortex_driver/src/non-generated/tests/kortex_simulator_unit_tests.cc b/kortex_driver/src/non-generated/tests/kortex_simulator_unit_tests.cc index 1b907e92..215335d5 100644 --- a/kortex_driver/src/non-generated/tests/kortex_simulator_unit_tests.cc +++ b/kortex_driver/src/non-generated/tests/kortex_simulator_unit_tests.cc @@ -41,6 +41,23 @@ TEST_F(KortexSimulatorTest, DefaultActions) ASSERT_FALSE(other); } +// Tests DeleteAction so default actions are not deleted +TEST_F(KortexSimulatorTest, DeleteDefaultActions) +{ + static const std::vector DEFAULT_ACTIONS_IDENTIFIERS{1,2,3}; + // Make sure the action can be deleted properly + kortex_driver::DeleteAction::Request req; + kortex_driver::ActionHandle handle; + for (unsigned int i : DEFAULT_ACTIONS_IDENTIFIERS) + { + handle.identifier = i; + req.input = handle; + m_simulator->DeleteAction(req); + auto actions_map = m_simulator->GetActionsMap(); + ASSERT_EQ(actions_map.count(i), 1); + } +} + // Tests ReadAllActions TEST_F(KortexSimulatorTest, ReadAllActions) { @@ -61,7 +78,7 @@ TEST_F(KortexSimulatorTest, ReadAllActions) ASSERT_TRUE(action_list.action_list.empty()); } -// Tests CreateAction handler for a supported Action +// Tests CreateAction handler for a supported Action, and DeleteAction TEST_F(KortexSimulatorTest, CreateSupportedAction) { static const std::string name = "MyNewAction"; @@ -88,6 +105,13 @@ TEST_F(KortexSimulatorTest, CreateSupportedAction) ASSERT_EQ(actions_map.count(handle.identifier), 1); ASSERT_EQ(actions_map[handle.identifier].name, name); ASSERT_EQ(actions_map[handle.identifier].handle.action_type, kortex_driver::ActionType::REACH_JOINT_ANGLES); + + // Make sure the action can be deleted properly + kortex_driver::DeleteAction::Request del_req; + del_req.input = handle; + m_simulator->DeleteAction(del_req); + actions_map = m_simulator->GetActionsMap(); + ASSERT_EQ(actions_map.count(handle.identifier), 0); } // Tests CreateAction handler for an unsupported Action From 634e36bd2652e47a7af065849ffa4ac80b11167b Mon Sep 17 00:00:00 2001 From: Alexandre Vannobel Date: Tue, 16 Jun 2020 10:37:26 -0400 Subject: [PATCH 25/33] Add implementation and tests for UpdateAction --- .../non-generated/kortex_arm_simulation.h | 1 + kortex_driver/launch/test_simulator.launch | 4 - .../driver/kortex_arm_simulation.cpp | 27 +++- .../tests/kortex_simulator_unit_tests.cc | 123 ++++++++++++++---- 4 files changed, 124 insertions(+), 31 deletions(-) diff --git a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h index de50faa2..4977d2a3 100644 --- a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h +++ b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h @@ -41,6 +41,7 @@ class KortexArmSimulation KortexArmSimulation(ros::NodeHandle& nh); ~KortexArmSimulation(); std::unordered_map GetActionsMap() const; + int GetDOF() const {return m_degrees_of_freedom;} // Handlers for simulated Kortex API functions // Actions API diff --git a/kortex_driver/launch/test_simulator.launch b/kortex_driver/launch/test_simulator.launch index fa61683d..b425760b 100644 --- a/kortex_driver/launch/test_simulator.launch +++ b/kortex_driver/launch/test_simulator.launch @@ -27,7 +27,6 @@ - @@ -83,9 +82,6 @@ - - - diff --git a/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp b/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp index 072f0ccc..ba9789ca 100644 --- a/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp +++ b/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp @@ -167,7 +167,32 @@ kortex_driver::DeleteAction::Response KortexArmSimulation::DeleteAction(const ko kortex_driver::UpdateAction::Response KortexArmSimulation::UpdateAction(const kortex_driver::UpdateAction::Request& req) { - auto input = req.input; + auto action = req.input; + // If the action is not a default action + if (DEFAULT_ACTIONS_IDENTIFIERS.find(action.handle.identifier) == DEFAULT_ACTIONS_IDENTIFIERS.end()) + { + auto it = m_map_actions.find(action.handle.identifier); + if (it != m_map_actions.end()) + { + if (it->second.handle.action_type == action.handle.action_type) + { + it->second = action; + ROS_INFO("Simulated action #%u properly updated.", action.handle.identifier); + } + else + { + ROS_ERROR("Cannot update action with different type."); + } + } + else + { + ROS_ERROR("Could not find simulated action #%u to update in actions map.", action.handle.identifier); + } + } + else + { + ROS_ERROR("Cannot update default simulated actions."); + } kortex_driver::UpdateAction::Response response; return response; } diff --git a/kortex_driver/src/non-generated/tests/kortex_simulator_unit_tests.cc b/kortex_driver/src/non-generated/tests/kortex_simulator_unit_tests.cc index 215335d5..c9c8a232 100644 --- a/kortex_driver/src/non-generated/tests/kortex_simulator_unit_tests.cc +++ b/kortex_driver/src/non-generated/tests/kortex_simulator_unit_tests.cc @@ -5,20 +5,47 @@ #include class KortexSimulatorTest : public ::testing::Test { - protected: + protected: - void SetUp() override + void SetUp() override { // Create Simulator m_simulator.reset(new KortexArmSimulation(n)); + + // Create dummy action + dummy_action.name = "MyDummyAction"; + dummy_action.handle.action_type = kortex_driver::ActionType::REACH_JOINT_ANGLES; + dummy_action.handle.permission = 7; + kortex_driver::ConstrainedJointAngles angles; + for (int i = 0; i < m_simulator->GetDOF(); i++) + { + kortex_driver::JointAngle angle; + angle.joint_identifier = i; + angle.value = 10.0f*i; + angles.joint_angles.joint_angles.push_back(angle); + } + dummy_action.oneof_action_parameters.reach_joint_angles.push_back(angles); + } + + void TearDown() override + { } - void TearDown() override + void CompareReachJointAnglesActions(const kortex_driver::Action& a1, const kortex_driver::Action& a2, bool same) { + ASSERT_EQ(a1.oneof_action_parameters.reach_joint_angles.size(), a2.oneof_action_parameters.reach_joint_angles.size()); + auto angles1 = a1.oneof_action_parameters.reach_joint_angles[0]; + auto angles2 = a2.oneof_action_parameters.reach_joint_angles[0]; + ASSERT_EQ(angles1.joint_angles.joint_angles.size(), angles2.joint_angles.joint_angles.size()); + for (int i = 0; i < angles1.joint_angles.joint_angles.size() && same; i++) + { + ASSERT_EQ(same, angles1.joint_angles.joint_angles.at(i) == angles2.joint_angles.joint_angles.at(i)); + } } - ros::NodeHandle n; - std::unique_ptr m_simulator; + kortex_driver::Action dummy_action; + ros::NodeHandle n; + std::unique_ptr m_simulator; }; @@ -58,6 +85,23 @@ TEST_F(KortexSimulatorTest, DeleteDefaultActions) } } +// Tests UpdateAction so default actions are not updated +TEST_F(KortexSimulatorTest, UpdateDefaultActions) +{ + static const std::vector DEFAULT_ACTIONS_IDENTIFIERS{1,2,3}; + // Make sure the action cannot be updated + kortex_driver::UpdateAction::Request req; + for (unsigned int i : DEFAULT_ACTIONS_IDENTIFIERS) + { + dummy_action.handle.identifier = i; + req.input = dummy_action; + m_simulator->UpdateAction(req); + auto actions_map = m_simulator->GetActionsMap(); + ASSERT_EQ(actions_map.count(i), 1); + CompareReachJointAnglesActions(actions_map[i], dummy_action, false); + } +} + // Tests ReadAllActions TEST_F(KortexSimulatorTest, ReadAllActions) { @@ -84,19 +128,8 @@ TEST_F(KortexSimulatorTest, CreateSupportedAction) static const std::string name = "MyNewAction"; kortex_driver::CreateAction::Request req; - kortex_driver::Action action; - action.handle.action_type = kortex_driver::ActionType::REACH_JOINT_ANGLES; - action.name = name; - kortex_driver::ConstrainedJointAngles angles; - for (int i = 0; i < 7; i++) - { - kortex_driver::JointAngle angle; - angle.joint_identifier = i; - angle.value = 10.0f*i; - angles.joint_angles.joint_angles.push_back(angle); - } - action.oneof_action_parameters.reach_joint_angles.push_back(angles); - req.input = action; + dummy_action.name = name; + req.input = dummy_action; auto res = m_simulator->CreateAction(req); auto handle = res.output; auto actions_map = m_simulator->GetActionsMap(); @@ -117,22 +150,19 @@ TEST_F(KortexSimulatorTest, CreateSupportedAction) // Tests CreateAction handler for an unsupported Action TEST_F(KortexSimulatorTest, CreateUnsupportedAction) { - static const std::string name = "MyNewAction"; - kortex_driver::CreateAction::Request req; - kortex_driver::Action action; - action.handle.action_type = kortex_driver::ActionType::SEND_JOINT_SPEEDS; - action.name = name; + dummy_action.oneof_action_parameters.reach_joint_angles.clear(); + dummy_action.handle.action_type = kortex_driver::ActionType::SEND_JOINT_SPEEDS; kortex_driver::Base_JointSpeeds speeds; - for (int i = 0; i < 7; i++) + for (int i = 0; i < m_simulator->GetDOF(); i++) { kortex_driver::JointSpeed speed; speed.joint_identifier = i; speed.value = 10.0f*i; speeds.joint_speeds.push_back(speed); } - action.oneof_action_parameters.send_joint_speeds.push_back(speeds); - req.input = action; + dummy_action.oneof_action_parameters.send_joint_speeds.push_back(speeds); + req.input = dummy_action; auto res = m_simulator->CreateAction(req); auto handle = res.output; auto actions_map = m_simulator->GetActionsMap(); @@ -140,3 +170,44 @@ TEST_F(KortexSimulatorTest, CreateUnsupportedAction) // Make sure the action was added to the map ASSERT_EQ(actions_map.count(handle.identifier), 0); } + +// Tests UpdateAction on existing and non-existing actions +TEST_F(KortexSimulatorTest, UpdateAction) +{ + // Create Action at first + kortex_driver::CreateAction::Request req; + req.input = dummy_action; + auto res = m_simulator->CreateAction(req); + auto handle = res.output; + auto actions_map = m_simulator->GetActionsMap(); + + // Make sure the action was added to the map + ASSERT_EQ(actions_map.count(handle.identifier), 1); + ASSERT_EQ(actions_map[handle.identifier].handle.action_type, kortex_driver::ActionType::REACH_JOINT_ANGLES); + + // Modify and update the Action + dummy_action.name = "MyUpdatedName"; + dummy_action.handle.identifier = handle.identifier; + dummy_action.oneof_action_parameters.reach_joint_angles[0].joint_angles.joint_angles[3].value = 0.0f; + kortex_driver::UpdateAction::Request update_req; + update_req.input = dummy_action; + m_simulator->UpdateAction(update_req); + actions_map = m_simulator->GetActionsMap(); + ASSERT_EQ(actions_map.count(handle.identifier), 1); + ASSERT_EQ(dummy_action.name, actions_map[handle.identifier].name); + CompareReachJointAnglesActions(dummy_action, actions_map[handle.identifier], true); + + // Modify and update the Action with a different type + kortex_driver::Action wrong_type_action; + wrong_type_action.name = "WrongType"; + wrong_type_action.handle.identifier = handle.identifier; + wrong_type_action.handle.action_type = kortex_driver::ActionType::SEND_JOINT_SPEEDS; + wrong_type_action.oneof_action_parameters.reach_joint_angles.clear(); + wrong_type_action.oneof_action_parameters.send_joint_speeds.push_back(kortex_driver::Base_JointSpeeds()); + update_req.input = wrong_type_action; + m_simulator->UpdateAction(update_req); + actions_map = m_simulator->GetActionsMap(); + ASSERT_EQ(actions_map.count(handle.identifier), 1); + ASSERT_EQ(dummy_action.name, actions_map[handle.identifier].name); + ASSERT_EQ(kortex_driver::ActionType::REACH_JOINT_ANGLES, actions_map[handle.identifier].handle.action_type); +} From b7be7b72d60c720fd38af80134433e04aacd37c0 Mon Sep 17 00:00:00 2001 From: Alexandre Vannobel Date: Tue, 16 Jun 2020 11:55:09 -0400 Subject: [PATCH 26/33] Added basic frame for a threaded action executor --- .../non-generated/kortex_arm_simulation.h | 20 ++- .../driver/kortex_arm_driver.cpp | 1 - .../driver/kortex_arm_simulation.cpp | 119 ++++++++++++++++-- 3 files changed, 128 insertions(+), 12 deletions(-) diff --git a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h index 4977d2a3..80603931 100644 --- a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h +++ b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h @@ -16,10 +16,12 @@ #include #include +#include #include "kortex_driver/non-generated/kortex_math_util.h" #include "kortex_driver/ActionType.h" +#include "kortex_driver/KortexError.h" #include "kortex_driver/CreateAction.h" #include "kortex_driver/ReadAction.h" @@ -52,7 +54,6 @@ class KortexArmSimulation kortex_driver::UpdateAction::Response UpdateAction(const kortex_driver::UpdateAction::Request& req); kortex_driver::ExecuteActionFromReference::Response ExecuteActionFromReference(const kortex_driver::ExecuteActionFromReference::Request& req); kortex_driver::ExecuteAction::Response ExecuteAction(const kortex_driver::ExecuteAction::Request& req); - kortex_driver::PauseAction::Response PauseAction(const kortex_driver::PauseAction::Request& req); kortex_driver::StopAction::Response StopAction(const kortex_driver::StopAction::Request& req); // Sequences API // Velocity control RPCs @@ -77,9 +78,14 @@ class KortexArmSimulation // Action-related std::unordered_map m_map_actions; - // ROS and thread objects to publish the feedback from the robot + // Math utility KortexMathUtil m_math_util; + // Threading + std::atomic m_is_action_being_executed; + std::atomic m_action_preempted; + std::thread m_action_executor_thread; + // MoveIt-related std::unique_ptr m_moveit_arm_interface; std::unique_ptr m_moveit_gripper_interface; @@ -87,6 +93,16 @@ class KortexArmSimulation // Helper functions bool IsGripperPresent() const {return !m_gripper_name.empty();} void CreateDefaultActions(); + void CancelAction(); + void PlayAction(const kortex_driver::Action& action); + + // Executors + kortex_driver::KortexError ExecuteReachJointAngles(const kortex_driver::Action& action); + kortex_driver::KortexError ExecuteReachPose(const kortex_driver::Action& action); + kortex_driver::KortexError ExecuteSendJointSpeeds(const kortex_driver::Action& action); + kortex_driver::KortexError ExecuteSendTwist(const kortex_driver::Action& action); + kortex_driver::KortexError ExecuteSendGripperCommand(const kortex_driver::Action& action); + kortex_driver::KortexError ExecuteTimeDelay(const kortex_driver::Action& action); }; #endif //_KORTEX_ARM_SIMULATION_H_ diff --git a/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp b/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp index 2f312fd2..274b44db 100644 --- a/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp +++ b/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp @@ -650,6 +650,5 @@ void KortexArmDriver::registerSimulationHandlers() base_services_simulation->UpdateActionHandler = std::bind(&KortexArmSimulation::UpdateAction, m_simulator.get(), std::placeholders::_1); base_services_simulation->ExecuteActionFromReferenceHandler = std::bind(&KortexArmSimulation::ExecuteActionFromReference, m_simulator.get(), std::placeholders::_1); base_services_simulation->ExecuteActionHandler = std::bind(&KortexArmSimulation::ExecuteAction, m_simulator.get(), std::placeholders::_1); - base_services_simulation->PauseActionHandler = std::bind(&KortexArmSimulation::PauseAction, m_simulator.get(), std::placeholders::_1); base_services_simulation->StopActionHandler = std::bind(&KortexArmSimulation::StopAction, m_simulator.get(), std::placeholders::_1); } \ No newline at end of file diff --git a/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp b/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp index ba9789ca..2a86a3f0 100644 --- a/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp +++ b/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp @@ -11,6 +11,8 @@ */ #include "kortex_driver/non-generated/kortex_arm_simulation.h" +#include "kortex_driver/ErrorCodes.h" +#include "kortex_driver/SubErrorCodes.h" #include namespace @@ -22,7 +24,9 @@ namespace } KortexArmSimulation::KortexArmSimulation(ros::NodeHandle& node_handle): m_node_handle(node_handle), - m_map_actions{} + m_map_actions{}, + m_is_action_being_executed{false}, + m_action_preempted{false} { // Namespacing and prefixing information ros::param::get("~robot_name", m_robot_name); @@ -211,13 +215,6 @@ kortex_driver::ExecuteAction::Response KortexArmSimulation::ExecuteAction(const return response; } -kortex_driver::PauseAction::Response KortexArmSimulation::PauseAction(const kortex_driver::PauseAction::Request& req) -{ - auto input = req.input; - kortex_driver::PauseAction::Response response; - return response; -} - kortex_driver::StopAction::Response KortexArmSimulation::StopAction(const kortex_driver::StopAction::Request& req) { auto input = req.input; @@ -278,4 +275,108 @@ void KortexArmSimulation::CreateDefaultActions() m_map_actions.emplace(std::make_pair(retract.handle.identifier, retract)); m_map_actions.emplace(std::make_pair(home.handle.identifier, home)); m_map_actions.emplace(std::make_pair(zero.handle.identifier, zero)); -} \ No newline at end of file +} + +void KortexArmSimulation::CancelAction() +{ + m_action_preempted = true; + if (m_action_executor_thread.joinable()) + { + m_action_executor_thread.join(); + } + m_action_preempted = false; +} + +void KortexArmSimulation::PlayAction(const kortex_driver::Action& action) +{ + kortex_driver::KortexError action_result; + m_is_action_being_executed = true; + + // Switch on the action type + switch (action.handle.action_type) + { + case kortex_driver::ActionType::REACH_JOINT_ANGLES: + action_result = ExecuteReachJointAngles(action); + break; + case kortex_driver::ActionType::REACH_POSE: + action_result = ExecuteReachPose(action); + break; + case kortex_driver::ActionType::SEND_JOINT_SPEEDS: + action_result = ExecuteSendJointSpeeds(action); + break; + case kortex_driver::ActionType::SEND_TWIST_COMMAND: + action_result = ExecuteSendTwist(action); + break; + case kortex_driver::ActionType::SEND_GRIPPER_COMMAND: + action_result = ExecuteSendGripperCommand(action); + break; + case kortex_driver::ActionType::TIME_DELAY: + action_result = ExecuteTimeDelay(action); + break; + default: + action_result.code = kortex_driver::ErrorCodes::ERROR_DEVICE; + action_result.subCode = kortex_driver::SubErrorCodes::UNSUPPORTED_ACTION; + break; + } + + // Action was cancelled by user + if (m_action_preempted.load()) + { + // Notify ACTION_ABORT + } + // Action ended on its own + else + { + // Notify ACTION_END + } + + m_is_action_being_executed = false; +} + +kortex_driver::KortexError KortexArmSimulation::ExecuteReachJointAngles(const kortex_driver::Action& action) +{ + kortex_driver::KortexError result; + result.code = kortex_driver::ErrorCodes::ERROR_NONE; + result.subCode = kortex_driver::SubErrorCodes::SUB_ERROR_NONE; + return result; +} + +kortex_driver::KortexError KortexArmSimulation::ExecuteReachPose(const kortex_driver::Action& action) +{ + kortex_driver::KortexError result; + result.code = kortex_driver::ErrorCodes::ERROR_NONE; + result.subCode = kortex_driver::SubErrorCodes::SUB_ERROR_NONE; + return result; +} + +kortex_driver::KortexError KortexArmSimulation::ExecuteSendJointSpeeds(const kortex_driver::Action& action) +{ + kortex_driver::KortexError result; + result.code = kortex_driver::ErrorCodes::ERROR_NONE; + result.subCode = kortex_driver::SubErrorCodes::SUB_ERROR_NONE; + return result; +} + +kortex_driver::KortexError KortexArmSimulation::ExecuteSendTwist(const kortex_driver::Action& action) +{ + kortex_driver::KortexError result; + result.code = kortex_driver::ErrorCodes::ERROR_NONE; + result.subCode = kortex_driver::SubErrorCodes::SUB_ERROR_NONE; + return result; +} + +kortex_driver::KortexError KortexArmSimulation::ExecuteSendGripperCommand(const kortex_driver::Action& action) +{ + kortex_driver::KortexError result; + result.code = kortex_driver::ErrorCodes::ERROR_NONE; + result.subCode = kortex_driver::SubErrorCodes::SUB_ERROR_NONE; + return result; +} + +kortex_driver::KortexError KortexArmSimulation::ExecuteTimeDelay(const kortex_driver::Action& action) +{ + kortex_driver::KortexError result; + result.code = kortex_driver::ErrorCodes::ERROR_NONE; + result.subCode = kortex_driver::SubErrorCodes::SUB_ERROR_NONE; + return result; +} From c5d5785ed68f2db503afb422df55ed78f0206d86 Mon Sep 17 00:00:00 2001 From: Alexandre Vannobel Date: Tue, 16 Jun 2020 13:54:51 -0400 Subject: [PATCH 27/33] Added publishing for action topic before and after action execution --- .../non-generated/kortex_arm_simulation.h | 7 ++-- .../driver/kortex_arm_simulation.cpp | 36 +++++++++++++++++-- 2 files changed, 39 insertions(+), 4 deletions(-) diff --git a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h index 80603931..0c21cec8 100644 --- a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h +++ b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h @@ -62,6 +62,9 @@ class KortexArmSimulation ros::NodeHandle m_node_handle; + // Publishers + ros::Publisher m_pub_action_topic; + // Namespacing and prefixing information std::string m_prefix; std::string m_robot_name; @@ -93,10 +96,10 @@ class KortexArmSimulation // Helper functions bool IsGripperPresent() const {return !m_gripper_name.empty();} void CreateDefaultActions(); - void CancelAction(); - void PlayAction(const kortex_driver::Action& action); // Executors + void CancelAction(); + void PlayAction(const kortex_driver::Action& action); kortex_driver::KortexError ExecuteReachJointAngles(const kortex_driver::Action& action); kortex_driver::KortexError ExecuteReachPose(const kortex_driver::Action& action); kortex_driver::KortexError ExecuteSendJointSpeeds(const kortex_driver::Action& action); diff --git a/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp b/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp index 2a86a3f0..c42d3b22 100644 --- a/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp +++ b/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp @@ -13,6 +13,9 @@ #include "kortex_driver/non-generated/kortex_arm_simulation.h" #include "kortex_driver/ErrorCodes.h" #include "kortex_driver/SubErrorCodes.h" +#include "kortex_driver/ActionNotification.h" +#include "kortex_driver/ActionEvent.h" + #include namespace @@ -68,6 +71,9 @@ KortexArmSimulation::KortexArmSimulation(ros::NodeHandle& node_handle): m_node_h // Create default actions CreateDefaultActions(); + + // Create publishers + m_pub_action_topic = m_node_handle.advertise("action_topic", 1000); } KortexArmSimulation::~KortexArmSimulation() @@ -290,9 +296,15 @@ void KortexArmSimulation::CancelAction() void KortexArmSimulation::PlayAction(const kortex_driver::Action& action) { kortex_driver::KortexError action_result; + + // Notify action started + kortex_driver::ActionNotification start_notif; + start_notif.handle = action.handle; + start_notif.action_event = kortex_driver::ActionEvent::ACTION_START; + m_pub_action_topic.publish(start_notif); m_is_action_being_executed = true; - // Switch on the action type + // Switch executor on the action type switch (action.handle.action_type) { case kortex_driver::ActionType::REACH_JOINT_ANGLES: @@ -319,16 +331,36 @@ void KortexArmSimulation::PlayAction(const kortex_driver::Action& action) break; } + kortex_driver::ActionNotification end_notif; + end_notif.handle = action.handle; // Action was cancelled by user if (m_action_preempted.load()) { // Notify ACTION_ABORT + end_notif.action_event = kortex_driver::ActionEvent::ACTION_ABORT; + ROS_WARN("Action was aborted by user."); } // Action ended on its own else { - // Notify ACTION_END + if (action_result.code != kortex_driver::ErrorCodes::ERROR_NONE) + { + // Notify ACTION_ABORT + end_notif.action_event = kortex_driver::ActionEvent::ACTION_ABORT; + end_notif.abort_details = action_result.subCode; + ROS_WARN("Action was failed : \nError code is %d\nSub-error code is %d\nError description is : %s", + action_result.code, + action_result.subCode, + action_result.description.c_str()); + } + else + { + // Notify ACTION_END + end_notif.action_event = kortex_driver::ActionEvent::ACTION_END; + ROS_WARN("Action ended."); + } } + m_pub_action_topic.publish(end_notif); m_is_action_being_executed = false; } From 798b43c1b3252ed4c4ba1aa52339a7dd58e0d991 Mon Sep 17 00:00:00 2001 From: Alexandre Vannobel Date: Tue, 16 Jun 2020 15:39:12 -0400 Subject: [PATCH 28/33] Added StopAction logic and tests for executing and aborting actions --- .../driver/kortex_arm_simulation.cpp | 87 +++++++++++--- .../tests/kortex_simulator_unit_tests.cc | 106 +++++++++++++++++- 2 files changed, 177 insertions(+), 16 deletions(-) diff --git a/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp b/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp index c42d3b22..5419e771 100644 --- a/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp +++ b/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp @@ -17,6 +17,7 @@ #include "kortex_driver/ActionEvent.h" #include +#include namespace { @@ -78,6 +79,7 @@ KortexArmSimulation::KortexArmSimulation(ros::NodeHandle& node_handle): m_node_h KortexArmSimulation::~KortexArmSimulation() { + CancelAction(); } std::unordered_map KortexArmSimulation::GetActionsMap() const @@ -171,8 +173,8 @@ kortex_driver::DeleteAction::Response KortexArmSimulation::DeleteAction(const ko { ROS_ERROR("Cannot delete default simulated actions."); } - kortex_driver::DeleteAction::Response response; - return response; + + return kortex_driver::DeleteAction::Response(); } kortex_driver::UpdateAction::Response KortexArmSimulation::UpdateAction(const kortex_driver::UpdateAction::Request& req) @@ -203,29 +205,65 @@ kortex_driver::UpdateAction::Response KortexArmSimulation::UpdateAction(const ko { ROS_ERROR("Cannot update default simulated actions."); } - kortex_driver::UpdateAction::Response response; - return response; + + return kortex_driver::UpdateAction::Response(); } kortex_driver::ExecuteActionFromReference::Response KortexArmSimulation::ExecuteActionFromReference(const kortex_driver::ExecuteActionFromReference::Request& req) { - auto input = req.input; - kortex_driver::ExecuteActionFromReference::Response response; - return response; + auto handle = req.input; + auto it = m_map_actions.find(handle.identifier); + if (it != m_map_actions.end()) + { + // If an action is ongoing, cancel it first + if (m_is_action_being_executed.load()) + { + CancelAction(); // this will block until the thread is joined and current action finished + } + m_action_executor_thread = std::thread(&KortexArmSimulation::PlayAction, this, it->second); + } + else + { + ROS_ERROR("Could not find action with given identifier %d", handle.identifier); + } + + return kortex_driver::ExecuteActionFromReference::Response(); } kortex_driver::ExecuteAction::Response KortexArmSimulation::ExecuteAction(const kortex_driver::ExecuteAction::Request& req) { - auto input = req.input; - kortex_driver::ExecuteAction::Response response; - return response; + auto action = req.input; + + // Add Action to map if type is supported + switch (action.handle.action_type) + { + case kortex_driver::ActionType::REACH_JOINT_ANGLES: + case kortex_driver::ActionType::REACH_POSE: + case kortex_driver::ActionType::SEND_GRIPPER_COMMAND: + case kortex_driver::ActionType::TIME_DELAY: + // If an action is ongoing, cancel it first + if (m_is_action_being_executed.load()) + { + CancelAction(); // this will block until the thread is joined and current action finished + } + m_action_executor_thread = std::thread(&KortexArmSimulation::PlayAction, this, action); + break; + default: + ROS_ERROR("Unsupported action type %d : could not execute simulated action.", action.handle.action_type); + break; + } + + return kortex_driver::ExecuteAction::Response(); } kortex_driver::StopAction::Response KortexArmSimulation::StopAction(const kortex_driver::StopAction::Request& req) { - auto input = req.input; - kortex_driver::StopAction::Response response; - return response; + if (m_is_action_being_executed.load()) + { + CancelAction(); // this will block until the thread is joined and current action finished + } + + return kortex_driver::StopAction::Response(); } void KortexArmSimulation::CreateDefaultActions() @@ -357,7 +395,6 @@ void KortexArmSimulation::PlayAction(const kortex_driver::Action& action) { // Notify ACTION_END end_notif.action_event = kortex_driver::ActionEvent::ACTION_END; - ROS_WARN("Action ended."); } } m_pub_action_topic.publish(end_notif); @@ -365,6 +402,7 @@ void KortexArmSimulation::PlayAction(const kortex_driver::Action& action) m_is_action_being_executed = false; } +// TODO Fill implementation kortex_driver::KortexError KortexArmSimulation::ExecuteReachJointAngles(const kortex_driver::Action& action) { kortex_driver::KortexError result; @@ -373,6 +411,7 @@ kortex_driver::KortexError KortexArmSimulation::ExecuteReachJointAngles(const ko return result; } +// TODO Fill implementation kortex_driver::KortexError KortexArmSimulation::ExecuteReachPose(const kortex_driver::Action& action) { kortex_driver::KortexError result; @@ -381,6 +420,7 @@ kortex_driver::KortexError KortexArmSimulation::ExecuteReachPose(const kortex_dr return result; } +// TODO Fill implementation kortex_driver::KortexError KortexArmSimulation::ExecuteSendJointSpeeds(const kortex_driver::Action& action) { kortex_driver::KortexError result; @@ -389,6 +429,7 @@ kortex_driver::KortexError KortexArmSimulation::ExecuteSendJointSpeeds(const kor return result; } +// TODO Fill implementation kortex_driver::KortexError KortexArmSimulation::ExecuteSendTwist(const kortex_driver::Action& action) { kortex_driver::KortexError result; @@ -397,6 +438,7 @@ kortex_driver::KortexError KortexArmSimulation::ExecuteSendTwist(const kortex_dr return result; } +// TODO Fill implementation kortex_driver::KortexError KortexArmSimulation::ExecuteSendGripperCommand(const kortex_driver::Action& action) { kortex_driver::KortexError result; @@ -410,5 +452,22 @@ kortex_driver::KortexError KortexArmSimulation::ExecuteTimeDelay(const kortex_dr kortex_driver::KortexError result; result.code = kortex_driver::ErrorCodes::ERROR_NONE; result.subCode = kortex_driver::SubErrorCodes::SUB_ERROR_NONE; + if (!action.oneof_action_parameters.delay.empty()) + { + auto start = std::chrono::system_clock::now(); + uint32_t delay_seconds = action.oneof_action_parameters.delay[0].duration; + // While not preempted and duration not elapsed + while (!m_action_preempted.load() && (std::chrono::system_clock::now() - start) < std::chrono::seconds(delay_seconds)) + { + // sleep a bit + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } + else + { + result.code = kortex_driver::ErrorCodes::ERROR_DEVICE; + result.subCode = kortex_driver::SubErrorCodes::INVALID_PARAM; + result.description = "Error playing time delay action : action is malformed."; + } return result; } diff --git a/kortex_driver/src/non-generated/tests/kortex_simulator_unit_tests.cc b/kortex_driver/src/non-generated/tests/kortex_simulator_unit_tests.cc index c9c8a232..6ecf2a39 100644 --- a/kortex_driver/src/non-generated/tests/kortex_simulator_unit_tests.cc +++ b/kortex_driver/src/non-generated/tests/kortex_simulator_unit_tests.cc @@ -3,6 +3,7 @@ #include #include #include +#include "kortex_driver/ActionEvent.h" class KortexSimulatorTest : public ::testing::Test { protected: @@ -25,7 +26,11 @@ class KortexSimulatorTest : public ::testing::Test { angles.joint_angles.joint_angles.push_back(angle); } dummy_action.oneof_action_parameters.reach_joint_angles.push_back(angles); - } + + // Create action topic subscriber + m_action_topic_sub = n.subscribe("action_topic", 5, &KortexSimulatorTest::ActionTopicHandler, this); + m_received_notifications.clear(); + } void TearDown() override { @@ -43,10 +48,16 @@ class KortexSimulatorTest : public ::testing::Test { } } + void ActionTopicHandler(const kortex_driver::ActionNotification& notif) + { + m_received_notifications.push_back(notif); + } + kortex_driver::Action dummy_action; ros::NodeHandle n; std::unique_ptr m_simulator; - + ros::Subscriber m_action_topic_sub; + std::vector m_received_notifications; }; // Make sure after initialisation the default actions are @@ -211,3 +222,94 @@ TEST_F(KortexSimulatorTest, UpdateAction) ASSERT_EQ(dummy_action.name, actions_map[handle.identifier].name); ASSERT_EQ(kortex_driver::ActionType::REACH_JOINT_ANGLES, actions_map[handle.identifier].handle.action_type); } + +// This uses a TIME_DELAY action to test execution +TEST_F(KortexSimulatorTest, ExecuteAction) +{ + static constexpr uint32_t SLEEP_DURATION_SECONDS = 4; + + dummy_action.oneof_action_parameters.reach_joint_angles.clear(); + dummy_action.handle.action_type = kortex_driver::ActionType::TIME_DELAY; + kortex_driver::Delay delay; + delay.duration = SLEEP_DURATION_SECONDS; + dummy_action.oneof_action_parameters.delay.push_back(delay); + kortex_driver::ExecuteAction::Request req; + req.input = dummy_action; + + ASSERT_EQ(m_received_notifications.size(), 0); + m_simulator->ExecuteAction(req); + std::this_thread::sleep_for(std::chrono::seconds(2)); + // Make sure after a couple seconds we received the ACTION_START + ASSERT_EQ(m_received_notifications.size(), 1); + ASSERT_EQ(m_received_notifications[0].action_event, kortex_driver::ActionEvent::ACTION_START); + std::this_thread::sleep_for(std::chrono::seconds(3)); + ASSERT_EQ(m_received_notifications.size(), 2); + ASSERT_EQ(m_received_notifications[1].action_event, kortex_driver::ActionEvent::ACTION_END); +} + +// This uses a TIME_DELAY action to test execution from reference +TEST_F(KortexSimulatorTest, ExecuteActionFromReference) +{ + static constexpr uint32_t SLEEP_DURATION_SECONDS = 4; + + // Create the Delay action + dummy_action.oneof_action_parameters.reach_joint_angles.clear(); + dummy_action.handle.action_type = kortex_driver::ActionType::TIME_DELAY; + kortex_driver::Delay delay; + delay.duration = SLEEP_DURATION_SECONDS; + dummy_action.oneof_action_parameters.delay.push_back(delay); + kortex_driver::CreateAction::Request req; + req.input = dummy_action; + auto res = m_simulator->CreateAction(req); + dummy_action.handle = res.output; + auto actions_map = m_simulator->GetActionsMap(); + ASSERT_EQ(actions_map.count(dummy_action.handle.identifier), 1); + + // Execute the Delay action by reference + ASSERT_EQ(m_received_notifications.size(), 0); + kortex_driver::ExecuteActionFromReference::Request execute_req; + execute_req.input = dummy_action.handle; + m_simulator->ExecuteActionFromReference(execute_req); + std::this_thread::sleep_for(std::chrono::seconds(2)); + // Make sure after a couple seconds we received the ACTION_START + ASSERT_EQ(m_received_notifications.size(), 1); + ASSERT_EQ(m_received_notifications[0].action_event, kortex_driver::ActionEvent::ACTION_START); + std::this_thread::sleep_for(std::chrono::seconds(3)); + ASSERT_EQ(m_received_notifications.size(), 2); + ASSERT_EQ(m_received_notifications[1].action_event, kortex_driver::ActionEvent::ACTION_END); +} + +// This uses a TIME_DELAY action to test aborting +TEST_F(KortexSimulatorTest, StopAction) +{ + static constexpr uint32_t SLEEP_DURATION_SECONDS = 4; + + // Create delay action object + dummy_action.oneof_action_parameters.reach_joint_angles.clear(); + dummy_action.handle.action_type = kortex_driver::ActionType::TIME_DELAY; + kortex_driver::Delay delay; + delay.duration = SLEEP_DURATION_SECONDS; + dummy_action.oneof_action_parameters.delay.push_back(delay); + kortex_driver::ExecuteAction::Request req; + req.input = dummy_action; + + // Prepare StopAction request + kortex_driver::StopAction::Request stop_req; + + // Execute the action + ASSERT_EQ(m_received_notifications.size(), 0); + m_simulator->ExecuteAction(req); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // Make sure after one second we received the ACTION_START + ASSERT_EQ(m_received_notifications.size(), 1); + ASSERT_EQ(m_received_notifications[0].action_event, kortex_driver::ActionEvent::ACTION_START); + + // Abort the action now + m_simulator->StopAction(stop_req); + + // Wait a biut and make sure we received the ACTION_ABORT + std::this_thread::sleep_for(std::chrono::seconds(1)); + ASSERT_EQ(m_received_notifications.size(), 2); + ASSERT_EQ(m_received_notifications[1].action_event, kortex_driver::ActionEvent::ACTION_ABORT); +} \ No newline at end of file From 45ae1212f44e9722d6d452314bb6e1f367e9e00a Mon Sep 17 00:00:00 2001 From: Alexandre Vannobel Date: Tue, 16 Jun 2020 16:03:31 -0400 Subject: [PATCH 29/33] Added stubs for other RPCs we want to support at first in simulation --- .../non-generated/kortex_arm_simulation.h | 20 ++++++- .../driver/kortex_arm_driver.cpp | 16 ++++++ .../driver/kortex_arm_simulation.cpp | 56 +++++++++++++++++++ 3 files changed, 90 insertions(+), 2 deletions(-) diff --git a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h index 0c21cec8..50246f6c 100644 --- a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h +++ b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h @@ -34,6 +34,15 @@ #include "kortex_driver/StopAction.h" #include "kortex_driver/ResumeAction.h" +#include "kortex_driver/PlayCartesianTrajectory.h" +#include "kortex_driver/Stop.h" +#include "kortex_driver/GetMeasuredCartesianPose.h" +#include "kortex_driver/SendTwistCommand.h" +#include "kortex_driver/PlayJointTrajectory.h" +#include "kortex_driver/SendJointSpeedsCommand.h" +#include "kortex_driver/SendGripperCommand.h" +#include "kortex_driver/ApplyEmergencyStop.h" + #include class KortexArmSimulation @@ -55,8 +64,15 @@ class KortexArmSimulation kortex_driver::ExecuteActionFromReference::Response ExecuteActionFromReference(const kortex_driver::ExecuteActionFromReference::Request& req); kortex_driver::ExecuteAction::Response ExecuteAction(const kortex_driver::ExecuteAction::Request& req); kortex_driver::StopAction::Response StopAction(const kortex_driver::StopAction::Request& req); - // Sequences API - // Velocity control RPCs + // Other RPCs + kortex_driver::PlayCartesianTrajectory::Response PlayCartesianTrajectory(const kortex_driver::PlayCartesianTrajectory::Request& req); + kortex_driver::Stop::Response Stop(const kortex_driver::Stop::Request& req); + kortex_driver::GetMeasuredCartesianPose::Response GetMeasuredCartesianPose(const kortex_driver::GetMeasuredCartesianPose::Request& req); + kortex_driver::SendTwistCommand::Response SendTwistCommand(const kortex_driver::SendTwistCommand::Request& req); + kortex_driver::PlayJointTrajectory::Response PlayJointTrajectory(const kortex_driver::PlayJointTrajectory::Request& req); + kortex_driver::SendJointSpeedsCommand::Response SendJointSpeedsCommand(const kortex_driver::SendJointSpeedsCommand::Request& req); + kortex_driver::SendGripperCommand::Response SendGripperCommand(const kortex_driver::SendGripperCommand::Request& req); + kortex_driver::ApplyEmergencyStop::Response ApplyEmergencyStop(const kortex_driver::ApplyEmergencyStop::Request& req); private: diff --git a/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp b/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp index 274b44db..adc44072 100644 --- a/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp +++ b/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp @@ -643,6 +643,7 @@ void KortexArmDriver::registerSimulationHandlers() { BaseSimulationServices* base_services_simulation = dynamic_cast(m_base_ros_services); // Link the m_simulator handlers to the ROS services callbacks + // Action services base_services_simulation->CreateActionHandler = std::bind(&KortexArmSimulation::CreateAction, m_simulator.get(), std::placeholders::_1); base_services_simulation->ReadActionHandler = std::bind(&KortexArmSimulation::ReadAction, m_simulator.get(), std::placeholders::_1); base_services_simulation->ReadAllActionsHandler = std::bind(&KortexArmSimulation::ReadAllActions, m_simulator.get(), std::placeholders::_1); @@ -651,4 +652,19 @@ void KortexArmDriver::registerSimulationHandlers() base_services_simulation->ExecuteActionFromReferenceHandler = std::bind(&KortexArmSimulation::ExecuteActionFromReference, m_simulator.get(), std::placeholders::_1); base_services_simulation->ExecuteActionHandler = std::bind(&KortexArmSimulation::ExecuteAction, m_simulator.get(), std::placeholders::_1); base_services_simulation->StopActionHandler = std::bind(&KortexArmSimulation::StopAction, m_simulator.get(), std::placeholders::_1); + // Other services + base_services_simulation->PlayCartesianTrajectoryHandler = std::bind(&KortexArmSimulation::PlayCartesianTrajectory, m_simulator.get(), std::placeholders::_1); + base_services_simulation->StopHandler = std::bind(&KortexArmSimulation::Stop, m_simulator.get(), std::placeholders::_1); + base_services_simulation->GetMeasuredCartesianPoseHandler = std::bind(&KortexArmSimulation::GetMeasuredCartesianPose, m_simulator.get(), std::placeholders::_1); + base_services_simulation->SendTwistCommandHandler = std::bind(&KortexArmSimulation::SendTwistCommand, m_simulator.get(), std::placeholders::_1); + base_services_simulation->PlayJointTrajectoryHandler = std::bind(&KortexArmSimulation::PlayJointTrajectory, m_simulator.get(), std::placeholders::_1); + base_services_simulation->SendJointSpeedsCommandHandler = std::bind(&KortexArmSimulation::SendJointSpeedsCommand, m_simulator.get(), std::placeholders::_1); + base_services_simulation->SendGripperCommandHandler = std::bind(&KortexArmSimulation::SendGripperCommand, m_simulator.get(), std::placeholders::_1); + base_services_simulation->ApplyEmergencyStopHandler = std::bind(&KortexArmSimulation::ApplyEmergencyStop, m_simulator.get(), std::placeholders::_1); + + // Prospects + //SendSelectedJointSpeedCommand + //PlaySelectedJointTrajectory + //PlayCartesianTrajectoryPosition + //PlayCartesianTrajectoryOrientation } \ No newline at end of file diff --git a/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp b/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp index 5419e771..bf6b38cf 100644 --- a/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp +++ b/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp @@ -266,6 +266,62 @@ kortex_driver::StopAction::Response KortexArmSimulation::StopAction(const kortex return kortex_driver::StopAction::Response(); } +kortex_driver::PlayCartesianTrajectory::Response KortexArmSimulation::PlayCartesianTrajectory(const kortex_driver::PlayCartesianTrajectory::Request& req) +{ + auto input = req.input; + kortex_driver::PlayCartesianTrajectory::Response response; + return response; +} + +kortex_driver::Stop::Response KortexArmSimulation::Stop(const kortex_driver::Stop::Request& req) +{ + auto input = req.input; + kortex_driver::Stop::Response response; + return response; +} + +kortex_driver::GetMeasuredCartesianPose::Response KortexArmSimulation::GetMeasuredCartesianPose(const kortex_driver::GetMeasuredCartesianPose::Request& req) +{ + auto input = req.input; + kortex_driver::GetMeasuredCartesianPose::Response response; + return response; +} + +kortex_driver::SendTwistCommand::Response KortexArmSimulation::SendTwistCommand(const kortex_driver::SendTwistCommand::Request& req) +{ + auto input = req.input; + kortex_driver::SendTwistCommand::Response response; + return response; +} + +kortex_driver::PlayJointTrajectory::Response KortexArmSimulation::PlayJointTrajectory(const kortex_driver::PlayJointTrajectory::Request& req) +{ + auto input = req.input; + kortex_driver::PlayJointTrajectory::Response response; + return response; +} + +kortex_driver::SendJointSpeedsCommand::Response KortexArmSimulation::SendJointSpeedsCommand(const kortex_driver::SendJointSpeedsCommand::Request& req) +{ + auto input = req.input; + kortex_driver::SendJointSpeedsCommand::Response response; + return response; +} + +kortex_driver::SendGripperCommand::Response KortexArmSimulation::SendGripperCommand(const kortex_driver::SendGripperCommand::Request& req) +{ + auto input = req.input; + kortex_driver::SendGripperCommand::Response response; + return response; +} + +kortex_driver::ApplyEmergencyStop::Response KortexArmSimulation::ApplyEmergencyStop(const kortex_driver::ApplyEmergencyStop::Request& req) +{ + auto input = req.input; + kortex_driver::ApplyEmergencyStop::Response response; + return response; +} + void KortexArmSimulation::CreateDefaultActions() { kortex_driver::Action retract, home, zero; From 3f33b022b3f98d6a52db7fd56a532777d3d2f380 Mon Sep 17 00:00:00 2001 From: Alexandre Vannobel Date: Tue, 16 Jun 2020 16:31:34 -0400 Subject: [PATCH 30/33] Added implementation for other RPCs worth simulating --- .../non-generated/kortex_arm_simulation.h | 1 - .../driver/kortex_arm_driver.cpp | 3 +- .../driver/kortex_arm_simulation.cpp | 116 +++++++++++++----- 3 files changed, 86 insertions(+), 34 deletions(-) diff --git a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h index 50246f6c..0d0da555 100644 --- a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h +++ b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h @@ -67,7 +67,6 @@ class KortexArmSimulation // Other RPCs kortex_driver::PlayCartesianTrajectory::Response PlayCartesianTrajectory(const kortex_driver::PlayCartesianTrajectory::Request& req); kortex_driver::Stop::Response Stop(const kortex_driver::Stop::Request& req); - kortex_driver::GetMeasuredCartesianPose::Response GetMeasuredCartesianPose(const kortex_driver::GetMeasuredCartesianPose::Request& req); kortex_driver::SendTwistCommand::Response SendTwistCommand(const kortex_driver::SendTwistCommand::Request& req); kortex_driver::PlayJointTrajectory::Response PlayJointTrajectory(const kortex_driver::PlayJointTrajectory::Request& req); kortex_driver::SendJointSpeedsCommand::Response SendJointSpeedsCommand(const kortex_driver::SendJointSpeedsCommand::Request& req); diff --git a/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp b/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp index adc44072..c4801fa7 100644 --- a/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp +++ b/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp @@ -654,12 +654,11 @@ void KortexArmDriver::registerSimulationHandlers() base_services_simulation->StopActionHandler = std::bind(&KortexArmSimulation::StopAction, m_simulator.get(), std::placeholders::_1); // Other services base_services_simulation->PlayCartesianTrajectoryHandler = std::bind(&KortexArmSimulation::PlayCartesianTrajectory, m_simulator.get(), std::placeholders::_1); - base_services_simulation->StopHandler = std::bind(&KortexArmSimulation::Stop, m_simulator.get(), std::placeholders::_1); - base_services_simulation->GetMeasuredCartesianPoseHandler = std::bind(&KortexArmSimulation::GetMeasuredCartesianPose, m_simulator.get(), std::placeholders::_1); base_services_simulation->SendTwistCommandHandler = std::bind(&KortexArmSimulation::SendTwistCommand, m_simulator.get(), std::placeholders::_1); base_services_simulation->PlayJointTrajectoryHandler = std::bind(&KortexArmSimulation::PlayJointTrajectory, m_simulator.get(), std::placeholders::_1); base_services_simulation->SendJointSpeedsCommandHandler = std::bind(&KortexArmSimulation::SendJointSpeedsCommand, m_simulator.get(), std::placeholders::_1); base_services_simulation->SendGripperCommandHandler = std::bind(&KortexArmSimulation::SendGripperCommand, m_simulator.get(), std::placeholders::_1); + base_services_simulation->StopHandler = std::bind(&KortexArmSimulation::Stop, m_simulator.get(), std::placeholders::_1); base_services_simulation->ApplyEmergencyStopHandler = std::bind(&KortexArmSimulation::ApplyEmergencyStop, m_simulator.get(), std::placeholders::_1); // Prospects diff --git a/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp b/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp index bf6b38cf..383b9d2b 100644 --- a/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp +++ b/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp @@ -268,58 +268,112 @@ kortex_driver::StopAction::Response KortexArmSimulation::StopAction(const kortex kortex_driver::PlayCartesianTrajectory::Response KortexArmSimulation::PlayCartesianTrajectory(const kortex_driver::PlayCartesianTrajectory::Request& req) { - auto input = req.input; - kortex_driver::PlayCartesianTrajectory::Response response; - return response; -} - -kortex_driver::Stop::Response KortexArmSimulation::Stop(const kortex_driver::Stop::Request& req) -{ - auto input = req.input; - kortex_driver::Stop::Response response; - return response; -} + auto constrained_pose = req.input; + kortex_driver::Action action; + action.name = "PlayCartesianTrajectory"; + action.handle.action_type = kortex_driver::ActionType::REACH_POSE; + action.oneof_action_parameters.reach_pose.push_back(constrained_pose); -kortex_driver::GetMeasuredCartesianPose::Response KortexArmSimulation::GetMeasuredCartesianPose(const kortex_driver::GetMeasuredCartesianPose::Request& req) -{ - auto input = req.input; - kortex_driver::GetMeasuredCartesianPose::Response response; - return response; + // If an action is ongoing, cancel it first + if (m_is_action_being_executed.load()) + { + CancelAction(); // this will block until the thread is joined and current action finished + } + m_action_executor_thread = std::thread(&KortexArmSimulation::PlayAction, this, action); + + return kortex_driver::PlayCartesianTrajectory::Response(); } kortex_driver::SendTwistCommand::Response KortexArmSimulation::SendTwistCommand(const kortex_driver::SendTwistCommand::Request& req) { - auto input = req.input; - kortex_driver::SendTwistCommand::Response response; - return response; + auto twist_command = req.input; + kortex_driver::Action action; + action.name = "SendTwistCommand"; + action.handle.action_type = kortex_driver::ActionType::SEND_TWIST_COMMAND; + action.oneof_action_parameters.send_twist_command.push_back(twist_command); + + // If an action is ongoing, cancel it first + if (m_is_action_being_executed.load()) + { + CancelAction(); // this will block until the thread is joined and current action finished + } + m_action_executor_thread = std::thread(&KortexArmSimulation::PlayAction, this, action); + + return kortex_driver::SendTwistCommand::Response(); } kortex_driver::PlayJointTrajectory::Response KortexArmSimulation::PlayJointTrajectory(const kortex_driver::PlayJointTrajectory::Request& req) { - auto input = req.input; - kortex_driver::PlayJointTrajectory::Response response; - return response; + auto constrained_joint_angles = req.input; + kortex_driver::Action action; + action.name = "PlayJointTrajectory"; + action.handle.action_type = kortex_driver::ActionType::REACH_JOINT_ANGLES; + action.oneof_action_parameters.reach_joint_angles.push_back(constrained_joint_angles); + + // If an action is ongoing, cancel it first + if (m_is_action_being_executed.load()) + { + CancelAction(); // this will block until the thread is joined and current action finished + } + m_action_executor_thread = std::thread(&KortexArmSimulation::PlayAction, this, action); + + return kortex_driver::PlayJointTrajectory::Response(); } kortex_driver::SendJointSpeedsCommand::Response KortexArmSimulation::SendJointSpeedsCommand(const kortex_driver::SendJointSpeedsCommand::Request& req) { - auto input = req.input; - kortex_driver::SendJointSpeedsCommand::Response response; - return response; + auto joint_speeds = req.input; + kortex_driver::Action action; + action.name = "SendJointSpeedsCommand"; + action.handle.action_type = kortex_driver::ActionType::SEND_JOINT_SPEEDS; + action.oneof_action_parameters.send_joint_speeds.push_back(joint_speeds); + + // If an action is ongoing, cancel it first + if (m_is_action_being_executed.load()) + { + CancelAction(); // this will block until the thread is joined and current action finished + } + m_action_executor_thread = std::thread(&KortexArmSimulation::PlayAction, this, action); + + return kortex_driver::SendJointSpeedsCommand::Response(); } kortex_driver::SendGripperCommand::Response KortexArmSimulation::SendGripperCommand(const kortex_driver::SendGripperCommand::Request& req) { - auto input = req.input; - kortex_driver::SendGripperCommand::Response response; - return response; + auto gripper_command = req.input; + kortex_driver::Action action; + action.name = "GripperCommand"; + action.handle.action_type = kortex_driver::ActionType::SEND_GRIPPER_COMMAND; + action.oneof_action_parameters.send_gripper_command.push_back(gripper_command); + + // If an action is ongoing, cancel it first + if (m_is_action_being_executed.load()) + { + CancelAction(); // this will block until the thread is joined and current action finished + } + m_action_executor_thread = std::thread(&KortexArmSimulation::PlayAction, this, action); + + return kortex_driver::SendGripperCommand::Response(); +} + +kortex_driver::Stop::Response KortexArmSimulation::Stop(const kortex_driver::Stop::Request& req) +{ + // If an action is ongoing, cancel it first + if (m_is_action_being_executed.load()) + { + CancelAction(); // this will block until the thread is joined and current action finished + } + return kortex_driver::Stop::Response(); } kortex_driver::ApplyEmergencyStop::Response KortexArmSimulation::ApplyEmergencyStop(const kortex_driver::ApplyEmergencyStop::Request& req) { - auto input = req.input; - kortex_driver::ApplyEmergencyStop::Response response; - return response; + // If an action is ongoing, cancel it first + if (m_is_action_being_executed.load()) + { + CancelAction(); // this will block until the thread is joined and current action finished + } + return kortex_driver::ApplyEmergencyStop::Response(); } void KortexArmSimulation::CreateDefaultActions() From 6fd9104f403f8802dcdb1f541e096f891318a829 Mon Sep 17 00:00:00 2001 From: Alexandre Vannobel Date: Tue, 16 Jun 2020 16:46:52 -0400 Subject: [PATCH 31/33] Added some data structure checks and TODOs in executors --- .../driver/kortex_arm_simulation.cpp | 76 ++++++++++++++++++- 1 file changed, 75 insertions(+), 1 deletion(-) diff --git a/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp b/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp index 383b9d2b..2ac73bff 100644 --- a/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp +++ b/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp @@ -512,12 +512,30 @@ void KortexArmSimulation::PlayAction(const kortex_driver::Action& action) m_is_action_being_executed = false; } -// TODO Fill implementation kortex_driver::KortexError KortexArmSimulation::ExecuteReachJointAngles(const kortex_driver::Action& action) { kortex_driver::KortexError result; result.code = kortex_driver::ErrorCodes::ERROR_NONE; result.subCode = kortex_driver::SubErrorCodes::SUB_ERROR_NONE; + if (action.oneof_action_parameters.reach_joint_angles.size() != 1) + { + result.code = kortex_driver::ErrorCodes::ERROR_DEVICE; + result.subCode = kortex_driver::SubErrorCodes::INVALID_PARAM; + result.description = "Error playing joint angles action : action is malformed."; + return result; + } + auto constrained_joint_angles = action.oneof_action_parameters.reach_joint_angles[0]; + if (constrained_joint_angles.joint_angles.joint_angles.size() != GetDOF()) + { + result.code = kortex_driver::ErrorCodes::ERROR_DEVICE; + result.subCode = kortex_driver::SubErrorCodes::INVALID_PARAM; + result.description = "Error playing joint angles action : action contains " + std::to_string(constrained_joint_angles.joint_angles.joint_angles.size()) + " joint angles but arm has " + std::to_string(GetDOF()); + return result; + } + + // TODO Handle constraints and warn if some cannot be applied in simulation + // TODO Fill implementation to move simulated arm to angular position + return result; } @@ -527,6 +545,18 @@ kortex_driver::KortexError KortexArmSimulation::ExecuteReachPose(const kortex_dr kortex_driver::KortexError result; result.code = kortex_driver::ErrorCodes::ERROR_NONE; result.subCode = kortex_driver::SubErrorCodes::SUB_ERROR_NONE; + if (action.oneof_action_parameters.reach_pose.size() != 1) + { + result.code = kortex_driver::ErrorCodes::ERROR_DEVICE; + result.subCode = kortex_driver::SubErrorCodes::INVALID_PARAM; + result.description = "Error playing pose action : action is malformed."; + return result; + } + auto constrained_pose = action.oneof_action_parameters.reach_pose[0]; + + // TODO Handle constraints and warn if some cannot be applied in simulation + // TODO Fill implementation to move simulated arm to Cartesian pose + return result; } @@ -536,6 +566,25 @@ kortex_driver::KortexError KortexArmSimulation::ExecuteSendJointSpeeds(const kor kortex_driver::KortexError result; result.code = kortex_driver::ErrorCodes::ERROR_NONE; result.subCode = kortex_driver::SubErrorCodes::SUB_ERROR_NONE; + if (action.oneof_action_parameters.send_joint_speeds.size() != 1) + { + result.code = kortex_driver::ErrorCodes::ERROR_DEVICE; + result.subCode = kortex_driver::SubErrorCodes::INVALID_PARAM; + result.description = "Error playing joints speeds : action is malformed."; + return result; + } + auto joint_speeds = action.oneof_action_parameters.send_joint_speeds[0]; + if (joint_speeds.joint_speeds.size() != GetDOF()) + { + result.code = kortex_driver::ErrorCodes::ERROR_DEVICE; + result.subCode = kortex_driver::SubErrorCodes::INVALID_PARAM; + result.description = "Error playing joint speeds action : action contains " + std::to_string(constrained_joint_angles.joint_angles.joint_angles.size()) + " joint speeds but arm has " + std::to_string(GetDOF()); + return result; + } + + // TODO Handle constraints and warn if some cannot be applied in simulation + // TODO Fill implementation to move simulated arm at angular speeds + return result; } @@ -545,6 +594,18 @@ kortex_driver::KortexError KortexArmSimulation::ExecuteSendTwist(const kortex_dr kortex_driver::KortexError result; result.code = kortex_driver::ErrorCodes::ERROR_NONE; result.subCode = kortex_driver::SubErrorCodes::SUB_ERROR_NONE; + if (action.oneof_action_parameters.send_twist_command.size() != 1) + { + result.code = kortex_driver::ErrorCodes::ERROR_DEVICE; + result.subCode = kortex_driver::SubErrorCodes::INVALID_PARAM; + result.description = "Error playing twist action : action is malformed."; + return result; + } + auto twist = action.oneof_action_parameters.send_twist_command[0]; + + // TODO Handle constraints and warn if some cannot be applied in simulation + // TODO Fill implementation to move simulated arm at Cartesian twist + return result; } @@ -554,6 +615,19 @@ kortex_driver::KortexError KortexArmSimulation::ExecuteSendGripperCommand(const kortex_driver::KortexError result; result.code = kortex_driver::ErrorCodes::ERROR_NONE; result.subCode = kortex_driver::SubErrorCodes::SUB_ERROR_NONE; + if (action.oneof_action_parameters.send_gripper_command.size() != 1) + { + result.code = kortex_driver::ErrorCodes::ERROR_DEVICE; + result.subCode = kortex_driver::SubErrorCodes::INVALID_PARAM; + result.description = "Error playing gripper command action : action is malformed."; + return result; + } + auto gripper_command = action.oneof_action_parameters.send_gripper_command[0]; + + // TODO Handle constraints and warn if some cannot be applied in simulation + // TODO Handle velocity mode too? + // TODO Fill implementation to move simulated gripper to given position + return result; } From a78256f6b3e904ecfac7fecb39262c87b1d57e97 Mon Sep 17 00:00:00 2001 From: Alexandre Vannobel Date: Tue, 16 Jun 2020 17:09:18 -0400 Subject: [PATCH 32/33] Added tests for newly added RPCs --- .../non-generated/kortex_arm_simulation.h | 2 +- .../driver/kortex_arm_simulation.cpp | 2 +- .../tests/kortex_simulator_unit_tests.cc | 195 +++++++++++++++++- 3 files changed, 196 insertions(+), 3 deletions(-) diff --git a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h index 0d0da555..208a5b48 100644 --- a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h +++ b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h @@ -66,11 +66,11 @@ class KortexArmSimulation kortex_driver::StopAction::Response StopAction(const kortex_driver::StopAction::Request& req); // Other RPCs kortex_driver::PlayCartesianTrajectory::Response PlayCartesianTrajectory(const kortex_driver::PlayCartesianTrajectory::Request& req); - kortex_driver::Stop::Response Stop(const kortex_driver::Stop::Request& req); kortex_driver::SendTwistCommand::Response SendTwistCommand(const kortex_driver::SendTwistCommand::Request& req); kortex_driver::PlayJointTrajectory::Response PlayJointTrajectory(const kortex_driver::PlayJointTrajectory::Request& req); kortex_driver::SendJointSpeedsCommand::Response SendJointSpeedsCommand(const kortex_driver::SendJointSpeedsCommand::Request& req); kortex_driver::SendGripperCommand::Response SendGripperCommand(const kortex_driver::SendGripperCommand::Request& req); + kortex_driver::Stop::Response Stop(const kortex_driver::Stop::Request& req); kortex_driver::ApplyEmergencyStop::Response ApplyEmergencyStop(const kortex_driver::ApplyEmergencyStop::Request& req); private: diff --git a/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp b/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp index 2ac73bff..f0c6d47b 100644 --- a/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp +++ b/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp @@ -578,7 +578,7 @@ kortex_driver::KortexError KortexArmSimulation::ExecuteSendJointSpeeds(const kor { result.code = kortex_driver::ErrorCodes::ERROR_DEVICE; result.subCode = kortex_driver::SubErrorCodes::INVALID_PARAM; - result.description = "Error playing joint speeds action : action contains " + std::to_string(constrained_joint_angles.joint_angles.joint_angles.size()) + " joint speeds but arm has " + std::to_string(GetDOF()); + result.description = "Error playing joint speeds action : action contains " + std::to_string(joint_speeds.joint_speeds.size()) + " joint speeds but arm has " + std::to_string(GetDOF()); return result; } diff --git a/kortex_driver/src/non-generated/tests/kortex_simulator_unit_tests.cc b/kortex_driver/src/non-generated/tests/kortex_simulator_unit_tests.cc index 6ecf2a39..9b05c1fb 100644 --- a/kortex_driver/src/non-generated/tests/kortex_simulator_unit_tests.cc +++ b/kortex_driver/src/non-generated/tests/kortex_simulator_unit_tests.cc @@ -4,6 +4,8 @@ #include #include #include "kortex_driver/ActionEvent.h" +#include "kortex_driver/CartesianReferenceFrame.h" +#include "kortex_driver/GripperMode.h" class KortexSimulatorTest : public ::testing::Test { protected: @@ -312,4 +314,195 @@ TEST_F(KortexSimulatorTest, StopAction) std::this_thread::sleep_for(std::chrono::seconds(1)); ASSERT_EQ(m_received_notifications.size(), 2); ASSERT_EQ(m_received_notifications[1].action_event, kortex_driver::ActionEvent::ACTION_ABORT); -} \ No newline at end of file +} + +TEST_F(KortexSimulatorTest, PlayCartesianTrajectory) +{ + kortex_driver::PlayCartesianTrajectory::Request req; + kortex_driver::ConstrainedPose pose; + pose.target_pose.x = 0.1; + pose.target_pose.y = 0.1; + pose.target_pose.z = 0.1; + pose.target_pose.theta_x = 0.1; + pose.target_pose.theta_y = 0.1; + pose.target_pose.theta_z = 0.1; + req.input = pose; + + // Execute the action + ASSERT_EQ(m_received_notifications.size(), 0); + m_simulator->PlayCartesianTrajectory(req); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // TODO Change when implementation is filled + // Make sure after one second we received the ACTION_START and ACTION_END + ASSERT_EQ(m_received_notifications.size(), 2); + ASSERT_EQ(m_received_notifications[0].action_event, kortex_driver::ActionEvent::ACTION_START); + ASSERT_EQ(m_received_notifications[1].action_event, kortex_driver::ActionEvent::ACTION_END); +} + +TEST_F(KortexSimulatorTest, SendTwistCommand) +{ + kortex_driver::SendTwistCommand::Request req; + kortex_driver::TwistCommand twist_command; + twist_command.reference_frame = kortex_driver::CartesianReferenceFrame::CARTESIAN_REFERENCE_FRAME_BASE; + twist_command.twist.linear_x = 0.1; + twist_command.twist.linear_y = 0.1; + twist_command.twist.linear_z = 0.1; + twist_command.twist.angular_x = 0.1; + twist_command.twist.angular_y = 0.1; + twist_command.twist.angular_z = 0.1; + req.input = twist_command; + + // Execute the action + ASSERT_EQ(m_received_notifications.size(), 0); + m_simulator->SendTwistCommand(req); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // TODO Change when implementation is filled + // Make sure after one second we received the ACTION_START and ACTION_END + ASSERT_EQ(m_received_notifications.size(), 2); + ASSERT_EQ(m_received_notifications[0].action_event, kortex_driver::ActionEvent::ACTION_START); + ASSERT_EQ(m_received_notifications[1].action_event, kortex_driver::ActionEvent::ACTION_END); +} + +TEST_F(KortexSimulatorTest, PlayJointTrajectory) +{ + kortex_driver::PlayJointTrajectory::Request req; + kortex_driver::ConstrainedJointAngles constrained_joint_angles; + for (int i = 0; i < m_simulator->GetDOF(); i++) + { + kortex_driver::JointAngle angle; + angle.joint_identifier = i; + angle.value = i*10.0f; + constrained_joint_angles.joint_angles.joint_angles.push_back(angle); + } + req.input = constrained_joint_angles; + + // Execute the action + ASSERT_EQ(m_received_notifications.size(), 0); + m_simulator->PlayJointTrajectory(req); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // TODO Change when implementation is filled + // Make sure after one second we received the ACTION_START and ACTION_END + ASSERT_EQ(m_received_notifications.size(), 2); + ASSERT_EQ(m_received_notifications[0].action_event, kortex_driver::ActionEvent::ACTION_START); + ASSERT_EQ(m_received_notifications[1].action_event, kortex_driver::ActionEvent::ACTION_END); +} + +TEST_F(KortexSimulatorTest, SendJointSpeedsCommand) +{ + kortex_driver::SendJointSpeedsCommand::Request req; + kortex_driver::Base_JointSpeeds joint_speeds; + for (int i = 0; i < m_simulator->GetDOF(); i++) + { + kortex_driver::JointSpeed speed; + speed.joint_identifier = i; + speed.value = i*10.0f; + joint_speeds.joint_speeds.push_back(speed); + } + req.input = joint_speeds; + + // Execute the action + ASSERT_EQ(m_received_notifications.size(), 0); + m_simulator->SendJointSpeedsCommand(req); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // TODO Change when implementation is filled + // Make sure after one second we received the ACTION_START and ACTION_END + ASSERT_EQ(m_received_notifications.size(), 2); + ASSERT_EQ(m_received_notifications[0].action_event, kortex_driver::ActionEvent::ACTION_START); + ASSERT_EQ(m_received_notifications[1].action_event, kortex_driver::ActionEvent::ACTION_END); +} + +TEST_F(KortexSimulatorTest, SendGripperCommand) +{ + kortex_driver::SendGripperCommand::Request req; + kortex_driver::GripperCommand gripper_command; + gripper_command.mode = kortex_driver::GripperMode::GRIPPER_POSITION; + kortex_driver::Finger finger; + finger.finger_identifier = 0; + finger.value = 10.0f; + gripper_command.gripper.finger.push_back(finger); + req.input = gripper_command; + + // Execute the action + ASSERT_EQ(m_received_notifications.size(), 0); + m_simulator->SendGripperCommand(req); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // TODO Change when implementation is filled + // Make sure after one second we received the ACTION_START and ACTION_END + ASSERT_EQ(m_received_notifications.size(), 2); + ASSERT_EQ(m_received_notifications[0].action_event, kortex_driver::ActionEvent::ACTION_START); + ASSERT_EQ(m_received_notifications[1].action_event, kortex_driver::ActionEvent::ACTION_END); +} + +// Use a TIME_DELAY action to test the Stop RPC +TEST_F(KortexSimulatorTest, Stop) +{ + static constexpr uint32_t SLEEP_DURATION_SECONDS = 4; + + // Create delay action object + dummy_action.oneof_action_parameters.reach_joint_angles.clear(); + dummy_action.handle.action_type = kortex_driver::ActionType::TIME_DELAY; + kortex_driver::Delay delay; + delay.duration = SLEEP_DURATION_SECONDS; + dummy_action.oneof_action_parameters.delay.push_back(delay); + kortex_driver::ExecuteAction::Request req; + req.input = dummy_action; + + // Prepare Stop request + kortex_driver::Stop::Request stop_req; + + // Execute the action + ASSERT_EQ(m_received_notifications.size(), 0); + m_simulator->ExecuteAction(req); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // Make sure after one second we received the ACTION_START + ASSERT_EQ(m_received_notifications.size(), 1); + ASSERT_EQ(m_received_notifications[0].action_event, kortex_driver::ActionEvent::ACTION_START); + + // Abort the action now + m_simulator->Stop(stop_req); + + // Wait a biut and make sure we received the ACTION_ABORT + std::this_thread::sleep_for(std::chrono::seconds(1)); + ASSERT_EQ(m_received_notifications.size(), 2); + ASSERT_EQ(m_received_notifications[1].action_event, kortex_driver::ActionEvent::ACTION_ABORT); +} + +TEST_F(KortexSimulatorTest, ApplyEmergencyStop) +{ + static constexpr uint32_t SLEEP_DURATION_SECONDS = 4; + + // Create delay action object + dummy_action.oneof_action_parameters.reach_joint_angles.clear(); + dummy_action.handle.action_type = kortex_driver::ActionType::TIME_DELAY; + kortex_driver::Delay delay; + delay.duration = SLEEP_DURATION_SECONDS; + dummy_action.oneof_action_parameters.delay.push_back(delay); + kortex_driver::ExecuteAction::Request req; + req.input = dummy_action; + + // Prepare Stop request + kortex_driver::ApplyEmergencyStop::Request stop_req; + + // Execute the action + ASSERT_EQ(m_received_notifications.size(), 0); + m_simulator->ExecuteAction(req); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // Make sure after one second we received the ACTION_START + ASSERT_EQ(m_received_notifications.size(), 1); + ASSERT_EQ(m_received_notifications[0].action_event, kortex_driver::ActionEvent::ACTION_START); + + // Abort the action now + m_simulator->ApplyEmergencyStop(stop_req); + + // Wait a biut and make sure we received the ACTION_ABORT + std::this_thread::sleep_for(std::chrono::seconds(1)); + ASSERT_EQ(m_received_notifications.size(), 2); + ASSERT_EQ(m_received_notifications[1].action_event, kortex_driver::ActionEvent::ACTION_ABORT); +} From e490b556137af825a06a9028c4c277b1010f0307 Mon Sep 17 00:00:00 2001 From: Alexandre Vannobel Date: Wed, 17 Jun 2020 07:22:40 -0400 Subject: [PATCH 33/33] Remove merge artefacts --- .../include/kortex_driver/non-generated/kortex_arm_driver.h | 3 --- kortex_gazebo/launch/spawn_kortex_robot.launch | 3 --- 2 files changed, 6 deletions(-) diff --git a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h index 384bbe77..63126621 100644 --- a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h +++ b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h @@ -163,10 +163,7 @@ class KortexArmDriver bool isGripperPresent(); void setAngularTrajectorySoftLimitsToMax(); void publishRobotFeedback(); -<<<<<<< HEAD void registerSimulationHandlers(); -======= ->>>>>>> feature/improve-simulation }; #endif diff --git a/kortex_gazebo/launch/spawn_kortex_robot.launch b/kortex_gazebo/launch/spawn_kortex_robot.launch index b0e06189..44f4bf5b 100644 --- a/kortex_gazebo/launch/spawn_kortex_robot.launch +++ b/kortex_gazebo/launch/spawn_kortex_robot.launch @@ -138,10 +138,7 @@ -<<<<<<< HEAD -======= ->>>>>>> feature/improve-simulation