diff --git a/.gitignore b/.gitignore index f38b8a2..43c59aa 100644 --- a/.gitignore +++ b/.gitignore @@ -1,5 +1,6 @@ devel/ logs/ +log/ build/ bin/ install/ diff --git a/README.md b/README.md index daf6df5..33949f1 100644 --- a/README.md +++ b/README.md @@ -25,6 +25,16 @@ source ./install/local_setup.sh ros2 launch robotiq_hande_driver gripper_controller_preview.launch.py use_fake_hardware:=true ``` +### Test Serial Connection + +There is an additional test tool with hardcoded parameters to test the connection with the Hand-E without any ROS dependencies. +Before build, change the hardcoded parameters in the beginning of the `robotiq_hande_driver/test/communication_test.cpp` file. + +To run test: +```bash +cd cd ~/ceai_ws/src/robotiq_hande_driver/build/robotiq_hande_driver/ +./communication_test +``` --- ## Development notes diff --git a/robotiq_hande_driver/CHANGELOG.md b/robotiq_hande_driver/CHANGELOG.md index 025712c..0f4f725 100644 --- a/robotiq_hande_driver/CHANGELOG.md +++ b/robotiq_hande_driver/CHANGELOG.md @@ -9,6 +9,11 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Added +* [PR-7](https://github.com/AGH-CEAI/robotiq_hande_driver/pull/7) - Integration of ModbusRTU communication with ros2_control Hardware Interface +* [PR-2](https://github.com/AGH-CEAI/robotiq_hande_driver/pull/2) - ModbusRTU communication +* [PR-2](https://github.com/AGH-CEAI/robotiq_hande_driver/pull/2) - Protocol abstraction +* [PR-1](https://github.com/AGH-CEAI/robotiq_hande_driver/pull/1) - Prepared ros2_control Hardware Interface. + ### Changed ### Deprecated diff --git a/robotiq_hande_driver/CMakeLists.txt b/robotiq_hande_driver/CMakeLists.txt index 482726b..0ff5b32 100644 --- a/robotiq_hande_driver/CMakeLists.txt +++ b/robotiq_hande_driver/CMakeLists.txt @@ -6,8 +6,12 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic) endif() +include(FindPkgConfig) +pkg_check_modules(LIBMODBUS REQUIRED libmodbus) + +set(DEPENDS modbus) + set(HW_IF_INCLUDE_DEPENDS - fmt hardware_interface pluginlib rclcpp @@ -15,11 +19,20 @@ set(HW_IF_INCLUDE_DEPENDS rcpputils) find_package(ament_cmake REQUIRED) + foreach(dependency IN ITEMS ${HW_IF_INCLUDE_DEPENDS}) find_package(${dependency} REQUIRED) endforeach() -add_library(${PROJECT_NAME} SHARED hardware/src/hande_hardware_interface.cpp) +add_library( + ${PROJECT_NAME} SHARED hardware/src/hande_hardware_interface.cpp hardware/src/application.cpp + hardware/src/communication.cpp hardware/src/protocol_logic.cpp) + +target_link_libraries(${PROJECT_NAME} PUBLIC ${DEPENDS}) + +# TODO(issue#8) Build the communication_test only with BUILD_TESTING flag +add_executable(communication_test test/communication_test.cpp) +target_link_libraries(communication_test ${catkin_LIBRARIES} ${DEPENDS}) target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17) target_include_directories( @@ -47,9 +60,9 @@ if(BUILD_TESTING) ament_add_gmock(test_load_hw_interface test/test_load_hw_interface.cpp) target_link_libraries(test_load_hw_interface ${PROJECT_NAME}) - # TODO: investigate ament_add_pytest_test(example_7_urdf_xacro test/test_urdf_xacro.py) - # ament_add_pytest_test(view_example_7_launch test/test_view_robot_launch.py) - # ament_add_pytest_test(run_example_7_launch test/test_r6bot_controller_launch.py) + # TODO(issue#8) Build the communication_test only with BUILD_TESTING flag + # ament_add_gmock(application_test test/application_test.cpp ) + # target_link_libraries(application_test ${PROJECT_NAME}) endif() # EXPORTS diff --git a/robotiq_hande_driver/hardware/include/robotiq_hande_driver/application.hpp b/robotiq_hande_driver/hardware/include/robotiq_hande_driver/application.hpp new file mode 100644 index 0000000..670fab3 --- /dev/null +++ b/robotiq_hande_driver/hardware/include/robotiq_hande_driver/application.hpp @@ -0,0 +1,324 @@ +#ifndef ROBOTIQ_HANDE_DRIVER__APPLICATION_HPP_ +#define ROBOTIQ_HANDE_DRIVER__APPLICATION_HPP_ + +#include +#include + +#include "protocol_logic.hpp" + +namespace robotiq_hande_driver { + +static constexpr auto GRIPPER_CURRENT_SCALE = 0.01; +static constexpr auto MAX_SPEED = 255; +static constexpr auto MAX_FORCE = 255; + +/** + * @brief This class contains high-level gripper commands and status. + */ +class GripperApplication { + public: + struct Status { + bool is_reset; + bool is_ready; + bool is_moving; + bool is_stopped; + bool is_opened; + bool is_closed; + bool object_detected; + }; + + struct FaultStatus { + bool is_error; + }; + + GripperApplication(); + + ~GripperApplication() {}; + + /** + * @brief Initializes driver parameters. + * + * @param gripper_position_min Minimal gripper position in meters. + * @param gripper_position_max Maximal gripper position in meters. + * @param tty_port Modbus virtual port. + * @param baudrate Modbus serial baudrate. + * @param parity Modbus serial parity. + * @param data_bits Modbus serial data bits. + * @param stop_bit Modbus serial stopbit. + * @param slave_id Modbus slave id. + * @return None. + * @note The status should be checked to verify successful execution. An exception is thrown if + * communication issues occur. + */ + void initialize( + double gripper_position_min, + double gripper_position_max, + const std::string& tty_port, + int baudrate, + char parity, + int data_bits, + int stop_bit, + int slave_id) { + gripper_position_min_ = gripper_position_min; + gripper_position_max_ = gripper_position_max; + gripper_postion_step_ = (gripper_position_max_ - gripper_position_min_) / 255.0; + protocol_logic_.initialize(tty_port, baudrate, parity, data_bits, stop_bit, slave_id); + }; + + /** + * @brief Configures driver session. + * @return int Connection status code. + * @note The status should be checked to verify successful execution. An exception is thrown if + * communication issues occur. + */ + int configure() { + int result; + + result = protocol_logic_.configure(); + + return result; + }; + + /** + * @brief Deinitializes driver. + * + * @param none + * @return None. + * @note The status should be checked to verify successful execution. An exception is thrown if + * communication issues occur. + */ + void cleanup() { + protocol_logic_.cleanup(); + }; + + /** + * @brief Stops the gripper movement. + * + * @param none + * @return None. + * @note The status should be checked to verify successful execution. An exception is thrown if + * communication issues occur. + */ + void stop() { + protocol_logic_.stop(); + }; + + /** + * @brief Resets the gripper by deactivating and reactivating it. + * + * @param none + * @return None. + * @note The status should be checked to verify successful execution. An exception is thrown if + * communication issues occur. + */ + void reset() { + protocol_logic_.reset(); + }; + + /** + * Emergency auto-release, gripper fingers are slowly opened, reactivation necessary + * + * @param none + * @return None. + * @note The status should be checked to verify successful execution. An exception is thrown if + * communication issues occur. + */ + void auto_release() { + protocol_logic_.auto_release(); + }; + + /** + * @brief Activates the gripper, making it ready for use. + * + * @return None. + * @note The status should be checked to verify successful execution. An exception is thrown if + * communication issues occur. + */ + void activate() { + protocol_logic_.activate(); + }; + + /** + * @brief Deactivates the gripper. + * + * @param none + * @return None. + * @note The status should be checked to verify successful execution. An exception is thrown if + * communication issues occur. + */ + void deactivate() { + protocol_logic_.reset(); + }; + + /** + * @brief Deactivates the gripper. + * + * @param none + * @return None. + * @note The status should be checked to verify successful execution. An exception is thrown if + * communication issues occur. + */ + void shutdown() { + deactivate(); + cleanup(); + }; + + /** + * @brief Opens the gripper. + * + * @param none + * @return None. + * @note The status should be checked to verify successful execution. An exception is thrown if + * communication issues occur. + */ + void open() { + set_position(gripper_position_max_); + }; + + /** + * @brief Closes the gripper. + * + * @param none + * @return None. + * @note The status should be checked to verify successful execution. An exception is thrown if + * communication issues occur. + */ + void close() { + set_position(gripper_position_min_); + }; + + /** + * @brief Retrieves the gripper status. + * + * @param none + * @return The current gripper status. + * @note The status should be checked to verify successful execution. An exception is thrown if + * communication issues occur. + */ + Status get_status() { + return status_; + }; + + /** + * @brief Retrieves the gripper fault status. + * + * @param none + * @return The current gripper fault status. + * @note The status should be checked to verify successful execution. An exception is thrown if + * communication issues occur. + */ + FaultStatus get_fault_status() { + return fault_status_; + }; + + /** + * @brief Retrieves the requested position of the gripper. + * + * @param none + * @return The requested gripper position in meters. + * @note The status should be checked to verify successful execution. An exception is thrown if + * communication issues occur. + */ + double get_requested_position() { + return requested_position_; + }; + + /** + * @brief Retrieves the actual position of the gripper. + * + * @param none + * @return The actual gripper position in meters. + * @note The status should be checked to verify successful execution. An exception is thrown if + * communication issues occur. + */ + double get_position() { + return position_; + }; + + /** + * @brief Moves the gripper to the requested position. + * + * @param position The target position in meters. + * @return None. + * @note The status should be checked to verify successful execution. An exception is thrown if + * communication issues occur. + */ + void set_position(double position, double force = 1.0) { + uint8_t scaled_force = static_cast(force * MAX_FORCE); + protocol_logic_.go_to( + (uint8_t)((gripper_position_max_ - position) / gripper_postion_step_), + MAX_SPEED, + scaled_force); + }; + + /** + * @brief Retrieves the electric current drawn by the gripper. + * + * @param none + * @return The electric current in amperes (range: 0–2.55 A) + * @note The status should be checked to verify successful execution. An exception is thrown if + * communication issues occur. + */ + double get_current() { + return current_; + }; + + /** + * @brief Reads gripper data. + * + * @param none + * @return None. + * @note The status should be checked to verify successful execution. An exception is thrown if + * communication issues occur. + */ + void read(); + + /** + * @brief Writes gripper data. + * + * @param none + * @return None. + * @note The status should be checked to verify successful execution. An exception is thrown if + * communication issues occur. + */ + void write() { + protocol_logic_.refresh_registers(); + }; + + private: + /** + * Handles protocol logic for mid-level abstraction. + */ + ProtocolLogic protocol_logic_; + + /** + * Stores the gripper status bits. + */ + Status status_; + + /** + * Stores the fault status bits. + */ + FaultStatus fault_status_; + + /** + * Stores the requested position of the gripper in meters. + */ + double requested_position_; + + /** + * Stores the actual position of the gripper in meters. + */ + double position_; + + /** + * Stores the electric current drawn by the gripper in amperes. + */ + double current_; + + double gripper_position_min_; + double gripper_position_max_; + double gripper_postion_step_; +}; +} // namespace robotiq_hande_driver +#endif // ROBOTIQ_HANDE_DRIVER__APPLICATION_HPP_ diff --git a/robotiq_hande_driver/hardware/include/robotiq_hande_driver/communication.hpp b/robotiq_hande_driver/hardware/include/robotiq_hande_driver/communication.hpp new file mode 100644 index 0000000..95692f4 --- /dev/null +++ b/robotiq_hande_driver/hardware/include/robotiq_hande_driver/communication.hpp @@ -0,0 +1,241 @@ +#ifndef ROBOTIQ_HANDE_DRIVER__COMMUNICATION_HPP_ +#define ROBOTIQ_HANDE_DRIVER__COMMUNICATION_HPP_ + +#include +#include +#include + +namespace robotiq_hande_driver { + +static constexpr auto DEBUG_MODBUS = false; +static constexpr auto FAILURE_MODBUS = -1; + +static constexpr uint16_t GRIPPER_OUTPUT_FIRST_REG = 0x07D0; +static constexpr uint16_t GRIPPER_INPUT_FIRST_REG = 0x03E8; + +enum class OutputBytes : uint8_t { + RESERVED_1 = 0u, + ACTION_REQUEST, + POSITION_REQUEST, + RESERVED_2, + FORCE, + SPEED, + BYTES_MAX +}; +static constexpr auto OUTPUT_REGISTER_WORD_LENGTH = static_cast(OutputBytes::BYTES_MAX) / 2; + +enum class InputBytes : uint8_t { + RESERVED_1 = 0u, + GRIPPER_STATUS, + POSITION_REQUEST_ECHO, + FAULT_STATUS, + CURRENT, + POSITION, + BYTES_MAX +}; +static constexpr auto INPUT_REGISTER_WORD_LENGTH = static_cast(InputBytes::BYTES_MAX) / 2; + +/** + * @brief This class contains low level gripper commands and status + */ +class Communication { + public: + Communication(); + + ~Communication() { + cleanup(); + }; + + /** + * @brief Initializes driver parameters. + * + * @param tty_port Modbus virtual port. + * @param baudrate Modbus serial baudrate. + * @param parity Modbus serial parity. + * @param data_bits Modbus serial data bits. + * @param stop_bit Modbus serial stopbit. + * @param slave_id Modbus slave id. + * @return None. + * @note The status should be checked to verify successful execution. An exception is thrown if + * communication issues occur. + */ + void initialize( + const std::string& tty_port, + int baudrate, + char parity, + int data_bits, + int stop_bit, + int slave_id) { + tty_port_ = tty_port.c_str(); + baudrate_ = baudrate; + parity_ = parity; + data_bits_ = data_bits; + stop_bit_ = stop_bit; + slave_id_ = slave_id; + }; + + /** + * @brief Initializes communication layer. + * + * @param none + * @return int Connection status code. + * @note The status should be checked to verify successful execution. An exception is thrown if + * communication issues occur. + */ + int configure() { + int result; + + mb_ = modbus_new_rtu(tty_port_, baudrate_, parity_, data_bits_, stop_bit_); + modbus_set_slave(mb_, slave_id_); + modbus_set_debug(mb_, DEBUG_MODBUS); + result = connect(); + + return result; + }; + + /** + * @brief Deinitializes communication layer. + * + * @param none + * @return None. + * @note The status should be checked to verify successful execution. An exception is thrown if + * communication issues occur. + */ + void cleanup() { + disconnect(); + modbus_free(mb_); + }; + + /** + * @brief Connects to the gripper using Modbus RTU and a virtual socket. + * + * @param none + * @return connection status code. + * @note The status should be checked to verify successful execution. An exception is thrown if + * communication issues occur. + */ + int connect(); + + /** + * @brief Disconnects from the gripper using Modbus RTU and a virtual socket. + * + * @param none + * @return None. + * @note The status should be checked to verify successful execution. An exception is thrown if + * communication issues occur. + */ + void disconnect() { + modbus_close(mb_); + }; + + /** + * @brief Reads and writes input/output registers at once. + * + * @param none + * @return None. + * @note The status should be checked to verify successful execution. An exception is thrown if + * communication issues occur. + */ + void read_write_registers() { + /** + * @brief Read and write modbus registers at once + * + * @param modbus_t *ctx + * @param int write_addr + * @param int write_nb + * @param uint16_t *src + * @param int read_addr + * @param int read_nb + * @param uint16_t *dest + * @return int >0 on success + * @note The status should be checked to verify successful execution. An exception is thrown + * if communication issues occur. + */ + modbus_write_and_read_registers( + mb_, + GRIPPER_INPUT_FIRST_REG, + OUTPUT_REGISTER_WORD_LENGTH, + reinterpret_cast(output_bytes_), + GRIPPER_OUTPUT_FIRST_REG, + INPUT_REGISTER_WORD_LENGTH, + reinterpret_cast(input_bytes_)); + }; + + /** + * @brief Sets output bytes to zeros. + * + * @param none + * @return None. + * @note The status should be checked to verify successful execution. An exception is thrown if + * communication issues occur. + */ + void clear_output_bytes() { + memset(output_bytes_, 0, sizeof(output_bytes_)); + }; + + /** + * @brief Retrieves the input byte value at the specified index. + * + * @param index The InputBytes byte index. + * @return Requested byte value. + * @note The status should be checked to verify successful execution. An exception is thrown if + * communication issues occur. + */ + uint8_t get_input_byte(InputBytes index) { + return input_bytes_[static_cast(index)]; + }; + + /** + * @brief Sets the output byte value at the specified index. + * + * @param index OutputBytes byte index + * @param value The value to be set. + * @return none + * @note The status should be checked to verify successful execution. An exception is thrown if + * communication issues occur. + */ + void set_output_byte(OutputBytes index, uint8_t value) { + output_bytes_[static_cast(index)] = value; + }; + + /** + * @brief Sets the n-th bit of the action request byte. + * + * @param position_bit n-th bit in byte + * @param value The boolean value to set for the bit. + * @return None. + */ + void write_action_bit(uint8_t position_bit, bool value) { + output_bytes_[static_cast(OutputBytes::ACTION_REQUEST)] = bit_set_to( + output_bytes_[static_cast(OutputBytes::ACTION_REQUEST)], position_bit, value); + }; + + /** + * @brief Sets the n-th bit of a number to the specified boolean value. + * + * @param number + * @param n The n-th bit position to set. + * @param x The boolean value to set the bit to. + * @return The modified number with the n-th bit set to the specified value. + */ + uint bit_set_to(uint value, uint n, bool x) { + uint reset_n_bit = ~(1u << n); + uint set_n_bit = static_cast(x) << n; + return (value & reset_n_bit) | set_n_bit; + }; + + private: + const char* tty_port_; + int baudrate_; + char parity_; + int data_bits_; + int stop_bit_; + int slave_id_; + + modbus_t* mb_; + + uint8_t input_bytes_[static_cast(InputBytes::BYTES_MAX)]; + uint8_t output_bytes_[static_cast(OutputBytes::BYTES_MAX)]; +}; +} // namespace robotiq_hande_driver +#endif // ROBOTIQ_HANDE_DRIVER__COMMUNICATION_HPP_ diff --git a/robotiq_hande_driver/hardware/include/robotiq_hande_driver/hande_hardware_interface.hpp b/robotiq_hande_driver/hardware/include/robotiq_hande_driver/hande_hardware_interface.hpp index d44238d..6f6070e 100644 --- a/robotiq_hande_driver/hardware/include/robotiq_hande_driver/hande_hardware_interface.hpp +++ b/robotiq_hande_driver/hardware/include/robotiq_hande_driver/hande_hardware_interface.hpp @@ -5,6 +5,8 @@ #include #include +#include "application.hpp" + namespace robotiq_hande_driver { namespace HWI = hardware_interface; @@ -35,14 +37,28 @@ class RobotiqHandeHardwareInterface : public HWI::SystemInterface { return *logger_; } + rclcpp::Clock::SharedPtr get_clock() const { + return clock_; + } + private: - // TODO(modbus integration): composition of the modbus communication + GripperApplication gripper_driver_; std::shared_ptr logger_; + rclcpp::Clock::SharedPtr clock_; + double gripper_position_min_; + double gripper_position_max_; std::string tty_port_; + int baudrate_; + char parity_; + int data_bits_; + int stop_bit_; + int slave_id_; + double state_position_; double state_velocity_; double cmd_position_; + double cmd_force_; }; } // namespace robotiq_hande_driver diff --git a/robotiq_hande_driver/hardware/include/robotiq_hande_driver/protocol_logic.hpp b/robotiq_hande_driver/hardware/include/robotiq_hande_driver/protocol_logic.hpp new file mode 100644 index 0000000..8cb68da --- /dev/null +++ b/robotiq_hande_driver/hardware/include/robotiq_hande_driver/protocol_logic.hpp @@ -0,0 +1,378 @@ +#ifndef ROBOTIQ_HANDE_DRIVER__PROTOCOL_LOGIC_HPP_ +#define ROBOTIQ_HANDE_DRIVER__PROTOCOL_LOGIC_HPP_ + +#include + +#include "communication.hpp" + +namespace robotiq_hande_driver { + +/* Register mapping done based on Hand-E documentation: + * https://assets.robotiq.com/website-assets/support_documents/document/Hand-E_Instruction_Manual_e-Series_PDF_20190306.pdf + */ +enum class ActionRequestPositionBit : uint8_t { + ACTIVATE = 0u, /* rACT */ + GO_TO = 3u, /* rGTO */ + AUTOMATIC_RELEASE = 4u, /* rATR */ + AUTOMATIC_RELEASE_DIRECTION = 5u, /* rARD */ +}; + +enum class Activate : uint8_t { DEACTIVATE_GRIPPER = 0u, ACTIVATE_GRIPPER }; + +enum class GoTo : uint8_t { STOP = 0u, GO_TO_REQ_POS }; + +enum class AutomaticRelease : uint8_t { NORMAL = 0u, EMERGENCY_AUTO_RELEASE }; + +enum class AutoReleaseDirection : uint8_t { CLOSING = 0u, OPENING }; + +/* Gripper Status */ +enum class ResponseByte : uint8_t { + STATUS = 0u, + FAULT_STATUS = 2u, + SPEED = 4u, + FORCE = 5u, +}; + +enum class StatusPositionBit : uint8_t { + ACTIVATION_STATUS = 0u, /* gACT */ + ACTION_STATUS = 3u, /* gGTO */ + GRIPPER_STATUS = 4u, /* gSTA */ + OBJECT_DETECTION_STATUS = 6u, /* gObj */ +}; + +static constexpr auto ACTIVATION_STATUS_BITS = 0b1; +enum class ActivationStatus : uint8_t { GRIPPER_RESET = 0u, GRIPPER_ACTIVATION }; + +static constexpr auto ACTION_STATUS_BITS = 0b1; +enum class ActionStatus : uint8_t { STOPPED = 0u, GO_TO_POSITION_REQUEST }; + +static constexpr auto GRIPPER_STATUS_BITS = 0b11; +enum class GripperStatus : uint8_t { + GRIPPER_IN_RESET = 0u, + ACTIVATION_IN_PROGRESS, + NOT_USED, + ACTIVATION_COMPLETE +}; + +static constexpr auto OBJECT_DETECTION_STATUS_BITS = 0b11; +enum class ObjectDetectionStatus : uint8_t { + MOTION_NO_OBJECT = 0u, + STOPPED_OPENING_DETECTED, + STOPPED_CLOSING_DETECTED, + REQ_POS_NO_OBJECT +}; + +static constexpr auto GRIPPER_POSITION_OPENED_THRESHOLD = 230; +static constexpr auto GRIPPER_POSITION_CLOSED_THRESHOLD = 13; + +/** + * @brief This class contains protocol oriented functions and definitions. + */ +class ProtocolLogic { + public: + ProtocolLogic(); + + ~ProtocolLogic() {}; + + /** + * @brief Initializes driver parameters. + * + * @param tty_port Modbus virtual port. + * @param baudrate Modbus serial baudrate. + * @param parity Modbus serial parity. + * @param data_bits Modbus serial data bits. + * @param stop_bit Modbus serial stopbit. + * @param slave_id Modbus slave id. + * @return None. + * @note The status should be checked to verify successful execution. An exception is thrown if + * communication issues occur. + */ + void initialize( + const std::string& tty_port, + int baudrate, + char parity, + int data_bits, + int stop_bit, + int slave_id) { + communication_.initialize(tty_port, baudrate, parity, data_bits, stop_bit, slave_id); + }; + + /** + * @brief Configures protocol layer. + * + * @param none + * @return int Connection status code. + * @note The status should be checked to verify successful execution. An exception is thrown if + * communication issues occur. + */ + int configure() { + int result; + + activation_status_ = ActivationStatus::GRIPPER_RESET; + action_status_ = ActionStatus::STOPPED; + gripper_status_ = GripperStatus::NOT_USED; + object_detection_status_ = ObjectDetectionStatus::REQ_POS_NO_OBJECT; + fault_status_ = 0; + position_request_echo_ = 0; + position_ = 0; + current_ = 0; + result = communication_.configure(); + + return result; + }; + + /** + * @brief Deinitializes protocol layer. + * + * @param none + * @return None. + * @note The status should be checked to verify successful execution. An exception is thrown if + * communication issues occur. + */ + void cleanup() { + communication_.cleanup(); + }; + + /** + * @brief Resets the gripper. + * + * @param none + * @return None. + * @note The status should be checked to verify successful execution. An exception is thrown if + * communication issues occur. + */ + void reset() { + communication_.clear_output_bytes(); + communication_.write_action_bit( + static_cast(ActionRequestPositionBit::ACTIVATE), + static_cast(Activate::DEACTIVATE_GRIPPER)); + communication_.read_write_registers(); + }; + + /** + * @brief Sets the gripper. + * + * @param none + * @return None. + * @note The status should be checked to verify successful execution. An exception is thrown if + * communication issues occur. + */ + void set() { + communication_.clear_output_bytes(); + communication_.write_action_bit( + static_cast(ActionRequestPositionBit::ACTIVATE), + static_cast(Activate::ACTIVATE_GRIPPER)); + communication_.read_write_registers(); + }; + + /** + * @brief Performs an emergency auto-release. The fingers slowly open, requiring reactivation. + * + * @param none + * @return None. + * @note The status should be checked to verify successful execution. An exception is thrown if + * communication issues occur. + */ + void auto_release() { + communication_.write_action_bit( + static_cast(ActionRequestPositionBit::AUTOMATIC_RELEASE), + static_cast(AutomaticRelease::EMERGENCY_AUTO_RELEASE)); + communication_.write_action_bit( + static_cast(ActionRequestPositionBit::AUTOMATIC_RELEASE_DIRECTION), + static_cast(AutoReleaseDirection::OPENING)); + communication_.read_write_registers(); + }; + + /** + * @brief Activates the gripper, making it ready for use. + * + * @param none + * @return None. + * @note The status should be checked to verify successful execution. An exception is thrown if + * communication issues occur. + */ + void activate() { + reset(); + set(); + }; + + /** + * @brief Deactivates the gripper. + * + * @param none + * @return None. + * @note The status should be checked to verify successful execution. An exception is thrown if + * communication issues occur. + */ + void deactivate() { + reset(); + }; + + /** + * @brief Moves the gripper to the requested position with specified velocity and force. + * + * @param position The requested position. + * @param velocity The requested velocity. + * @param force The requested force. + * @return None. + * @note The status should be checked to verify successful execution. An exception is thrown if + * communication issues occur. + */ + void go_to(uint8_t position, uint8_t velocity, uint8_t force) { + communication_.write_action_bit( + static_cast(ActionRequestPositionBit::GO_TO), + static_cast(GoTo::GO_TO_REQ_POS)); + communication_.set_output_byte(OutputBytes::POSITION_REQUEST, position); + communication_.set_output_byte(OutputBytes::SPEED, velocity); + communication_.set_output_byte(OutputBytes::FORCE, force); + communication_.read_write_registers(); + }; + + /** + * @brief Stops the gripper. + * + * @return None. + * @note The status should be checked to verify successful execution. An exception is thrown if + * communication issues occur. + */ + void stop() { + communication_.write_action_bit( + static_cast(ActionRequestPositionBit::GO_TO), static_cast(GoTo::STOP)); + communication_.read_write_registers(); + }; + + /** + * @brief Checks if the gripper is in reset state. + * + * @return True if the gripper is in reset state. + */ + bool is_reset() { + return ( + gripper_status_ == GripperStatus::GRIPPER_IN_RESET + && activation_status_ == ActivationStatus::GRIPPER_RESET); + }; + + /** + * @brief Checks if the gripper is in ready state. + * + * @return True if the gripper is in ready state. + */ + bool is_ready() { + return ( + gripper_status_ == GripperStatus::ACTIVATION_COMPLETE + && activation_status_ == ActivationStatus::GRIPPER_ACTIVATION); + }; + + /** + * @brief Checks if the gripper is moving. + * + * @return True if gripper is moving. + */ + bool is_moving() { + return ( + action_status_ == ActionStatus::GO_TO_POSITION_REQUEST + && object_detection_status_ == ObjectDetectionStatus::MOTION_NO_OBJECT); + }; + + /** + * @brief Checks if the gripper is stopped. + * + * @return True if gripper is stopped. + */ + bool is_stopped() { + return object_detection_status_ != ObjectDetectionStatus::MOTION_NO_OBJECT; + }; + + /** + * @brief Checks if the gripper is closed. + * + * @return True if gripper is closed. + */ + bool is_closed() { + return position_ >= GRIPPER_POSITION_OPENED_THRESHOLD; + }; + + /** + * @brief Checks if the gripper is opened. + * + * @return True if gripper is opened. + */ + bool is_opened() { + return position_ <= GRIPPER_POSITION_CLOSED_THRESHOLD; + }; + + /** + * @brief Checks if the gripper has detected an object. + * + * @return True if gripper has detected an object. + */ + bool obj_detected() { + return ( + object_detection_status_ == ObjectDetectionStatus::STOPPED_OPENING_DETECTED + || object_detection_status_ == ObjectDetectionStatus::STOPPED_CLOSING_DETECTED); + }; + + /** + * @brief Retrieves the requested position of the gripper. + * + * @return Requested gripper position. + */ + uint8_t get_reg_pos() { + return position_request_echo_; + }; + + /** + * @brief Retrieves the actual position of the gripper. + * + * @return The actual gripper position. + */ + uint8_t get_pos() { + return position_; + }; + + /** + * @brief Retrieves the electric current drawn by the gripper. + * + * @return The electric current. + */ + uint8_t get_current() { + return current_; + }; + + /** + * @brief Decodes Modbus registers and refreshes appropriate data. + * + * @return None. + */ + void refresh_registers(); + + private: + /* Gripper */ + uint8_t status_; + ActivationStatus activation_status_; + ActionStatus action_status_; + GripperStatus gripper_status_; + ObjectDetectionStatus object_detection_status_; + + /* Fault */ + uint8_t fault_status_; + + /** + * @brief Stores the requested position in normalized 0–255 value. + */ + uint8_t position_request_echo_; + + /** + * @brief Stores the actual position in normalized 0–255 value. + */ + uint8_t position_; + + /** + * @brief Stores the electric current drawn by the gripper in 10 mA. + */ + uint8_t current_; + + Communication communication_; +}; +} // namespace robotiq_hande_driver +#endif // ROBOTIQ_HANDE_DRIVER__PROTOCOL_LOGIC_HPP_ diff --git a/robotiq_hande_driver/hardware/src/application.cpp b/robotiq_hande_driver/hardware/src/application.cpp new file mode 100644 index 0000000..a5369ba --- /dev/null +++ b/robotiq_hande_driver/hardware/src/application.cpp @@ -0,0 +1,33 @@ +#include "robotiq_hande_driver/application.hpp" + +#include + +namespace robotiq_hande_driver { + +GripperApplication::GripperApplication() + : requested_position_(), + position_(), + current_(), + gripper_position_min_(), + gripper_position_max_(), + gripper_postion_step_() {} + +void GripperApplication::read() { + protocol_logic_.refresh_registers(); + + status_.is_reset = protocol_logic_.is_reset(); + status_.is_ready = protocol_logic_.is_ready(); + status_.is_moving = protocol_logic_.is_moving(); + status_.is_stopped = protocol_logic_.is_stopped(); + status_.is_opened = protocol_logic_.is_opened(); + status_.is_closed = protocol_logic_.is_closed(); + status_.object_detected = protocol_logic_.obj_detected(); + + // fault_status + + requested_position_ = gripper_position_max_ + - (double)protocol_logic_.get_reg_pos() * gripper_postion_step_; + position_ = gripper_position_max_ - (double)protocol_logic_.get_pos() * gripper_postion_step_; + current_ = (double)protocol_logic_.get_current() * GRIPPER_CURRENT_SCALE; +} +} // namespace robotiq_hande_driver diff --git a/robotiq_hande_driver/hardware/src/communication.cpp b/robotiq_hande_driver/hardware/src/communication.cpp new file mode 100644 index 0000000..972e4bd --- /dev/null +++ b/robotiq_hande_driver/hardware/src/communication.cpp @@ -0,0 +1,19 @@ +#include "robotiq_hande_driver/communication.hpp" + +#include + +namespace robotiq_hande_driver { + +Communication::Communication() : input_bytes_{}, output_bytes_{} {} + +int Communication::connect() { + uint16_t activation_status[1] = {0x0000}; + int result; + + modbus_connect(mb_); + + result = modbus_read_registers(mb_, GRIPPER_OUTPUT_FIRST_REG, 1, activation_status); + + return result; +} +} // namespace robotiq_hande_driver diff --git a/robotiq_hande_driver/hardware/src/hande_hardware_interface.cpp b/robotiq_hande_driver/hardware/src/hande_hardware_interface.cpp index 7ce76ee..d24d78b 100644 --- a/robotiq_hande_driver/hardware/src/hande_hardware_interface.cpp +++ b/robotiq_hande_driver/hardware/src/hande_hardware_interface.cpp @@ -5,39 +5,97 @@ namespace robotiq_hande_driver { +static constexpr auto THROTTLE_1000_MS = 1000; +static constexpr auto ACTIVATION_MAX_ITER = 100; +inline void wait_100ms() { + usleep(100 * 1000); +} + RobotiqHandeHardwareInterface::RobotiqHandeHardwareInterface() {} HWI::CallbackReturn RobotiqHandeHardwareInterface::on_init(const HWI::HardwareInfo& info) { if(HWI::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { return HWI::CallbackReturn::ERROR; } + clock_ = std::make_shared(rclcpp::Clock()); + logger_ = std::make_shared( + rclcpp::get_logger("controller_manager.resource_manager.hardware_component.system." + "RobotiqHandeHardwareInterface")); + + RCLCPP_DEBUG( + get_logger(), "grip_pos_min: %s", info_.hardware_parameters["grip_pos_min"].c_str()); + RCLCPP_DEBUG( + get_logger(), "grip_pos_max: %s", info_.hardware_parameters["grip_pos_max"].c_str()); + RCLCPP_DEBUG(get_logger(), "tty: %s", info_.hardware_parameters["tty"].c_str()); + RCLCPP_DEBUG(get_logger(), "baudrate: %s", info_.hardware_parameters["baudrate"].c_str()); + RCLCPP_DEBUG(get_logger(), "parity: %s", info_.hardware_parameters["parity"].c_str()); + RCLCPP_DEBUG(get_logger(), "data_bits: %s", info_.hardware_parameters["data_bits"].c_str()); + RCLCPP_DEBUG(get_logger(), "stop_bit: %s", info_.hardware_parameters["stop_bit"].c_str()); + RCLCPP_DEBUG(get_logger(), "slave_id: %s", info_.hardware_parameters["slave_id"].c_str()); - state_position_ = 0.025; state_velocity_ = 0.0; - cmd_position_ = 0.025; + cmd_force_ = 1.0; + gripper_position_min_ = std::stod(info_.hardware_parameters["grip_pos_min"]); + gripper_position_max_ = std::stod(info_.hardware_parameters["grip_pos_max"]); tty_port_ = info_.hardware_parameters["tty"]; - - logger_ = std::make_shared( - rclcpp::get_logger("controller_manager.resource_manager.hardware_" - "component.system.RobotiqHandeHardwareInterface")); - - // TODO(modbus integration): Set parameters for the modbus communication - - RCLCPP_INFO(get_logger(), "Initialized ModbusRTU for %s", tty_port_.c_str()); + baudrate_ = std::stoi(info_.hardware_parameters["baudrate"]); + parity_ = (info_.hardware_parameters["parity"].c_str())[0]; + data_bits_ = std::stoi(info_.hardware_parameters["data_bits"]); + stop_bit_ = std::stoi(info_.hardware_parameters["stop_bit"]); + slave_id_ = std::stoi(info_.hardware_parameters["slave_id"]); + + cmd_position_ = gripper_position_max_; + state_position_ = gripper_position_max_; + + gripper_driver_.initialize( + gripper_position_min_, + gripper_position_max_, + tty_port_, + baudrate_, + parity_, + data_bits_, + stop_bit_, + slave_id_); + + RCLCPP_INFO( + get_logger(), + "Initialized ModbusRTU for %s, %d, %c, %d, %d", + tty_port_.c_str(), + baudrate_, + parity_, + data_bits_, + stop_bit_); return HWI::CallbackReturn::SUCCESS; } HWI::CallbackReturn RobotiqHandeHardwareInterface::on_configure( const rlccp_lc::State& /*previous_state*/) { - // TODO(modbus integration): Initialize the ModbusRTU communication session - RCLCPP_INFO(get_logger(), "configure()"); + int result; + + RCLCPP_INFO( + get_logger(), + "Connecting to tty:%s, baudrate:%d, parity:%c, data_bits:%d, stop_bit:%d", + tty_port_.c_str(), + baudrate_, + parity_, + data_bits_, + stop_bit_); + result = gripper_driver_.configure(); + + // TODO(issue#10) Modbus connection check always fails + // if(result == FAILURE_MODBUS) { + // RCLCPP_INFO(get_logger(), "Failed to configure Hand-E Gripper"); + // return HWI::CallbackReturn::FAILURE; + // } + RCLCPP_INFO(get_logger(), "Configured Hand-E Gripper: %d", result); return HWI::CallbackReturn::SUCCESS; } HWI::CallbackReturn RobotiqHandeHardwareInterface::on_cleanup( const rlccp_lc::State& /*previous_state*/) { - // TODO(modbus integration): Deinitalize the ModbusRTU session - RCLCPP_INFO(get_logger(), "cleanup()"); + gripper_driver_.cleanup(); + + RCLCPP_INFO(get_logger(), "Cleaned up Hand-E connection"); return HWI::CallbackReturn::SUCCESS; } @@ -64,48 +122,77 @@ std::vector RobotiqHandeHardwareInterface::export_command info_.joints[LEFT_FINGER_JOINT_ID].name, hardware_interface::HW_IF_POSITION, &cmd_position_)); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[LEFT_FINGER_JOINT_ID].name, hardware_interface::HW_IF_POSITION, &cmd_force_)); - RCLCPP_INFO(get_logger(), "export_command_interfaces()"); return command_interfaces; } HWI::CallbackReturn RobotiqHandeHardwareInterface::on_activate( const rlccp_lc::State& /*previous_state*/) { - // TODO(modbus integration): Power on the gripper (activation) - RCLCPP_INFO(get_logger(), "activate()"); + int iter = 0; + gripper_driver_.read(); + + if(gripper_driver_.get_status().is_ready) + RCLCPP_INFO(get_logger(), "Hand-E already active"); + else { + RCLCPP_INFO(get_logger(), "Hand-E activation in progress"); + gripper_driver_.activate(); + + while(!gripper_driver_.get_status().is_ready) { + RCLCPP_DEBUG_SKIPFIRST_THROTTLE( + get_logger(), + *get_clock(), + THROTTLE_1000_MS, + "Waiting for activation to be finished: %d", + iter); + wait_100ms(); + gripper_driver_.read(); + if(iter++ > ACTIVATION_MAX_ITER) { + RCLCPP_INFO(get_logger(), "Hand-E NOT activated, failure"); + return HWI::CallbackReturn::FAILURE; + } + } + } + + RCLCPP_INFO(get_logger(), "Hand-E successfully activated"); return HWI::CallbackReturn::SUCCESS; } HWI::CallbackReturn RobotiqHandeHardwareInterface::on_deactivate( const rlccp_lc::State& /*previous_state*/) { - // TODO(modbus integration): Deactiavte the gripper (set the reset flag) - RCLCPP_INFO(get_logger(), "deactivate()"); + gripper_driver_.deactivate(); + + RCLCPP_INFO(get_logger(), "Hand-E successfully deactivated"); return HWI::CallbackReturn::SUCCESS; } HWI::CallbackReturn RobotiqHandeHardwareInterface::on_shutdown( const rlccp_lc::State& /*previous_state*/) { - // TODO(modbus integration): Deactivate the gripper, deinitalize the modbus - // RTU session, - RCLCPP_INFO(get_logger(), "shutdown()"); + gripper_driver_.shutdown(); + + RCLCPP_INFO(get_logger(), "Hand-E shutdown"); return HWI::CallbackReturn::SUCCESS; } HWI::CallbackReturn RobotiqHandeHardwareInterface::on_error( const rlccp_lc::State& /*previous_state*/) { - // TODO(modbus integration): Placeholder for error handling - RCLCPP_INFO(get_logger(), "Handled error"); - return HWI::CallbackReturn::SUCCESS; + RCLCPP_INFO(get_logger(), "Handled error with FAILURE on purpose - check previous logs"); + return HWI::CallbackReturn::FAILURE; } HWI::return_type RobotiqHandeHardwareInterface::read( const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { - // TODO(modbus integration): data = driver_->receive_data(); + gripper_driver_.read(); + state_position_ = gripper_driver_.get_position(); + return hardware_interface::return_type::OK; } HWI::return_type RobotiqHandeHardwareInterface::write( const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { - // TODO(modbus integration): driver_->send_data(cmd_position_); + gripper_driver_.set_position(cmd_position_, cmd_force_); + gripper_driver_.write(); + return hardware_interface::return_type::OK; } diff --git a/robotiq_hande_driver/hardware/src/protocol_logic.cpp b/robotiq_hande_driver/hardware/src/protocol_logic.cpp new file mode 100644 index 0000000..0bf6372 --- /dev/null +++ b/robotiq_hande_driver/hardware/src/protocol_logic.cpp @@ -0,0 +1,46 @@ +#include "robotiq_hande_driver/protocol_logic.hpp" + +#include + +namespace robotiq_hande_driver { + +ProtocolLogic::ProtocolLogic() + : status_(), + activation_status_(ActivationStatus::GRIPPER_RESET), + action_status_(ActionStatus::STOPPED), + gripper_status_(GripperStatus::NOT_USED), + object_detection_status_(ObjectDetectionStatus::REQ_POS_NO_OBJECT), + fault_status_(), + position_request_echo_(), + position_(), + current_() {} + +void ProtocolLogic::refresh_registers() { + communication_.read_write_registers(); + + status_ = communication_.get_input_byte(InputBytes::GRIPPER_STATUS); + + activation_status_ = + (ActivationStatus)((status_ >> static_cast(StatusPositionBit::ACTIVATION_STATUS)) + & ACTIVATION_STATUS_BITS); + + action_status_ = (ActionStatus)((status_ >> static_cast(StatusPositionBit::ACTION_STATUS)) + & ACTION_STATUS_BITS); + + gripper_status_ = + (GripperStatus)((status_ >> static_cast(StatusPositionBit::GRIPPER_STATUS)) + & GRIPPER_STATUS_BITS); + + object_detection_status_ = + (ObjectDetectionStatus)((status_ + >> static_cast(StatusPositionBit::OBJECT_DETECTION_STATUS)) + & OBJECT_DETECTION_STATUS_BITS); + + // TODO(issue#9) Read Hand-E fault status flags + fault_status_ = communication_.get_input_byte(InputBytes::FAULT_STATUS); + + position_request_echo_ = communication_.get_input_byte(InputBytes::POSITION_REQUEST_ECHO); + position_ = communication_.get_input_byte(InputBytes::POSITION); + current_ = communication_.get_input_byte(InputBytes::CURRENT); +} +} // namespace robotiq_hande_driver diff --git a/robotiq_hande_driver/package.xml b/robotiq_hande_driver/package.xml index 311dab6..74d025d 100644 --- a/robotiq_hande_driver/package.xml +++ b/robotiq_hande_driver/package.xml @@ -17,6 +17,7 @@ gripper_controllers hardware_interface + libmodbus-dev pluginlib rclcpp rclcpp_lifecycle diff --git a/robotiq_hande_driver/test/communication_test.cpp b/robotiq_hande_driver/test/communication_test.cpp new file mode 100644 index 0000000..ac678bf --- /dev/null +++ b/robotiq_hande_driver/test/communication_test.cpp @@ -0,0 +1,147 @@ +#include +#include +#include + +constexpr auto REGISTER_READ_LENGTH = 6; +constexpr auto DEVICE_NAME = "/tmp/ttyUR"; +constexpr auto BAUDRATE = 115200; +constexpr auto PARITY = 'N'; +constexpr auto DATA_BITS = 8; +constexpr auto STOP_BIT = 1; +constexpr auto DEBUG_MODBUS = true; + +constexpr uint8_t SERVER_ID = 0x09; + +constexpr uint16_t GRIPPER_OUTPUT_FIRST_REG = 0x07D0; +constexpr uint16_t GRIPPER_INPUT_FIRST_REG = 0x03E8; +constexpr uint8_t READ_WRITE_REG_LENGTH = 3; + +constexpr uint16_t GRIPPER_FLAGS_DEACTIVATE = 0x0000; +constexpr uint16_t GRIPPER_FLAGS_ACTIVATE = 0x0100; +constexpr uint16_t GRIPPER_FLAGS_ACTIVATED = 0x3100; + +constexpr uint16_t GRIPPER_FLAGS_MOVE = 0x0900; +constexpr uint16_t FULL_SPEED_FORCE = 0xFFFF; +constexpr uint16_t GRIPPER_OPENED = 0x0000; +constexpr uint16_t GRIPPER_CLOSED = 0x00FF; + +constexpr uint16_t GRIPPER_FLAGS_CLOSED = 0xB900; +constexpr uint16_t GRIPPER_FLAGS_NO_OBJECT = 0xF900; + +void sleep_100ms() { + usleep(100 * 1000); // ms * 1000 +} + +void test_connection(modbus_t* ctx) { + uint16_t tab_reg[REGISTER_READ_LENGTH]; + + // Read 5 registers from the address 0 + modbus_read_registers(ctx, GRIPPER_OUTPUT_FIRST_REG, REGISTER_READ_LENGTH, tab_reg); + + printf( + "Status: %.4X %.4X %.4X %.4X %.4X %.4X\n", + tab_reg[0], + tab_reg[1], + tab_reg[2], + tab_reg[3], + tab_reg[4], + tab_reg[5]); +} + +void activate(modbus_t* ctx) { + printf("Step: Activation Request (clear and set rACT)\n"); + uint16_t activation_request_register_reset[READ_WRITE_REG_LENGTH] = { + GRIPPER_FLAGS_DEACTIVATE, 0x0000, 0x0000}; + modbus_write_registers( + ctx, GRIPPER_INPUT_FIRST_REG, READ_WRITE_REG_LENGTH, activation_request_register_reset); + + uint16_t activation_request_register_set[READ_WRITE_REG_LENGTH] = { + GRIPPER_FLAGS_ACTIVATE, 0x0000, 0x0000}; + modbus_write_registers( + ctx, GRIPPER_INPUT_FIRST_REG, READ_WRITE_REG_LENGTH, activation_request_register_set); +} + +void wait_activation_complete(modbus_t* ctx) { + printf("Step: Read Gripper status until the activation is completed\n"); + uint16_t activation_status[READ_WRITE_REG_LENGTH] = {}; + + modbus_read_registers(ctx, GRIPPER_OUTPUT_FIRST_REG, READ_WRITE_REG_LENGTH, activation_status); + + while(activation_status[0] != GRIPPER_FLAGS_ACTIVATED) { + printf( + "Gripper not yet activated: %.4X vs. %.4X\n", + activation_status[0], + GRIPPER_FLAGS_ACTIVATED); + modbus_read_registers(ctx, GRIPPER_OUTPUT_FIRST_REG, 1, activation_status); + } +} + +void close_gripper(modbus_t* ctx) { + printf("Step: Close the Gripper at full speed and full force\n"); + uint16_t close_gripper_request_register[READ_WRITE_REG_LENGTH] = { + GRIPPER_FLAGS_MOVE, GRIPPER_CLOSED, FULL_SPEED_FORCE}; + modbus_write_registers( + ctx, GRIPPER_INPUT_FIRST_REG, READ_WRITE_REG_LENGTH, close_gripper_request_register); +} + +void open_gripper(modbus_t* ctx) { + printf("Step: Open the Gripper at full speed and full force\n"); + uint16_t open_gripper_request_register[READ_WRITE_REG_LENGTH] = { + GRIPPER_FLAGS_MOVE, GRIPPER_OPENED, FULL_SPEED_FORCE}; + modbus_write_registers( + ctx, GRIPPER_INPUT_FIRST_REG, READ_WRITE_REG_LENGTH, open_gripper_request_register); +} + +void wait_movement_complete(modbus_t* ctx) { + printf("Step: Read Gripper status until movement is complete\n"); + uint16_t gripper_status_movement[READ_WRITE_REG_LENGTH] = {}; + + modbus_read_registers( + ctx, GRIPPER_OUTPUT_FIRST_REG, READ_WRITE_REG_LENGTH, gripper_status_movement); + + while(gripper_status_movement[0] != GRIPPER_FLAGS_CLOSED + && gripper_status_movement[0] != GRIPPER_FLAGS_NO_OBJECT) { + printf( + "Gripper not yet closed: %.4X vs. %.4X or %.4X\n", + gripper_status_movement[0], + GRIPPER_FLAGS_NO_OBJECT, + GRIPPER_FLAGS_CLOSED); + modbus_read_registers( + ctx, GRIPPER_OUTPUT_FIRST_REG, READ_WRITE_REG_LENGTH, gripper_status_movement); + sleep_100ms(); + } +} + +int main(void) { + modbus_t* mb; + + mb = modbus_new_rtu(DEVICE_NAME, BAUDRATE, PARITY, DATA_BITS, STOP_BIT); + modbus_set_slave(mb, SERVER_ID); + modbus_set_debug(mb, DEBUG_MODBUS); + modbus_connect(mb); + printf("\nConnected\n"); + + test_connection(mb); + + activate(mb); + wait_activation_complete(mb); + printf("Gripper activated, press any key to continue\n"); + getc(stdin); + + printf("BYPASS: Step: Move the robot to the pick-up location\n"); + + close_gripper(mb); + wait_movement_complete(mb); + printf("Gripper closed, press any key to continue\n"); + getc(stdin); + + printf("BYPASS: Step: Move the robot to the release location\n"); + + open_gripper(mb); + wait_movement_complete(mb); + printf("Gripper opened\n"); + + printf("Test finished, closing connection to the Gripper\n"); + modbus_close(mb); + modbus_free(mb); +} diff --git a/robotiq_hande_driver/test/test_load_hw_interface.cpp b/robotiq_hande_driver/test/test_load_hw_interface.cpp index 63b9f1a..bef683b 100644 --- a/robotiq_hande_driver/test/test_load_hw_interface.cpp +++ b/robotiq_hande_driver/test/test_load_hw_interface.cpp @@ -26,7 +26,14 @@ class TestHWInterface : public ::testing::Test { robotiq_hande_driver/RobotiqHandeHardwareInterface - /tmp/ttyX + 0.0 + /tmp/ttyUR + 115200 + N + 8 + 1 + 9