diff --git a/.gitignore b/.gitignore index 5778205..a9d2efb 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,4 @@ /build/ /install/ /log/ -/.vscode \ No newline at end of file +/.vscode diff --git a/LICENSES/license_notice.cpp b/LICENSES/license_notice.cpp index c45378b..1ecb658 100644 --- a/LICENSES/license_notice.cpp +++ b/LICENSES/license_notice.cpp @@ -4,12 +4,12 @@ // under the terms of the GNU General Public License as published by the Free // Software Foundation, either version 3 of the License, or (at your option) // any later version. -// +// // This program is distributed in the hope that it will be useful, but WITHOUT // ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or // FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for // more details. -// +// // You should have received a copy of the GNU General Public License along with // this program. If not, see . // -------------------------------------------------------------------------------- diff --git a/LICENSES/license_notice.py b/LICENSES/license_notice.py index d9979f3..311c495 100644 --- a/LICENSES/license_notice.py +++ b/LICENSES/license_notice.py @@ -4,12 +4,12 @@ # under the terms of the GNU General Public License as published by the Free # Software Foundation, either version 3 of the License, or (at your option) # any later version. -# +# # This program is distributed in the hope that it will be useful, but WITHOUT # ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or # FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for # more details. -# +# # You should have received a copy of the GNU General Public License along with # this program. If not, see . # -------------------------------------------------------------------------------- diff --git a/README.md b/README.md index 4579855..86d6dfe 100644 --- a/README.md +++ b/README.md @@ -45,7 +45,7 @@ sudo apt update -qq rosdep update rosdep install --from-paths ./ --ignore-src -y cd .. -colcon build --symlink-install --packages-select schunk_egu_egk_gripper_driver schunk_egu_egk_gripper_library schunk_egu_egk_gripper_interfaces +colcon build --symlink-install --packages-select schunk_egu_egk_gripper_driver schunk_egu_egk_gripper_library schunk_egu_egk_gripper_interfaces ``` ## Getting started @@ -130,7 +130,7 @@ All other services can be used whenever you like. (**Note:** Fast stop is an abo `reconnect` is the only method for altering the IP address during runtime. If nothing is connected to the IP address or a gripper is connected, it undergoes a change. If something else is linked to this IP, errors will occur, and the old address will be retained in such cases. Exercise caution when using this service! -With `parameter_get` and `parameter_set` you can read and set all allowed Parameter of the gripper. For getting and setting you need always the parameter instance. After that +With `parameter_get` and `parameter_set` you can read and set all allowed Parameter of the gripper. For getting and setting you need always the parameter instance. After that ## Parameters @@ -170,7 +170,7 @@ Open: - Plugins/Services/Service Caller: For calling services. - Plugins/Topic/Topic Monitor: For viewing all messages. -Additionally, you can refer to 'gripper_example.cpp' for guidance on using this driver in your code. To run the example, start 'schunk.launch.py' (or 'schunk_rqt_launch.py') and then execute the example: +Additionally, you can refer to 'gripper_example.cpp' for guidance on using this driver in your code. To run the example, start 'schunk.launch.py' (or 'schunk_rqt_launch.py') and then execute the example: **!!!WARNING!!! This will move the gripper jaws** ``` ros2 run schunk_gripper schunk_example diff --git a/schunk_egu_egk_gripper_driver/CMakeLists.txt b/schunk_egu_egk_gripper_driver/CMakeLists.txt index b9999ea..bf4d95c 100644 --- a/schunk_egu_egk_gripper_driver/CMakeLists.txt +++ b/schunk_egu_egk_gripper_driver/CMakeLists.txt @@ -50,7 +50,7 @@ ament_target_dependencies(${PROJECT_NAME} rclcpp_action rclcpp_components diagnostic_updater - diagnostic_msgs + diagnostic_msgs schunk_egu_egk_gripper_interfaces ) target_link_libraries(${PROJECT_NAME} @@ -82,4 +82,3 @@ endif() # Ament package configuration ament_package() - diff --git a/schunk_egu_egk_gripper_driver/include/schunk_egu_egk_gripper_driver/schunk_gripper_wrapper.hpp b/schunk_egu_egk_gripper_driver/include/schunk_egu_egk_gripper_driver/schunk_gripper_wrapper.hpp index 9c957a2..f6924fa 100644 --- a/schunk_egu_egk_gripper_driver/include/schunk_egu_egk_gripper_driver/schunk_gripper_wrapper.hpp +++ b/schunk_egu_egk_gripper_driver/include/schunk_egu_egk_gripper_driver/schunk_gripper_wrapper.hpp @@ -4,12 +4,12 @@ // under the terms of the GNU General Public License as published by the Free // Software Foundation, either version 3 of the License, or (at your option) // any later version. -// +// // This program is distributed in the hope that it will be useful, but WITHOUT // ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or // FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for // more details. -// +// // You should have received a copy of the GNU General Public License along with // this program. If not, see . // -------------------------------------------------------------------------------- @@ -56,7 +56,7 @@ extern std::map param_inst; extern std::map inst_param; class SchunkGripperNode : public rclcpp::Node, public Gripper -{ +{ private: std::mutex lock_service_post; @@ -86,7 +86,7 @@ class SchunkGripperNode : public rclcpp::Node, public Gripper //Flags bool doing_something; bool action_active; - bool wrong_version; + bool wrong_version; //Topic publishing void publishJointState(); void publishState(); @@ -102,7 +102,7 @@ class SchunkGripperNode : public rclcpp::Node, public Gripper //Diagnostic updater std::shared_ptr gripper_updater; - std::array old_diagnosis; + std::array old_diagnosis; std::string error_str; std::string warn_str; @@ -118,10 +118,10 @@ class SchunkGripperNode : public rclcpp::Node, public Gripper void callback_move_parameter(const rclcpp::Parameter &); double abs_pos_param; - std::string actual_command; + std::string actual_command; std::shared_ptr cycletime; rclcpp::Time last_time; - rclcpp::Rate limiting_rate; + rclcpp::Rate limiting_rate; //Basic Functions void publishStateMsg(); void finishedCommand(); @@ -137,7 +137,7 @@ class SchunkGripperNode : public rclcpp::Node, public Gripper rcl_interfaces::msg::ParameterDescriptor parameter_descriptor(const std::string&); rcl_interfaces::msg::FloatingPointRange FloatingPointRange(double, double, double step = 0.0); rcl_interfaces::msg::IntegerRange IntegerRange(int64_t, int64_t, uint64_t step = 0); - + //Services void acknowledge_srv(const std::shared_ptr, std::shared_ptr ); void brake_test_srv(const std::shared_ptr, std::shared_ptr ); diff --git a/schunk_egu_egk_gripper_driver/launch/schunk.launch.py b/schunk_egu_egk_gripper_driver/launch/schunk.launch.py index 39508ba..acd1cde 100644 --- a/schunk_egu_egk_gripper_driver/launch/schunk.launch.py +++ b/schunk_egu_egk_gripper_driver/launch/schunk.launch.py @@ -4,12 +4,12 @@ # under the terms of the GNU General Public License as published by the Free # Software Foundation, either version 3 of the License, or (at your option) # any later version. -# +# # This program is distributed in the hope that it will be useful, but WITHOUT # ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or # FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for # more details. -# +# # You should have received a copy of the GNU General Public License along with # this program. If not, see . # -------------------------------------------------------------------------------- @@ -21,34 +21,34 @@ def generate_launch_description(): container = Node( - name='gripper_container', - package='rclcpp_components', - executable='component_container_mt', - output='screen', + name="gripper_container", + package="rclcpp_components", + executable="component_container_mt", + output="screen", emulate_tty=True, ) - + load_composable_nodes = LoadComposableNodes( - target_container='gripper_container', + target_container="gripper_container", composable_node_descriptions=[ - ComposableNode( - package='schunk_egu_egk_gripper_driver', - plugin='SchunkGripperNode', - name='schunk_gripper_driver', - namespace='EGK_50_M_B', - parameters=[ - {'IP': '10.49.60.86'}, - {'state_frq': 60.0}, - {'rate': 10.0}, - {'use_brk': False}, - {'grp_pos_margin': 2.0}, - {'grp_prepos_delta': 5.0}, - {'zero_pos_ofs': 0.0}, - {'grp_prehold_time': 0}, - {'wp_release_delta': 5.0}, - {'wp_lost_distance': 1.0}, - ] - ) - ], + ComposableNode( + package="schunk_egu_egk_gripper_driver", + plugin="SchunkGripperNode", + name="schunk_gripper_driver", + namespace="EGK_50_M_B", + parameters=[ + {"IP": "10.49.60.86"}, + {"state_frq": 60.0}, + {"rate": 10.0}, + {"use_brk": False}, + {"grp_pos_margin": 2.0}, + {"grp_prepos_delta": 5.0}, + {"zero_pos_ofs": 0.0}, + {"grp_prehold_time": 0}, + {"wp_release_delta": 5.0}, + {"wp_lost_distance": 1.0}, + ], + ) + ], ) return launch.LaunchDescription([container, load_composable_nodes]) diff --git a/schunk_egu_egk_gripper_driver/launch/schunk_rqt.launch.py b/schunk_egu_egk_gripper_driver/launch/schunk_rqt.launch.py index f128877..8e08b16 100644 --- a/schunk_egu_egk_gripper_driver/launch/schunk_rqt.launch.py +++ b/schunk_egu_egk_gripper_driver/launch/schunk_rqt.launch.py @@ -4,12 +4,12 @@ # under the terms of the GNU General Public License as published by the Free # Software Foundation, either version 3 of the License, or (at your option) # any later version. -# +# # This program is distributed in the hope that it will be useful, but WITHOUT # ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or # FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for # more details. -# +# # You should have received a copy of the GNU General Public License along with # this program. If not, see . # -------------------------------------------------------------------------------- @@ -21,39 +21,35 @@ def generate_launch_description(): container = Node( - name='gripper_container', - package='rclcpp_components', - executable='component_container_mt', - output='screen', + name="gripper_container", + package="rclcpp_components", + executable="component_container_mt", + output="screen", emulate_tty=True, ) - rqt_launch = Node( - package='rqt_gui', - executable='rqt_gui', - name='rqt_gui' - ) + rqt_launch = Node(package="rqt_gui", executable="rqt_gui", name="rqt_gui") load_composable_nodes = LoadComposableNodes( - target_container='gripper_container', + target_container="gripper_container", composable_node_descriptions=[ - ComposableNode( - package='schunk_gripper', - plugin='SchunkGripperNode', - name='schunk_gripper_driver', - namespace='EGK_50_M_B', - parameters=[ - {'state_frq': 10.0}, - {'rate': 10.0}, - {'IP': '10.49.60.86'}, - {'Gripper_Parameter.use_brk': False}, - {'Gripper_Parameter.grp_pos_margin': 2.0}, - {'Gripper_Parameter.grp_prepos_delta': 5.0}, - {'Gripper_Parameter.zero_pos_ofs': 0.0}, - {'Gripper_Parameter.grp_prehold_time': 0}, - {'Gripper_Parameter.wp_release_delta': 5.0}, - {'Gripper_Parameter.wp_lost_distance': 1.0} - ], - ) - ], + ComposableNode( + package="schunk_gripper", + plugin="SchunkGripperNode", + name="schunk_gripper_driver", + namespace="EGK_50_M_B", + parameters=[ + {"state_frq": 10.0}, + {"rate": 10.0}, + {"IP": "10.49.60.86"}, + {"Gripper_Parameter.use_brk": False}, + {"Gripper_Parameter.grp_pos_margin": 2.0}, + {"Gripper_Parameter.grp_prepos_delta": 5.0}, + {"Gripper_Parameter.zero_pos_ofs": 0.0}, + {"Gripper_Parameter.grp_prehold_time": 0}, + {"Gripper_Parameter.wp_release_delta": 5.0}, + {"Gripper_Parameter.wp_lost_distance": 1.0}, + ], + ) + ], ) return launch.LaunchDescription([container, load_composable_nodes, rqt_launch]) diff --git a/schunk_egu_egk_gripper_driver/src/schunk_gripper_wrapper.cpp b/schunk_egu_egk_gripper_driver/src/schunk_gripper_wrapper.cpp index 9b51786..5f4f68e 100644 --- a/schunk_egu_egk_gripper_driver/src/schunk_gripper_wrapper.cpp +++ b/schunk_egu_egk_gripper_driver/src/schunk_gripper_wrapper.cpp @@ -4,12 +4,12 @@ // under the terms of the GNU General Public License as published by the Free // Software Foundation, either version 3 of the License, or (at your option) // any later version. -// +// // This program is distributed in the hope that it will be useful, but WITHOUT // ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or // FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for // more details. -// +// // You should have received a copy of the GNU General Public License along with // this program. If not, see . // -------------------------------------------------------------------------------- @@ -21,18 +21,18 @@ #include "schunk_egu_egk_gripper_driver/schunk_gripper_wrapper.hpp" - std::map param_inst = + std::map param_inst = { - { "Gripper_Parameter.grp_pos_margin", GRP_POS_MARGIN_INST}, - { "Gripper_Parameter.grp_prepos_delta", GRP_PREPOS_DELTA_INST }, + { "Gripper_Parameter.grp_pos_margin", GRP_POS_MARGIN_INST}, + { "Gripper_Parameter.grp_prepos_delta", GRP_PREPOS_DELTA_INST }, { "Gripper_Parameter.zero_pos_ofs", ZERO_POS_OFS_INST }, { "Gripper_Parameter.grp_prehold_time", GRP_PREHOLD_TIME_INST }, { "Gripper_Parameter.wp_release_delta", WP_RELEASE_DELTA_INST }, { "Gripper_Parameter.wp_lost_dst", WP_LOST_DISTANCE_INST }, }; -std::map inst_param = -{ +std::map inst_param = +{ {GRP_POS_MARGIN_INST, "Gripper_Parameter.grp_pos_margin"}, {GRP_PREPOS_DELTA_INST, "Gripper_Parameter.grp_prepos_delta"}, {ZERO_POS_OFS_INST, "Gripper_Parameter.zero_pos_ofs"}, @@ -42,18 +42,18 @@ std::map inst_param = }; //Initialize the ROS Driver -SchunkGripperNode::SchunkGripperNode(const rclcpp::NodeOptions &options) : +SchunkGripperNode::SchunkGripperNode(const rclcpp::NodeOptions &options) : rclcpp::Node("schunk_gripper_driver", options), Gripper(this->declare_parameter("IP", "0.0.0.0", parameter_descriptor("IP-Address of the gripper"))), limiting_rate(1000) //limiting_rate for loops -{ +{ //Callback groups messages_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); services_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); // actions_group = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); // rest = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - + rclcpp::PublisherOptions option_messages; option_messages.callback_group = messages_group; @@ -62,7 +62,7 @@ SchunkGripperNode::SchunkGripperNode(const rclcpp::NodeOptions &options) : //ParameterDescription read_only rcl_interfaces::msg::ParameterDescriptor paramDesc; paramDesc.read_only = true; - + paramDesc.description = "Frequency of state"; state_frq = this->declare_parameter("state_frq", 60.0, paramDesc); @@ -71,11 +71,11 @@ SchunkGripperNode::SchunkGripperNode(const rclcpp::NodeOptions &options) : //Cycletime cycletime = std::make_shared(std::chrono::milliseconds(static_cast(1/state_frq)*1000)); - + //Set flags doing_something = false; action_active = false; - + //Set Parametereventhandler parameter_event_handler = std::make_shared(this); //Actually no Command @@ -95,13 +95,13 @@ SchunkGripperNode::SchunkGripperNode(const rclcpp::NodeOptions &options) : callback_gripper_param = std::bind(&SchunkGripperNode::callback_gripper_parameter,this,std::placeholders::_1); callback_move_param = std::bind(&SchunkGripperNode::callback_move_parameter,this ,std::placeholders::_1); - if(start_connection == true) + if(start_connection == true) { advertiseConnectionRelevant(); ip_changed_with_all_param = true; connection_error = "OK"; } - else + else { ip_changed_with_all_param = false; connection_error = ""; @@ -117,7 +117,7 @@ SchunkGripperNode::SchunkGripperNode(const rclcpp::NodeOptions &options) : //State msg update void SchunkGripperNode::publishStateMsg() { - msg.command_received_toggle = gripperBitInput(COMMAND_RECEIVED_TOGGLE); + msg.command_received_toggle = gripperBitInput(COMMAND_RECEIVED_TOGGLE); msg.doing_command = actual_command; //It is the only msg...,that is not updated through "runGets()" //It is asynchronous with the others @@ -161,42 +161,42 @@ void SchunkGripperNode::advertiseServices() parameter_set_service= this->create_service("parameter_set", std::bind(&SchunkGripperNode::parameter_set_srv,this,std::placeholders::_1,std::placeholders::_2), rmw_qos_profile_services_default, services_group); info_service = this->create_service("gripper_info", std::bind(&SchunkGripperNode::info_srv,this,std::placeholders::_1,std::placeholders::_2), rmw_qos_profile_services_default, rest); change_ip_service = this->create_service("reconnect", std::bind(&SchunkGripperNode::change_ip_srv,this,std::placeholders::_1,std::placeholders::_2), rmw_qos_profile_services_default, services_group); - + } //Advertise Actions void SchunkGripperNode::advertiseActions() { - //Create Actions + //Create Actions move_abs_server = rclcpp_action::create_server(this, "move_to_absolute_position", std::bind(&SchunkGripperNode::handle_goal, this, std::placeholders::_1, std::placeholders::_2), std::bind(&SchunkGripperNode::handle_cancel, this, std::placeholders::_1), std::bind(&SchunkGripperNode::handle_accepted_abs, this, std::placeholders::_1),rcl_action_server_get_default_options(), services_group); - + move_rel_server = rclcpp_action::create_server(this, "move_to_relative_position", std::bind(&SchunkGripperNode::handle_goal, this, std::placeholders::_1, std::placeholders::_2), std::bind(&SchunkGripperNode::handle_cancel, this, std::placeholders::_1), - std::bind(&SchunkGripperNode::handle_accepted_rel, this, std::placeholders::_1),rcl_action_server_get_default_options(), services_group); - + std::bind(&SchunkGripperNode::handle_accepted_rel, this, std::placeholders::_1),rcl_action_server_get_default_options(), services_group); + release_wp_server = rclcpp_action::create_server(this, "release_workpiece", std::bind(&SchunkGripperNode::handle_goal, this, std::placeholders::_1, std::placeholders::_2), std::bind(&SchunkGripperNode::handle_cancel, this, std::placeholders::_1), std::bind(&SchunkGripperNode::handle_accepted_release, this, std::placeholders::_1),rcl_action_server_get_default_options(), services_group); - + control_server = rclcpp_action::create_server(this, "gripper_control", std::bind(&SchunkGripperNode::handle_goal, this, std::placeholders::_1, std::placeholders::_2), std::bind(&SchunkGripperNode::handle_cancel, this, std::placeholders::_1), std::bind(&SchunkGripperNode::handle_accepted_control, this, std::placeholders::_1),rcl_action_server_get_default_options(), services_group); - + } //Advertise topics void SchunkGripperNode::advertiseTopics() { //Advertise state - last_time = this->now(); //Initialize last_time + last_time = this->now(); //Initialize last_time statePublisher = this->create_publisher("state", 1); publish_state_timer=this->create_wall_timer(std::chrono::duration(1 / state_frq), std::bind(&SchunkGripperNode::publishState, this), messages_group); - + //Initialize diagnostics gripper_updater = std::make_shared(this); gripper_updater->setHardwareID("Module"); gripper_updater->add(model, std::bind(&SchunkGripperNode::gripperDiagnostics,this,std::placeholders::_1)); - + //Look if joint_state_frq is less than state_frq if(j_state_frq > state_frq) { @@ -209,7 +209,7 @@ void SchunkGripperNode::advertiseTopics() } //Advertise all that needs to be read out the gripper void SchunkGripperNode::advertiseConnectionRelevant() -{ +{ checkVersions(); //Needed Firmware if(sw_version >= 502) brake_test_service = this->create_service("brake_test", std::bind(&SchunkGripperNode::brake_test_srv, this,std::placeholders::_1,std::placeholders::_2), rmw_qos_profile_services_default, services_group); @@ -219,18 +219,18 @@ void SchunkGripperNode::advertiseConnectionRelevant() grip_server = rclcpp_action::create_server(this, "grip", std::bind(&SchunkGripperNode::handle_goal, this, std::placeholders::_1, std::placeholders::_2), std::bind(&SchunkGripperNode::handle_cancel, this, std::placeholders::_1), std::bind(&SchunkGripperNode::handle_accepted_grip_egk, this, std::placeholders::_1),rcl_action_server_get_default_options(), services_group); - + grip_w_pos_server = rclcpp_action::create_server(this, "grip_with_position", std::bind(&SchunkGripperNode::handle_goal, this, std::placeholders::_1, std::placeholders::_2), std::bind(&SchunkGripperNode::handle_cancel, this, std::placeholders::_1), std::bind(&SchunkGripperNode::handle_accepted_gripPos_egk, this, std::placeholders::_1),rcl_action_server_get_default_options(), services_group); - + } else if(model.find("EGU") != std::string::npos) { grip_egu_server = rclcpp_action::create_server(this, "grip", std::bind(&SchunkGripperNode::handle_goal, this, std::placeholders::_1, std::placeholders::_2), std::bind(&SchunkGripperNode::handle_cancel, this, std::placeholders::_1), std::bind(&SchunkGripperNode::handle_accepted_grip_egu, this, std::placeholders::_1),rcl_action_server_get_default_options(), services_group); - + grip_w_pos_egu_server = rclcpp_action::create_server(this, "grip_with_position", std::bind(&SchunkGripperNode::handle_goal, this, std::placeholders::_1, std::placeholders::_2), std::bind(&SchunkGripperNode::handle_cancel, this, std::placeholders::_1), std::bind(&SchunkGripperNode::handle_accepted_gripPos_egu, this, std::placeholders::_1),rcl_action_server_get_default_options(), services_group); @@ -264,13 +264,13 @@ void SchunkGripperNode::advertiseConnectionRelevant() void SchunkGripperNode::checkVersions() { comm_version_double = std::stod(comm_version.substr(0, sizeof(MIN_SUPPORTED_COMMUNICATION_VERSION) - 1)); //minus termination null - if( MIN_SUPPORTED_COMMUNICATION_VERSION > comm_version_double - || MAX_SUPPORTED_COMMUNICATION_VERSION < comm_version_double - || MIN_SUPPORTED_FIRMWARE_VERSION > sw_version - || MAX_SUPPORTED_FIRMWARE_VERSION < sw_version) //Depends on + if( MIN_SUPPORTED_COMMUNICATION_VERSION > comm_version_double + || MAX_SUPPORTED_COMMUNICATION_VERSION < comm_version_double + || MIN_SUPPORTED_FIRMWARE_VERSION > sw_version + || MAX_SUPPORTED_FIRMWARE_VERSION < sw_version) //Depends on { RCLCPP_WARN_STREAM(this->get_logger(), "Gripper Versions not supported.\n" << "Used communictation software version: " << comm_version_double - << "\nNeeded: " << MIN_SUPPORTED_COMMUNICATION_VERSION << " to " << MAX_SUPPORTED_COMMUNICATION_VERSION << "\nUsed software version: " << sw_version + << "\nNeeded: " << MIN_SUPPORTED_COMMUNICATION_VERSION << " to " << MAX_SUPPORTED_COMMUNICATION_VERSION << "\nUsed software version: " << sw_version << "\nNeeded: " << MIN_SUPPORTED_FIRMWARE_VERSION << " to " << MAX_SUPPORTED_FIRMWARE_VERSION << std::endl); @@ -283,7 +283,7 @@ void SchunkGripperNode::checkVersions() } //Declares Parameter void SchunkGripperNode::declareParameter() -{ +{ // Gripper Parameter this->declare_parameter("Gripper_Parameter.use_brk", false, parameter_descriptor("Use brake")); this->declare_parameter("Gripper_Parameter.grp_pos_margin", 2.0, parameter_descriptor("Grip position margin in mm", FloatingPointRange(1.0, 10.0, 0.01))); @@ -299,7 +299,7 @@ void SchunkGripperNode::declareParameter() this->declare_parameter("Control_Parameter.release_workpiece", false, parameter_descriptor("Release Workpiece")); } // Helper function to create a FloatingPointRange -rcl_interfaces::msg::FloatingPointRange SchunkGripperNode::FloatingPointRange(double from, double to, double step) +rcl_interfaces::msg::FloatingPointRange SchunkGripperNode::FloatingPointRange(double from, double to, double step) { rcl_interfaces::msg::FloatingPointRange range; range.from_value = from; @@ -309,7 +309,7 @@ rcl_interfaces::msg::FloatingPointRange SchunkGripperNode::FloatingPointRange(do return range; } // Helper function to create an IntegerRange -rcl_interfaces::msg::IntegerRange SchunkGripperNode::IntegerRange(int64_t from, int64_t to, uint64_t step) +rcl_interfaces::msg::IntegerRange SchunkGripperNode::IntegerRange(int64_t from, int64_t to, uint64_t step) { rcl_interfaces::msg::IntegerRange range; range.from_value = from; @@ -319,7 +319,7 @@ rcl_interfaces::msg::IntegerRange SchunkGripperNode::IntegerRange(int64_t from, return range; } // Overloaded helper function to set parameter descriptor with range -rcl_interfaces::msg::ParameterDescriptor SchunkGripperNode::parameter_descriptor(const std::string& description, const rcl_interfaces::msg::FloatingPointRange& range) +rcl_interfaces::msg::ParameterDescriptor SchunkGripperNode::parameter_descriptor(const std::string& description, const rcl_interfaces::msg::FloatingPointRange& range) { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = description; @@ -329,7 +329,7 @@ rcl_interfaces::msg::ParameterDescriptor SchunkGripperNode::parameter_descriptor return descriptor; } -rcl_interfaces::msg::ParameterDescriptor SchunkGripperNode::parameter_descriptor(const std::string& description, const rcl_interfaces::msg::IntegerRange& range) +rcl_interfaces::msg::ParameterDescriptor SchunkGripperNode::parameter_descriptor(const std::string& description, const rcl_interfaces::msg::IntegerRange& range) { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; @@ -338,7 +338,7 @@ rcl_interfaces::msg::ParameterDescriptor SchunkGripperNode::parameter_descriptor return descriptor; } -rcl_interfaces::msg::ParameterDescriptor SchunkGripperNode::parameter_descriptor(const std::string& description) +rcl_interfaces::msg::ParameterDescriptor SchunkGripperNode::parameter_descriptor(const std::string& description) { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.dynamic_typing = true; @@ -359,33 +359,33 @@ rclcpp_action::GoalResponse SchunkGripperNode::handle_goal(const rclcpp_action:: (void)uuid; (void)goal; - if(gripperBitInput(GRIPPER_ERROR)) + if(gripperBitInput(GRIPPER_ERROR)) { RCLCPP_ERROR(this->get_logger(), "Action will not be performed as long an error is active"); } - else if(doing_something == false) + else if(doing_something == false) { action_active = true; doing_something = true; return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } - else + else { std::unique_lock lock(lock_mutex, std::defer_lock); try { //send fast stop std::unique_lock lock_service(lock_service_post); - + set_command = FAST_STOP; - - + + sendService(lock); lock_service.unlock(); //if command received, get values - if(handshake != gripperBitInput(COMMAND_RECEIVED_TOGGLE)) + if(handshake != gripperBitInput(COMMAND_RECEIVED_TOGGLE)) while (rclcpp::ok() && check()) - runGets(); + runGets(); gripper_updater->force_update(); doing_something = false; @@ -404,7 +404,7 @@ rclcpp_action::GoalResponse SchunkGripperNode::handle_goal(const rclcpp_action:: //Deal with gripper, if the Command where successful done. Else throw an Exception template void SchunkGripperNode::setFinalState(std::shared_ptr res, const std::shared_ptr> goal_handle) -{ +{ if(gripperBitOutput(STOP)) { finishedCommand(); @@ -414,7 +414,7 @@ void SchunkGripperNode::setFinalState(std::shared_ptr res, const std::s //Gripper succeeded or Workpiece lost... else if(gripperBitInput(SUCCESS) ||gripperBitInput(NO_WORKPIECE_DETECTED) - ||gripperBitInput(WORK_PIECE_LOST) + ||gripperBitInput(WORK_PIECE_LOST) ||gripperBitInput(WRONG_WORKPIECE_DETECTED)) { finishedCommand(); @@ -423,37 +423,37 @@ void SchunkGripperNode::setFinalState(std::shared_ptr res, const std::s } //Gripper was fast stopped else if(!gripperBitOutput(FAST_STOP)) - { + { finishedCommand(); RCLCPP_WARN(this->get_logger(),"Fast stopped. Reason could be another goal or fast stop service."); goal_handle->abort(res); return; } //Cancel or new goal arrived - else if(goal_handle->is_canceling()) + else if(goal_handle->is_canceling()) { RCLCPP_ERROR(this->get_logger(),"Cancel Request received. Fast stop."); - + std::unique_lock lock(lock_mutex, std::defer_lock); std::unique_lock lock_service(lock_service_post); set_command = FAST_STOP; - - + + sendService(lock); lock_service.unlock(); - - if(handshake != gripperBitInput(COMMAND_RECEIVED_TOGGLE)) + + if(handshake != gripperBitInput(COMMAND_RECEIVED_TOGGLE)) while (rclcpp::ok() && check()) runGets(); - + gripper_updater->force_update(); finishedCommand(); - + goal_handle->canceled(res); return; } - else + else { throw static_cast(plc_sync_input[3]); } @@ -465,7 +465,7 @@ void SchunkGripperNode::exceptionHandling(const std::shared_ptrforce_update(); //If gripper did not receive command else if(i == -1) - { + { RCLCPP_ERROR(this->get_logger(),"GRIPPER DID NOT RECEIVE THE COMMAND!"); //ToDo Feedback to robot } else RCLCPP_WARN(this->get_logger(),"Action aborted!"); @@ -473,11 +473,11 @@ void SchunkGripperNode::exceptionHandling(const std::shared_ptrabort(res); return; -} +} //Action publishing its feedback as long the Gripper is moving template void SchunkGripperNode::runActionMove(std::shared_ptr feedback, std::shared_ptr> goal_handle) -{ +{ start_time = this->now(); while(!endCondition() && rclcpp::ok() && (!goal_handle->is_canceling()) && connection_error == "OK") @@ -510,7 +510,7 @@ void SchunkGripperNode::runActionMove(std::shared_ptr feedback, st //Action publishing feedback as long the gripper is moving template void SchunkGripperNode::runActionGrip(std::shared_ptr feedback, std::shared_ptr> goal_handle) -{ +{ start_time = this->now(); while(!endCondition() && rclcpp::ok() && (!goal_handle->is_canceling()) && connection_error == "OK") @@ -526,7 +526,7 @@ void SchunkGripperNode::runActionGrip(std::shared_ptr feedback, st RCLCPP_INFO(this->get_logger(),"PRE-HOLDING: %i ms", grp_prehold_time); feedback->workpiece_pre_grip_started = gripperBitInput(PRE_HOLDING); } - + goal_handle->publish_feedback(feedback); start_time = this->now(); } @@ -559,17 +559,17 @@ void SchunkGripperNode::moveAbsExecute(const std::shared_ptr(); try -{ +{ std::unique_lock lock_service(lock_service_post); - + set_position = mm2mu(goal->absolute_position); set_speed = mm2mu(goal->velocity_of_movement); set_command = commands_str.at(actual_command); - - + + sendAction(); lock_service.unlock(); - + runActionMove(feedback, goal_handle); //set result res->absolute_position = actual_position; @@ -595,7 +595,7 @@ catch(const char* response) } //Move relative action callback void SchunkGripperNode::moveRelExecute(const std::shared_ptr> goal_handle) -{ +{ actual_command = "MOVE TO RELATIVE POSITION"; const auto goal = goal_handle->get_goal(); @@ -604,14 +604,14 @@ void SchunkGripperNode::moveRelExecute(const std::shared_ptr(); try -{ +{ std::unique_lock lock_service(lock_service_post); - + set_position = mm2mu(goal->signed_relative_position); set_speed = mm2mu(goal->velocity_of_movement); set_command = commands_str.at(actual_command); - - + + sendAction(); lock_service.unlock(); @@ -652,7 +652,7 @@ void SchunkGripperNode::gripExecute(const std::shared_ptr lock_service(lock_service_post); - + feedback->workpiece_pre_grip_started = false; auto goal = goal_handle->get_goal(); @@ -664,7 +664,7 @@ try if(gripperBitOutput(GRIP_DIRECTION) != goal->grip_direction) set_command |= GRIP_DIRECTION; - + sendAction(); lock_service.unlock(); @@ -716,7 +716,7 @@ try if(gripperBitOutput(GRIP_DIRECTION) != goal->grip_direction) set_command |= GRIP_DIRECTION; - + sendAction(); lock_service.unlock(); @@ -748,18 +748,18 @@ catch(const char* response) } //Grip workpiece with position (EGK) action callback void SchunkGripperNode::gripWithPositionExecute(const std::shared_ptr> goal_handle) -{ +{ actual_command = "GRIP WORKPIECE WITH POSITION"; auto feedback = std::make_shared(); auto res = std::make_shared(); - + auto goal = goal_handle->get_goal(); try -{ +{ std::unique_lock lock_service(lock_service_post); - + set_position = mm2mu(goal->absolute_position); set_speed = mm2mu(goal->velocity_of_movement); set_gripping_force = static_cast(goal->gripping_force); @@ -768,7 +768,7 @@ try if(gripperBitOutput(GRIP_DIRECTION) != goal->grip_direction) set_command |= GRIP_DIRECTION; - + sendAction(); lock_service.unlock(); @@ -801,18 +801,18 @@ catch(const char* response) } //Grip with position (EGU) action callback void SchunkGripperNode::gripWithPosition_eguExecute(const std::shared_ptr> goal_handle) -{ +{ actual_command = "GRIP WORKPIECE WITH POSITION"; auto feedback = std::make_shared(); auto res = std::make_shared(); - + auto goal = goal_handle->get_goal(); try -{ +{ std::unique_lock lock_service(lock_service_post); - + set_position = mm2mu(goal->absolute_position); set_gripping_force = static_cast(goal->gripping_force); set_command = GRIP_WORKPIECE_WITH_POSITION; @@ -820,7 +820,7 @@ try if(gripperBitOutput(GRIP_DIRECTION) != goal->grip_direction) set_command |= GRIP_DIRECTION; - + sendAction(); lock_service.unlock(); @@ -853,19 +853,19 @@ catch(const char* response) } //release workpiece action callback void SchunkGripperNode::releaseExecute(const std::shared_ptr> goal_handle) -{ +{ actual_command = "RELEASE WORKPIECE"; auto feedback = std::make_shared(); auto res = std::make_shared(); try -{ +{ std::unique_lock lock_service(lock_service_post); - + set_command = commands_str.at(actual_command); - - + + sendAction(); lock_service.unlock(); @@ -892,7 +892,7 @@ catch(const char* response) } //Grip: control_msgs void SchunkGripperNode::controlExecute(const std::shared_ptr> goal_handle) -{ +{ auto feedback = std::make_shared(); auto res = std::make_shared(); auto goal = goal_handle->get_goal(); @@ -900,7 +900,7 @@ void SchunkGripperNode::controlExecute(const std::shared_ptr lock_service(lock_service_post); - + feedback->stalled = false; feedback->reached_goal = false; @@ -912,7 +912,7 @@ try //position is zero and gripping_force is not zero if(set_position == 0 && set_gripping_force != 0) actual_command = "GRIP WORKPIECE"; //gripping_force is zero - else if(set_gripping_force == 0) + else if(set_gripping_force == 0) { actual_command = "MOVE TO ABSOLUTE POSITION"; set_speed = mm2mu(max_vel/2); @@ -925,7 +925,7 @@ try if(gripperBitOutput(GRIP_DIRECTION) != 0 && set_command != MOVE_TO_ABSOLUTE_POSITION) set_command |= GRIP_DIRECTION; - + sendAction(); lock_service.unlock(); @@ -996,11 +996,11 @@ catch(const char* response) } //Updates final successful state and zero position offset void SchunkGripperNode::finishedCommand() -{ - if(connection_error != "OK") - { +{ + if(connection_error != "OK") + { doing_something = false; - action_active = false; + action_active = false; actual_command = "NO COMMAND"; return; } @@ -1008,7 +1008,7 @@ void SchunkGripperNode::finishedCommand() std::vector params = {rclcpp::Parameter("Control_Parameter.release_workpiece", false), rclcpp::Parameter("Control_Parameter.grip", false), rclcpp::Parameter("Control_Parameter.move_gripper", actualPosInterval())}; - + abs_pos_param = actualPosInterval(); this->set_parameters(params); @@ -1017,21 +1017,21 @@ void SchunkGripperNode::finishedCommand() { RCLCPP_WARN(this->get_logger(),"No workpiece detected!"); } - if(gripperBitInput(WRONG_WORKPIECE_DETECTED)) + if(gripperBitInput(WRONG_WORKPIECE_DETECTED)) { RCLCPP_WARN(this->get_logger(),"Wrong workpiece detected!"); } //Was Command successfully processed? - if(gripperBitInput(SUCCESS) && gripperBitInput(POSITION_REACHED) && doing_something == true) + if(gripperBitInput(SUCCESS) && gripperBitInput(POSITION_REACHED) && doing_something == true) { RCLCPP_INFO(this->get_logger(),"%s SUCCEEDED", actual_command.c_str()); } - if(gripperBitInput(GRIPPED)) + if(gripperBitInput(GRIPPED)) { RCLCPP_WARN(this->get_logger(),"Gripped Workpiece!"); } doing_something = false; - action_active = false; + action_active = false; actual_command = "NO COMMAND"; @@ -1049,7 +1049,7 @@ void SchunkGripperNode::publishState() try { runGets(); - + if(!(connection_error == "OK")) //If after loss of connection reconnected { reconnect(); @@ -1058,7 +1058,7 @@ void SchunkGripperNode::publishState() lock.unlock(); } catch(const char *res) - { + { lock.unlock(); connection_error = res; rclcpp::Duration sleep_time(1,0); @@ -1087,7 +1087,7 @@ void SchunkGripperNode::publishState() //Reconnect void SchunkGripperNode::reconnect() { - + if(ip_changed_with_all_param == false) //If connection was found after the IP-changed or a connection was never established { startGripper(); @@ -1109,7 +1109,7 @@ void SchunkGripperNode::reconnect() else if(ip_changed_with_all_param == true && start_connection == true ) //If the node had connection and nothing was changed through IP-changed { getWithInstance(PLC_SYNC_OUTPUT_INST); - getActualParameters(); + getActualParameters(); } else if(start_connection == false) return; @@ -1122,9 +1122,9 @@ void SchunkGripperNode::reconnect() this->set_parameter(rclcpp::Parameter("Gripper_Parameter.wp_release_delta", wp_release_delta)); this->set_parameter(rclcpp::Parameter("Gripper_Parameter.wp_lost_dst", wp_lost_dst)); this->set_parameter(rclcpp::Parameter("Control_Parameter.move_gripper", actualPosInterval())); - abs_pos_param = actualPosInterval(); + abs_pos_param = actualPosInterval(); - connection_error = "OK"; + connection_error = "OK"; } //JointStatePublisher void SchunkGripperNode::publishJointState() @@ -1141,7 +1141,7 @@ void SchunkGripperNode::publishJointState() void SchunkGripperNode::acknowledge_srv(const std::shared_ptr, std::shared_ptr res) { const std::lock_guard lock(lock_mutex); - + actual_command = "ACKNOWLEDGE"; try { @@ -1153,7 +1153,7 @@ void SchunkGripperNode::acknowledge_srv(const std::shared_ptracknowledged = true; RCLCPP_WARN(this->get_logger(),"Acknowledged"); } - else + else { res->acknowledged = false; RCLCPP_WARN(this->get_logger(),"Acknowledge failed!"); @@ -1178,15 +1178,15 @@ void SchunkGripperNode::brake_test_srv(const std::shared_ptr std::unique_lock lock(lock_mutex, std::defer_lock); //send stop std::unique_lock lock_service(lock_service_post); - + set_command = BRAKE_TEST; - - + + sendService(lock); lock_service.unlock(); //if command received, get values - if(handshake != gripperBitInput(COMMAND_RECEIVED_TOGGLE)) - while (rclcpp::ok() && check() && !gripperBitInput(SUCCESS)) + if(handshake != gripperBitInput(COMMAND_RECEIVED_TOGGLE)) + while (rclcpp::ok() && check() && !gripperBitInput(SUCCESS)) { runGets(); } @@ -1203,7 +1203,7 @@ void SchunkGripperNode::brake_test_srv(const std::shared_ptr res->error_code = error_str; res->brake_test_successful = false; RCLCPP_INFO(this->get_logger(),"Brake test failed!"); - + } } catch(const char* server_response) @@ -1212,12 +1212,12 @@ void SchunkGripperNode::brake_test_srv(const std::shared_ptr res->error_code = server_response; res->brake_test_successful = false; RCLCPP_ERROR(this->get_logger(), "Failed Connection! %s", connection_error.c_str()); - } + } } //Ip change service, if the ip should change during runtime of the program void SchunkGripperNode::change_ip_srv(const std::shared_ptr req, std::shared_ptr res) -{ +{ try { std::lock_guard lock(lock_mutex); @@ -1230,13 +1230,13 @@ void SchunkGripperNode::change_ip_srv(const std::shared_ptr r return; } - if(res->ip_changed == true) + if(res->ip_changed == true) { this->set_parameter(rclcpp::Parameter("IP", req->new_ip)); } if(start_connection == false && ip_changed_with_all_param == true) - { + { advertiseConnectionRelevant(); start_connection = true; } @@ -1244,7 +1244,7 @@ void SchunkGripperNode::change_ip_srv(const std::shared_ptr r if(ip_changed_with_all_param == true) { this->set_parameter(rclcpp::Parameter("model", model)); - + if(connection_error == "OK") { reconnect(); @@ -1264,20 +1264,20 @@ void SchunkGripperNode::stop_srv(const std::shared_ptr, std::shar { //send stop std::unique_lock lock(lock_mutex, std::defer_lock); - + std::unique_lock lock_service(lock_service_post); - + set_command = STOP; - + sendService(lock); lock_service.unlock(); //if command received, get values - if(handshake != gripperBitInput(COMMAND_RECEIVED_TOGGLE)) - while (rclcpp::ok() && check() && !gripperBitInput(SUCCESS)) + if(handshake != gripperBitInput(COMMAND_RECEIVED_TOGGLE)) + while (rclcpp::ok() && check() && !gripperBitInput(SUCCESS)) { runGets(); - } + } } catch(const char* res) { @@ -1303,16 +1303,16 @@ void SchunkGripperNode::fast_stop_srv(const std::shared_ptr, try { std::unique_lock lock(lock_mutex, std::defer_lock); - + std::unique_lock lock_service(lock_service_post); - + set_command = FAST_STOP; - + sendService(lock); lock_service.unlock(); //if command received, get values - if(handshake != gripperBitInput(COMMAND_RECEIVED_TOGGLE)) + if(handshake != gripperBitInput(COMMAND_RECEIVED_TOGGLE)) while (rclcpp::ok() && check()) { runGets(); @@ -1338,26 +1338,26 @@ void SchunkGripperNode::fast_stop_srv(const std::shared_ptr, } gripper_updater->force_update(); } -//Softreset service callback +//Softreset service callback void SchunkGripperNode::softreset_srv(const std::shared_ptr, std::shared_ptr res) -{ +{ std::unique_lock lock(lock_mutex, std::defer_lock); try { RCLCPP_INFO(this->get_logger(),"SOFTRESET"); - + std::unique_lock lock_service(lock_service_post); - + set_command = SOFT_RESET; - + sendService(lock); lock_service.unlock(); } catch(const char* res){} bool connection_once_lost = false; - + if(handshake != gripperBitInput(COMMAND_RECEIVED_TOGGLE)) { for(int i = 0; rclcpp::ok() && i <= 1000 && check(); i++) @@ -1365,7 +1365,7 @@ void SchunkGripperNode::softreset_srv(const std::shared_ptr, try { runGets(); //if connection is back, don't catch - if(connection_once_lost == true) + if(connection_once_lost == true) { connection_error = "OK"; break; @@ -1387,7 +1387,7 @@ void SchunkGripperNode::softreset_srv(const std::shared_ptr, RCLCPP_INFO(this->get_logger(), "Softreset succeeded!"); res->reset_success = true; } - else + else { RCLCPP_INFO(this->get_logger(), "Softreset failed!"); res->reset_success = false; @@ -1398,7 +1398,7 @@ void SchunkGripperNode::softreset_srv(const std::shared_ptr, } //Prepare for shutdown service callback void SchunkGripperNode::prepare_for_shutdown_srv(const std::shared_ptr, std::shared_ptr res) -{ +{ try { std::unique_lock lock(lock_mutex, std::defer_lock); @@ -1406,7 +1406,7 @@ void SchunkGripperNode::prepare_for_shutdown_srv(const std::shared_ptr lock_service(lock_service_post); set_command = PREPARE_FOR_SHUTDOWN; - + sendService(lock); lock_service.unlock(); //if command received, get values @@ -1421,7 +1421,7 @@ void SchunkGripperNode::prepare_for_shutdown_srv(const std::shared_ptrget_logger(), "Failed Connection! %s", connection_error.c_str()); } - + if((gripperBitInput(READY_FOR_SHUTDOWN) == true) && (handshake != gripperBitInput(COMMAND_RECEIVED_TOGGLE)) ) { RCLCPP_WARN(this->get_logger(),"READY FOR SHUTDOWN"); @@ -1437,23 +1437,23 @@ void SchunkGripperNode::prepare_for_shutdown_srv(const std::shared_ptr, std::shared_ptr res) -{ +{ try { std::unique_lock lock(lock_mutex, std::defer_lock); //If no Errors - if(splitted_Diagnosis[2] == 0) - { + if(splitted_Diagnosis[2] == 0) + { //Send fast stop std::unique_lock lock_service(lock_service_post); - + set_command = FAST_STOP; - + sendService(lock); lock_service.unlock(); - //if command received, get values - if(handshake != gripperBitInput(COMMAND_RECEIVED_TOGGLE)) + //if command received, get values + if(handshake != gripperBitInput(COMMAND_RECEIVED_TOGGLE)) while (rclcpp::ok() && check()) { runGets(); @@ -1465,21 +1465,21 @@ void SchunkGripperNode::releaseForManualMov_srv(const std::shared_ptr lock_service(lock_service_post); - + set_command = EMERGENCY_RELEASE; - - + + sendService(lock); lock_service.unlock(); //if command received, get values - if(handshake != gripperBitInput(COMMAND_RECEIVED_TOGGLE)) + if(handshake != gripperBitInput(COMMAND_RECEIVED_TOGGLE)) while(rclcpp::ok() && !gripperBitInput(EMERGENCY_RELEASED)) { runGets(); } lock.unlock(); } - + catch(const char* res) { connection_error = res; @@ -1501,7 +1501,7 @@ void SchunkGripperNode::releaseForManualMov_srv(const std::shared_ptr, std::shared_ptr) -{ +{ const std::lock_guard lock(lock_mutex); try { @@ -1514,19 +1514,19 @@ void SchunkGripperNode::info_srv(const std::shared_ptr, st RCLCPP_INFO_STREAM(this->get_logger(),"\n\n\nGRIPPER TYPE: " << model.c_str() << "\nIP: " << ip << std::endl); - + getEnums(FIELDBUS_TYPE_INST,fieldbus_type); getParameter(MAC_ADDR_INST, 6, CHAR_DATA); std::array mac; - + for(size_t i = 0; i < 6; i++){mac[i] = static_cast(static_cast(char_vector.at(i)));} - + RCLCPP_INFO_STREAM(this->get_logger(), - "\nFieldbustype: " << json_data["string"] + "\nFieldbustype: " << json_data["string"] << "\nMac-address: " << std::hex << mac[0] << ":" << mac[1] << ":" << mac[2] << ":" << mac[3] << ":" << mac[4] << ":" << mac[5] << std::endl); - + //Get the char-Parameter and save them as strings in char_strings getParameter(SERIAL_NO_TXT_INST, 16, CHAR_DATA); char_strings.push_back(char_vector.data()); @@ -1538,7 +1538,7 @@ void SchunkGripperNode::info_srv(const std::shared_ptr, st char_strings.push_back(char_vector.data()); getParameter(SW_VERSION_TXT_INST, 22, CHAR_DATA); char_strings.push_back(char_vector.data()); - + getWithInstance(SERIAL_NO_NUM_INST, &data); getWithInstance(SW_VERSION_NUM_INST, &data2); @@ -1553,29 +1553,29 @@ void SchunkGripperNode::info_srv(const std::shared_ptr, st getWithInstance(UPTIME_INST, &data); RCLCPP_INFO_STREAM(this->get_logger(),"\nSystem uptime: " << data << " s" << std::endl); - + getWithInstance(DEAD_LOAD_KG_INST, &data3); RCLCPP_INFO_STREAM(this->get_logger(),"\nNet mass of the Gripper: " << data3 << " kg" << std::endl); - + getParameter(TOOL_CENT_POINT_INST, 6, FLOAT_DATA); - RCLCPP_INFO_STREAM(this->get_logger(),"\nTool center point 6D-Frame: \n" - << float_vector[0] << " mm " << float_vector[1] << " mm " << float_vector[2] << " mm\n" + RCLCPP_INFO_STREAM(this->get_logger(),"\nTool center point 6D-Frame: \n" + << float_vector[0] << " mm " << float_vector[1] << " mm " << float_vector[2] << " mm\n" << float_vector[3] << " " << float_vector[4] << " " << float_vector[5] << std::endl); - + getParameter(CENT_OF_MASS_INST, 6, FLOAT_DATA); - RCLCPP_INFO_STREAM(this->get_logger(), "\nCenter of Mass 6D-frame: \n" + RCLCPP_INFO_STREAM(this->get_logger(), "\nCenter of Mass 6D-frame: \n" << float_vector[0] << " mm " << float_vector[1] << " mm " << float_vector[2] << " mm\n" << float_vector[3] << " kg*m^2 " << float_vector[4] << " kg*m^2 "<< float_vector[5] << " kg*m^2"<< std::endl); - RCLCPP_INFO_STREAM(this->get_logger(),"\nMin. absolute position: " << min_pos << " mm\n" + RCLCPP_INFO_STREAM(this->get_logger(),"\nMin. absolute position: " << min_pos << " mm\n" << "Max. absolute position: " << max_pos << " mm\n" << "Zero_pos_offset: " << zero_pos_ofs << " mm" << std::endl); RCLCPP_INFO_STREAM(this->get_logger(),"\nMin. velocity_of_movement: " << min_vel << " mm/s\n" << "Max. velocity_of_movement: " << max_vel << " mm/s" <get_logger(),"\nMax. grip velocity_of_movement: "<< max_grp_vel << " mm/s\n" - << "Min. grip force: " << min_grip_force << " N\n" + RCLCPP_INFO_STREAM(this->get_logger(),"\nMax. grip velocity_of_movement: "<< max_grp_vel << " mm/s\n" + << "Min. grip force: " << min_grip_force << " N\n" << "Max. grip force: " << max_grip_force << " N" << std::endl); if(model.find("EGU") != std::string::npos) @@ -1594,7 +1594,7 @@ void SchunkGripperNode::info_srv(const std::shared_ptr, st << "Max. error logic voltage: " << float_vector[3] << " V\n" << "Min. error logic temperature: " << float_vector[4] << " C\n" << "Max. error logic temperature: " << float_vector[5] << " C" << std::endl); - + getWithInstance(MEAS_LGC_TEMP_INST, &data3); RCLCPP_INFO_STREAM(this->get_logger(), "Measured logic temperature: " << data3 << " C" << std::endl); @@ -1608,7 +1608,7 @@ void SchunkGripperNode::info_srv(const std::shared_ptr, st << "Max. warning logic voltage: " << float_vector[5] << " V\n" << "Min. warning logic temperature: " << float_vector[6] << " C\n" << "Max. warning logic temperature: " << float_vector[7] << " C" << std::endl); - + } catch(const char* res) { @@ -1629,27 +1629,27 @@ void SchunkGripperNode::parameter_get_srv(const std::shared_ptr lock(lock_mutex); - + getMetadata(req->instance); - std::string data_name = json_data["name"]; + std::string data_name = json_data["name"]; datatype = json_data["datatype"]; numelements = json_data["numelements"]; - + RCLCPP_INFO(this->get_logger(), "Get: %s\n", data_name.c_str()); - + getParameter(req->instance, numelements, datatype); - + switch(datatype) { case BOOL_DATA: res->bool_value = bool_vector; for(auto element : bool_vector) std::cout << element << "" ; break; - + case UINT8_DATA: res->uint8_value = uint8_vector; for(auto element : uint8_vector) std::cout << static_cast(element) << " "; break; - + case UINT16_DATA: res->uint16_value = uint16_vector; for(auto element : uint16_vector) std::cout << element << " "; break; @@ -1657,7 +1657,7 @@ void SchunkGripperNode::parameter_get_srv(const std::shared_ptruint32_value = uint32_vector; for(auto element : uint32_vector) std::cout << element << " "; break; - + case INT32_DATA: res->int32_value = int32_vector; for(auto element : int32_vector) std::cout <float_value = float_vector; for(auto element : float_vector) std::cout <char_value = char_vector.data(); std::cout << char_vector.data(); break; case ENUM_DATA: res->enum_value = uint8_vector; - for(auto element : uint8_vector) - { + for(auto element : uint8_vector) + { getEnums(req->instance.c_str(), static_cast(element)); res->enum_string.push_back(json_data["string"]); std::cout << json_data["string"] << " \nEnumCode: "<< static_cast(element) << std::endl; @@ -1686,27 +1686,27 @@ void SchunkGripperNode::parameter_get_srv(const std::shared_ptrget_logger(), res); } catch(std::exception &e) - { + { not_double_word = true; RCLCPP_ERROR_STREAM(this->get_logger(), e.what()); } } //set Parameter with Instance void SchunkGripperNode::parameter_set_srv(const std::shared_ptr req, std::shared_ptr res) -{ +{ try { std::lock_guard lock(lock_mutex); - + char inst[7]; - if(req->instance.size() < 7) + if(req->instance.size() < 7) { std::strcpy(inst, req->instance.c_str()); } @@ -1718,8 +1718,8 @@ void SchunkGripperNode::parameter_set_srv(const std::shared_ptrget_logger(), "Changing: %s\n", data.c_str()); - - std::string instance_upper = "0x"; + + std::string instance_upper = "0x"; for(const auto character: req->instance.substr(2)) { instance_upper.append(1, std::toupper(character)); @@ -1729,31 +1729,31 @@ void SchunkGripperNode::parameter_set_srv(const std::shared_ptrbool_value); getParameter(req->instance, elements, datatype); - + if(search != inst_param.end()){ this->set_parameter(rclcpp::Parameter(search->second, rclcpp::ParameterValue(bool_vector.at(0)))); } res->actual_bool_value = bool_vector; break; - + case UINT8_DATA: - + changeVectorParameter(inst, req->uint8_value); getParameter(req->instance, elements, datatype); - + if(search != inst_param.end()){ this->set_parameter(rclcpp::Parameter(search->second, rclcpp::ParameterValue(uint8_vector.at(0)))); } res->actual_uint8_value = uint8_vector; break; - + case UINT16_DATA: handshake = gripperBitInput(COMMAND_RECEIVED_TOGGLE); changeVectorParameter(inst, req->uint16_value); getParameter(req->instance, elements, datatype); getWithInstance(PLC_SYNC_INPUT_INST); - - if(search != inst_param.end()){ + + if(search != inst_param.end()){ this->set_parameter(rclcpp::Parameter(search->second, rclcpp::ParameterValue(uint16_vector.at(0)))); } res->actual_uint16_value = uint16_vector; break; @@ -1764,31 +1764,31 @@ void SchunkGripperNode::parameter_set_srv(const std::shared_ptruint32_value); getParameter(req->instance, elements, datatype); getWithInstance(PLC_SYNC_INPUT_INST); - + if(search != inst_param.end()){ this->set_parameter(rclcpp::Parameter(search->second, rclcpp::ParameterValue(static_cast(uint32_vector.at(0))))); } res->actual_uint32_value = uint32_vector; break; - + case INT32_DATA: - + changeVectorParameter(inst, req->int32_value); getParameter(req->instance, elements, datatype); - + if(search != inst_param.end()){ this->set_parameter(rclcpp::Parameter(search->second, rclcpp::ParameterValue(int32_vector.at(0)))); } res->actual_int32_value = int32_vector; break; case FLOAT_DATA: - + changeVectorParameter(inst, req->float_value); getParameter(req->instance, elements, datatype); - + if(search != inst_param.end()){ this->set_parameter(rclcpp::Parameter(search->second, rclcpp::ParameterValue(float_vector.at(0)))); } res->actual_float_value = float_vector; break; - + default: RCLCPP_ERROR(this->get_logger(), "Datatype not compatible") ; - } + } if(handshake != gripperBitInput(COMMAND_RECEIVED_TOGGLE)) { @@ -1806,19 +1806,19 @@ void SchunkGripperNode::parameter_set_srv(const std::shared_ptrget_logger(), res); return; } catch(std::exception &e) - { + { not_double_word = true; RCLCPP_ERROR_STREAM(this->get_logger(), e.what()); return; @@ -1830,7 +1830,7 @@ void SchunkGripperNode::callback_gripper_parameter(const rclcpp::Parameter &p) { try { - if(p.get_name() == "diagnostic_updater.period") + if(p.get_name() == "diagnostic_updater.period") { gripper_updater->setPeriod(p.as_double()); return; @@ -1839,19 +1839,19 @@ void SchunkGripperNode::callback_gripper_parameter(const rclcpp::Parameter &p) { const std::lock_guard lock(lock_mutex); changeOneParameter(param_inst.at(p.get_name()), static_cast(p.as_double())); - + RCLCPP_INFO_STREAM(this->get_logger(), p.get_name() << " changed to: " << p.as_double()); if(p.get_name() == "Gripper_Parameter.zero_pos_ofs") - { + { getWithInstance(MAX_POS_INST, &max_pos); getWithInstance(MIN_POS_INST,&min_pos); runGets(); parameter_event_handler->remove_parameter_callback(cb_handle[9]); - + this->undeclare_parameter("Control_Parameter.move_gripper"); this->declare_parameter("Control_Parameter.move_gripper", actualPosInterval(), parameter_descriptor("Moving the gripper with parameter in mm", FloatingPointRange(min_pos,max_pos)), true); - + abs_pos_param = actualPosInterval(); cb_handle[9] = parameter_event_handler->add_parameter_callback("Control_Parameter.move_gripper", callback_move_param); } @@ -1881,22 +1881,22 @@ catch(const char* res) void SchunkGripperNode::callback_move_parameter(const rclcpp::Parameter &p) { try -{ - if(p.get_name() == "Control_Parameter.move_gripper" && doing_something == false && p.as_double() != abs_pos_param && action_active == false) +{ + if(p.get_name() == "Control_Parameter.move_gripper" && doing_something == false && p.as_double() != abs_pos_param && action_active == false) { std::unique_lock lock_service(lock_service_post); - + actual_command = "MOVE TO ABSOLUTE POSITION"; set_command = MOVE_TO_ABSOLUTE_POSITION; set_position = static_cast(this->get_parameter("Control_Parameter.move_gripper").as_double()*1000); set_speed = static_cast(this->get_parameter("Control_Parameter.move_gripper_velocity").as_double()*1000); - + sendAction(); doing_something = true; lock_service.unlock(); } - else if(p.get_name() == "Control_Parameter.grip" && p.as_bool() == true && doing_something == false && action_active == false) + else if(p.get_name() == "Control_Parameter.grip" && p.as_bool() == true && doing_something == false && action_active == false) { std::unique_lock lock_service(lock_service_post); @@ -1906,28 +1906,28 @@ try set_gripping_force = static_cast(this->get_parameter("Control_Parameter.grip_force").as_double()); if(gripperBitOutput(GRIP_DIRECTION) != this->get_parameter("Control_Parameter.grip_direction").as_bool()) {set_command |= GRIP_DIRECTION;} - - + + sendAction(); doing_something = true; lock_service.unlock(); } - else if(p.get_name() == "Control_Parameter.release_workpiece" && p.as_bool() == true && doing_something == false && action_active == false) + else if(p.get_name() == "Control_Parameter.release_workpiece" && p.as_bool() == true && doing_something == false && action_active == false) { if (gripperBitInput(GRIPPED)) { std::unique_lock lock_service(lock_service_post); - + actual_command = "RELEASE WORKPIECE"; set_command = commands_str.at(actual_command); - - + + sendAction(); doing_something = true; lock_service.unlock(); } - else + else { RCLCPP_INFO(this->get_logger(),"No Workpiece Gripped"); this->set_parameter(rclcpp::Parameter("Control_Parameter.release_workpiece", false)); @@ -1939,9 +1939,9 @@ catch(const char* res) RCLCPP_ERROR(this->get_logger() , "Could't set the Parameter(s). \nConnection failed: %s", res); } } -//Diagnostics +//Diagnostics void SchunkGripperNode::gripperDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& status) -{ +{ splitted_Diagnosis = splitDiagnosis(); try @@ -1953,17 +1953,17 @@ void SchunkGripperNode::gripperDiagnostics(diagnostic_updater::DiagnosticStatusW } if(gripperBitInput(EMERGENCY_RELEASED)) //If release for manual Movement active status.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "If you want to end this mode, perform a fast stop and acknowledge!"); - + else if(gripperBitInput(READY_FOR_SHUTDOWN)) //If ready for shutdown status.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "Ready for shutdown!"); - - + + //If an error or waning occurred else if(plc_sync_input[3] != 0) { //if error if(splitted_Diagnosis[2]) - { - if(splitted_Diagnosis != old_diagnosis) + { + if(splitted_Diagnosis != old_diagnosis) { const std::lock_guard lock(lock_mutex); error_str = getErrorString(splitted_Diagnosis[2]); @@ -1973,8 +1973,8 @@ void SchunkGripperNode::gripperDiagnostics(diagnostic_updater::DiagnosticStatusW } //if not feasible if(splitted_Diagnosis[0]) - { - if(splitted_Diagnosis != old_diagnosis) + { + if(splitted_Diagnosis != old_diagnosis) { const std::lock_guard lock(lock_mutex); warn_str = getErrorString(splitted_Diagnosis[1]); @@ -1985,7 +1985,7 @@ void SchunkGripperNode::gripperDiagnostics(diagnostic_updater::DiagnosticStatusW } //if warning else if(splitted_Diagnosis[1]) - { + { if(splitted_Diagnosis != old_diagnosis) { const std::lock_guard lock(lock_mutex); @@ -1998,8 +1998,8 @@ void SchunkGripperNode::gripperDiagnostics(diagnostic_updater::DiagnosticStatusW } else //No errors - { - if(splitted_Diagnosis != old_diagnosis) + { + if(splitted_Diagnosis != old_diagnosis) { const std::lock_guard lock(lock_mutex); error_str = getErrorString(0); @@ -2015,13 +2015,13 @@ catch(const char* res) //Lost Connection catch RCLCPP_ERROR(this->get_logger(), "Failed Connection! %s", connection_error.c_str()); } - old_diagnosis = splitted_Diagnosis; + old_diagnosis = splitted_Diagnosis; status.add("Ready for operation", gripperBitInput(READY_FOR_OPERATION)); status.add("Command successfully processed", gripperBitInput(SUCCESS)); status.add("position reached", gripperBitInput(POSITION_REACHED)); status.add("Command received toggle", gripperBitInput(COMMAND_RECEIVED_TOGGLE)); - + status.add("Error", gripperBitInput(GRIPPER_ERROR)); status.add("Warning", gripperBitInput(WARNING)); status.add("Not feasible", gripperBitInput(NOT_FEASIBLE)); @@ -2171,13 +2171,13 @@ void SchunkGripperNode::handle_accepted_control(const std::shared_ptr lock(lock_mutex); if(action_active == true || doing_something == true) //If gripper is moving, fast stop @@ -2192,7 +2192,7 @@ SchunkGripperNode::~SchunkGripperNode() if(gripperBitInput(COMMAND_RECEIVED_TOGGLE) != handshake) break; } catch(...) - { + { RCLCPP_ERROR(this->get_logger(),"Could't perform fast stop."); } } diff --git a/schunk_egu_egk_gripper_examples/CMakeLists.txt b/schunk_egu_egk_gripper_examples/CMakeLists.txt index e766094..2d14b81 100644 --- a/schunk_egu_egk_gripper_examples/CMakeLists.txt +++ b/schunk_egu_egk_gripper_examples/CMakeLists.txt @@ -55,4 +55,3 @@ endif() # Ament package configuration ament_package() - diff --git a/schunk_egu_egk_gripper_examples/src/gripper_example.cpp b/schunk_egu_egk_gripper_examples/src/gripper_example.cpp index a51e817..cbb1bb1 100644 --- a/schunk_egu_egk_gripper_examples/src/gripper_example.cpp +++ b/schunk_egu_egk_gripper_examples/src/gripper_example.cpp @@ -4,12 +4,12 @@ // under the terms of the GNU General Public License as published by the Free // Software Foundation, either version 3 of the License, or (at your option) // any later version. -// +// // This program is distributed in the hope that it will be useful, but WITHOUT // ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or // FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for // more details. -// +// // You should have received a copy of the GNU General Public License along with // this program. If not, see . // -------------------------------------------------------------------------------- @@ -19,7 +19,7 @@ * Maintainer: Stefan Scherzinger (stefan.scherzinger@de.schunk.com) */ -//This Client shows you how to control your gripper in program. If you do something else, as long as the program works, +//This Client shows you how to control your gripper in program. If you do something else, as long as the program works, //this program will exhibit undefined behavior. #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" @@ -83,9 +83,9 @@ std::string name_space; /** * @brief Callback function for handling the state message. - * + * * This function is called when a new state message is received. It updates the state_msg variable with the contents of the message. - * + * * @param msg The shared pointer to the state message. */ void stateCallback(const State::SharedPtr msg) @@ -95,10 +95,10 @@ void stateCallback(const State::SharedPtr msg) /** * @brief Callback function for the joint state message. - * + * * This function is called whenever a new joint state message is received. * It updates the joint_state_msg variable with the latest joint state data. - * + * * @param msg The joint state message. */ void jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr msg) @@ -108,22 +108,22 @@ void jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr msg) /** * @brief Callback function for handling diagnostic messages. - * + * * This function is called when a diagnostic message is received. It updates the diagnostic_msg variable with the received message. - * + * * @param msg The received diagnostic message. */ void diagnosticsCallback(const diagnostic_msgs::msg::DiagnosticArray::SharedPtr msg) -{ +{ diagnostic_msg = *msg; } /** * @brief Callback function for the feedback of the MoveToAbsolutePosition action. - * + * * This function is called when the action server sends feedback about the progress of the goal. * It logs the current position of the gripper in millimeters. - * + * * @param goal_handle The goal handle associated with the action. * @param feedback The feedback received from the action server. */ @@ -134,21 +134,21 @@ void feedbackCB(rclcpp_action::ClientGoalHandle::SharedP /** * @brief Callback function called when the execution of the EGU action is done. - * + * * This function logs whether the workpiece has been gripped or not. - * + * * @param result The result of the EGU action. */ void doneEguCb(const rclcpp_action::ClientGoalHandle::WrappedResult & result) -{ +{ RCLCPP_INFO(rclcpp::get_logger("schunk_gripper_example"), "%s", result.result->workpiece_gripped ? "workpiece_gripped" : "Not workpiece_gripped"); } /** * @brief Callback function called when the goal is done for the EGK action client. - * + * * This function prints whether the workpiece is gripped or not based on the result. - * + * * @param result The result of the EGK action client goal. */ void doneEgkCb(const rclcpp_action::ClientGoalHandle::WrappedResult & result) @@ -158,31 +158,31 @@ void doneEgkCb(const rclcpp_action::ClientGoalHandle::WrappedR /** * @brief Callback function for grip action feedback. - * + * * This function is called when feedback is received from the grip action server. * It logs a message if the workpiece pre-grip has started. - * + * * @param goal_handle The goal handle for the grip action. * @param feedback The feedback received from the grip action server. */ void gripFeedback(rclcpp_action::ClientGoalHandle::SharedPtr, const std::shared_ptr feedback) -{ - if(feedback->workpiece_pre_grip_started == true) - RCLCPP_INFO_ONCE(rclcpp::get_logger("schunk_gripper_example"), "Pre_grip started"); +{ + if(feedback->workpiece_pre_grip_started == true) + RCLCPP_INFO_ONCE(rclcpp::get_logger("schunk_gripper_example"), "Pre_grip started"); } /** * @brief Callback function for grip velocity feedback. - * + * * This function is called when receiving feedback from the grip velocity action. * It checks if the workpiece pre-grip has started and logs a message if it is true. - * + * * @param goal_handle The goal handle for the grip velocity action. * @param feedback The feedback received from the grip velocity action. */ void gripVelFeedback(rclcpp_action::ClientGoalHandle::SharedPtr, const std::shared_ptr feedback) -{ - if(feedback->workpiece_pre_grip_started == true) +{ + if(feedback->workpiece_pre_grip_started == true) RCLCPP_INFO_ONCE(rclcpp::get_logger("schunk_gripper_example"), "Pre_grip started"); } @@ -191,7 +191,7 @@ void gripVelFeedback(rclcpp_action::ClientGoalHandle::SharedPt /** * Moves the gripper to an absolute position and waits for the result. - * + * * @param move_abs_client The client for the MoveToAbsolutePosition action server. */ void moveAbsoluteAndWaitForResult(rclcpp_action::Client::SharedPtr move_abs_client) @@ -205,12 +205,12 @@ void moveAbsoluteAndWaitForResult(rclcpp_action::Client: send_goal_options.feedback_callback = std::bind(feedbackCB, std::placeholders::_1, std::placeholders::_2); auto result = move_abs_client->async_send_goal(goal_abs, send_goal_options); - + result.get(); RCLCPP_WARN(rclcpp::get_logger("schunk_gripper_example"), "%s: %f mm", state_msg.doing_command.c_str(), goal_abs.absolute_position); if(move_abs_client->async_get_result(result.get()).get().code == rclcpp_action::ResultCode::SUCCEEDED) - RCLCPP_INFO(rclcpp::get_logger("schunk_gripper_example"), "At the right position"); + RCLCPP_INFO(rclcpp::get_logger("schunk_gripper_example"), "At the right position"); } /** @@ -235,21 +235,21 @@ void moveRelativeAndStop(rclcpp_action::Client::SharedPt auto stop_req = std::make_shared(); auto resp = stop_client->async_send_request(stop_req); bool stopped = resp.get()->stopped; - + RCLCPP_INFO(rclcpp::get_logger("schunk_gripper_example"), "%s", stopped ? "Stopped!" : "Not stopped!"); auto res = move_rel_client->async_get_result(goal_handle.get()); auto final_res = res.get(); - + RCLCPP_INFO_STREAM(rclcpp::get_logger("schunk_gripper_example"), "Current Position: " << final_res.result->absolute_position); RCLCPP_INFO(rclcpp::get_logger("schunk_gripper_example"), "ResultCode: %i", static_cast(final_res.code)); } /** * @brief Moves the gripper to a relative position and cancels the movement. - * + * * This function sends a goal to move the gripper to a relative position and then cancels the movement * after a specified duration. It performs a fast stop to halt the movement. - * + * * @param move_rel_client A shared pointer to the action client for moving the gripper to a relative position. */ void MoveRelativeAndCancel(rclcpp_action::Client::SharedPtr move_rel_client) @@ -268,10 +268,10 @@ void MoveRelativeAndCancel(rclcpp_action::Client::Shared /** * @brief Sends an acknowledge request to the acknowledge server. - * + * * This function sends an acknowledge request to the acknowledge server using the provided client. * It logs the result of the request as "Acknowledged" or "Not acknowledged". - * + * * @param acknowledge_client The client used to send the acknowledge request. */ void acknowledge(rclcpp::Client::SharedPtr acknowledge_client) @@ -287,10 +287,10 @@ void acknowledge(rclcpp::Client::SharedPtr acknowledge_client) /** * @brief Changes the configuration of the gripper. - * + * * This function retrieves the current configuration parameters of the gripper, * modifies them, and sets the new configuration parameters. - * + * * @param param_client A shared pointer to the AsyncParametersClient used to retrieve and set parameters. */ void changeConfiguration(std::shared_ptr param_client) @@ -313,7 +313,7 @@ void changeConfiguration(std::shared_ptr param_cl set_params.get(); RCLCPP_INFO(rclcpp::get_logger("schunk_gripper_example") ,"Parameter after:"); - + RCLCPP_INFO(rclcpp::get_logger("schunk_gripper_example") ,"%s: %f", parameter.at(0).get_name().c_str(), parameter.at(0).as_double()); RCLCPP_INFO(rclcpp::get_logger("schunk_gripper_example") ,"%s: %li", parameter.at(1).get_name().c_str(), parameter.at(1).as_int()); } @@ -335,14 +335,14 @@ void gripEgu(rclcpp_action::Client::SharedPtr grip_client) rclcpp_action::Client::SendGoalOptions goal_opt; goal_opt.result_callback = std::bind(&doneEguCb, std::placeholders::_1); goal_opt.feedback_callback = std::bind(&gripFeedback, std::placeholders::_1, std::placeholders::_2); - + auto goal_handle = grip_client->async_send_goal(goal_grip, goal_opt); if(goal_handle.get()->is_result_aware()) - { + { RCLCPP_WARN(rclcpp::get_logger("schunk_gripper_example"), "%s", state_msg.doing_command.c_str()); grip_client->async_get_result(goal_handle.get()).get(); } -} +} /** * @brief Executes the grip action with the EGK gripper. @@ -366,7 +366,7 @@ void gripEgk(rclcpp_action::Client::SharedPtr grip_client) goal_opt.feedback_callback = std::bind(&gripVelFeedback, std::placeholders::_1, std::placeholders::_2); handshake = state_msg.command_received_toggle; auto goal_handle = grip_client->async_send_goal(goal_grip, goal_opt); - + if(goal_handle.get()->is_result_aware()) { while(handshake == state_msg.command_received_toggle) std::this_thread::sleep_for(std::chrono::milliseconds(15)); @@ -379,7 +379,7 @@ void gripEgk(rclcpp_action::Client::SharedPtr grip_client) /** * @brief Releases the workpiece using the release_client. - * + * * @param release_client The client used to send the release goal. */ void releaseWorkpiece(rclcpp_action::Client::SharedPtr release_client) @@ -393,17 +393,17 @@ void releaseWorkpiece(rclcpp_action::Client::SharedPtr release RCLCPP_WARN(rclcpp::get_logger("schunk_gripper_example"), "%s", state_msg.doing_command.c_str()); auto result = (release_client->async_get_result(goal_handle.get())).get().result->released_workpiece; RCLCPP_WARN(rclcpp::get_logger("schunk_gripper_example"), "%s" , result ? "released_workpiece" : "not released_workpiece"); - + } /** * @brief Moves the gripper to an absolute position with the specified configuration parameters. - * + * * This function retrieves the control parameters for moving the gripper from the parameter server * using the provided parameter client. It then sets the configuration parameters to the desired values * and updates them on the parameter server. Finally, it waits for the gripper to reach the desired position * and logs a message indicating if the position was reached. - * + * * @param param_client A shared pointer to the parameter client used to retrieve and update the parameters. */ void moveAbsWithConfig(std::shared_ptr param_client) @@ -425,12 +425,12 @@ void moveAbsWithConfig(std::shared_ptr param_clie set_params.get(); RCLCPP_INFO(rclcpp::get_logger("schunk_gripper_example") ,"Parameter after:"); - + RCLCPP_INFO(rclcpp::get_logger("schunk_gripper_example") ,"%s: %f", parameter.at(0).get_name().c_str(), parameter.at(0).as_double()); RCLCPP_INFO(rclcpp::get_logger("schunk_gripper_example") ,"%s: %f", parameter.at(1).get_name().c_str(), parameter.at(1).as_double()); while(state_msg.command_successfully_processed == false && state_msg.position_reached == false && rclcpp::ok()) - if(diagnostic_msg.status.at(0).message != "NON_ERROR") break; + if(diagnostic_msg.status.at(0).message != "NON_ERROR") break; else std::this_thread::sleep_for(std::chrono::milliseconds(15)); if(state_msg.position_reached == true) RCLCPP_INFO(rclcpp::get_logger("schunk_gripper_example"), "Position reached"); @@ -439,19 +439,19 @@ void moveAbsWithConfig(std::shared_ptr param_clie /** * @brief Spins the given node using a multi-threaded executor. - * + * * This function adds the given node to a multi-threaded executor and spins it until shutdown is called. - * + * * @param node A shared pointer to the node to be spun. */ void spinFunction(std::shared_ptr node) { rclcpp::executors::MultiThreadedExecutor executor; - + executor.add_node(node); - + executor.spin(); - + rclcpp::shutdown(); } @@ -475,13 +475,13 @@ void spinFunction(std::shared_ptr node) */ int main(int argc, char** argv) { - + rclcpp::init(argc, argv); std::string model; std::shared_ptr node = std::make_shared("schunk_gripper_example"); - + std::thread spin_thread(&spinFunction, node); - + //IS SCHUNK GRIPPER ACTIVE name_space = "/EGK_50_M_B/"; @@ -510,7 +510,7 @@ int main(int argc, char** argv) auto move_abs_client = rclcpp_action::create_client(node, name_space+"move_to_absolute_position"); auto move_rel_client = rclcpp_action::create_client(node, name_space+"move_to_relative_position"); - + rclcpp_action::Client::SharedPtr grip_egu_client; rclcpp_action::Client::SharedPtr grp_w_pos_egu_client; @@ -528,7 +528,7 @@ int main(int argc, char** argv) grp_w_pos_egk_client = rclcpp_action::create_client(node, name_space+"grip_with_position"); } auto release_client = rclcpp_action::create_client(node, name_space+"release_workpiece"); - + auto control_client = rclcpp_action::create_client(node, name_space+"gripper_control"); //TOPICS @@ -539,7 +539,7 @@ int main(int argc, char** argv) //acknowledge if the gripper has an error rclcpp::wait_for_message(diagnostic_msg, node, "diagnostics", std::chrono::seconds(3)); Acknowledge::Request::SharedPtr acknowledge_req; - if(diagnostic_msg.status[0].level == diagnostic_msgs::msg::DiagnosticStatus::ERROR) + if(diagnostic_msg.status[0].level == diagnostic_msgs::msg::DiagnosticStatus::ERROR) { auto response = acknowledge_client->async_send_request(acknowledge_req); if(response.get()->acknowledged) @@ -550,25 +550,25 @@ int main(int argc, char** argv) //Move to 0 mm with 100 mm/s and get result moveAbsoluteAndWaitForResult(move_abs_client); - + //Move relative moveRelativeAndStop(move_rel_client, stop_client); - + //Cancel the goal MoveRelativeAndCancel(move_rel_client); - + acknowledge(acknowledge_client); //Change grip prehold time and workpiece release delta changeConfiguration(param_client); - + //Grip if(model_param[0].as_string().find("EGU") != std::string::npos) gripEgu(grip_egu_client); else if(model_param[0].as_string().find("EGK") != std::string::npos) gripEgk(grip_egk_client); else RCLCPP_INFO(node->get_logger(), "No model"); - + //Release Workpiece if(state_msg.workpiece_gripped == true) releaseWorkpiece(release_client); else RCLCPP_INFO(node->get_logger(), "No Workpiece workpiece_gripped!"); diff --git a/schunk_egu_egk_gripper_interfaces/action/Grip.action b/schunk_egu_egk_gripper_interfaces/action/Grip.action index ebb4e49..252bd99 100644 --- a/schunk_egu_egk_gripper_interfaces/action/Grip.action +++ b/schunk_egu_egk_gripper_interfaces/action/Grip.action @@ -9,4 +9,4 @@ bool workpiece_lost float32 absolute_position float32 velocity_of_movement float32 motor_current -bool workpiece_pre_grip_started \ No newline at end of file +bool workpiece_pre_grip_started diff --git a/schunk_egu_egk_gripper_interfaces/action/GripWithPosition.action b/schunk_egu_egk_gripper_interfaces/action/GripWithPosition.action index 768d0a1..7b6334a 100644 --- a/schunk_egu_egk_gripper_interfaces/action/GripWithPosition.action +++ b/schunk_egu_egk_gripper_interfaces/action/GripWithPosition.action @@ -11,4 +11,4 @@ bool wrong_workpiece_detected float32 absolute_position float32 velocity_of_movement float32 motor_current -bool workpiece_pre_grip_started \ No newline at end of file +bool workpiece_pre_grip_started diff --git a/schunk_egu_egk_gripper_interfaces/action/GripWithPositionAndVelocity.action b/schunk_egu_egk_gripper_interfaces/action/GripWithPositionAndVelocity.action index 37a6889..205cbc7 100644 --- a/schunk_egu_egk_gripper_interfaces/action/GripWithPositionAndVelocity.action +++ b/schunk_egu_egk_gripper_interfaces/action/GripWithPositionAndVelocity.action @@ -12,4 +12,4 @@ bool wrong_workpiece_detected float32 absolute_position float32 velocity_of_movement float32 motor_current -bool workpiece_pre_grip_started \ No newline at end of file +bool workpiece_pre_grip_started diff --git a/schunk_egu_egk_gripper_interfaces/action/GripWithVelocity.action b/schunk_egu_egk_gripper_interfaces/action/GripWithVelocity.action index 81af4e0..d4a77ef 100644 --- a/schunk_egu_egk_gripper_interfaces/action/GripWithVelocity.action +++ b/schunk_egu_egk_gripper_interfaces/action/GripWithVelocity.action @@ -10,4 +10,4 @@ bool workpiece_lost float32 absolute_position float32 velocity_of_movement float32 motor_current -bool workpiece_pre_grip_started \ No newline at end of file +bool workpiece_pre_grip_started diff --git a/schunk_egu_egk_gripper_interfaces/action/MoveToAbsolutePosition.action b/schunk_egu_egk_gripper_interfaces/action/MoveToAbsolutePosition.action index 0450f9a..e1cc21b 100644 --- a/schunk_egu_egk_gripper_interfaces/action/MoveToAbsolutePosition.action +++ b/schunk_egu_egk_gripper_interfaces/action/MoveToAbsolutePosition.action @@ -6,4 +6,4 @@ bool position_reached --- float32 absolute_position float32 velocity_of_movement -float32 motor_current \ No newline at end of file +float32 motor_current diff --git a/schunk_egu_egk_gripper_interfaces/action/MoveToRelativePosition.action b/schunk_egu_egk_gripper_interfaces/action/MoveToRelativePosition.action index b073d4c..e161db7 100644 --- a/schunk_egu_egk_gripper_interfaces/action/MoveToRelativePosition.action +++ b/schunk_egu_egk_gripper_interfaces/action/MoveToRelativePosition.action @@ -6,4 +6,4 @@ bool position_reached --- float32 absolute_position float32 velocity_of_movement -float32 motor_current \ No newline at end of file +float32 motor_current diff --git a/schunk_egu_egk_gripper_interfaces/msg/State.msg b/schunk_egu_egk_gripper_interfaces/msg/State.msg index 1f0f127..838c719 100644 --- a/schunk_egu_egk_gripper_interfaces/msg/State.msg +++ b/schunk_egu_egk_gripper_interfaces/msg/State.msg @@ -21,4 +21,4 @@ bool wrong_workpiece_gripped bool no_workpiece_detected bool position_reached bool grip_force_and_position_maintenance_activated -bool ready_for_shutdown \ No newline at end of file +bool ready_for_shutdown diff --git a/schunk_egu_egk_gripper_interfaces/srv/Acknowledge.srv b/schunk_egu_egk_gripper_interfaces/srv/Acknowledge.srv index 6baef9e..4c2d7d7 100644 --- a/schunk_egu_egk_gripper_interfaces/srv/Acknowledge.srv +++ b/schunk_egu_egk_gripper_interfaces/srv/Acknowledge.srv @@ -1,2 +1,2 @@ --- -bool acknowledged \ No newline at end of file +bool acknowledged diff --git a/schunk_egu_egk_gripper_interfaces/srv/BrakeTest.srv b/schunk_egu_egk_gripper_interfaces/srv/BrakeTest.srv index 6bd7eb2..e0cac6d 100644 --- a/schunk_egu_egk_gripper_interfaces/srv/BrakeTest.srv +++ b/schunk_egu_egk_gripper_interfaces/srv/BrakeTest.srv @@ -1,3 +1,3 @@ --- bool brake_test_successful -string error_code \ No newline at end of file +string error_code diff --git a/schunk_egu_egk_gripper_interfaces/srv/ChangeIp.srv b/schunk_egu_egk_gripper_interfaces/srv/ChangeIp.srv index b60e062..6308764 100644 --- a/schunk_egu_egk_gripper_interfaces/srv/ChangeIp.srv +++ b/schunk_egu_egk_gripper_interfaces/srv/ChangeIp.srv @@ -1,3 +1,3 @@ string new_ip --- -bool ip_changed \ No newline at end of file +bool ip_changed diff --git a/schunk_egu_egk_gripper_interfaces/srv/FastStop.srv b/schunk_egu_egk_gripper_interfaces/srv/FastStop.srv index 3cb4c96..4d1c0b1 100644 --- a/schunk_egu_egk_gripper_interfaces/srv/FastStop.srv +++ b/schunk_egu_egk_gripper_interfaces/srv/FastStop.srv @@ -1,2 +1,2 @@ --- -bool fast_stopped \ No newline at end of file +bool fast_stopped diff --git a/schunk_egu_egk_gripper_interfaces/srv/GripperInfo.srv b/schunk_egu_egk_gripper_interfaces/srv/GripperInfo.srv index 73b314f..ed97d53 100644 --- a/schunk_egu_egk_gripper_interfaces/srv/GripperInfo.srv +++ b/schunk_egu_egk_gripper_interfaces/srv/GripperInfo.srv @@ -1 +1 @@ ---- \ No newline at end of file +--- diff --git a/schunk_egu_egk_gripper_interfaces/srv/ParameterSet.srv b/schunk_egu_egk_gripper_interfaces/srv/ParameterSet.srv index adc454f..7c4e9fd 100644 --- a/schunk_egu_egk_gripper_interfaces/srv/ParameterSet.srv +++ b/schunk_egu_egk_gripper_interfaces/srv/ParameterSet.srv @@ -5,11 +5,11 @@ uint8[] uint8_value uint16[] uint16_value uint32[] uint32_value int32[] int32_value -float32[] float_value +float32[] float_value --- bool[] actual_bool_value uint8[] actual_uint8_value uint16[] actual_uint16_value uint32[] actual_uint32_value int32[] actual_int32_value -float32[] actual_float_value \ No newline at end of file +float32[] actual_float_value diff --git a/schunk_egu_egk_gripper_interfaces/srv/PrepareForShutdown.srv b/schunk_egu_egk_gripper_interfaces/srv/PrepareForShutdown.srv index 7927b5a..bbc46d3 100644 --- a/schunk_egu_egk_gripper_interfaces/srv/PrepareForShutdown.srv +++ b/schunk_egu_egk_gripper_interfaces/srv/PrepareForShutdown.srv @@ -1,2 +1,2 @@ --- -bool prepared_for_shutdown \ No newline at end of file +bool prepared_for_shutdown diff --git a/schunk_egu_egk_gripper_interfaces/srv/ReleaseForManualMovement.srv b/schunk_egu_egk_gripper_interfaces/srv/ReleaseForManualMovement.srv index e91cbf7..f2fb068 100644 --- a/schunk_egu_egk_gripper_interfaces/srv/ReleaseForManualMovement.srv +++ b/schunk_egu_egk_gripper_interfaces/srv/ReleaseForManualMovement.srv @@ -1,2 +1,2 @@ --- -bool released_for_manual_movement \ No newline at end of file +bool released_for_manual_movement diff --git a/schunk_egu_egk_gripper_interfaces/srv/Softreset.srv b/schunk_egu_egk_gripper_interfaces/srv/Softreset.srv index e9bedff..4102fa4 100644 --- a/schunk_egu_egk_gripper_interfaces/srv/Softreset.srv +++ b/schunk_egu_egk_gripper_interfaces/srv/Softreset.srv @@ -1,2 +1,2 @@ --- -bool reset_success \ No newline at end of file +bool reset_success diff --git a/schunk_egu_egk_gripper_interfaces/srv/Stop.srv b/schunk_egu_egk_gripper_interfaces/srv/Stop.srv index dd12f87..91a705f 100644 --- a/schunk_egu_egk_gripper_interfaces/srv/Stop.srv +++ b/schunk_egu_egk_gripper_interfaces/srv/Stop.srv @@ -1,2 +1,2 @@ --- -bool stopped \ No newline at end of file +bool stopped diff --git a/schunk_egu_egk_gripper_library/include/schunk_egu_egk_gripper_library/communication.hpp b/schunk_egu_egk_gripper_library/include/schunk_egu_egk_gripper_library/communication.hpp index 08e93fd..56e37ec 100644 --- a/schunk_egu_egk_gripper_library/include/schunk_egu_egk_gripper_library/communication.hpp +++ b/schunk_egu_egk_gripper_library/include/schunk_egu_egk_gripper_library/communication.hpp @@ -4,12 +4,12 @@ // under the terms of the GNU General Public License as published by the Free // Software Foundation, either version 3 of the License, or (at your option) // any later version. -// +// // This program is distributed in the hope that it will be useful, but WITHOUT // ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or // FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for // more details. -// +// // You should have received a copy of the GNU General Public License along with // this program. If not, see . // -------------------------------------------------------------------------------- @@ -22,7 +22,7 @@ /* * Definitions to communicate with the Gripper via AnybusCom 40. - * + * * This task involves receiving a ByteString via HTTP, interpreting it, * and posting a ByteString if any action needs to be performed by the gripper. */ @@ -72,7 +72,7 @@ #define POSITION_REACHED 0x00200000 #define PRE_HOLDING 0x00400000 #define WORK_PIECE_LOST 0x00000100 -#define WRONG_WORKPIECE_DETECTED 0x00000200 +#define WRONG_WORKPIECE_DETECTED 0x00000200 #define BRAKE_ACTIVE 0x00000080 //Instance @@ -158,7 +158,7 @@ class AnybusCom {PLC_SYNC_INPUT_INST, &plc_sync_input}, {PLC_SYNC_OUTPUT_INST, &plc_sync_output} }; - + void updatePlc(std::string &, const std::string &); void updateFeedback(const std::string &); @@ -175,7 +175,7 @@ class AnybusCom uint8_t endian_format; //Flag for big/little Endian bool not_double_word; //Flag if double word is requested (double words are always big Endian) - const uint32_t mask = FAST_STOP | USE_GPE | GRIP_DIRECTION | REPEAT_COMMAND_TOGGLE; //Mask for setting Command bits to zero + const uint32_t mask = FAST_STOP | USE_GPE | GRIP_DIRECTION | REPEAT_COMMAND_TOGGLE; //Mask for setting Command bits to zero uint32_t last_command; //Saves last Command //Software @@ -189,7 +189,7 @@ class AnybusCom template std::string writeValueToString(paramtype); //Writes a given value to a hexadecimals string - template + template paramtype readParam(std::string); //Reads a hexadecimal string to value template void updateSaveData(std::vector &vector, const size_t &elements = 1, const size_t &counts = 1); //Updates variable save_data @@ -198,9 +198,9 @@ class AnybusCom void postCommand(); //Post plc_sync_output[4] void postParameter(std::string, std::string); //Post a Parameter with instance void getEnums(const char[7],const uint16_t &); //Get to an enum number the string - void getMetadata(const std::string &); + void getMetadata(const std::string &); void getInfo(); - template + template void getWithInstance(const char inst[7], paramtype *param = NULL, const size_t &elements = 1); //Get a Parameter with Instance template //Function to get Endian format void getWithOffset(const std::string &offset, const size_t & count, std::vector &vector, const size_t & elements = 1); //A Function just for Gripper Feedback @@ -212,9 +212,9 @@ class AnybusCom float grp_pos_margin; //Margin for workpiece detections float grp_prepos_delta; //Gripping pre-position delta float zero_pos_ofs; //zero position offset - + float max_pos; //max absolute position of gripper - float min_pos; //min absolute position of gripper + float min_pos; //min absolute position of gripper float min_vel; //min velocity float max_vel; //max velocity float max_grip_force; //max. grip force @@ -222,15 +222,15 @@ class AnybusCom float max_allow_force; //strong grip float max_grp_vel; //max. grip velocity - float actual_position, actual_velocity, actual_motor_current; //actual values (feedback) + float actual_position, actual_velocity, actual_motor_current; //actual values (feedback) public: - plc_Array plc_sync_input; // [0] -> Status double word, [1] ->actual Position, [2] ->reserved, [3] ->diagnose - plc_Array plc_sync_output; // [0] -> Control double word, [1] ->set_position, [2] -> set_velocity, [3] ->set_effort + plc_Array plc_sync_input; // [0] -> Status double word, [1] ->actual Position, [2] ->reserved, [3] ->diagnose + plc_Array plc_sync_output; // [0] -> Control double word, [1] ->set_position, [2] -> set_velocity, [3] ->set_effort + + nlohmann::json json_data; //raw Json-Data - nlohmann::json json_data; //raw Json-Data - std::vector bool_vector; std::vector uint8_vector; std::vector uint16_vector; @@ -249,7 +249,7 @@ class AnybusCom //Get something with Instance template inline void AnybusCom::getWithInstance(const char inst[7], paramtype *param, const size_t &elements) -{ +{ std::string response; CURLcode res; @@ -258,7 +258,7 @@ inline void AnybusCom::getWithInstance(const char inst[7], paramtype *param, con std::string address = data_address; address.append("inst=" + instance + "&count=1"); - if(curl) + if(curl) { //GET CURL curl_easy_setopt(curl, CURLOPT_URL, address.c_str()); @@ -267,7 +267,7 @@ inline void AnybusCom::getWithInstance(const char inst[7], paramtype *param, con curl_easy_setopt(curl, CURLOPT_WRITEDATA, &response); curl_easy_setopt(curl, CURLOPT_TIMEOUT, 1L); res = curl_easy_perform(curl); - + if (res != CURLE_OK) { fprintf(stderr, "curl_easy_perform_failed: %s\n", curl_easy_strerror(res)); @@ -277,8 +277,8 @@ inline void AnybusCom::getWithInstance(const char inst[7], paramtype *param, con else { if ((instance == PLC_SYNC_INPUT_INST || instance == PLC_SYNC_OUTPUT_INST) && elements == 1) updatePlc(response, instance); //If double word - else if(param != NULL && elements == 1) *param = readParam(response); //If single Parameter; - else save_response_string = response; //If an array just save the response + else if(param != NULL && elements == 1) *param = readParam(response); //If single Parameter; + else save_response_string = response; //If an array just save the response } curl_easy_reset(curl); } @@ -286,16 +286,16 @@ inline void AnybusCom::getWithInstance(const char inst[7], paramtype *param, con //Receive data with an offset template inline void AnybusCom::getWithOffset(const std::string &offset, const size_t & count, std::vector &vector,const size_t & elements) -{ +{ std::string response; CURLcode res; std::string address = data_address; - + if(offset.length() <= 5) address.append("offset=" + offset + "&count=" + std::to_string(count)); else throw "Offset to many symbols"; - if(curl) + if(curl) { //GET curl_easy_setopt(curl, CURLOPT_URL, address.c_str()); curl_easy_setopt(curl, CURLOPT_HTTPGET, 1); @@ -303,8 +303,8 @@ inline void AnybusCom::getWithOffset(const std::string &offset, const size_t & c curl_easy_setopt(curl, CURLOPT_WRITEDATA, &response); curl_easy_setopt(curl, CURLOPT_TIMEOUT, 1); - res = curl_easy_perform(curl); - + res = curl_easy_perform(curl); + if (res != CURLE_OK) { fprintf(stderr, "curl_easy_perform_failed: %s\n", curl_easy_strerror(res)); @@ -313,13 +313,13 @@ inline void AnybusCom::getWithOffset(const std::string &offset, const size_t & c } else - { + { if(count == 3 && offset == ACTUAL_POS_OFFSET) updateFeedback(response); //If ROS-Feedback - else - { + else + { save_response_string = response; - updateSaveData(vector, elements, count); - } + updateSaveData(vector, elements, count); + } } curl_easy_reset(curl); } @@ -327,7 +327,7 @@ inline void AnybusCom::getWithOffset(const std::string &offset, const size_t & c //Interprets the received bytestring into an variable of float or int(all except int8_t) template inline paramtype AnybusCom::readParam(std::string hex_str) -{ +{ if(hex_str.find("[",0) != std::string::npos) { hex_str.erase(hex_str.begin(), hex_str.begin() + 2); @@ -339,7 +339,7 @@ inline paramtype AnybusCom::readParam(std::string hex_str) if(endian_format == 0 && not_double_word == true) hex_str = changeEndianFormat(hex_str); //If little Endian //if float - if (std::is_same::value) + if (std::is_same::value) { uint32_t intValue; std::stringstream stream; @@ -348,7 +348,7 @@ inline paramtype AnybusCom::readParam(std::string hex_str) float floatValue; std::memcpy(&floatValue, &intValue, sizeof(float)); - + return floatValue; } else if(std::is_same::value || std::is_same::value || std::is_same::value) @@ -358,7 +358,7 @@ inline paramtype AnybusCom::readParam(std::string hex_str) hexStream >> std::hex >> value; return static_cast(value); } - else //if an integer + else //if an integer { uint32_t l = std::stoul(hex_str, nullptr, 16); return static_cast(l);; @@ -372,7 +372,7 @@ inline std::string AnybusCom::writeValueToString(paramtype param) std::stringstream stream; //if float if (std::is_same::value) - { + { int32_t* pa= reinterpret_cast(¶m); stream << std::hex << std::setw(8) << std::setfill('0') << *pa; } @@ -389,7 +389,7 @@ inline std::string AnybusCom::writeValueToString(paramtype param) template inline void AnybusCom::updateSaveData(std::vector &vector, const size_t &elements, const size_t &count) { - //If multiple parameter of the same size at once (saves all in a vector-> No matrix!) + //If multiple parameter of the same size at once (saves all in a vector-> No matrix!) if(count >= 2) { std::vector splitted; @@ -400,19 +400,19 @@ inline void AnybusCom::updateSaveData(std::vector &vector, const size for(size_t i = 0; i < count; i++) //which Parameter { for(size_t k = 0; k < elements; k++) //If parameter array - { + { vector[i*elements + k] = readParam(splitted[i].substr((k * sizeof(paramtype) * 2), sizeof(paramtype) * 2)); } } } - else //Else less complex + else //Else less complex { vector.clear(); vector.resize(elements); for(size_t i = 0; i < elements; i++) { - vector[i] = readParam(save_response_string.substr((i * sizeof(paramtype) * 2) + 2 , sizeof(paramtype) * 2)); + vector[i] = readParam(save_response_string.substr((i * sizeof(paramtype) * 2) + 2 , sizeof(paramtype) * 2)); not_double_word = true; } } diff --git a/schunk_egu_egk_gripper_library/include/schunk_egu_egk_gripper_library/schunk_gripper_lib.hpp b/schunk_egu_egk_gripper_library/include/schunk_egu_egk_gripper_library/schunk_gripper_lib.hpp index 4a50f26..15e6bad 100644 --- a/schunk_egu_egk_gripper_library/include/schunk_egu_egk_gripper_library/schunk_gripper_lib.hpp +++ b/schunk_egu_egk_gripper_library/include/schunk_egu_egk_gripper_library/schunk_gripper_lib.hpp @@ -4,12 +4,12 @@ // under the terms of the GNU General Public License as published by the Free // Software Foundation, either version 3 of the License, or (at your option) // any later version. -// +// // This program is distributed in the hope that it will be useful, but WITHOUT // ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or // FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for // more details. -// +// // You should have received a copy of the GNU General Public License along with // this program. If not, see . // -------------------------------------------------------------------------------- @@ -22,9 +22,9 @@ /* * Definitions for implementing common procedures to communicate with the gripper. * - * The gripper initializes at the beginning. It acts as the middleware between + * The gripper initializes at the beginning. It acts as the middleware between * communication and the Ros-Wrapper. - * + * */ #ifndef SCHUNK_GRIPPER_HPP #define SCHUNK_GRIPPER_HPP @@ -46,18 +46,18 @@ class Gripper : protected AnybusCom void getActualParameters(); //Get all necessary parameters void getModel(); //Get the string of the model and set flags void getVersions(); - + bool start_connection; bool model_M; - bool handshake; - std::string model; + bool handshake; + std::string model; bool gripperBitInput(const uint32_t&) const; //retrieve individual bits from the status double word bool gripperBitOutput(const uint32_t&) const; //retrieve individual bits from the control double word - + void acknowledge(); //acknowledge bool changeIp(const std::string &); //changeIP - + bool ip_changed_with_all_param; //IP and parameter changed uint32_t mm2mu(const float &); //Convert millimeters to micrometers. All unsigned -> For control double word @@ -70,14 +70,14 @@ class Gripper : protected AnybusCom void runPost(uint32_t command, uint32_t position = 0, uint32_t velocity = 0, uint32_t effort = 0); //Post control d void getParameter(const std::string& instance, const size_t&, const uint8_t&); std::string getErrorString(const uint8_t &); //Get for the error code the error string - + void sendAction(); void sendService(std::unique_lock &); bool check(); //Check for errors bool endCondition(); //Is the gripper in an endCondition? -> example Successbit is set - std::array splitDiagnosis(); //Split the diagnosis to get error, warning and not-feasible + std::array splitDiagnosis(); //Split the diagnosis to get error, warning and not-feasible bool post_requested; uint32_t set_position; @@ -97,7 +97,7 @@ inline void Gripper::changeOneParameter(const char inst[7], parametertype new_va { std::string value = writeValueToString(new_value); //Hexstring Value postParameter(inst, value); //Post - std::string instance = inst; + std::string instance = inst; getWithInstance(inst, store); } diff --git a/schunk_egu_egk_gripper_library/src/communication.cpp b/schunk_egu_egk_gripper_library/src/communication.cpp index c53b1dd..a65bbf4 100644 --- a/schunk_egu_egk_gripper_library/src/communication.cpp +++ b/schunk_egu_egk_gripper_library/src/communication.cpp @@ -4,12 +4,12 @@ // under the terms of the GNU General Public License as published by the Free // Software Foundation, either version 3 of the License, or (at your option) // any later version. -// +// // This program is distributed in the hope that it will be useful, but WITHOUT // ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or // FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for // more details. -// +// // You should have received a copy of the GNU General Public License along with // this program. If not, see . // -------------------------------------------------------------------------------- @@ -21,7 +21,7 @@ /* * Implementation to communicate with the Gripper via AnybusCom 40. - * + * * This task involves receiving a ByteString via HTTP, interpreting it, * and posting a ByteString if any action needs to be performed by the gripper. */ @@ -38,7 +38,7 @@ size_t writeCallback(void* contents, size_t size, size_t nmemb, void* userp) //Initialize the plcs and Addresses AnybusCom::AnybusCom(const std::string &ip) : ip(ip) -{ +{ initAddresses(); //Init addresses for post and get curl = curl_easy_init(); @@ -50,9 +50,9 @@ std::vector AnybusCom::splitResponse(const std::string &hex_str, co { std::vector splitted; splitted.resize(count); - for(size_t i = 0; i < count; i++) + for(size_t i = 0; i < count; i++) { - splitted[i] = hex_str.substr(2+(data_type_size*2 + 3)*i, data_type_size*2); + splitted[i] = hex_str.substr(2+(data_type_size*2 + 3)*i, data_type_size*2); } return splitted; } @@ -72,11 +72,11 @@ std::string AnybusCom::changeEndianFormat(const std::string &hexString) /** * @brief Performs a CURL request with the given POST data. - * + * * This function sets up the CURL options, including the URL, POST data, and write callback. * It then performs the CURL request and handles the response. * If the request fails, an exception is thrown with the corresponding error message. - * + * * @param post The POST data to send in the request. * @throws const char* An exception with the error message if the request fails. */ @@ -100,11 +100,11 @@ void AnybusCom::performCurlRequest(std::string post) curl_easy_reset(curl); throw curl_easy_strerror(res); } - else + else { json_data.clear(); json_data = nlohmann::json::parse(response); //Parse server response - if(json_data["result"] != 0) + if(json_data["result"] != 0) { std::string sever_res = "Server response: "; sever_res.append(to_string(json_data["result"])); @@ -117,25 +117,25 @@ void AnybusCom::performCurlRequest(std::string post) /** * @brief Posts a command to the Anybus communication module. - * + * * This function constructs a command string and sends it to the Anybus communication module. * The command string is constructed by appending the values of the plc_sync_output array as hexadecimal strings. * If the endian_format is 0, indicating big endian, the double_words are treated as big endian. - * + * * @note This function requires the curl library to be initialized. */ void AnybusCom::postCommand() -{ +{ std::string post = "inst=0x0048&value="; if(endian_format == 0) not_double_word = false; //Double_words are always big endian - for(int i = 0; i < 4 ; i++) + for(int i = 0; i < 4 ; i++) { post.append(writeValueToString(plc_sync_output[i])); //Array to HexString not_double_word = true; } - if (curl) + if (curl) { performCurlRequest(post); } @@ -143,13 +143,13 @@ void AnybusCom::postCommand() /** * @brief Posts a parameter to the Anybus communication module. - * + * * This function posts a parameter to the Anybus communication module * using the specified instruction and value. The instruction and value * are passed as strings. The function checks the length of the instruction * and value to ensure they are within the allowed limits. If the length * of either the instruction or value exceeds the limits, an exception is thrown. - * + * * @param inst The instruction to be posted. * @param value The value to be posted. * @throws An exception if the length of the instruction or value exceeds the limits. @@ -164,7 +164,7 @@ void AnybusCom::postParameter(std::string inst, std::string value) } else throw "Too many symbols."; - if (curl) + if (curl) { performCurlRequest(post); } @@ -172,7 +172,7 @@ void AnybusCom::postParameter(std::string inst, std::string value) //Inits used Addresses with the ip void AnybusCom::initAddresses() -{ +{ data_address = "http:///adi/data.json?"; update_address = "http:///adi/update.json"; enum_address = "http:///adi/enum.json?"; @@ -180,7 +180,7 @@ void AnybusCom::initAddresses() metadata_address = "http:///adi/metadata.json?"; if(ip.size() >= 100) ip = "0.0.0.0"; - + data_address.insert(7, ip); update_address.insert(7, ip); enum_address.insert(7,ip); @@ -191,8 +191,8 @@ void AnybusCom::initAddresses() //Translates the received string of double_word to an integer[4] an saves it in plc_sync_input void AnybusCom::updatePlc(std::string &hex_str,const std::string &inst) -{ - plc_Array *plc = plc_pairs.at(inst); +{ + plc_Array *plc = plc_pairs.at(inst); plc->fill(0); hex_str.erase(hex_str.begin(), hex_str.begin() + 2); @@ -211,7 +211,7 @@ void AnybusCom::updatePlc(std::string &hex_str,const std::string &inst) //Feedback strings getting translate and stored in corresponding variables void AnybusCom::updateFeedback(const std::string &hex_str) -{ +{ std::vector splitted; splitted = splitResponse(hex_str, 3, sizeof(float)); @@ -222,12 +222,12 @@ void AnybusCom::updateFeedback(const std::string &hex_str) //Update the plcOutput if a Command is send void AnybusCom::updatePlcOutput(uint32_t command, uint32_t position, uint32_t velocity, uint32_t effort) -{ +{ uint32_t actual_command = command & (~GRIP_DIRECTION); //Command without Grip direction if(last_command != actual_command && command != FAST_STOP) //If not repeating command so do this, and reset the sended Bits plc_sync_output[0] &= mask; - + if(GRIP_DIRECTION == (command & GRIP_DIRECTION)) plc_sync_output[0] ^= GRIP_DIRECTION; //if command have an positive Grip direction bit //Change the direction @@ -236,22 +236,22 @@ void AnybusCom::updatePlcOutput(uint32_t command, uint32_t position, uint32_t ve plc_sync_output[0] &= ~FAST_STOP; last_command = 0; } - else if(command == USE_GPE) //If GPE is requested toggle + else if(command == USE_GPE) //If GPE is requested toggle { plc_sync_output[0] ^= command; last_command = actual_command; } else if(last_command == actual_command) plc_sync_output[0] ^= REPEAT_COMMAND_TOGGLE; //REPEAT_COMMAND - else if(command == EMERGENCY_RELEASE) plc_sync_output[0] |= (FAST_STOP | EMERGENCY_RELEASE); - else - { + else if(command == EMERGENCY_RELEASE) plc_sync_output[0] |= (FAST_STOP | EMERGENCY_RELEASE); + else + { last_command = actual_command; plc_sync_output[0] |= actual_command; } plc_sync_output[1] = position; plc_sync_output[2] = velocity; - plc_sync_output[3] = effort; + plc_sync_output[3] = effort; } //Gets to enumNumber a corresponding Error String. response is saved in json_data @@ -264,15 +264,15 @@ void AnybusCom::getEnums(const char inst[7], const uint16_t &enumNum) address.append("inst=" + instance); address.append("&value=" + std::to_string(enumNum)); //Enum code to string - if(curl) - { + if(curl) + { //GET curl_easy_setopt(curl, CURLOPT_URL, address.c_str()); curl_easy_setopt(curl,CURLOPT_WRITEFUNCTION, writeCallback); curl_easy_setopt(curl, CURLOPT_HTTPGET, 1); curl_easy_setopt(curl, CURLOPT_WRITEDATA, &response); curl_easy_setopt(curl, CURLOPT_TIMEOUT, 1L); - + res = curl_easy_perform(curl); if (res != CURLE_OK) @@ -299,7 +299,7 @@ void AnybusCom::getInfo() std::string response; CURLcode res; - if(curl) + if(curl) { //GET curl_easy_setopt(curl, CURLOPT_URL, info_address.c_str()); @@ -337,7 +337,7 @@ void AnybusCom::getMetadata(const std::string &instance) address.append("inst=" + instance); address.append("&count=1"); //Metadata - if(curl) + if(curl) { //GET curl_easy_setopt(curl, CURLOPT_URL, address.c_str()); @@ -363,7 +363,7 @@ void AnybusCom::getMetadata(const std::string &instance) } curl_easy_reset(curl); - } + } } AnybusCom::~AnybusCom() diff --git a/schunk_egu_egk_gripper_library/src/schunk_gripper_lib.cpp b/schunk_egu_egk_gripper_library/src/schunk_gripper_lib.cpp index 0386848..b3a9653 100644 --- a/schunk_egu_egk_gripper_library/src/schunk_gripper_lib.cpp +++ b/schunk_egu_egk_gripper_library/src/schunk_gripper_lib.cpp @@ -4,12 +4,12 @@ // under the terms of the GNU General Public License as published by the Free // Software Foundation, either version 3 of the License, or (at your option) // any later version. -// +// // This program is distributed in the hope that it will be useful, but WITHOUT // ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or // FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for // more details. -// +// // You should have received a copy of the GNU General Public License along with // this program. If not, see . // -------------------------------------------------------------------------------- @@ -23,7 +23,7 @@ * Implementation for common procedures to communicate with the gripper. * * The gripper initializes at the beginning. It acts as the middleware between communication and the Ros-Wrapper. - * + * */ #include "schunk_egu_egk_gripper_library/schunk_gripper_lib.hpp" @@ -44,12 +44,12 @@ std::map commands_str {"SOFT RESET", SOFT_RESET} }; //Start th gripper, so it is ready to operate -Gripper::Gripper(const std::string &ip): -AnybusCom(ip), +Gripper::Gripper(const std::string &ip): +AnybusCom(ip), post_requested(false) -{ +{ try - { + { startGripper(); set_command = 0; set_position = actual_position; @@ -102,7 +102,7 @@ void Gripper::getModel() getEnums(MODULE_TYPE_INST, module_type); model = json_data["string"]; //Is it model M this means it has a break - if(model.find("_M_") != std::string::npos) + if(model.find("_M_") != std::string::npos) { std::cout << "Grip force and position maintenance!" << std::endl; model_M = true; @@ -113,16 +113,16 @@ void Gripper::getModel() model_M = false; } if(model.find("EGU") != std::string::npos) getWithInstance(MAX_ALLOW_FORCE_INST,&max_allow_force); - + std::cout << model << " CONNECTED!" << std::endl; } void Gripper::getParameter(const std::string& instance, const size_t& elements, const uint8_t& datatype) -{ +{ if(instance == PLC_SYNC_INPUT_INST || instance == PLC_SYNC_OUTPUT_INST) not_double_word = false; - + char inst[7]; - if(instance.size() < 7) + if(instance.size() < 7) { std::strcpy(inst, instance.c_str()); } @@ -135,12 +135,12 @@ void Gripper::getParameter(const std::string& instance, const size_t& elements, updateSaveData(uint8_vector, elements); //ROS 1 updateSaveData(bool_vector, elements); // ROS2 break; - + case UINT8_DATA : getWithInstance(inst, NULL, elements); updateSaveData( uint8_vector, elements); break; - + case UINT16_DATA: getWithInstance(inst, NULL, elements); updateSaveData(uint16_vector, elements); @@ -150,7 +150,7 @@ void Gripper::getParameter(const std::string& instance, const size_t& elements, getWithInstance(inst, NULL, elements); updateSaveData(uint32_vector, elements); break; - + case INT32_DATA: getWithInstance(inst, NULL, elements); updateSaveData(int32_vector, elements); @@ -160,7 +160,7 @@ void Gripper::getParameter(const std::string& instance, const size_t& elements, getWithInstance(inst, NULL, elements); updateSaveData(float_vector, elements); break; - + case CHAR_DATA: getWithInstance(inst, NULL, elements); updateSaveData(char_vector, elements); @@ -172,7 +172,7 @@ void Gripper::getParameter(const std::string& instance, const size_t& elements, break; } } - + bool Gripper::check() { if(plc_sync_input[3] == 0) return 1; @@ -184,26 +184,26 @@ std::array Gripper::splitDiagnosis() std::array error_codes; error_codes[0] = (plc_sync_input[3] >> 3*8) & 0xFF; error_codes[1] = (plc_sync_input[3] >> 2*8) & 0xFF; - error_codes[2] = plc_sync_input[3] & 0xFF; + error_codes[2] = plc_sync_input[3] & 0xFF; return error_codes; } //Return true when the Gripper is in end condition -bool Gripper::endCondition() +bool Gripper::endCondition() { - return (gripperBitInput(SUCCESS) || gripperBitInput(POSITION_REACHED) || gripperBitInput(NO_WORKPIECE_DETECTED) - || gripperBitInput(GRIPPED) || gripperBitInput(GRIPPER_ERROR) || gripperBitInput(WARNING) + return (gripperBitInput(SUCCESS) || gripperBitInput(POSITION_REACHED) || gripperBitInput(NO_WORKPIECE_DETECTED) + || gripperBitInput(GRIPPED) || gripperBitInput(GRIPPER_ERROR) || gripperBitInput(WARNING) || gripperBitInput(WORK_PIECE_LOST) || gripperBitInput(WRONG_WORKPIECE_DETECTED)); } //Post a Command and receive Gripper response void Gripper::runPost(uint32_t command, uint32_t position, uint32_t velocity, uint32_t effort) -{ +{ updatePlcOutput(command, position, velocity, effort); postCommand(); } //Receive Gripper response and actual Data void Gripper::runGets() -{ - if(post_requested) +{ + if(post_requested) { handshake = gripperBitInput(COMMAND_RECEIVED_TOGGLE); runPost(set_command, set_position, set_speed, set_gripping_force); @@ -213,8 +213,8 @@ void Gripper::runGets() } getWithOffset(ACTUAL_POS_OFFSET, 3, float_vector); - - if(post_requested) + + if(post_requested) { handshake = gripperBitInput(COMMAND_RECEIVED_TOGGLE); runPost(set_command, set_position, set_speed, set_gripping_force); @@ -222,10 +222,10 @@ void Gripper::runGets() post_requested = false; return; } - + getWithInstance(PLC_SYNC_INPUT_INST); - if(post_requested) + if(post_requested) { handshake = gripperBitInput(COMMAND_RECEIVED_TOGGLE); runPost(set_command, set_position, set_speed, set_gripping_force); @@ -235,7 +235,7 @@ void Gripper::runGets() } //If the gripper is ready for shutdown, so do softreset. DO: Acknowledge and get receive void Gripper::startGripper() -{ +{ //get dataformat -> big/little endian getInfo(); //Get plc Values @@ -260,7 +260,7 @@ void Gripper::startGripper() std::cout << ("SOFTRESET FINISHED") << std::endl; } - + //Get parameters getActualParameters(); //Model @@ -294,7 +294,7 @@ bool Gripper::gripperBitOutput(const uint32_t &bitmakro) const return bitmakro & plc_sync_output[0]; } //Get to an Error the corresponding string -std::string Gripper::getErrorString(const uint8_t &error) +std::string Gripper::getErrorString(const uint8_t &error) { try { @@ -323,7 +323,7 @@ void Gripper::acknowledge() bool Gripper::changeIp(const std::string &new_ip) { if(new_ip.size() > 100) return false; - + std::string old_ip = ip; ip = new_ip; std::string old_model = model; @@ -332,9 +332,9 @@ bool Gripper::changeIp(const std::string &new_ip) try { startGripper(); - + ip_changed_with_all_param = true; - + return true; } catch(const char* res) @@ -346,7 +346,7 @@ bool Gripper::changeIp(const std::string &new_ip) } catch(const nlohmann::json::parse_error &e) { - ip_changed_with_all_param = true; + ip_changed_with_all_param = true; ip = old_ip; std::cout << "message: " << e.what() << "\nexception id: " << e.id << std::endl; std::cout << "Setting to old IP: " << ip << std::endl; @@ -361,7 +361,7 @@ bool Gripper::changeIp(const std::string &new_ip) initAddresses(); return false; } - + } //send action directly after current (cyclic-) http or immediately void Gripper::sendAction() @@ -370,30 +370,30 @@ void Gripper::sendAction() std::unique_lock lock(lock_mutex); - if(post_requested == true) + if(post_requested == true) { handshake = gripperBitInput(COMMAND_RECEIVED_TOGGLE); - + runPost(set_command, set_position, set_speed, set_gripping_force); getWithInstance(PLC_SYNC_INPUT_INST); post_requested = false; } - if(handshake == gripperBitInput(COMMAND_RECEIVED_TOGGLE)) + if(handshake == gripperBitInput(COMMAND_RECEIVED_TOGGLE)) { post_requested = false; - throw int32_t(-1); + throw int32_t(-1); } lock.unlock(); } //send service directly after current (cyclic-) http or immediately. Locks the mutex. Does not unlock! void Gripper::sendService(std::unique_lock &lock) -{ +{ post_requested = true; lock.lock(); - if(post_requested == true) + if(post_requested == true) { handshake = gripperBitInput(COMMAND_RECEIVED_TOGGLE); runPost(set_command, set_position, set_speed, set_gripping_force); diff --git a/schunk_egu_egk_gripper_library/third_party_licenses/LICENSE.md b/schunk_egu_egk_gripper_library/third_party_licenses/LICENSE.md index cd6ee6b..5ebc263 100644 --- a/schunk_egu_egk_gripper_library/third_party_licenses/LICENSE.md +++ b/schunk_egu_egk_gripper_library/third_party_licenses/LICENSE.md @@ -2,7 +2,7 @@ This license applies only to the `json.hpp` file. The `json.hpp` file was copied from its official [Github repository](https://github.com/nlohmann/json). -MIT License +MIT License Copyright (c) 2013-2022 Niels Lohmann