diff --git a/.github/workflows/industrial_ci_humble_action.yml b/.github/workflows/industrial_ci_humble_action.yml
index 7c1c1d9..7e3308b 100644
--- a/.github/workflows/industrial_ci_humble_action.yml
+++ b/.github/workflows/industrial_ci_humble_action.yml
@@ -19,6 +19,6 @@ jobs:
- {ROS_DISTRO: humble, ROS_REPO: main}
runs-on: ubuntu-latest
steps:
- - uses: actions/checkout@v1
+ - uses: actions/checkout@v4
- uses: 'ros-industrial/industrial_ci@master'
env: ${{matrix.env}}
diff --git a/.github/workflows/industrial_ci_iron_action.yml b/.github/workflows/industrial_ci_iron_action.yml
index 4e518c0..3d1ffd4 100644
--- a/.github/workflows/industrial_ci_iron_action.yml
+++ b/.github/workflows/industrial_ci_iron_action.yml
@@ -19,6 +19,6 @@ jobs:
- {ROS_DISTRO: iron, ROS_REPO: main}
runs-on: ubuntu-latest
steps:
- - uses: actions/checkout@v1
+ - uses: actions/checkout@v4
- uses: 'ros-industrial/industrial_ci@master'
env: ${{matrix.env}}
diff --git a/.github/workflows/industrial_ci_jazzy_action.yml b/.github/workflows/industrial_ci_jazzy_action.yml
new file mode 100644
index 0000000..ec906f8
--- /dev/null
+++ b/.github/workflows/industrial_ci_jazzy_action.yml
@@ -0,0 +1,24 @@
+name: Jazzy
+
+on:
+ push:
+ pull_request:
+ schedule:
+ # Run every Friday at 6:30 am to detect breaking APIs
+ - cron: '30 6 * * 5'
+
+
+jobs:
+ industrial_ci:
+ env:
+ BEFORE_BUILD_TARGET_WORKSPACE: '.github/script/install_dependencies.sh'
+ strategy:
+ fail-fast: false
+ matrix:
+ env:
+ - {ROS_DISTRO: jazzy, ROS_REPO: main}
+ runs-on: ubuntu-latest
+ steps:
+ - uses: actions/checkout@v4
+ - uses: 'ros-industrial/industrial_ci@master'
+ env: ${{matrix.env}}
diff --git a/.github/workflows/industrial_ci_rolling_action.yml b/.github/workflows/industrial_ci_rolling_action.yml
index 78a21f9..6868f50 100644
--- a/.github/workflows/industrial_ci_rolling_action.yml
+++ b/.github/workflows/industrial_ci_rolling_action.yml
@@ -20,6 +20,6 @@ jobs:
- {ROS_DISTRO: rolling, ROS_REPO: main}
runs-on: ubuntu-latest
steps:
- - uses: actions/checkout@v1
+ - uses: actions/checkout@v4
- uses: 'ros-industrial/industrial_ci@master'
env: ${{matrix.env}}
diff --git a/README.md b/README.md
index 50dad2f..1c25ce4 100644
--- a/README.md
+++ b/README.md
@@ -14,6 +14,9 @@
+
+
+
@@ -82,7 +85,7 @@ a) Using a component manager – this process is also outlined in the launch fil
b) Within your custom executable, coupled with a (multithreaded-)executor.
-If you prefer starting the node in your main function, ensure that you include `Schunk::schunk_egu_egk_gripper_driver` in CMake's `target_link_libraries()`. Additionally, always specify the IP address, setting the parameter overrides "IP" to your designated IP. Alternatively, create the `SchunkGripperNode`-Component and utilize the `reconnect` service, specifying your IP.
+If you prefer starting the node in your main function, ensure that you include `Schunk::schunk_egu_egk_gripper_driver` in CMake's `target_link_libraries()`. Additionally, always specify the IP address, setting the parameter overrides "IP" to your designated IP.
## Actions
All functionalities of the gripper, including movement, are treated as actions. This implies that when gripping, moving, or releasing a workpiece, you need to send a goal and can receive a result or feedback. Releasing a workpiece is the only action where you send an empty goal:
@@ -108,10 +111,8 @@ Services are functionalities that do not involve movement or occur so rapidly th
- `softreset`
- `parameter_get`
- `parameter_set`
-- `reconnect`
- `release_for_manual_movement`
- `prepare_for_shutdown`
-- `gripper_info`
**Important:** During a soft reset, no topics will be published. This will last for approximately 7 seconds. Afterward, all publications resume, and you can modify parameters.
@@ -121,10 +122,6 @@ Services are functionalities that do not involve movement or occur so rapidly th
All other services can be used whenever you like. (**Note:** Fast stop is an abort of movement, so it always provokes an error).
-`gripper_info` publishes some information about the gripper on the terminal screen.
-
-`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
## Parameters
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 f6924fa..dd9173f 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
@@ -47,7 +47,6 @@
#include "schunk_egu_egk_gripper_interfaces/action/release_workpiece.hpp"
#include "schunk_egu_egk_gripper_interfaces/srv/release_for_manual_movement.hpp"
#include "schunk_egu_egk_gripper_interfaces/srv/gripper_info.hpp"
-#include "schunk_egu_egk_gripper_interfaces/srv/change_ip.hpp"
#include "schunk_egu_egk_gripper_interfaces/action/grip.hpp"
#include "schunk_egu_egk_gripper_interfaces/srv/parameter_get.hpp"
#include "schunk_egu_egk_gripper_interfaces/srv/parameter_set.hpp"
@@ -70,7 +69,6 @@ class SchunkGripperNode : public rclcpp::Node, public Gripper
using Softreset = schunk_egu_egk_gripper_interfaces::srv::Softreset;
using PrepareForShutdown = schunk_egu_egk_gripper_interfaces::srv::PrepareForShutdown;
using GripperInfo= schunk_egu_egk_gripper_interfaces::srv::GripperInfo;
- using ChangeIp = schunk_egu_egk_gripper_interfaces::srv::ChangeIp;
using ParameterGet = schunk_egu_egk_gripper_interfaces::srv::ParameterGet;
using ParameterSet = schunk_egu_egk_gripper_interfaces::srv::ParameterSet;
@@ -145,11 +143,10 @@ class SchunkGripperNode : public rclcpp::Node, public Gripper
void fast_stop_srv(const std::shared_ptr, std::shared_ptr);
void parameter_get_srv(const std::shared_ptr, std::shared_ptr);
void parameter_set_srv(const std::shared_ptr, std::shared_ptr);
- void change_ip_srv(const std::shared_ptr, std::shared_ptr);
void releaseForManualMov_srv(const std::shared_ptr, std::shared_ptr);
void softreset_srv(const std::shared_ptr, std::shared_ptr);
void prepare_for_shutdown_srv(const std::shared_ptr, std::shared_ptr);
- void info_srv(const std::shared_ptr, std::shared_ptr);
+ void info_srv(const std::shared_ptr, std::shared_ptr res);
//Action-basic-functions
template
@@ -198,7 +195,6 @@ class SchunkGripperNode : public rclcpp::Node, public Gripper
rclcpp::CallbackGroup::SharedPtr messages_group;
rclcpp::CallbackGroup::SharedPtr services_group;
rclcpp::CallbackGroup::SharedPtr actions_group;
- rclcpp::CallbackGroup::SharedPtr rest;
public:
@@ -224,7 +220,6 @@ class SchunkGripperNode : public rclcpp::Node, public Gripper
rclcpp::Service::SharedPtr softreset_service;
rclcpp::Service::SharedPtr prepare_for_shutdown_service;
rclcpp::Service::SharedPtr info_service;
- rclcpp::Service::SharedPtr change_ip_service;
};
#endif
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 5758742..6dd8db6 100644
--- a/schunk_egu_egk_gripper_driver/src/schunk_gripper_wrapper.cpp
+++ b/schunk_egu_egk_gripper_driver/src/schunk_gripper_wrapper.cpp
@@ -55,13 +55,10 @@ SchunkGripperNode::SchunkGripperNode(const rclcpp::NodeOptions &options) :
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;
- rclcpp::SubscriptionOptions option;
- option.callback_group = rest;
//ParameterDescription read_only
rcl_interfaces::msg::ParameterDescriptor paramDesc;
paramDesc.read_only = true;
@@ -162,8 +159,7 @@ void SchunkGripperNode::advertiseServices()
fast_stop_service = this->create_service("fast_stop", std::bind(&SchunkGripperNode::fast_stop_srv,this,std::placeholders::_1,std::placeholders::_2), rmw_qos_profile_services_default, services_group);
parameter_get_service = this->create_service("parameter_get", std::bind(&SchunkGripperNode::parameter_get_srv,this,std::placeholders::_1,std::placeholders::_2), rmw_qos_profile_services_default, services_group);
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);
+ info_service = this->create_service("gripper_info", std::bind(&SchunkGripperNode::info_srv,this,std::placeholders::_1,std::placeholders::_2), rmw_qos_profile_services_default, services_group);
}
//Advertise Actions
@@ -1195,7 +1191,7 @@ void SchunkGripperNode::brake_test_srv(const std::shared_ptr
}
if((gripperBitInput(SUCCESS) == true) && (handshake != gripperBitInput(COMMAND_RECEIVED_TOGGLE)))
{
- res->brake_test_successful = true;
+ res->success = true;
res->error_code = error_str;
RCLCPP_INFO(this->get_logger(),"Brake test successful!");
}
@@ -1204,7 +1200,7 @@ void SchunkGripperNode::brake_test_srv(const std::shared_ptr
runGets();
gripper_updater->force_update();
res->error_code = error_str;
- res->brake_test_successful = false;
+ res->success = false;
RCLCPP_INFO(this->get_logger(),"Brake test failed!");
}
@@ -1213,53 +1209,12 @@ void SchunkGripperNode::brake_test_srv(const std::shared_ptr
{
connection_error = server_response;
res->error_code = server_response;
- res->brake_test_successful = false;
+ res->success = 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);
- res->ip_changed = changeIp(req->new_ip);
-
- if(res->ip_changed == false)
- {
- connection_error = "No gripper found. Using old IP";
- gripper_updater->force_update();
- return;
- }
-
- 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;
- }
-
- if(ip_changed_with_all_param == true)
- {
- this->set_parameter(rclcpp::Parameter("model", model));
-
- if(connection_error == "OK")
- {
- reconnect();
- }
- }
- }
- catch(...)
- {
- RCLCPP_ERROR(this->get_logger(), "Handshake failed!");
- }
-
-}
//Stop service callback
void SchunkGripperNode::stop_srv(const std::shared_ptr, std::shared_ptr res)
{
@@ -1290,12 +1245,12 @@ void SchunkGripperNode::stop_srv(const std::shared_ptr, std::shar
if((gripperBitInput(SUCCESS) == 1) && (handshake != gripperBitInput(COMMAND_RECEIVED_TOGGLE)))
{
- res->stopped = 1;
+ res->success = true;
RCLCPP_WARN(this->get_logger(),"Stopped!");
}
else
{
- res->stopped = 0;
+ res->success = false;
last_command = 0;
}
@@ -1323,12 +1278,12 @@ void SchunkGripperNode::fast_stop_srv(const std::shared_ptr,
if(gripperBitInput(GRIPPER_ERROR))
{
- res->fast_stopped = 1;
+ res->success = true;
RCLCPP_WARN(this->get_logger(),"Fast stopped!");
}
else
{
- res->fast_stopped = 0;
+ res->success = false;
RCLCPP_WARN(this->get_logger(),"Fast stop failed!");
}
}
@@ -1336,7 +1291,7 @@ void SchunkGripperNode::fast_stop_srv(const std::shared_ptr,
{
connection_error = server_err;
RCLCPP_ERROR(this->get_logger(), "Failed Connection! %s", connection_error.c_str());
- res->fast_stopped = 0;
+ res->success = false;
RCLCPP_WARN(this->get_logger(), "Fast stop failed!");
}
gripper_updater->force_update();
@@ -1345,59 +1300,40 @@ void SchunkGripperNode::fast_stop_srv(const std::shared_ptr,
void SchunkGripperNode::softreset_srv(const std::shared_ptr, std::shared_ptr res)
{
std::unique_lock lock(lock_mutex, std::defer_lock);
+ getParameter(SYSTEM_UPTIME_INST, 1, UINT32_DATA);
+ const auto uptime_before_reset = uint32_vector[0];
+ auto uptime = uptime_before_reset;
+
+ // Detect soft resets by checking the system uptime.
+ auto system_reset = [&](){
+ try {
+ getParameter(SYSTEM_UPTIME_INST, 1, UINT32_DATA);
+ uptime = uint32_vector[0];
+ }
+ catch(const char* error){}
+ return (uptime < uptime_before_reset) ? true : false;
+ };
+
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++)
- {
- try
- {
- runGets(); //if connection is back, don't catch
- if(connection_once_lost == true)
- {
- connection_error = "OK";
- break;
- }
- }
- catch(const char* res)
- {
- connection_error = res;
- std::chrono::milliseconds sleep_time(10);
- std::this_thread::sleep_for(sleep_time);
- connection_once_lost = true;
- }
- }
+ catch(const char* error){
+ RCLCPP_INFO(this->get_logger(), "Softreset failed!");
+ res->success = false;
}
- else RCLCPP_WARN(this->get_logger(), "Handshake failed!");
- if(connection_once_lost == true && rclcpp::ok() && connection_error == "OK")
+ while (!system_reset())
{
- RCLCPP_INFO(this->get_logger(), "Softreset succeeded!");
- res->reset_success = true;
- }
- else
- {
- RCLCPP_INFO(this->get_logger(), "Softreset failed!");
- res->reset_success = false;
+ runGets();
}
+ res->success = true;
lock.unlock();
gripper_updater->force_update();
-
}
//Prepare for shutdown service callback
void SchunkGripperNode::prepare_for_shutdown_srv(const std::shared_ptr, std::shared_ptr res)
@@ -1428,12 +1364,12 @@ void SchunkGripperNode::prepare_for_shutdown_srv(const std::shared_ptrget_logger(),"READY FOR SHUTDOWN");
- res->prepared_for_shutdown = true;
+ res->success = true;
}
else
{
RCLCPP_INFO(this->get_logger(),"COMMAND FAILED");
- res->prepared_for_shutdown = false;
+ res->success = false;
}
gripper_updater->force_update();
@@ -1493,30 +1429,33 @@ void SchunkGripperNode::releaseForManualMov_srv(const std::shared_ptrget_logger(),"YOU CAN TAKE THE WORKPIECE!\n");
RCLCPP_WARN(this->get_logger(),"If you want to end this mode, perform a fast stop and acknowledge!");
- res->released_for_manual_movement = true;
+ res->success = true;
}
else
{
RCLCPP_WARN(this->get_logger(),"COMMAND FAILED");
- res->released_for_manual_movement = false;
+ res->success = false;
}
gripper_updater->force_update();
}
//Get infos of the gripper service
-void SchunkGripperNode::info_srv(const std::shared_ptr, std::shared_ptr)
+void SchunkGripperNode::info_srv(const std::shared_ptr, std::shared_ptr res)
{
+ auto info_stream = std::stringstream();
const std::lock_guard lock(lock_mutex);
try
{
- if(wrong_version) RCLCPP_WARN_STREAM(this->get_logger(),"Using not suitable software-version. Some informations may be misleading");
-
+ if(wrong_version)
+ {
+ info_stream << "Using not suitable software-version. Some informations may be misleading";
+ }
std::vector char_strings;
uint32_t data;
uint16_t data2;
float data3;
- RCLCPP_INFO_STREAM(this->get_logger(),"\n\n\nGRIPPER TYPE: " << model.c_str()
- << "\nIP: " << ip << std::endl);
+ info_stream << "\n\n\nGRIPPER TYPE: " << model.c_str()
+ << "\nIP: " << ip << std::endl;
getEnums(FIELDBUS_TYPE_INST,fieldbus_type);
getParameter(MAC_ADDR_INST, 6, CHAR_DATA);
@@ -1524,10 +1463,9 @@ void SchunkGripperNode::info_srv(const std::shared_ptr, st
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"]
+ info_stream << "\nFieldbustype: " << json_data["string"]
<< "\nMac-address: " << std::hex << mac[0] << ":" << mac[1] << ":" << mac[2] << ":" << mac[3] << ":"
- << mac[4] << ":" << mac[5] << std::endl);
+ << mac[4] << ":" << mac[5] << std::endl;
//Get the char-Parameter and save them as strings in char_strings
@@ -1545,64 +1483,64 @@ void SchunkGripperNode::info_srv(const std::shared_ptr, st
getWithInstance(SERIAL_NO_NUM_INST, &data);
getWithInstance(SW_VERSION_NUM_INST, &data2);
- RCLCPP_INFO_STREAM(this->get_logger(),"\nSerial number text: " << char_strings[0]
+ info_stream << "\nSerial number text: " << char_strings[0]
<<"\nOrder number text: " << char_strings[1]
<<"\nDevice serial number encoded: " << data
<<"\nMain software build date: " << char_strings[2]
<<"\nMain software build time: " << char_strings[3]
<<"\nMain software version short: " << data2
<<"\nMain software version: " << char_strings[4]
- <<"\nCommunication software version: " << comm_version << std::endl);
+ <<"\nCommunication software version: " << comm_version << std::endl;
getWithInstance(UPTIME_INST, &data);
- RCLCPP_INFO_STREAM(this->get_logger(),"\nSystem uptime: " << data << " s" << std::endl);
+ info_stream << "\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);
+ info_stream << "\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"
+ info_stream << "\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);
+ << 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"
+ info_stream << "\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);
+ << 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"
+ info_stream << "\nMin. absolute position: " << min_pos << " mm\n"
<< "Max. absolute position: " << max_pos << " mm\n"
- << "Zero_pos_offset: " << zero_pos_ofs << " mm" << std::endl);
+ << "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"
+ info_stream << "\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);
+ << "Max. grip force: " << max_grip_force << " N" << std::endl;
if(model.find("EGU") != std::string::npos)
{
- RCLCPP_INFO_STREAM(this->get_logger(),"\nMax allowed grip force StrongGrip: " << max_allow_force << " N " << std::endl);
+ info_stream << "\nMax allowed grip force StrongGrip: " << max_allow_force << " N " << std::endl;
}
getWithInstance(USED_CUR_LIMIT_INST, &data3);
- RCLCPP_INFO_STREAM(this->get_logger(),"\nUsed current limit: " << data3 << " A" << std::endl);
+ info_stream << "\nUsed current limit: " << data3 << " A" << std::endl;
getWithInstance(MAX_PHYS_STROKE_INST, &data3);
- RCLCPP_INFO_STREAM(this->get_logger(),"Max. physical stroke: " << data3 << " mm" << std::endl);
+ info_stream << "Max. physical stroke: " << data3 << " mm" << std::endl;
getWithOffset(MIN_ERR_MOT_VOLT_OFFSET, 6, float_vector);
- RCLCPP_INFO_STREAM(this->get_logger(),"\nMin. error motor voltage: " << float_vector[0] << " V\n"
+ info_stream << "\nMin. error motor voltage: " << float_vector[0] << " V\n"
<< "Max. error motor voltage: " << float_vector[1] << " V\n"
<< "Min. error logic voltage: " << float_vector[2] << " V\n"
<< "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);
+ << "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);
+ info_stream << "Measured logic temperature: " << data3 << " C" << std::endl;
getWithOffset(MEAS_LGC_VOLT_OFFSET, 8, float_vector);
- RCLCPP_INFO_STREAM(this->get_logger(),
+ info_stream <<
"\nMeasured logic voltage: " << float_vector[0] << " V\n"
<< "Measured motor voltage: " << float_vector[1] << " V\n"
<< "Min. warning motor voltage: " << float_vector[2] << " V\n"
@@ -1610,16 +1548,20 @@ void SchunkGripperNode::info_srv(const std::shared_ptr, st
<< "Min. warning logic voltage: " << float_vector[4] << " V\n"
<< "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);
+ << "Max. warning logic temperature: " << float_vector[7] << " C" << std::endl;
+ res->info = info_stream.str();
+ res->success = true;
}
- catch(const char* res)
+ catch(const char* error)
{
- connection_error = res;
- RCLCPP_ERROR(this->get_logger(), "Failed Connection! %s", connection_error.c_str());
+ connection_error = error;
+ res->success = false;
+ RCLCPP_ERROR(this->get_logger(), "Failed Connection! %s", connection_error.c_str());
}
catch(const std::exception &e)
{
+ res->success = false;
RCLCPP_ERROR(this->get_logger(), "%s", e.what());
}
}
diff --git a/schunk_egu_egk_gripper_dummy/README.md b/schunk_egu_egk_gripper_dummy/README.md
index 66e8d24..82e2346 100644
--- a/schunk_egu_egk_gripper_dummy/README.md
+++ b/schunk_egu_egk_gripper_dummy/README.md
@@ -10,7 +10,7 @@ pip install --user fastapi uvicorn
## Getting started
1. Start the dummy standalone with
```bash
- uvicorn schunk_egu_egk_gripper_dummy.main:server --port 8000 --reload
+ ./start_dummy.sh
```
## Run tests locally
diff --git a/schunk_egu_egk_gripper_dummy/config/data.json b/schunk_egu_egk_gripper_dummy/config/data.json
index 371216e..b28eda0 100644
--- a/schunk_egu_egk_gripper_dummy/config/data.json
+++ b/schunk_egu_egk_gripper_dummy/config/data.json
@@ -24,11 +24,14 @@
"00000000"
],
"0x0230": [
- "42263D87"
+ "00000000"
],
"0x0238": [
"00000000"
],
+ "0x0240": [
+ "3C56B24F"
+ ],
"0x0380": [
"0000"
],
@@ -69,19 +72,19 @@
"00000000"
],
"0x0628": [
- "40B00000"
+ "0000B040"
],
"0x0630": [
- "42E60000"
+ "0000E642"
],
"0x0650": [
"41B00000"
],
"0x0658": [
- "42960000"
+ "00009642"
],
"0x0660": [
- "43160000"
+ "0000B442"
],
"0x06A8": [
"00000000"
diff --git a/schunk_egu_egk_gripper_dummy/config/system_parameter_codes b/schunk_egu_egk_gripper_dummy/config/system_parameter_codes
index 0b38c21..2464603 100644
--- a/schunk_egu_egk_gripper_dummy/config/system_parameter_codes
+++ b/schunk_egu_egk_gripper_dummy/config/system_parameter_codes
@@ -13,6 +13,7 @@
0x0210
0x0230
0x0238
+0x0240
0x0380
0x03A8
0x03B0
diff --git a/schunk_egu_egk_gripper_dummy/schunk_egu_egk_gripper_dummy/main.py b/schunk_egu_egk_gripper_dummy/schunk_egu_egk_gripper_dummy/main.py
index 46ec41f..ac3a06d 100644
--- a/schunk_egu_egk_gripper_dummy/schunk_egu_egk_gripper_dummy/main.py
+++ b/schunk_egu_egk_gripper_dummy/schunk_egu_egk_gripper_dummy/main.py
@@ -7,6 +7,7 @@
# Components
dummy = Dummy()
+dummy.start()
server = FastAPI()
client = ["http://localhost:8001"]
diff --git a/schunk_egu_egk_gripper_dummy/src/dummy.py b/schunk_egu_egk_gripper_dummy/src/dummy.py
index 2d7a045..3115813 100644
--- a/schunk_egu_egk_gripper_dummy/src/dummy.py
+++ b/schunk_egu_egk_gripper_dummy/src/dummy.py
@@ -38,6 +38,7 @@ def sample(self, t: float) -> Tuple[float, float]:
class Dummy(object):
def __init__(self):
+ self.starttime = time.time()
self.thread = Thread(target=self._run)
self.running = False
self.done = False
@@ -48,6 +49,7 @@ def __init__(self):
self.plc_output = "0x0048"
self.actual_position = "0x0230"
self.actual_speed = "0x0238"
+ self.system_uptime = "0x1400"
self.error_byte = 12
self.diagnostics_byte = 15
self.valid_status_bits = list(range(0, 10)) + [11, 12, 13, 14, 16, 17, 31]
@@ -73,6 +75,7 @@ def __init__(self):
self.plc_input_buffer = bytearray(bytes.fromhex(self.data[self.plc_input][0]))
self.plc_output_buffer = bytearray(bytes.fromhex(self.data[self.plc_output][0]))
+ self.initial_state = [self.plc_input_buffer.hex().upper()]
def start(self) -> None:
if self.running:
@@ -87,6 +90,8 @@ def stop(self) -> None:
def _run(self) -> None:
while not self.done:
+ elapsed = time.time() - self.starttime
+ self.set_system_uptime(int(elapsed))
time.sleep(1)
print("Done")
@@ -99,7 +104,7 @@ def move(self, target_pos: float, target_speed: float) -> None:
)
start = time.time()
actual_pos, actual_speed = motion.sample(0)
- while abs(actual_pos) < abs(target_pos):
+ while abs(target_pos - actual_pos) > 0.001: # mm
t = time.time() - start
actual_pos, actual_speed = motion.sample(t)
self.set_actual_position(actual_pos)
@@ -140,7 +145,10 @@ def get_data(self, query: dict[str, str]) -> list:
if offset + count >= len(self.metadata):
return result
for i in range(count):
- result.append(self.metadata[offset + i])
+ id = self.metadata[offset + i]["instance"]
+ inst = hex(id)[2:].upper().zfill(4)
+ inst = "0x" + inst
+ result.append(self.data[inst][0])
return result
if "inst" in query and "count" in query:
@@ -223,11 +231,25 @@ def get_control_bit(self, bit: int) -> int | bool:
byte_index, bit_index = divmod(bit, 8)
return 1 if self.plc_output_buffer[byte_index] & (1 << bit_index) != 0 else 0
+ def set_control_bit(self, bit: int, value: bool) -> bool:
+ if bit < 0 or bit > 31:
+ return False
+ if bit in self.reserved_control_bits:
+ return False
+ byte_index, bit_index = divmod(bit, 8)
+ if value:
+ self.plc_output_buffer[byte_index] |= 1 << bit_index
+ else:
+ self.plc_output_buffer[byte_index] &= ~(1 << bit_index)
+ return True
+
def get_target_position(self) -> float:
- return struct.unpack("f", self.plc_output_buffer[4:8])[0]
+ return struct.unpack("i", self.plc_output_buffer[4:8])[0] / 1000.0 # um to mm
def get_target_speed(self) -> float:
- return struct.unpack("f", self.plc_output_buffer[8:12])[0]
+ return (
+ struct.unpack("i", self.plc_output_buffer[8:12])[0] / 1000.0
+ ) # um/s to mm/s
def set_actual_position(self, position: float) -> None:
self.data[self.actual_position] = [
@@ -245,7 +267,27 @@ def get_actual_speed(self) -> float:
read_speed = self.data[self.actual_speed][0]
return struct.unpack("f", bytes.fromhex(read_speed))[0]
+ def set_system_uptime(self, uptime: int) -> None:
+ self.data[self.system_uptime] = [bytes(struct.pack("i", uptime)).hex().upper()]
+
+ def get_system_uptime(self) -> int:
+ uptime = self.data[self.system_uptime][0]
+ return struct.unpack("i", bytes.fromhex(uptime))[0]
+
def process_control_bits(self) -> None:
+ """
+ See the gripper's firmware documentation for EtherNet/IP [1]:
+ https://stb.cloud.schunk.com/media/IM0046706.PDF
+
+ """
+ # Reset success of previous commands
+ self.set_status_bit(bit=4, value=False)
+ self.set_status_bit(bit=8, value=False)
+ self.set_status_bit(bit=12, value=False)
+ self.set_status_bit(bit=13, value=False)
+
+ # Command received toggle
+ self.toggle_status_bit(bit=5)
# Acknowledge
if self.get_control_bit(2) == 1:
@@ -254,12 +296,71 @@ def process_control_bits(self) -> None:
self.set_status_error("00")
self.set_status_diagnostics("00")
+ # Brake test
+ if self.get_control_bit(30) == 1:
+ self.set_status_bit(bit=4, value=True)
+
+ # Fast stop
+ if self.get_control_bit(0) == 0: # fail-safe behavior
+ self.set_status_bit(bit=7, value=True)
+ self.set_status_diagnostics("D9")
+
+ # Controlled stop
+ if self.get_control_bit(1) == 1:
+ self.set_status_bit(bit=13, value=True)
+ self.set_status_bit(bit=4, value=True)
+
+ # Manual release
+ if self.get_control_bit(5) == 1:
+ if self.get_status_bit(7) == 1:
+ self.set_status_bit(bit=8, value=True)
+
+ # Shutdown
+ if self.get_control_bit(3) == 1:
+ self.set_status_bit(bit=2, value=True)
+
+ # Release workpiece
+ if self.get_control_bit(bit=11) == 1:
+ self.set_status_bit(bit=4, value=True)
+ self.set_status_bit(bit=13, value=True)
+ self.set_status_bit(bit=14, value=False)
+ self.set_status_bit(bit=12, value=False)
+ self.set_status_bit(bit=17, value=False)
+
+ # Soft reset
+ if self.get_control_bit(bit=4) == 1:
+ self.set_system_uptime(0)
+ self.data[self.plc_input] = self.initial_state
+
# Move to absolute position
- if self.get_control_bit(13) == 1:
- self.toggle_status_bit(5)
+ if self.get_control_bit(bit=13) == 1:
self.move(
target_pos=self.get_target_position(),
target_speed=self.get_target_speed(),
)
self.set_status_bit(bit=13, value=True)
self.set_status_bit(bit=4, value=True)
+
+ # Move to relative position
+ if self.get_control_bit(bit=14) == 1:
+ target_pos = (
+ self.get_actual_position()
+ + self.get_target_position() # interpret relative
+ )
+ self.move(
+ target_pos=target_pos,
+ target_speed=self.get_target_speed(),
+ )
+ self.set_status_bit(bit=13, value=True)
+ self.set_status_bit(bit=4, value=True)
+
+ # Grip workpiece
+ if self.get_control_bit(bit=12) == 1:
+ self.set_status_bit(bit=12, value=True)
+ self.set_status_bit(bit=4, value=True)
+
+ # Grip workpiece at position
+ if self.get_control_bit(bit=16) == 1:
+ self.set_status_bit(bit=12, value=True)
+ self.set_status_bit(bit=4, value=True)
+ self.set_status_bit(bit=31, value=True)
diff --git a/schunk_egu_egk_gripper_dummy/start_dummy.sh b/schunk_egu_egk_gripper_dummy/start_dummy.sh
new file mode 100755
index 0000000..1551d71
--- /dev/null
+++ b/schunk_egu_egk_gripper_dummy/start_dummy.sh
@@ -0,0 +1 @@
+uvicorn schunk_egu_egk_gripper_dummy.main:server --port 8000 --reload
diff --git a/schunk_egu_egk_gripper_dummy/tests/test_dummy.py b/schunk_egu_egk_gripper_dummy/tests/test_dummy.py
index b8a610a..557fb4b 100644
--- a/schunk_egu_egk_gripper_dummy/tests/test_dummy.py
+++ b/schunk_egu_egk_gripper_dummy/tests/test_dummy.py
@@ -1,6 +1,7 @@
from src.dummy import Dummy
import pytest
import struct
+import time
# [1]: https://stb.cloud.schunk.com/media/IM0046706.PDF
@@ -44,42 +45,168 @@ def test_dummy_starts_in_error_state():
def test_dummy_is_ready_after_acknowledge():
dummy = Dummy()
- control_double_word = "04000000"
- set_position = "00000000"
- set_speed = "00000000"
- gripping_force = "00000000"
- command = {
- "inst": dummy.plc_output,
- "value": control_double_word + set_position + set_speed + gripping_force,
- }
- dummy.post(command)
+ dummy.set_control_bit(bit=2, value=True)
+ dummy.process_control_bits()
assert dummy.get_status_bit(0) == 1 # ready
assert dummy.get_status_bit(7) == 0 # no error
assert dummy.get_status_error() == "0"
assert dummy.get_status_diagnostics() == "0"
-def test_dummy_moves_to_absolute_position():
+def test_dummy_always_toggles_command_received_bit():
dummy = Dummy()
- target_pos = 12.345
- target_speed = 50.3
+ before = dummy.get_status_bit(bit=5) # command received toggle
+ dummy.process_control_bits()
+ after = dummy.get_status_bit(bit=5)
+ assert after != before
- control_double_word = "00200000"
- set_position = bytes(struct.pack("f", target_pos)).hex().upper()
- set_speed = bytes(struct.pack("f", target_speed)).hex().upper()
+
+def test_dummy_moves_to_absolute_position():
+ dummy = Dummy()
+ target_positions = [12345, 10555, 77000, 1500] # mu
+ target_speeds = [50300, 40000, 10500, 20999] # mu / s
+ for target_pos, target_speed in zip(target_positions, target_speeds):
+ control_double_word = "00200000" # bit 13
+ set_position = bytes(struct.pack("i", target_pos)).hex().upper()
+ set_speed = bytes(struct.pack("i", target_speed)).hex().upper()
+ gripping_force = "00000000"
+ command = {
+ "inst": dummy.plc_output,
+ "value": control_double_word + set_position + set_speed + gripping_force,
+ }
+
+ dummy.post(command)
+ assert dummy.get_actual_position() == pytest.approx(
+ target_pos / 1000.0, rel=1e-3
+ )
+ assert dummy.get_status_bit(bit=13) == 1 # position reached
+ assert dummy.get_status_bit(bit=4) == 1 # command successfully processed
+
+
+def test_dummy_moves_to_relative_position():
+ dummy = Dummy()
+ target_pos = -5000 # mu
+ target_speed = 12000 # mu / s
+ control_double_word = "00400000" # bit 14
+ set_position = bytes(struct.pack("i", target_pos)).hex().upper()
+ set_speed = bytes(struct.pack("i", target_speed)).hex().upper()
gripping_force = "00000000"
command = {
"inst": dummy.plc_output,
"value": control_double_word + set_position + set_speed + gripping_force,
}
- before = dummy.get_status_bit(bit=5) # command received toggle
-
- # Motion
+ before = dummy.get_actual_position()
dummy.post(command)
+ after = dummy.get_actual_position()
+ assert after < before # we are decreasing
+ assert after == pytest.approx(before + target_pos / 1000.0, rel=1e-3)
+ assert dummy.get_status_bit(bit=13) == 1 # position reached
+ assert dummy.get_status_bit(bit=4) == 1 # command successfully processed
+
- # Done
+def test_dummy_updates_internal_state_when_moving():
+ dummy = Dummy()
+ query = {"offset": 15, "count": 3} # actual position, speed, and current
+ before = dummy.get_data(query)
+
+ # Move
+ target_pos = 10.34
+ target_speed = 15.0
+ assert pytest.approx(dummy.get_actual_position()) != target_pos
+ dummy.move(target_pos=target_pos, target_speed=target_speed)
assert pytest.approx(dummy.get_actual_position()) == target_pos
- after = dummy.get_status_bit(bit=5)
- assert after != before
+
+ after = dummy.get_data(query)
+ assert before != after
+
+
+def test_dummy_performs_break_test():
+ dummy = Dummy()
+ dummy.set_control_bit(bit=30, value=True)
+ dummy.process_control_bits()
+ assert dummy.get_status_bit(bit=4) == 1 # command successfully processed
+
+
+def test_dummy_performs_fast_stop():
+ dummy = Dummy()
+ dummy.set_control_bit(bit=0, value=False) # fail-safe behavior
+ dummy.process_control_bits()
+ assert dummy.get_status_bit(bit=7) == 1 # error
+ assert dummy.get_status_diagnostics() == "D9" # ERR_FAST_STOP
+
+
+def test_dummy_performs_controlled_stop():
+ dummy = Dummy()
+ dummy.set_control_bit(bit=1, value=True)
+ dummy.process_control_bits()
assert dummy.get_status_bit(bit=13) == 1 # position reached
assert dummy.get_status_bit(bit=4) == 1 # command successfully processed
+
+
+def test_dummy_supports_manual_release():
+ dummy = Dummy()
+
+ # Reject when not in error state
+ dummy.set_status_bit(bit=7, value=False) # clear error
+ dummy.set_status_diagnostics("00")
+ dummy.set_control_bit(bit=5, value=True) # release
+ dummy.process_control_bits()
+ assert dummy.get_status_bit(bit=8) == 0 # not released
+
+ # Accept when in error state
+ dummy.set_control_bit(bit=0, value=False) # trigger fast stop
+ dummy.process_control_bits()
+ dummy.set_control_bit(bit=5, value=True) # release
+ dummy.process_control_bits()
+ assert dummy.get_status_bit(bit=8) == 1 # released for manual movement
+
+
+def test_dummy_supports_prepare_for_shutdown():
+ dummy = Dummy()
+ dummy.set_control_bit(bit=3, value=True)
+ dummy.process_control_bits()
+ assert dummy.get_status_bit(bit=2) == 1 # ready for shutdown
+
+
+def test_dummy_supports_release_workpiece():
+ dummy = Dummy()
+ dummy.set_control_bit(bit=11, value=True)
+ dummy.process_control_bits()
+ assert dummy.get_status_bit(bit=4) == 1 # command successfully processed
+ assert dummy.get_status_bit(bit=13) == 1 # position reached
+ assert dummy.get_status_bit(bit=14) == 0 # pre-grip started
+ assert dummy.get_status_bit(bit=12) == 0 # workpiece gripped
+ assert dummy.get_status_bit(bit=17) == 0 # wrong workpiece gripped
+
+
+def test_dummy_supports_softreset():
+ dummy = Dummy()
+ dummy.start() # fake some system uptime
+ initial = dummy.get_system_uptime()
+ time.sleep(1.5)
+ later = dummy.get_system_uptime()
+ time.sleep(1.5)
+ dummy.set_control_bit(bit=4, value=True)
+ dummy.process_control_bits()
+ after_reset = dummy.get_system_uptime()
+ dummy.stop()
+
+ assert initial < later
+ assert after_reset < later # restart resets the uptime
+
+
+def test_dummy_supports_grip():
+ dummy = Dummy()
+ dummy.set_control_bit(bit=12, value=True) # grip workpiece
+ dummy.process_control_bits()
+ assert dummy.get_status_bit(bit=12) == 1 # workpiece gripped
+ assert dummy.get_status_bit(bit=4) == 1 # command successfully processed
+
+
+def test_dummy_supports_grip_at_position():
+ dummy = Dummy()
+ dummy.set_control_bit(bit=16, value=True) # grip workpiece at expected position
+ dummy.process_control_bits()
+ assert dummy.get_status_bit(bit=12) == 1 # workpiece gripped
+ assert dummy.get_status_bit(bit=4) == 1 # command successfully processed
+ assert dummy.get_status_bit(bit=31) == 1 # GPE activated
diff --git a/schunk_egu_egk_gripper_dummy/tests/test_motion_profile.py b/schunk_egu_egk_gripper_dummy/tests/test_motion_profile.py
new file mode 100644
index 0000000..120df57
--- /dev/null
+++ b/schunk_egu_egk_gripper_dummy/tests/test_motion_profile.py
@@ -0,0 +1,129 @@
+from src.dummy import LinearMotion
+import pytest
+import time
+
+
+def test_motion_initializes_with_initial_state_and_target_state():
+ initial_pos = 0.0
+ initial_speed = 0.0
+ target_pos = 0.1
+ target_speed = 55.1
+ motion = LinearMotion(
+ initial_pos=initial_pos,
+ initial_speed=initial_speed,
+ target_pos=target_pos,
+ target_speed=target_speed,
+ )
+ assert pytest.approx(motion.initial_pos) == initial_pos
+ assert pytest.approx(motion.initial_speed) == initial_speed
+ assert pytest.approx(motion.target_pos) == target_pos
+ assert pytest.approx(motion.target_speed) == target_speed
+
+
+def test_motion_corrects_negative_or_zero_speed_arguments():
+ initial_pos = 0.0
+ initial_speed = -0.3
+ target_pos = 0.1
+ target_speed = 0.0
+ motion = LinearMotion(
+ initial_pos=initial_pos,
+ initial_speed=initial_speed,
+ target_pos=target_pos,
+ target_speed=target_speed,
+ )
+ assert pytest.approx(motion.target_speed) == motion.min_speed
+ assert pytest.approx(motion.initial_speed) == 0.0
+
+ target_speed = -0.123
+ motion = LinearMotion(
+ initial_pos=initial_pos,
+ initial_speed=initial_speed,
+ target_pos=target_pos,
+ target_speed=target_speed,
+ )
+ assert pytest.approx(motion.target_speed) == motion.min_speed
+
+
+def test_motion_samples_current_state_at_given_time():
+ initial_pos = 0.0
+ initial_speed = 0.0
+ target_pos = 1.3
+ target_speed = 55.1
+ motion = LinearMotion(
+ initial_pos=initial_pos,
+ initial_speed=initial_speed,
+ target_pos=target_pos,
+ target_speed=target_speed,
+ )
+ start = time.time()
+ time.sleep(0.01)
+ now = time.time() - start
+ current_pos, current_speed = motion.sample(t=now)
+ assert isinstance(current_pos, float)
+ assert isinstance(current_speed, float)
+ assert current_pos >= initial_pos
+ assert current_pos <= target_pos
+ assert current_speed >= initial_speed
+ assert current_speed <= target_speed
+
+
+def test_motion_samples_initial_state_at_time_less_or_equal_zero():
+ initial_pos = 0.0
+ initial_speed = 0.0
+ target_pos = 0.1
+ target_speed = 55.1
+ motion = LinearMotion(
+ initial_pos=initial_pos,
+ initial_speed=initial_speed,
+ target_pos=target_pos,
+ target_speed=target_speed,
+ )
+ current_pos, current_speed = motion.sample(t=-0.001)
+ assert pytest.approx(current_pos) == motion.initial_pos
+ assert pytest.approx(current_speed) == motion.initial_speed
+
+
+def test_motion_samples_target_position_at_time_after_finish():
+ initial_pos = 0.0
+ initial_speed = 0.0
+ target_pos = 0.1
+ target_speed = 55.1
+ motion = LinearMotion(
+ initial_pos=initial_pos,
+ initial_speed=initial_speed,
+ target_pos=target_pos,
+ target_speed=target_speed,
+ )
+ current_pos, current_speed = motion.sample(t=1000)
+ assert pytest.approx(current_pos) == motion.target_pos
+ assert pytest.approx(current_speed) == 0.0
+
+
+def test_motion_supports_both_directions():
+ initial_pos = 5.0
+ initial_speed = 0.0
+ target_speed = 5.678
+
+ # Positive direction
+ target_pos = 12.34
+ motion = LinearMotion(
+ initial_pos=initial_pos,
+ initial_speed=initial_speed,
+ target_pos=target_pos,
+ target_speed=target_speed,
+ )
+ current_pos_1, _ = motion.sample(t=0.01)
+ current_pos_2, _ = motion.sample(t=0.02)
+ assert current_pos_1 < current_pos_2
+
+ # Negative direction
+ target_pos = 1.234
+ motion = LinearMotion(
+ initial_pos=initial_pos,
+ initial_speed=initial_speed,
+ target_pos=target_pos,
+ target_speed=target_speed,
+ )
+ current_pos_1, _ = motion.sample(t=0.01)
+ current_pos_2, _ = motion.sample(t=0.02)
+ assert current_pos_1 > current_pos_2
diff --git a/schunk_egu_egk_gripper_dummy/tests/test_plc_communication.py b/schunk_egu_egk_gripper_dummy/tests/test_plc_communication.py
index 310692f..cee1a03 100644
--- a/schunk_egu_egk_gripper_dummy/tests/test_plc_communication.py
+++ b/schunk_egu_egk_gripper_dummy/tests/test_plc_communication.py
@@ -81,11 +81,13 @@ def test_dummy_rejects_writing_invalid_status_diagnostics():
assert not dummy.set_status_diagnostics(code)
-def test_dummy_supports_reading_bits_in_plc_control():
+def test_dummy_supports_reading_and_writing_bits_in_plc_control():
dummy = Dummy()
for bit in dummy.valid_control_bits:
+ dummy.set_control_bit(bit=bit, value=True)
result = dummy.get_control_bit(bit=bit)
assert isinstance(result, int) # successful calls get the bit's value
+ assert result == 1
def test_dummy_rejects_reading_reserved_control_bits():
@@ -95,6 +97,13 @@ def test_dummy_rejects_reading_reserved_control_bits():
assert not dummy.get_control_bit(bit)
+def test_dummy_rejects_writing_reserved_control_bits():
+ dummy = Dummy()
+ invalid_bits = [-1, 999]
+ for bit in invalid_bits + dummy.reserved_control_bits:
+ assert not dummy.set_control_bit(bit, True)
+
+
def test_dummy_supports_toggling_status_bits():
dummy = Dummy()
for bit in dummy.valid_status_bits:
@@ -114,16 +123,16 @@ def test_dummy_rejects_toggling_reserved_status_bits():
def test_dummy_supports_reading_target_position():
dummy = Dummy()
- target_pos = 0.0123
- dummy.plc_output_buffer[4:8] = bytes(struct.pack("f", target_pos))
- assert pytest.approx(dummy.get_target_position()) == target_pos
+ target_pos = 12300 # um
+ dummy.plc_output_buffer[4:8] = bytes(struct.pack("i", target_pos))
+ assert pytest.approx(dummy.get_target_position(), rel=1e-3) == target_pos / 1000.0
def test_dummy_supports_reading_target_speed():
dummy = Dummy()
- target_speed = 55.3
- dummy.plc_output_buffer[8:12] = bytes(struct.pack("f", target_speed))
- assert pytest.approx(dummy.get_target_speed()) == target_speed
+ target_speed = 55300
+ dummy.plc_output_buffer[8:12] = bytes(struct.pack("i", target_speed))
+ assert pytest.approx(dummy.get_target_speed(), rel=1e-3) == target_speed / 1000.0
def test_dummy_supports_writing_actual_position():
@@ -156,3 +165,10 @@ def test_dummy_supports_reading_actual_speed():
speed = 66.5
dummy.set_actual_speed(speed)
assert pytest.approx(dummy.get_actual_speed()) == speed
+
+
+def test_dummy_supports_reading_and_writing_system_uptime():
+ dummy = Dummy()
+ uptime = 1234 # secs
+ dummy.set_system_uptime(uptime)
+ assert dummy.get_system_uptime() == uptime
diff --git a/schunk_egu_egk_gripper_dummy/tests/test_requests.py b/schunk_egu_egk_gripper_dummy/tests/test_requests.py
index 48667c3..c951791 100644
--- a/schunk_egu_egk_gripper_dummy/tests/test_requests.py
+++ b/schunk_egu_egk_gripper_dummy/tests/test_requests.py
@@ -1,5 +1,5 @@
from src.dummy import Dummy
-from schunk_egu_egk_gripper_dummy.main import server
+from schunk_egu_egk_gripper_dummy.main import server, dummy as client_dummy
from fastapi.testclient import TestClient
@@ -30,7 +30,19 @@ def test_dummy_survives_invalid_enum_requests():
def test_dummy_responds_correctly_to_data_offset_requests():
dummy = Dummy()
query = {"offset": 15, "count": 3}
- expected = [dummy.metadata[15], dummy.metadata[16], dummy.metadata[17]]
+
+ def get_hex(number: int) -> str:
+ plain = hex(number)[2:].upper().zfill(4)
+ return "0x" + plain
+
+ inst1 = get_hex(dummy.metadata[15]["instance"])
+ inst2 = get_hex(dummy.metadata[16]["instance"])
+ inst3 = get_hex(dummy.metadata[17]["instance"])
+ data1 = dummy.data[inst1][0]
+ data2 = dummy.data[inst2][0]
+ data3 = dummy.data[inst3][0]
+
+ expected = [data1, data2, data3]
assert dummy.get_data(query) == expected
@@ -83,6 +95,7 @@ def test_dummy_stores_post_requests():
def test_dummy_rejects_invalid_post_requests():
client = TestClient(server)
+ client_dummy.start()
valid_data = "AABBCCDD"
valid_inst = "0x0238"
@@ -98,3 +111,17 @@ def test_dummy_rejects_invalid_post_requests():
invalid_inst = "0x9999"
data = {"inst": invalid_inst, "value": valid_data}
assert client.post("/adi/update.json", data=data).json() == {"result": 1}
+ client_dummy.stop()
+
+
+def test_dummy_resets_success_status_bits_with_new_post_requests():
+ dummy = Dummy()
+ dummy.set_status_bit(bit=13, value=True) # position reached
+ dummy.set_status_bit(bit=4, value=True) # command successful
+ dummy.set_status_bit(bit=12, value=True) # workpiece gripped
+ empty_command = "01" + "".zfill(30) # only fast stop active
+ data = {"inst": dummy.plc_output, "value": empty_command}
+ dummy.post(data)
+ assert dummy.get_status_bit(bit=13) == 0
+ assert dummy.get_status_bit(bit=4) == 0
+ assert dummy.get_status_bit(bit=12) == 0
diff --git a/schunk_egu_egk_gripper_dummy/tests/test_webserver.py b/schunk_egu_egk_gripper_dummy/tests/test_webserver.py
index 91444c1..1a22094 100644
--- a/schunk_egu_egk_gripper_dummy/tests/test_webserver.py
+++ b/schunk_egu_egk_gripper_dummy/tests/test_webserver.py
@@ -1,23 +1,27 @@
-from schunk_egu_egk_gripper_dummy.main import server
+from schunk_egu_egk_gripper_dummy.main import server, dummy
from fastapi.testclient import TestClient
def test_info_route_is_available():
client = TestClient(server)
assert client.get("/adi/info.json").is_success
+ dummy.stop()
def test_enum_route_is_available():
client = TestClient(server)
assert client.get("/adi/enum.json").is_success
+ dummy.stop()
def test_data_route_is_available():
client = TestClient(server)
assert client.get("/adi/data.json").is_success
+ dummy.stop()
def test_update_route_is_available():
client = TestClient(server)
data = {"inst": 0, "value": "0"}
assert client.post("/adi/update.json", data=data).is_success
+ dummy.stop()
diff --git a/schunk_egu_egk_gripper_examples/src/gripper_example.cpp b/schunk_egu_egk_gripper_examples/src/gripper_example.cpp
index 2aea3cc..d4e7c0c 100644
--- a/schunk_egu_egk_gripper_examples/src/gripper_example.cpp
+++ b/schunk_egu_egk_gripper_examples/src/gripper_example.cpp
@@ -36,7 +36,6 @@
#include "schunk_egu_egk_gripper_interfaces/srv/prepare_for_shutdown.hpp"
#include "schunk_egu_egk_gripper_interfaces/srv/softreset.hpp"
#include "schunk_egu_egk_gripper_interfaces/srv/brake_test.hpp"
-#include "schunk_egu_egk_gripper_interfaces/srv/change_ip.hpp"
#include "schunk_egu_egk_gripper_interfaces/srv/parameter_get.hpp"
#include "schunk_egu_egk_gripper_interfaces/srv/parameter_set.hpp"
#include "schunk_egu_egk_gripper_interfaces/srv/release_for_manual_movement.hpp"
@@ -66,7 +65,6 @@ std::string name_space;
using Softreset = schunk_egu_egk_gripper_interfaces::srv::Softreset;
using PrepareForShutdown = schunk_egu_egk_gripper_interfaces::srv::PrepareForShutdown;
using GripperInfo= schunk_egu_egk_gripper_interfaces::srv::GripperInfo;
- using ChangeIp = schunk_egu_egk_gripper_interfaces::srv::ChangeIp;
using ParameterGet = schunk_egu_egk_gripper_interfaces::srv::ParameterGet;
using ParameterSet = schunk_egu_egk_gripper_interfaces::srv::ParameterSet;
@@ -234,7 +232,7 @@ 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;
+ bool stopped = resp.get()->success;
RCLCPP_INFO(rclcpp::get_logger("schunk_gripper_example"), "%s", stopped ? "Stopped!" : "Not stopped!");
auto res = move_rel_client->async_get_result(goal_handle.get());
diff --git a/schunk_egu_egk_gripper_interfaces/CMakeLists.txt b/schunk_egu_egk_gripper_interfaces/CMakeLists.txt
index 26e6ca1..6c87a1e 100644
--- a/schunk_egu_egk_gripper_interfaces/CMakeLists.txt
+++ b/schunk_egu_egk_gripper_interfaces/CMakeLists.txt
@@ -26,7 +26,6 @@ rosidl_generate_interfaces(
msg/State.msg
srv/Acknowledge.srv
srv/BrakeTest.srv
- srv/ChangeIp.srv
srv/FastStop.srv
srv/GripperInfo.srv
srv/ParameterGet.srv
diff --git a/schunk_egu_egk_gripper_interfaces/srv/BrakeTest.srv b/schunk_egu_egk_gripper_interfaces/srv/BrakeTest.srv
index e0cac6d..7e0b3bf 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
+bool success
string error_code
diff --git a/schunk_egu_egk_gripper_interfaces/srv/ChangeIp.srv b/schunk_egu_egk_gripper_interfaces/srv/ChangeIp.srv
deleted file mode 100644
index 6308764..0000000
--- a/schunk_egu_egk_gripper_interfaces/srv/ChangeIp.srv
+++ /dev/null
@@ -1,3 +0,0 @@
-string new_ip
----
-bool ip_changed
diff --git a/schunk_egu_egk_gripper_interfaces/srv/FastStop.srv b/schunk_egu_egk_gripper_interfaces/srv/FastStop.srv
index 4d1c0b1..410e0f9 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
+bool success
diff --git a/schunk_egu_egk_gripper_interfaces/srv/GripperInfo.srv b/schunk_egu_egk_gripper_interfaces/srv/GripperInfo.srv
index ed97d53..fafb7ac 100644
--- a/schunk_egu_egk_gripper_interfaces/srv/GripperInfo.srv
+++ b/schunk_egu_egk_gripper_interfaces/srv/GripperInfo.srv
@@ -1 +1,3 @@
---
+bool success
+string info
diff --git a/schunk_egu_egk_gripper_interfaces/srv/PrepareForShutdown.srv b/schunk_egu_egk_gripper_interfaces/srv/PrepareForShutdown.srv
index bbc46d3..410e0f9 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
+bool success
diff --git a/schunk_egu_egk_gripper_interfaces/srv/ReleaseForManualMovement.srv b/schunk_egu_egk_gripper_interfaces/srv/ReleaseForManualMovement.srv
index f2fb068..410e0f9 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
+bool success
diff --git a/schunk_egu_egk_gripper_interfaces/srv/Softreset.srv b/schunk_egu_egk_gripper_interfaces/srv/Softreset.srv
index 4102fa4..410e0f9 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
+bool success
diff --git a/schunk_egu_egk_gripper_interfaces/srv/Stop.srv b/schunk_egu_egk_gripper_interfaces/srv/Stop.srv
index 91a705f..410e0f9 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
+bool success
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 469d725..8762199 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
@@ -113,6 +113,8 @@
#define SW_BUILD_DATE_INST "0x1100"
#define SW_BUILD_TIME_INST "0x1108"
#define SW_VERSION_TXT_INST "0x1118"
+#define SYSTEM_UPTIME_INST "0x1400"
+
//Datatypes in JSON
#define BOOL_DATA 0
#define INT32_DATA 3
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 a52b69c..0c88a8a 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
@@ -56,7 +56,6 @@ class Gripper : protected AnybusCom
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
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 d3c4234..537b309 100644
--- a/schunk_egu_egk_gripper_library/src/schunk_gripper_lib.cpp
+++ b/schunk_egu_egk_gripper_library/src/schunk_gripper_lib.cpp
@@ -319,50 +319,7 @@ void Gripper::acknowledge()
getWithInstance(PLC_SYNC_INPUT_INST);
plc_sync_output[0] &= mask;
}
-//change the ip-address
-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;
- initAddresses();
- //Control if it is the same gripper
- try
- {
- startGripper();
-
- ip_changed_with_all_param = true;
-
- return true;
- }
- catch(const char* res)
- {
- ip_changed_with_all_param = false;
- std::cout << "No Gripper found. New IP: " << ip << std::endl;
- initAddresses();
- return true;
- }
- catch(const nlohmann::json::parse_error &e)
- {
- 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;
- initAddresses();
- return false;
- }
- catch(const std::exception &e)
- {
- ip_changed_with_all_param = true;
- ip = old_ip;
- std::cout << "Wrong data found. Setting to old IP: " << ip << std::endl;
- initAddresses();
- return false;
- }
-}
//send action directly after current (cyclic-) http or immediately
void Gripper::sendAction()
{
diff --git a/schunk_egu_egk_gripper_tests/test/conftest.py b/schunk_egu_egk_gripper_tests/test/conftest.py
index cd37991..4c35d3d 100644
--- a/schunk_egu_egk_gripper_tests/test/conftest.py
+++ b/schunk_egu_egk_gripper_tests/test/conftest.py
@@ -7,6 +7,8 @@
import launch_pytest
import subprocess
from ament_index_python.packages import get_package_share_directory
+from test.helpers import get_current_state
+import time
# We avoid black's F811, F401 linting warnings
@@ -15,14 +17,14 @@
# https://docs.pytest.org/en/7.1.x/reference/fixtures.html#conftest-py-sharing-fixtures-across-multiple-files # noqa: E501
-@pytest.fixture()
+@pytest.fixture(scope="module")
def isolated():
rclpy.init()
yield
rclpy.shutdown()
-@pytest.fixture()
+@pytest.fixture(scope="module")
def gripper_dummy():
package_name = "schunk_egu_egk_gripper_dummy"
dummy_dir = get_package_share_directory(package_name)
@@ -37,7 +39,7 @@ def gripper_dummy():
print("Stopped gripper dummy")
-@launch_pytest.fixture
+@launch_pytest.fixture(scope="module")
def launch_description():
setup = IncludeLaunchDescription(
PathJoinSubstitution(
@@ -53,3 +55,13 @@ def launch_description():
}.items(),
)
return LaunchDescription([setup, launch_pytest.actions.ReadyToTest()])
+
+
+@pytest.fixture(scope="module")
+def running_driver(gripper_dummy, isolated, launch_context):
+
+ print("Waiting until the driver is connected")
+ while not get_current_state(variable="ready_for_operation") is True:
+ time.sleep(0.1)
+ yield
+ ...
diff --git a/schunk_egu_egk_gripper_tests/test/helpers.py b/schunk_egu_egk_gripper_tests/test/helpers.py
index 966c404..2a950ff 100644
--- a/schunk_egu_egk_gripper_tests/test/helpers.py
+++ b/schunk_egu_egk_gripper_tests/test/helpers.py
@@ -1,21 +1,28 @@
from threading import Thread, Event
-import rclpy
from rclpy.node import Node
from typing import Any
from schunk_egu_egk_gripper_interfaces.msg import State # type: ignore[attr-defined]
import uuid
+from rclpy.executors import MultiThreadedExecutor
+import time
+from rclpy.action import ActionClient
class TopicGetsPublished(Node):
def __init__(self, topic: str, type: Any):
- node_name = "check_topic" + str(uuid.uuid4()).replace("-", "")
- super().__init__(node_name)
+ self.node_name = "check_topic" + str(uuid.uuid4()).replace("-", "")
+ super().__init__(self.node_name)
self.event = Event()
self.data = None
self.sub = self.create_subscription(type, topic, self.msg_cb, 3)
- self.thread = Thread(target=lambda node: rclpy.spin(node), args=(self,))
+ self.thread = Thread(target=self.spin)
self.thread.start()
+ def spin(self) -> None:
+ executor = MultiThreadedExecutor()
+ executor.add_node(self)
+ executor.spin()
+
def msg_cb(self, data: Any) -> None:
self.data = data
self.event.set()
@@ -25,3 +32,58 @@ def get_current_state(variable: str):
topic = TopicGetsPublished("/state", State)
topic.event.wait()
return getattr(topic.data, variable)
+
+
+class ServiceReturnsResult(Node):
+ def __init__(self, service: str, type: Any, msg: Any):
+ self.node_name = "call_service" + str(uuid.uuid4()).replace("-", "")
+ super().__init__(self.node_name)
+ self.event = Event()
+ srv = self.create_client(type, service)
+ srv.wait_for_service(1.0)
+ self.future = srv.call_async(msg)
+ self.result = None
+ self.thread = Thread(target=self.spin)
+ self.thread.start()
+
+ def spin(self) -> None:
+ executor = MultiThreadedExecutor()
+ executor.add_node(self)
+ executor.spin_until_future_complete(self.future)
+ self.result = self.future.result()
+ self.event.set()
+
+
+class ActionReturnsResult(Node):
+ def __init__(self, action: str, type: Any, goal: Any):
+ self.node_name = "call_action" + str(uuid.uuid4()).replace("-", "")
+ super().__init__(self.node_name)
+ self.event = Event()
+
+ client = ActionClient(self, type, action)
+ client.wait_for_server(1.0)
+
+ self.goal_future = client.send_goal_async(goal)
+ self.result = None
+ self.thread = Thread(target=self.spin)
+ self.thread.start()
+
+ def spin(self) -> None:
+ executor = MultiThreadedExecutor()
+ executor.add_node(self)
+ executor.spin_until_future_complete(self.goal_future)
+ goal_handle = self.goal_future.result()
+ result_future = goal_handle.get_result_async()
+ executor.spin_until_future_complete(result_future)
+ self.result = result_future.result().result
+ self.event.set()
+
+
+def check_each_in(elements: list, node_method: str) -> None:
+ node = Node("test")
+ until_ready = 2.0 # sec
+ time.sleep(until_ready)
+ existing = getattr(node, node_method)()
+ advertised = [i[0] for i in existing]
+ for element in elements:
+ assert element in advertised
diff --git a/schunk_egu_egk_gripper_tests/test/test_actions.py b/schunk_egu_egk_gripper_tests/test/test_actions.py
new file mode 100644
index 0000000..6524906
--- /dev/null
+++ b/schunk_egu_egk_gripper_tests/test/test_actions.py
@@ -0,0 +1,118 @@
+import pytest
+from test.conftest import launch_description
+from test.helpers import check_each_in, ActionReturnsResult, get_current_state
+from schunk_egu_egk_gripper_interfaces.action import ( # type: ignore[attr-defined]
+ GripWithVelocity,
+ GripWithPositionAndVelocity,
+ MoveToAbsolutePosition,
+ MoveToRelativePosition,
+ ReleaseWorkpiece,
+)
+from control_msgs.action import GripperCommand
+
+
+@pytest.mark.launch(fixture=launch_description)
+def test_driver_advertices_all_relevant_actions(running_driver):
+ action_list = [
+ "/grip",
+ "/grip_with_position",
+ "/gripper_control",
+ "/move_to_absolute_position",
+ "/move_to_relative_position",
+ "/release_workpiece",
+ ]
+ action_list = [a + "/_action/status" for a in action_list]
+ check_each_in(action_list, "get_topic_names_and_types")
+
+
+@pytest.mark.launch(fixture=launch_description)
+def test_driver_grips(running_driver):
+ goal = (
+ GripWithVelocity.Goal()
+ ) # Our dummy is an EGK, which uses an additional velocity
+ goal.gripping_force = 65.0 # % of max force
+ goal.velocity_of_movement = 11.0
+ goal.grip_direction = False # close
+ action = ActionReturnsResult("/grip", GripWithVelocity, goal)
+ action.event.wait()
+ assert action.result.workpiece_gripped
+ assert action.result.no_workpiece_detected is False
+ assert action.result.workpiece_lost is False
+
+
+@pytest.mark.launch(fixture=launch_description)
+def test_driver_grips_with_position(running_driver):
+ goal = (
+ GripWithPositionAndVelocity.Goal()
+ ) # Our dummy is an EGK, which uses an additional velocity
+ goal.absolute_position = 18.88
+ goal.gripping_force = 65.0 # % of max force
+ goal.velocity_of_movement = 13.0
+ goal.grip_direction = False # close
+ action = ActionReturnsResult(
+ "/grip_with_position", GripWithPositionAndVelocity, goal
+ )
+ action.event.wait()
+ assert action.result.workpiece_gripped
+ assert action.result.no_workpiece_detected is False
+ assert action.result.workpiece_lost is False
+
+
+@pytest.mark.launch(fixture=launch_description)
+def test_driver_supports_gripper_control(running_driver):
+
+ runs = [
+ {"position": 0.0, "max_effort": 77.0}, # grip workpiece
+ {"position": 82.4, "max_effort": 0.0}, # move to abs. position
+ {"position": 66.3, "max_effort": 85.1}, # grip with position
+ ]
+
+ for run in runs:
+ goal = GripperCommand.Goal()
+ goal.command.position = run["position"]
+ goal.command.max_effort = run["max_effort"]
+ action = ActionReturnsResult("/gripper_control", GripperCommand, goal)
+ action.event.wait()
+ assert action.result.reached_goal
+ assert action.result.stalled
+
+
+@pytest.mark.launch(fixture=launch_description)
+def test_driver_moves_to_absolute_position(running_driver):
+ test_positions = [43.55, 17.02, 38.55, 103.7]
+ test_speeds = [55.66, 10.5, 40.0, 88.8]
+ for test_pos, test_speed in zip(test_positions, test_speeds):
+ goal = MoveToAbsolutePosition.Goal()
+ goal.absolute_position = test_pos
+ goal.velocity_of_movement = test_speed
+ action = ActionReturnsResult(
+ "/move_to_absolute_position", MoveToAbsolutePosition, goal
+ )
+ action.event.wait()
+ assert action.result.position_reached
+ assert pytest.approx(action.result.absolute_position) == goal.absolute_position
+
+
+@pytest.mark.launch(fixture=launch_description)
+def test_driver_moves_to_relative_position(running_driver):
+ initial_pos = get_current_state(variable="actual_position")
+ goal = MoveToRelativePosition.Goal()
+ goal.signed_relative_position = -10.03
+ goal.velocity_of_movement = 15.0
+ action = ActionReturnsResult(
+ "/move_to_relative_position", MoveToRelativePosition, goal
+ )
+ action.event.wait()
+ assert action.result.position_reached
+ assert (
+ pytest.approx(action.result.absolute_position)
+ == initial_pos + goal.signed_relative_position
+ )
+
+
+@pytest.mark.launch(fixture=launch_description)
+def test_driver_releases_workpieces(running_driver):
+ goal = ReleaseWorkpiece.Goal()
+ action = ActionReturnsResult("/release_workpiece", ReleaseWorkpiece, goal)
+ action.event.wait()
+ assert action.result.released_workpiece
diff --git a/schunk_egu_egk_gripper_tests/test/test_functionality.py b/schunk_egu_egk_gripper_tests/test/test_functionality.py
deleted file mode 100644
index e0b9d5f..0000000
--- a/schunk_egu_egk_gripper_tests/test/test_functionality.py
+++ /dev/null
@@ -1,46 +0,0 @@
-import pytest
-import rclpy
-from rclpy.node import Node
-from test.conftest import launch_description
-from rclpy.action import ActionClient
-from schunk_egu_egk_gripper_interfaces.action import ( # type: ignore[attr-defined]
- MoveToAbsolutePosition,
-)
-from schunk_egu_egk_gripper_interfaces.srv import ( # type: ignore[attr-defined]
- Acknowledge,
-)
-from test.helpers import get_current_state
-
-
-@pytest.mark.launch(fixture=launch_description)
-def test_driver_starts_in_ready_state(launch_context, isolated, gripper_dummy):
- assert get_current_state(variable="ready_for_operation") is True
-
-
-@pytest.mark.launch(fixture=launch_description)
-def test_driver_is_ready_after_acknowledge(launch_context, isolated, gripper_dummy):
- node = Node("test")
- activate_srv = node.create_client(Acknowledge, "/acknowledge")
- while not activate_srv.wait_for_service(1.0):
- pass
- future = activate_srv.call_async(Acknowledge.Request())
- rclpy.spin_until_future_complete(node, future)
- assert future.result().success is True
-
-
-@pytest.mark.launch(fixture=launch_description)
-def test_driver_moves_to_absolute_position(launch_context, isolated, gripper_dummy):
- node = Node("move_test")
-
- client = ActionClient(node, MoveToAbsolutePosition, "/move_to_absolute_position")
- client.wait_for_server()
- goal = MoveToAbsolutePosition.Goal()
-
- goal_future = client.send_goal_async(goal)
- rclpy.spin_until_future_complete(node, goal_future)
- goal_handle = goal_future.result()
-
- result_future = goal_handle.get_result_async()
- rclpy.spin_until_future_complete(node, result_future)
- result = result_future.result().result
- assert result.position_reached
diff --git a/schunk_egu_egk_gripper_tests/test/test_http_interface.py b/schunk_egu_egk_gripper_tests/test/test_http_interface.py
deleted file mode 100644
index 7bf76ca..0000000
--- a/schunk_egu_egk_gripper_tests/test/test_http_interface.py
+++ /dev/null
@@ -1,14 +0,0 @@
-import pytest
-from test.conftest import launch_description
-import time
-from sensor_msgs.msg import JointState
-from test.helpers import TopicGetsPublished
-
-
-@pytest.mark.launch(fixture=launch_description)
-def test_driver_connnects_to_gripper_dummy(launch_context, isolated, gripper_dummy):
- until_dummy_ready = 3
- timeout = 3
-
- time.sleep(until_dummy_ready)
- assert TopicGetsPublished("/joint_states", JointState).event.wait(timeout)
diff --git a/schunk_egu_egk_gripper_tests/test/test_launch.py b/schunk_egu_egk_gripper_tests/test/test_launch.py
index 3b85f46..6b86c54 100644
--- a/schunk_egu_egk_gripper_tests/test/test_launch.py
+++ b/schunk_egu_egk_gripper_tests/test/test_launch.py
@@ -6,7 +6,7 @@
@pytest.mark.launch(fixture=launch_description)
-def test_normal_startup_works(launch_context, isolated, gripper_dummy):
+def test_normal_startup_works(launch_context, isolated):
node = Node("test_startup")
until_ready = 2.0 # sec
time.sleep(until_ready)
diff --git a/schunk_egu_egk_gripper_tests/test/test_ros_interfaces.py b/schunk_egu_egk_gripper_tests/test/test_ros_interfaces.py
deleted file mode 100644
index 48651fe..0000000
--- a/schunk_egu_egk_gripper_tests/test/test_ros_interfaces.py
+++ /dev/null
@@ -1,59 +0,0 @@
-#!/usr/bin/env python3
-import pytest
-from rclpy.node import Node
-import time
-from test.conftest import launch_description
-
-
-def check_each_in(elements: list, node_method: str) -> None:
- node = Node("test")
- until_ready = 2.0 # sec
- time.sleep(until_ready)
- existing = getattr(node, node_method)()
- advertised = [i[0] for i in existing]
- for element in elements:
- assert element in advertised
-
-
-@pytest.mark.launch(fixture=launch_description)
-def test_driver_advertices_all_relevant_topics(launch_context, isolated, gripper_dummy):
- topic_list = [
- "/diagnostics",
- "/joint_states",
- "/state",
- ]
- check_each_in(topic_list, "get_topic_names_and_types")
-
-
-@pytest.mark.launch(fixture=launch_description)
-def test_driver_advertices_all_relevant_services(
- launch_context, isolated, gripper_dummy
-):
- service_list = [
- "/acknowledge",
- "/brake_test",
- "/fast_stop",
- "/gripper_info",
- "/prepare_for_shutdown",
- "/reconnect",
- "/release_for_manual_movement",
- "/softreset",
- "/stop",
- ]
- check_each_in(service_list, "get_service_names_and_types")
-
-
-@pytest.mark.launch(fixture=launch_description)
-def test_driver_advertices_all_relevant_actions(
- launch_context, isolated, gripper_dummy
-):
- action_list = [
- "/grip",
- "/grip_with_position",
- "/gripper_control",
- "/move_to_absolute_position",
- "/move_to_relative_position",
- "/release_workpiece",
- ]
- action_list = [a + "/_action/status" for a in action_list]
- check_each_in(action_list, "get_topic_names_and_types")
diff --git a/schunk_egu_egk_gripper_tests/test/test_services.py b/schunk_egu_egk_gripper_tests/test/test_services.py
new file mode 100644
index 0000000..978ce49
--- /dev/null
+++ b/schunk_egu_egk_gripper_tests/test/test_services.py
@@ -0,0 +1,92 @@
+import pytest
+from test.conftest import launch_description
+from test.helpers import check_each_in
+from schunk_egu_egk_gripper_interfaces.srv import ( # type: ignore[attr-defined]
+ Acknowledge,
+ BrakeTest,
+ FastStop,
+ Stop,
+ ReleaseForManualMovement,
+ GripperInfo,
+ PrepareForShutdown,
+ Softreset,
+)
+from test.helpers import ServiceReturnsResult
+
+
+@pytest.mark.launch(fixture=launch_description)
+def test_driver_advertices_all_relevant_services(running_driver):
+ service_list = [
+ "/acknowledge",
+ "/brake_test",
+ "/fast_stop",
+ "/gripper_info",
+ "/prepare_for_shutdown",
+ "/release_for_manual_movement",
+ "/softreset",
+ "/stop",
+ ]
+ check_each_in(service_list, "get_service_names_and_types")
+
+
+@pytest.mark.launch(fixture=launch_description)
+def test_driver_is_ready_after_acknowledge(running_driver):
+ service = ServiceReturnsResult("/acknowledge", Acknowledge, Acknowledge.Request())
+ service.event.wait(timeout=1)
+ assert service.result.success is True
+
+
+@pytest.mark.launch(fixture=launch_description)
+def test_driver_supports_break_test(running_driver):
+ service = ServiceReturnsResult("/brake_test", BrakeTest, BrakeTest.Request())
+ service.event.wait(timeout=1)
+ assert service.result.success is True
+
+
+@pytest.mark.launch(fixture=launch_description)
+def test_driver_supports_fast_stop(running_driver):
+ service = ServiceReturnsResult("/fast_stop", FastStop, FastStop.Request())
+ service.event.wait(timeout=1)
+ assert service.result.success is True
+
+
+@pytest.mark.launch(fixture=launch_description)
+def test_driver_supports_gripper_info(running_driver):
+ service = ServiceReturnsResult("/gripper_info", GripperInfo, GripperInfo.Request())
+ service.event.wait(timeout=1)
+ assert service.result.success is True
+ assert service.result.info != ""
+
+
+@pytest.mark.launch(fixture=launch_description)
+def test_driver_supports_prepare_for_shutdown(running_driver):
+ service = ServiceReturnsResult(
+ "/prepare_for_shutdown", PrepareForShutdown, PrepareForShutdown.Request()
+ )
+ service.event.wait(timeout=1)
+ assert service.result.success is True
+
+
+@pytest.mark.launch(fixture=launch_description)
+def test_driver_supports_release_for_manual_movement(running_driver):
+ service = ServiceReturnsResult(
+ "/release_for_manual_movement",
+ ReleaseForManualMovement,
+ ReleaseForManualMovement.Request(),
+ )
+ service.event.wait(timeout=1)
+ assert service.result.success is True
+
+
+@pytest.mark.launch(fixture=launch_description)
+def test_driver_supports_softreset(running_driver):
+ service = ServiceReturnsResult("/softreset", Softreset, Softreset.Request())
+ service.event.wait(timeout=1)
+ assert service.result.success is True
+
+
+@pytest.mark.launch(fixture=launch_description)
+def test_driver_supports_stop(running_driver):
+ service = ServiceReturnsResult("/stop", Stop, Stop.Request())
+ service.event.wait(timeout=1)
+ assert service.result.success is True
diff --git a/schunk_egu_egk_gripper_tests/test/test_topics.py b/schunk_egu_egk_gripper_tests/test/test_topics.py
new file mode 100644
index 0000000..315e5a0
--- /dev/null
+++ b/schunk_egu_egk_gripper_tests/test/test_topics.py
@@ -0,0 +1,13 @@
+import pytest
+from test.conftest import launch_description
+from test.helpers import check_each_in
+
+
+@pytest.mark.launch(fixture=launch_description)
+def test_driver_advertices_all_relevant_topics(running_driver):
+ topic_list = [
+ "/diagnostics",
+ "/joint_states",
+ "/state",
+ ]
+ check_each_in(topic_list, "get_topic_names_and_types")