From 6b782ba3b2182366f26c8157d76bbfbde5cea2dc Mon Sep 17 00:00:00 2001 From: Giovanni Remigi Date: Tue, 5 Sep 2023 13:13:59 +0200 Subject: [PATCH 01/47] Refactoring for testability. --- robotiq_driver/CMakeLists.txt | 3 +- ...ipper_interface.hpp => default_driver.hpp} | 60 +++------- .../robotiq_driver/default_driver_factory.hpp | 58 +++++++++ .../include/robotiq_driver/driver.hpp | 113 ++++++++++++++++++ .../include/robotiq_driver/driver_factory.hpp | 51 ++++++++ .../robotiq_driver/hardware_interface.hpp | 36 ++++-- ...ipper_interface.cpp => default_driver.cpp} | 63 +++++----- robotiq_driver/src/default_driver_factory.cpp | 62 ++++++++++ robotiq_driver/src/gripper_interface_test.cpp | 37 +++--- robotiq_driver/src/hardware_interface.cpp | 59 +++++---- 10 files changed, 414 insertions(+), 128 deletions(-) rename robotiq_driver/include/robotiq_driver/{robotiq_gripper_interface.hpp => default_driver.hpp} (80%) create mode 100644 robotiq_driver/include/robotiq_driver/default_driver_factory.hpp create mode 100644 robotiq_driver/include/robotiq_driver/driver.hpp create mode 100644 robotiq_driver/include/robotiq_driver/driver_factory.hpp rename robotiq_driver/src/{robotiq_gripper_interface.cpp => default_driver.cpp} (84%) create mode 100644 robotiq_driver/src/default_driver_factory.cpp diff --git a/robotiq_driver/CMakeLists.txt b/robotiq_driver/CMakeLists.txt index bda81ea..5944589 100644 --- a/robotiq_driver/CMakeLists.txt +++ b/robotiq_driver/CMakeLists.txt @@ -28,7 +28,8 @@ add_library( SHARED src/crc.cpp src/hardware_interface.cpp - src/robotiq_gripper_interface.cpp + src/default_driver.cpp + src/default_driver_factory.cpp ) target_include_directories( ${PROJECT_NAME} diff --git a/robotiq_driver/include/robotiq_driver/robotiq_gripper_interface.hpp b/robotiq_driver/include/robotiq_driver/default_driver.hpp similarity index 80% rename from robotiq_driver/include/robotiq_driver/robotiq_gripper_interface.hpp rename to robotiq_driver/include/robotiq_driver/default_driver.hpp index c6203e2..5cf4c94 100644 --- a/robotiq_driver/include/robotiq_driver/robotiq_gripper_interface.hpp +++ b/robotiq_driver/include/robotiq_driver/default_driver.hpp @@ -30,6 +30,7 @@ #include +#include #include #include @@ -38,32 +39,33 @@ * the gripper's current state. * */ -class RobotiqGripperInterface +namespace robotiq_driver +{ +class DefaultDriver : public Driver { public: - explicit RobotiqGripperInterface( - const std::string & com_port = "/dev/ttyUSB0", uint8_t slave_id = 0x09); + explicit DefaultDriver(const std::string & com_port = "/dev/ttyUSB0", uint8_t slave_id = 0x09); /** * @brief Activates the gripper. * * @throw serial::IOException on failure to successfully communicate with gripper port */ - void activateGripper(); + void activate() override; /** * @brief Deactivates the gripper. * * @throw serial::IOException on failure to successfully communicate with gripper port */ - void deactivateGripper(); + void deactivate() override; /** * @brief Commands the gripper to move to the desired position. * * @param pos A value between 0x00 (fully open) and 0xFF (fully closed). */ - void setGripperPosition(uint8_t pos); + void set_gripper_position(uint8_t pos) override; /** * @brief Return the current position of the gripper. @@ -72,58 +74,31 @@ class RobotiqGripperInterface * * @return uint8_t A value between 0x00 (fully open) and 0xFF (fully closed). */ - uint8_t getGripperPosition(); + uint8_t get_gripper_position() override; /** * @brief Returns true if the gripper is currently moving, false otherwise. * */ - bool gripperIsMoving(); + bool gripper_is_moving() override; /** * @brief Set the speed of the gripper. * * @param speed A value between 0x00 (stopped) and 0xFF (full speed). */ - void setSpeed(uint8_t speed); + void set_speed(uint8_t speed) override; /** * @brief Set how forcefully the gripper opens or closes. * * @param force A value between 0x00 (no force) or 0xFF (maximum force). */ - void setForce(uint8_t force); - - enum class ActivationStatus - { - RESET, - ACTIVE - }; - - enum class ActionStatus - { - STOPPED, - MOVING - }; - - enum class GripperStatus - { - RESET, - IN_PROGRESS, - COMPLETED, - }; - - enum class ObjectDetectionStatus - { - MOVING, - OBJECT_DETECTED_OPENING, - OBJECT_DETECTED_CLOSING, - AT_REQUESTED_POSITION - }; + void set_force(uint8_t force) override; private: - std::vector createReadCommand(uint16_t first_register, uint8_t num_registers); - std::vector createWriteCommand( + std::vector create_read_command(uint16_t first_register, uint8_t num_registers); + std::vector create_write_command( uint16_t first_register, const std::vector & data); /** @@ -132,7 +107,7 @@ class RobotiqGripperInterface * @param num_bytes Number of bytes to be read from device port. * @throw serial::IOException on failure to successfully communicate with gripper port */ - std::vector readResponse(size_t num_bytes); + std::vector read_response(size_t num_bytes); /** * @brief Send a command to the gripper. @@ -140,14 +115,14 @@ class RobotiqGripperInterface * @param cmd The command. * @throw serial::IOException on failure to successfully communicate with gripper port */ - void sendCommand(const std::vector & cmd); + void send_command(const std::vector & cmd); /** * @brief Read the current status of the gripper, and update member variables as appropriate. * * @throw serial::IOException on failure to successfully communicate with gripper port */ - void updateStatus(); + void update_status(); serial::Serial port_; uint8_t slave_id_; @@ -162,3 +137,4 @@ class RobotiqGripperInterface uint8_t commanded_gripper_speed_; uint8_t commanded_gripper_force_; }; +} // namespace robotiq_driver diff --git a/robotiq_driver/include/robotiq_driver/default_driver_factory.hpp b/robotiq_driver/include/robotiq_driver/default_driver_factory.hpp new file mode 100644 index 0000000..a521e0e --- /dev/null +++ b/robotiq_driver/include/robotiq_driver/default_driver_factory.hpp @@ -0,0 +1,58 @@ +// Copyright (c) 2022 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#pragma once + +#include +#include +#include +#include + +namespace robotiq_driver +{ +/** + * This class is used to create a default driver to interact with the hardware. + */ +class DefaultDriverFactory : public DriverFactory +{ +public: + DefaultDriverFactory() = default; + + /** + * @brief Create a driver. + * @param info The hardware information. + * @return A driver to interact with the hardware. + */ + std::unique_ptr create(const hardware_interface::HardwareInfo & info) const; + +protected: + // Seam for testing. + virtual std::unique_ptr create_driver( + const hardware_interface::HardwareInfo & info) const; +}; +} // namespace robotiq_driver diff --git a/robotiq_driver/include/robotiq_driver/driver.hpp b/robotiq_driver/include/robotiq_driver/driver.hpp new file mode 100644 index 0000000..8ea37bf --- /dev/null +++ b/robotiq_driver/include/robotiq_driver/driver.hpp @@ -0,0 +1,113 @@ +// Copyright (c) 2022 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#pragma once + +#include + +/** + * @brief This interface describes how to communicate with the gripper hardware. + */ +namespace robotiq_driver +{ +class Driver +{ +public: + enum class ActivationStatus + { + RESET, + ACTIVE + }; + + enum class ActionStatus + { + STOPPED, + MOVING + }; + + enum class GripperStatus + { + RESET, + IN_PROGRESS, + COMPLETED, + }; + + enum class ObjectDetectionStatus + { + MOVING, + OBJECT_DETECTED_OPENING, + OBJECT_DETECTED_CLOSING, + AT_REQUESTED_POSITION + }; + + /** + * @brief Activates the gripper. + * @throw serial::IOException on failure to successfully communicate with gripper port + */ + virtual void activate() = 0; + + /** + * @brief Deactivates the gripper. + * @throw serial::IOException on failure to successfully communicate with gripper port + */ + virtual void deactivate() = 0; + + /** + * @brief Commands the gripper to move to the desired position. + * @param pos A value between 0x00 (fully open) and 0xFF (fully closed). + */ + virtual void set_gripper_position(uint8_t pos) = 0; + + /** + * @brief Return the current position of the gripper. + * + * @throw serial::IOException on failure to successfully communicate with gripper port + * + * @return uint8_t A value between 0x00 (fully open) and 0xFF (fully closed). + */ + virtual uint8_t get_gripper_position() = 0; + + /** + * @brief Returns true if the gripper is currently moving, false otherwise. + */ + virtual bool gripper_is_moving() = 0; + + /** + * @brief Set the speed of the gripper. + * + * @param speed A value between 0x00 (stopped) and 0xFF (full speed). + */ + virtual void set_speed(uint8_t speed) = 0; + + /** + * @brief Set how forcefully the gripper opens or closes. + * @param force A value between 0x00 (no force) or 0xFF (maximum force). + */ + virtual void set_force(uint8_t force) = 0; +}; +} // namespace robotiq_driver diff --git a/robotiq_driver/include/robotiq_driver/driver_factory.hpp b/robotiq_driver/include/robotiq_driver/driver_factory.hpp new file mode 100644 index 0000000..b380dd8 --- /dev/null +++ b/robotiq_driver/include/robotiq_driver/driver_factory.hpp @@ -0,0 +1,51 @@ +// Copyright (c) 2023 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#pragma once + +#include +#include +#include + +namespace robotiq_driver +{ +/** + * The hardware interface internally uses a factory to create and configure a + * driver to interact with the Robotiq Gripper. + * A factory is used to keep the code cleaner but also to simplify testing. + * With a factory, we can test that the parameters read by the hardware + * interface are correctly parsed and passed down to the driver. + * A factory can also be mocked to return different implementation of the + * Driver (or mocks) that do not require interaction with the real hardware. + */ +class DriverFactory +{ +public: + virtual std::unique_ptr create(const hardware_interface::HardwareInfo & info) const = 0; +}; +} // namespace robotiq_driver diff --git a/robotiq_driver/include/robotiq_driver/hardware_interface.hpp b/robotiq_driver/include/robotiq_driver/hardware_interface.hpp index 06fd8e3..a36cf2f 100644 --- a/robotiq_driver/include/robotiq_driver/hardware_interface.hpp +++ b/robotiq_driver/include/robotiq_driver/hardware_interface.hpp @@ -28,21 +28,22 @@ #pragma once +#include + #include +#include +#include +#include +#include #include #include +#include +#include +#include +#include #include #include -#include "hardware_interface/handle.hpp" -#include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/system_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "rclcpp/macros.hpp" -#include "rclcpp/rclcpp.hpp" -#include "robotiq_driver/robotiq_gripper_interface.hpp" -#include "robotiq_driver/visibility_control.h" - namespace robotiq_driver { class RobotiqGripperHardwareInterface : public hardware_interface::SystemInterface @@ -50,9 +51,18 @@ class RobotiqGripperHardwareInterface : public hardware_interface::SystemInterfa public: RCLCPP_SHARED_PTR_DEFINITIONS(RobotiqGripperHardwareInterface) + /** + * Default constructor. + */ ROBOTIQ_DRIVER_PUBLIC RobotiqGripperHardwareInterface(); + /** + * Constructor with a driver factory. This method is used for testing. + * @param driver_factory The driver that interact with the hardware. + */ + explicit RobotiqGripperHardwareInterface(std::unique_ptr driver_factory); + ROBOTIQ_DRIVER_PUBLIC CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; @@ -82,10 +92,14 @@ class RobotiqGripperHardwareInterface : public hardware_interface::SystemInterfa double gripper_position_; double gripper_velocity_; double gripper_position_command_; - std::unique_ptr gripper_interface_; + + // Interface to send binary data to the hardware using the serial port. + std::unique_ptr driver_; + + // Factory to create the driver during the initialization step. + std::unique_ptr driver_factory_; double gripper_closed_pos_; - std::string com_port_; std::thread command_interface_; std::atomic command_interface_is_running_; diff --git a/robotiq_driver/src/robotiq_gripper_interface.cpp b/robotiq_driver/src/default_driver.cpp similarity index 84% rename from robotiq_driver/src/robotiq_gripper_interface.cpp rename to robotiq_driver/src/default_driver.cpp index 58b6d24..2c574e7 100644 --- a/robotiq_driver/src/robotiq_gripper_interface.cpp +++ b/robotiq_driver/src/default_driver.cpp @@ -26,7 +26,7 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. -#include "robotiq_driver/robotiq_gripper_interface.hpp" +#include "robotiq_driver/default_driver.hpp" #include #include @@ -35,6 +35,8 @@ #include "robotiq_driver/crc.hpp" +namespace robotiq_driver +{ constexpr int kBaudRate = 115200; constexpr auto kTimeoutMilliseconds = 1000; @@ -67,10 +69,10 @@ static uint8_t getFirstByte(uint16_t val) { return (val & 0xFF00) >> 8; } static uint8_t getSecondByte(uint16_t val) { return val & 0x00FF; } -RobotiqGripperInterface::RobotiqGripperInterface(const std::string & com_port, uint8_t slave_id) +DefaultDriver::DefaultDriver(const std::string & com_port, uint8_t slave_id) : port_(com_port, kBaudRate, serial::Timeout::simpleTimeout(kTimeoutMilliseconds)), slave_id_(slave_id), - read_command_(createReadCommand(kFirstOutputRegister, kNumOutputRegisters)), + read_command_(create_read_command(kFirstOutputRegister, kNumOutputRegisters)), commanded_gripper_speed_(0x80), commanded_gripper_force_(0x80) { @@ -79,18 +81,18 @@ RobotiqGripperInterface::RobotiqGripperInterface(const std::string & com_port, u } } -void RobotiqGripperInterface::activateGripper() +void DefaultDriver::activate() { - const auto cmd = createWriteCommand( + const auto cmd = create_write_command( kActionRequestRegister, {0x0100, 0x0000, 0x0000} // set rACT to 1, clear all // other registers. ); try { - sendCommand(cmd); - readResponse(kWriteResponseSize); + send_command(cmd); + read_response(kWriteResponseSize); - updateStatus(); + update_status(); if (gripper_status_ == GripperStatus::COMPLETED) { return; @@ -98,7 +100,7 @@ void RobotiqGripperInterface::activateGripper() while (gripper_status_ == GripperStatus::IN_PROGRESS) { std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - updateStatus(); + update_status(); } } catch (const serial::IOException & e) { // catch connection error and rethrow @@ -107,12 +109,12 @@ void RobotiqGripperInterface::activateGripper() } } -void RobotiqGripperInterface::deactivateGripper() +void DefaultDriver::deactivate() { - const auto cmd = createWriteCommand(kActionRequestRegister, {0x0000, 0x0000, 0x0000}); + const auto cmd = create_write_command(kActionRequestRegister, {0x0000, 0x0000, 0x0000}); try { - sendCommand(cmd); - readResponse(kWriteResponseSize); + send_command(cmd); + read_response(kWriteResponseSize); } catch (const serial::IOException & e) { // catch connection error and rethrow std::cerr << "Failed to activate gripper"; @@ -120,19 +122,19 @@ void RobotiqGripperInterface::deactivateGripper() } } -void RobotiqGripperInterface::setGripperPosition(uint8_t pos) +void DefaultDriver::set_gripper_position(uint8_t pos) { uint8_t action_register = 0x09; uint8_t gripper_options_1 = 0x00; uint8_t gripper_options_2 = 0x00; - const auto cmd = createWriteCommand( + const auto cmd = create_write_command( kActionRequestRegister, {uint16_t(action_register << 8 | gripper_options_1), uint16_t(gripper_options_2 << 8 | pos), uint16_t(commanded_gripper_speed_ << 8 | commanded_gripper_force_)}); try { - sendCommand(cmd); - readResponse(kWriteResponseSize); + send_command(cmd); + read_response(kWriteResponseSize); } catch (const serial::IOException & e) { // catch connection error and rethrow std::cerr << "Failed to set gripper position\n"; @@ -147,10 +149,10 @@ void RobotiqGripperInterface::setGripperPosition(uint8_t pos) } } -uint8_t RobotiqGripperInterface::getGripperPosition() +uint8_t DefaultDriver::get_gripper_position() { try { - updateStatus(); + update_status(); } catch (const serial::IOException & e) { // catch connection error and rethrow std::cerr << "Failed to get gripper position\n"; @@ -160,10 +162,10 @@ uint8_t RobotiqGripperInterface::getGripperPosition() return gripper_position_; } -bool RobotiqGripperInterface::gripperIsMoving() +bool DefaultDriver::gripper_is_moving() { try { - updateStatus(); + update_status(); } catch (const serial::IOException & e) { // catch connection error and rethrow std::cerr << "Failed to get gripper position\n"; @@ -173,7 +175,7 @@ bool RobotiqGripperInterface::gripperIsMoving() return object_detection_status_ == ObjectDetectionStatus::MOVING; } -std::vector RobotiqGripperInterface::createReadCommand( +std::vector DefaultDriver::create_read_command( uint16_t first_register, uint8_t num_registers) { std::vector cmd = { @@ -189,11 +191,11 @@ std::vector RobotiqGripperInterface::createReadCommand( return cmd; } -void RobotiqGripperInterface::setSpeed(uint8_t speed) { commanded_gripper_speed_ = speed; } +void DefaultDriver::set_speed(uint8_t speed) { commanded_gripper_speed_ = speed; } -void RobotiqGripperInterface::setForce(uint8_t force) { commanded_gripper_force_ = force; } +void DefaultDriver::set_force(uint8_t force) { commanded_gripper_force_ = force; } -std::vector RobotiqGripperInterface::createWriteCommand( +std::vector DefaultDriver::create_write_command( uint16_t first_register, const std::vector & data) { uint16_t num_registers = data.size(); @@ -219,7 +221,7 @@ std::vector RobotiqGripperInterface::createWriteCommand( return cmd; } -std::vector RobotiqGripperInterface::readResponse(size_t num_bytes_requested) +std::vector DefaultDriver::read_response(size_t num_bytes_requested) { std::vector response; size_t num_bytes_read = port_.read(response, num_bytes_requested); @@ -233,7 +235,7 @@ std::vector RobotiqGripperInterface::readResponse(size_t num_bytes_requ return response; } -void RobotiqGripperInterface::sendCommand(const std::vector & cmd) +void DefaultDriver::send_command(const std::vector & cmd) { size_t num_bytes_written = port_.write(cmd); port_.flush(); @@ -244,13 +246,13 @@ void RobotiqGripperInterface::sendCommand(const std::vector & cmd) } } -void RobotiqGripperInterface::updateStatus() +void DefaultDriver::update_status() { // Tell the gripper that we want to read its status. try { - sendCommand(read_command_); + send_command(read_command_); - const auto response = readResponse(kReadResponseSize); + const auto response = read_response(kReadResponseSize); // Process the response. uint8_t gripper_status_byte = response[kResponseHeaderSize + kGripperStatusIndex]; @@ -306,3 +308,4 @@ void RobotiqGripperInterface::updateStatus() throw; } } +} // namespace robotiq_driver diff --git a/robotiq_driver/src/default_driver_factory.cpp b/robotiq_driver/src/default_driver_factory.cpp new file mode 100644 index 0000000..ad82a4e --- /dev/null +++ b/robotiq_driver/src/default_driver_factory.cpp @@ -0,0 +1,62 @@ +// Copyright (c) 2022 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include +#include +#include +#include + +namespace robotiq_driver +{ + +const auto kLogger = rclcpp::get_logger("DefaultDriverFactory"); + +std::unique_ptr DefaultDriverFactory::create( + const hardware_interface::HardwareInfo & info) const +{ + double gripper_speed = stod(info.hardware_parameters.at("gripper_speed_multiplier")); + double gripper_force = stod(info.hardware_parameters.at("gripper_force_multiplier")); + + // Speed and force must lie between 0.0 and 1.0. + gripper_speed = std::min(1.0, std::max(0.0, gripper_speed)); + gripper_force = std::min(1.0, std::max(0.0, gripper_force)); + + auto driver = create_driver(info); + driver->set_speed(gripper_speed * 0xFF); + driver->set_force(gripper_force * 0xFF); + + return driver; +} + +std::unique_ptr DefaultDriverFactory::create_driver( + const hardware_interface::HardwareInfo & info) const +{ + std::string com_port = info.hardware_parameters.at("COM_port"); + return std::make_unique(com_port); +} +} // namespace robotiq_driver diff --git a/robotiq_driver/src/gripper_interface_test.cpp b/robotiq_driver/src/gripper_interface_test.cpp index c93a8de..c584ef5 100644 --- a/robotiq_driver/src/gripper_interface_test.cpp +++ b/robotiq_driver/src/gripper_interface_test.cpp @@ -27,65 +27,64 @@ // POSSIBILITY OF SUCH DAMAGE. #include +#include #include -#include "robotiq_driver/robotiq_gripper_interface.hpp" - constexpr auto kComPort = "/dev/ttyUSB0"; constexpr auto kSlaveID = 0x09; int main() { try { - RobotiqGripperInterface gripper(kComPort, kSlaveID); + robotiq_driver::DefaultDriver gripper(kComPort, kSlaveID); std::cout << "Deactivating gripper...\n"; - gripper.deactivateGripper(); + gripper.deactivate(); std::cout << "Activating gripper...\n"; - gripper.activateGripper(); + gripper.activate(); std::cout << "Gripper successfully activated.\n"; std::cout << "Closing gripper...\n"; - gripper.setGripperPosition(0xFF); - while (gripper.gripperIsMoving()) { + gripper.set_gripper_position(0xFF); + while (gripper.gripper_is_moving()) { std::this_thread::sleep_for(std::chrono::milliseconds(500)); } std::cout << "Opening gripper...\n"; - gripper.setGripperPosition(0x00); - while (gripper.gripperIsMoving()) { + gripper.set_gripper_position(0x00); + while (gripper.gripper_is_moving()) { std::this_thread::sleep_for(std::chrono::milliseconds(500)); } std::cout << "Half closing gripper...\n"; - gripper.setGripperPosition(0x80); - while (gripper.gripperIsMoving()) { + gripper.set_gripper_position(0x80); + while (gripper.gripper_is_moving()) { std::this_thread::sleep_for(std::chrono::milliseconds(500)); } std::cout << "Opening gripper...\n"; - gripper.setGripperPosition(0x00); - while (gripper.gripperIsMoving()) { + gripper.set_gripper_position(0x00); + while (gripper.gripper_is_moving()) { std::this_thread::sleep_for(std::chrono::milliseconds(500)); } std::cout << "Decreasing gripper speed...\n"; - gripper.setSpeed(0x0F); + gripper.set_speed(0x0F); std::cout << "Closing gripper...\n"; - gripper.setGripperPosition(0xFF); - while (gripper.gripperIsMoving()) { + gripper.set_gripper_position(0xFF); + while (gripper.gripper_is_moving()) { std::this_thread::sleep_for(std::chrono::milliseconds(500)); } std::cout << "Increasing gripper speed...\n"; - gripper.setSpeed(0xFF); + gripper.set_speed(0xFF); std::cout << "Opening gripper...\n"; - gripper.setGripperPosition(0x00); - while (gripper.gripperIsMoving()) { + gripper.set_gripper_position(0x00); + while (gripper.gripper_is_moving()) { std::this_thread::sleep_for(std::chrono::milliseconds(500)); } } catch (const serial::IOException & e) { diff --git a/robotiq_driver/src/hardware_interface.cpp b/robotiq_driver/src/hardware_interface.cpp index 0f9aa8b..aa99b45 100644 --- a/robotiq_driver/src/hardware_interface.cpp +++ b/robotiq_driver/src/hardware_interface.cpp @@ -26,20 +26,19 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. -#include "robotiq_driver/hardware_interface.hpp" - #include #include #include +#include +#include #include #include +#include +#include +#include #include -#include "hardware_interface/actuator_interface.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/rclcpp.hpp" - constexpr uint8_t kGripperMinPos = 3; constexpr uint8_t kGripperMaxPos = 230; constexpr uint8_t kGripperRange = kGripperMaxPos - kGripperMinPos; @@ -48,24 +47,29 @@ const auto kLogger = rclcpp::get_logger("RobotiqGripperHardwareInterface"); namespace robotiq_driver { -RobotiqGripperHardwareInterface::RobotiqGripperHardwareInterface() {} +RobotiqGripperHardwareInterface::RobotiqGripperHardwareInterface() +{ + driver_factory_ = std::make_unique(); +} + +// This constructor is use for testing only. +RobotiqGripperHardwareInterface::RobotiqGripperHardwareInterface( + std::unique_ptr driver_factory) +: driver_factory_{std::move(driver_factory)} +{ +} hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init( const hardware_interface::HardwareInfo & info) { + RCLCPP_DEBUG(kLogger, "on_init"); + if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; } // Read parameters. gripper_closed_pos_ = stod(info_.hardware_parameters["gripper_closed_position"]); - com_port_ = info_.hardware_parameters["COM_port"]; - double gripper_speed = stod(info_.hardware_parameters["gripper_speed_multiplier"]); - double gripper_force = stod(info_.hardware_parameters["gripper_force_multiplier"]); - - // Speed and force must lie between 0.0 and 1.0. - gripper_speed = std::min(1.0, std::max(0.0, gripper_speed)); - gripper_force = std::min(1.0, std::max(0.0, gripper_force)); gripper_position_ = std::numeric_limits::quiet_NaN(); gripper_velocity_ = std::numeric_limits::quiet_NaN(); @@ -110,10 +114,7 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init( } try { - // Create the interface to the gripper. - gripper_interface_ = std::make_unique(com_port_); - gripper_interface_->setSpeed(gripper_speed * 0xFF); - gripper_interface_->setForce(gripper_force * 0xFF); + driver_ = driver_factory_->create(info_); } catch (const serial::IOException & e) { RCLCPP_FATAL(kLogger, "Failed to open gripper port."); return CallbackReturn::ERROR; @@ -125,6 +126,8 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init( std::vector RobotiqGripperHardwareInterface::export_state_interfaces() { + RCLCPP_DEBUG(kLogger, "export_state_interfaces"); + std::vector state_interfaces; state_interfaces.emplace_back(hardware_interface::StateInterface( @@ -138,6 +141,8 @@ RobotiqGripperHardwareInterface::export_state_interfaces() std::vector RobotiqGripperHardwareInterface::export_command_interfaces() { + RCLCPP_DEBUG(kLogger, "export_command_interfaces"); + std::vector command_interfaces; command_interfaces.emplace_back(hardware_interface::CommandInterface( @@ -153,6 +158,8 @@ RobotiqGripperHardwareInterface::export_command_interfaces() hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { + RCLCPP_DEBUG(kLogger, "on_activate"); + // set some default values for joints if (std::isnan(gripper_position_)) { gripper_position_ = 0; @@ -162,8 +169,8 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_activate( // Activate the gripper. try { - gripper_interface_->deactivateGripper(); - gripper_interface_->activateGripper(); + driver_->deactivate(); + driver_->activate(); } catch (const serial::IOException & e) { RCLCPP_FATAL(kLogger, "Failed to communicate with Gripper. Check Gripper connection."); return CallbackReturn::ERROR; @@ -185,17 +192,17 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_activate( // Re-activate the gripper // (this can be used, for example, to re-run the auto-calibration). if (reactivate_gripper_async_cmd_.load()) { - this->gripper_interface_->deactivateGripper(); - this->gripper_interface_->activateGripper(); + this->driver_->deactivate(); + this->driver_->activate(); reactivate_gripper_async_cmd_.store(false); reactivate_gripper_async_response_.store(true); } // Write the latest command to the gripper. - this->gripper_interface_->setGripperPosition(write_command_.load()); + this->driver_->set_gripper_position(write_command_.load()); // Read the state of the gripper. - gripper_current_state_.store(this->gripper_interface_->getGripperPosition()); + gripper_current_state_.store(this->driver_->get_gripper_position()); last_io = now; } catch (serial::IOException & e) { @@ -213,11 +220,13 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_activate( hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { + RCLCPP_DEBUG(kLogger, "on_deactivate"); + command_interface_is_running_.store(false); command_interface_.join(); try { - gripper_interface_->deactivateGripper(); + driver_->deactivate(); } catch (const std::exception & e) { RCLCPP_ERROR(kLogger, "Failed to deactivate gripper. Check Gripper connection"); return CallbackReturn::ERROR; From d32b44e3c2cc3c4ff5b9cda83981bdc6289b1bf2 Mon Sep 17 00:00:00 2001 From: Giovanni Remigi Date: Tue, 5 Sep 2023 13:32:51 +0200 Subject: [PATCH 02/47] Added simple test. --- robotiq_driver/CMakeLists.txt | 29 +++---- robotiq_driver/tests/CMakeLists.txt | 24 ++++++ ...est_robotiq_gripper_hardware_interface.cpp | 81 +++++++++++++++++++ 3 files changed, 117 insertions(+), 17 deletions(-) create mode 100644 robotiq_driver/tests/CMakeLists.txt create mode 100644 robotiq_driver/tests/test_robotiq_gripper_hardware_interface.cpp diff --git a/robotiq_driver/CMakeLists.txt b/robotiq_driver/CMakeLists.txt index 5944589..914011e 100644 --- a/robotiq_driver/CMakeLists.txt +++ b/robotiq_driver/CMakeLists.txt @@ -50,7 +50,9 @@ target_include_directories(gripper_interface_test PRIVATE include) ament_target_dependencies(gripper_interface_test serial) target_link_libraries(gripper_interface_test ${PROJECT_NAME}) +############################################################################### # INSTALL + install( TARGETS ${PROJECT_NAME} EXPORT export_${PROJECT_NAME} @@ -75,26 +77,19 @@ install( DESTINATION share/${PROJECT_NAME} ) +############################################################################### +# TESTS + +# CTest module automatically creates a BUILD_TESTING option that selects +# whether to enable testing support (ON by default). +include(CTest) if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - # the following skips uncrustify - # ament_uncrustify and ament_clang_format cannot both be satisfied - set(ament_cmake_uncrustify_FOUND TRUE) - # the following skips ament_flake8 - # ament_flake8 and black fight over double or single quotes - # flake8 is run via pre-commit with a .flake8 configuration file - set(ament_cmake_flake8_FOUND TRUE) - ament_lint_auto_find_test_dependencies() + add_subdirectory(tests) endif() -## EXPORTS +############################################################################### +# EXPORTS + ament_export_include_directories( include ) diff --git a/robotiq_driver/tests/CMakeLists.txt b/robotiq_driver/tests/CMakeLists.txt new file mode 100644 index 0000000..f8a1865 --- /dev/null +++ b/robotiq_driver/tests/CMakeLists.txt @@ -0,0 +1,24 @@ +# Add support for GTest. +find_package(ament_cmake_gtest REQUIRED) +find_package(ament_cmake_gmock REQUIRED) +find_package(ament_lint_auto REQUIRED) +find_package(ros2_control_test_assets REQUIRED) + +ament_lint_auto_find_test_dependencies() + +# GMock throws an error if we don't switch off this option in tests. +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wno-sign-conversion) +endif() + +############################################################################### +# test_robotiq_gripper_hardware_interface + +ament_add_gmock(test_robotiq_gripper_hardware_interface + test_robotiq_gripper_hardware_interface.cpp +) +target_include_directories(test_robotiq_gripper_hardware_interface + PRIVATE ${CMAKE_CURRENT_SOURCE_DIR} +) +target_link_libraries(test_robotiq_gripper_hardware_interface robotiq_driver) + diff --git a/robotiq_driver/tests/test_robotiq_gripper_hardware_interface.cpp b/robotiq_driver/tests/test_robotiq_gripper_hardware_interface.cpp new file mode 100644 index 0000000..2cea65d --- /dev/null +++ b/robotiq_driver/tests/test_robotiq_gripper_hardware_interface.cpp @@ -0,0 +1,81 @@ +// Copyright (c) 2023 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace robotiq_driver::test +{ + +/** + * This test generates a minimal xacro robot configuration and loads the + * hardware interface plugin. + */ +TEST(TestRobotiqGripperHardwareInterface, load_urdf) +{ + std::string urdf_control_ = + R"( + + + robotiq_driver/RobotiqGripperHardwareInterface + 1.0 + 0.5 + /dev/ttyUSB0 + 0.7929 + + + + + 0.7929 + + + + + )"; + + auto urdf = + ros2_control_test_assets::urdf_head + urdf_control_ + ros2_control_test_assets::urdf_tail; + hardware_interface::ResourceManager rm(urdf); + + // Check interfaces + EXPECT_EQ(1u, rm.system_components_size()); +} + +} // namespace robotiq_driver::test From 1a8aeebd477a17f6a7f23b4076082602ec42fcbf Mon Sep 17 00:00:00 2001 From: Giovanni Remigi Date: Tue, 5 Sep 2023 13:38:47 +0200 Subject: [PATCH 03/47] Pre-commits. --- robotiq_driver/tests/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/robotiq_driver/tests/CMakeLists.txt b/robotiq_driver/tests/CMakeLists.txt index f8a1865..e2181dc 100644 --- a/robotiq_driver/tests/CMakeLists.txt +++ b/robotiq_driver/tests/CMakeLists.txt @@ -21,4 +21,3 @@ target_include_directories(test_robotiq_gripper_hardware_interface PRIVATE ${CMAKE_CURRENT_SOURCE_DIR} ) target_link_libraries(test_robotiq_gripper_hardware_interface robotiq_driver) - From ddda5029a28ed170f2313ad307b76570a012a2b4 Mon Sep 17 00:00:00 2001 From: Giovanni Remigi Date: Tue, 5 Sep 2023 14:46:45 +0200 Subject: [PATCH 04/47] Added missing packages for testing. --- robotiq_driver/package.xml | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/robotiq_driver/package.xml b/robotiq_driver/package.xml index 5004d57..97f6e7b 100644 --- a/robotiq_driver/package.xml +++ b/robotiq_driver/package.xml @@ -20,9 +20,23 @@ gripper_controllers joint_state_broadcaster + + ament_clang_format + + ament_clang_tidy + + ament_cmake_copyright + + ament_cmake_lint_cmake + ament_lint_auto + ament_lint_common + + ament_cmake_gmock + ros2_control_test_assets + ament_cmake From 79dfec49f5e3008e9e362818542008310024efe8 Mon Sep 17 00:00:00 2001 From: Giovanni Remigi Date: Tue, 5 Sep 2023 15:02:18 +0200 Subject: [PATCH 05/47] In the on_configure method we try to connect to the hardware. --- .../include/robotiq_driver/default_driver.hpp | 3 + .../include/robotiq_driver/driver.hpp | 6 ++ .../robotiq_driver/hardware_interface.hpp | 56 ++++++++++++++++--- robotiq_driver/src/default_driver.cpp | 8 +++ robotiq_driver/src/hardware_interface.cpp | 36 ++++++++++-- 5 files changed, 95 insertions(+), 14 deletions(-) diff --git a/robotiq_driver/include/robotiq_driver/default_driver.hpp b/robotiq_driver/include/robotiq_driver/default_driver.hpp index 5cf4c94..c506a34 100644 --- a/robotiq_driver/include/robotiq_driver/default_driver.hpp +++ b/robotiq_driver/include/robotiq_driver/default_driver.hpp @@ -46,6 +46,9 @@ class DefaultDriver : public Driver public: explicit DefaultDriver(const std::string & com_port = "/dev/ttyUSB0", uint8_t slave_id = 0x09); + bool connect() override; + void disconnect() override; + /** * @brief Activates the gripper. * diff --git a/robotiq_driver/include/robotiq_driver/driver.hpp b/robotiq_driver/include/robotiq_driver/driver.hpp index 8ea37bf..7e66525 100644 --- a/robotiq_driver/include/robotiq_driver/driver.hpp +++ b/robotiq_driver/include/robotiq_driver/driver.hpp @@ -65,6 +65,12 @@ class Driver AT_REQUESTED_POSITION }; + /** Connect to the gripper serial connection. */ + virtual bool connect() = 0; + + /** Disconnect from the gripper serial connection. */ + virtual void disconnect() = 0; + /** * @brief Activates the gripper. * @throw serial::IOException on failure to successfully communicate with gripper port diff --git a/robotiq_driver/include/robotiq_driver/hardware_interface.hpp b/robotiq_driver/include/robotiq_driver/hardware_interface.hpp index a36cf2f..5eadd20 100644 --- a/robotiq_driver/include/robotiq_driver/hardware_interface.hpp +++ b/robotiq_driver/include/robotiq_driver/hardware_interface.hpp @@ -63,46 +63,86 @@ class RobotiqGripperHardwareInterface : public hardware_interface::SystemInterfa */ explicit RobotiqGripperHardwareInterface(std::unique_ptr driver_factory); + /** + * Initialization of the hardware interface from data parsed from the + * robot's URDF. + * @param hardware_info Structure with data from URDF. + * @returns CallbackReturn::SUCCESS if required data are provided and can be + * parsed or CallbackReturn::ERROR if any error happens or data are missing. + */ ROBOTIQ_DRIVER_PUBLIC CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; + /** + * Connect to the hardware. + * @param previous_state The previous state. + * @returns CallbackReturn::SUCCESS if required data are provided and can be + * parsed or CallbackReturn::ERROR if any error happens or data are missing. + */ + ROBOTIQ_DRIVER_PUBLIC + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + + /** + * This method exposes position and velocity of joints for reading. + */ ROBOTIQ_DRIVER_PUBLIC std::vector export_state_interfaces() override; + /** + * This method exposes the joints targets for writing. + */ ROBOTIQ_DRIVER_PUBLIC std::vector export_command_interfaces() override; + /** + * This method is invoked when the hardware is connected. + * @param previous_state Unconfigured, Inactive, Active or Finalized. + * @returns CallbackReturn::SUCCESS or CallbackReturn::ERROR. + */ ROBOTIQ_DRIVER_PUBLIC CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + /** + * This method is invoked when the hardware is disconnected. + * @param previous_state Unconfigured, Inactive, Active or Finalized. + * @returns CallbackReturn::SUCCESS or CallbackReturn::ERROR. + */ ROBOTIQ_DRIVER_PUBLIC CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + /** + * Read data from the hardware. + */ ROBOTIQ_DRIVER_PUBLIC hardware_interface::return_type read( const rclcpp::Time & time, const rclcpp::Duration & period) override; + /** + * Write data to hardware. + */ ROBOTIQ_DRIVER_PUBLIC hardware_interface::return_type write( const rclcpp::Time & time, const rclcpp::Duration & period) override; private: - static constexpr double NO_NEW_CMD_ = std::numeric_limits::quiet_NaN(); - - double gripper_position_; - double gripper_velocity_; - double gripper_position_command_; - // Interface to send binary data to the hardware using the serial port. std::unique_ptr driver_; // Factory to create the driver during the initialization step. std::unique_ptr driver_factory_; + // We use a thread to read/write to the driver so that we dont block the hardware_interface read/write. + std::thread communication_thread_; + std::atomic communication_thread_is_running_; + double gripper_closed_pos_; - std::thread command_interface_; - std::atomic command_interface_is_running_; + static constexpr double NO_NEW_CMD_ = std::numeric_limits::quiet_NaN(); + + double gripper_position_; + double gripper_velocity_; + double gripper_position_command_; + std::atomic write_command_; std::atomic gripper_current_state_; diff --git a/robotiq_driver/src/default_driver.cpp b/robotiq_driver/src/default_driver.cpp index 2c574e7..090091d 100644 --- a/robotiq_driver/src/default_driver.cpp +++ b/robotiq_driver/src/default_driver.cpp @@ -81,6 +81,14 @@ DefaultDriver::DefaultDriver(const std::string & com_port, uint8_t slave_id) } } +bool DefaultDriver::connect() +{ + port_.open(); + return port_.isOpen(); +} + +void DefaultDriver::disconnect() { port_.close(); } + void DefaultDriver::activate() { const auto cmd = create_write_command( diff --git a/robotiq_driver/src/hardware_interface.cpp b/robotiq_driver/src/hardware_interface.cpp index aa99b45..e29e3ef 100644 --- a/robotiq_driver/src/hardware_interface.cpp +++ b/robotiq_driver/src/hardware_interface.cpp @@ -123,6 +123,30 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init( return CallbackReturn::SUCCESS; } +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +RobotiqGripperHardwareInterface::on_configure(const rclcpp_lifecycle::State & previous_state) +{ + RCLCPP_DEBUG(kLogger, "on_configure"); + try { + if ( + hardware_interface::SystemInterface::on_configure(previous_state) != + CallbackReturn::SUCCESS) { + return CallbackReturn::ERROR; + } + + // Open the serial port and handshake. + bool connected = driver_->connect(); + if (!connected) { + RCLCPP_ERROR(kLogger, "Cannot connect to the Robotiq gripper"); + return CallbackReturn::ERROR; + } + } catch (const std::exception & e) { + RCLCPP_ERROR(kLogger, "Cannot configure the Robotiq gripper: %s", e.what()); + return CallbackReturn::ERROR; + } + return CallbackReturn::SUCCESS; +} + std::vector RobotiqGripperHardwareInterface::export_state_interfaces() { @@ -178,14 +202,14 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_activate( RCLCPP_INFO(kLogger, "Robotiq Gripper successfully activated!"); - command_interface_is_running_.store(true); + communication_thread_is_running_.store(true); - command_interface_ = std::thread([this] { + communication_thread_ = std::thread([this] { // Read from and write to the gripper at 100 Hz. const auto io_interval = std::chrono::milliseconds(10); auto last_io = std::chrono::high_resolution_clock::now(); - while (command_interface_is_running_.load()) { + while (communication_thread_is_running_.load()) { const auto now = std::chrono::high_resolution_clock::now(); if (now - last_io > io_interval) { try { @@ -208,7 +232,7 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_activate( } catch (serial::IOException & e) { RCLCPP_ERROR( kLogger, "Check Robotiq Gripper connection and restart drivers. ERROR: %s", e.what()); - command_interface_is_running_.store(false); + communication_thread_is_running_.store(false); } } } @@ -222,8 +246,8 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_deactivat { RCLCPP_DEBUG(kLogger, "on_deactivate"); - command_interface_is_running_.store(false); - command_interface_.join(); + communication_thread_is_running_.store(false); + communication_thread_.join(); try { driver_->deactivate(); From f37146889b191c0b8f215a51c26343b9fe6de26a Mon Sep 17 00:00:00 2001 From: Giovanni Remigi Date: Tue, 5 Sep 2023 15:57:58 +0200 Subject: [PATCH 06/47] Added missing package. --- robotiq_driver/package.xml | 10 +--------- 1 file changed, 1 insertion(+), 9 deletions(-) diff --git a/robotiq_driver/package.xml b/robotiq_driver/package.xml index 97f6e7b..4c98252 100644 --- a/robotiq_driver/package.xml +++ b/robotiq_driver/package.xml @@ -20,20 +20,12 @@ gripper_controllers joint_state_broadcaster - - ament_clang_format - - ament_clang_tidy - - ament_cmake_copyright - - ament_cmake_lint_cmake - ament_lint_auto ament_lint_common + ament_cmake_gtest ament_cmake_gmock ros2_control_test_assets From c11d6572cd92bec34fb931dd0313b79bf7890766 Mon Sep 17 00:00:00 2001 From: Giovanni Remigi Date: Tue, 5 Sep 2023 16:05:21 +0200 Subject: [PATCH 07/47] Refactoring. --- robotiq_driver/CMakeLists.txt | 51 ++++++++++++++++++++++------------- 1 file changed, 33 insertions(+), 18 deletions(-) diff --git a/robotiq_driver/CMakeLists.txt b/robotiq_driver/CMakeLists.txt index 914011e..11fd7f3 100644 --- a/robotiq_driver/CMakeLists.txt +++ b/robotiq_driver/CMakeLists.txt @@ -5,8 +5,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() - -# find dependencies find_package(ament_cmake REQUIRED) find_package(hardware_interface REQUIRED) find_package(pluginlib REQUIRED) @@ -43,13 +41,46 @@ ament_target_dependencies( ${THIS_PACKAGE_INCLUDE_DEPENDS} ) +############################################################################### +# PLUGINS + pluginlib_export_plugin_description_file(hardware_interface hardware_interface_plugin.xml) +############################################################################### +# ADDITIONAL OPTIONAL EXECUTABLES + add_executable(gripper_interface_test src/gripper_interface_test.cpp) target_include_directories(gripper_interface_test PRIVATE include) ament_target_dependencies(gripper_interface_test serial) target_link_libraries(gripper_interface_test ${PROJECT_NAME}) +############################################################################### +# EXPORTS + +# This is necessary to allow this library’s clients to use the syntax +# target_link_libraries(client ::) +# Without this a client cannot find this library. +# It can take an arbitrary list of targets named as EXPORT in an install call. +ament_export_targets( + export_${PROJECT_NAME} +) +# Help downstream packages to find transitive dependencies i.e. export all +# dependencies required by a package to use this library. +# When a package calls find_package(epick_driver), CMake looks for a file +# called epick_driverConfig.cmake which sets up everything another project +# would need to depend on this one. +ament_export_dependencies( + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) +# Tell downstream packages where to find our headers. +ament_export_include_directories( + include +) +# Tell downstream packages our libraries to link against. +ament_export_libraries( + ${PROJECT_NAME} +) + ############################################################################### # INSTALL @@ -87,20 +118,4 @@ if(BUILD_TESTING) add_subdirectory(tests) endif() -############################################################################### -# EXPORTS - -ament_export_include_directories( - include -) -ament_export_libraries( - ${PROJECT_NAME} -) -ament_export_targets( - export_${PROJECT_NAME} -) -ament_export_dependencies( - ${THIS_PACKAGE_INCLUDE_DEPENDS} -) - ament_package() From 76c54979e9a28432098e3bd173b5e4215aa470db Mon Sep 17 00:00:00 2001 From: Giovanni Remigi Date: Tue, 5 Sep 2023 16:17:23 +0200 Subject: [PATCH 08/47] Removed not existing package. --- .github/workflows/ci-ros-lint.yml | 46 ++++++++++++++----------------- 1 file changed, 21 insertions(+), 25 deletions(-) diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index be06560..e2eb79b 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -9,19 +9,17 @@ jobs: strategy: fail-fast: false matrix: - linter: [cppcheck, copyright, lint_cmake] + linter: [cppcheck, copyright, lint_cmake] steps: - - uses: actions/checkout@v3 - - uses: ros-tooling/setup-ros@0.6.2 - - uses: ros-tooling/action-ros-lint@v0.1 - with: - distribution: rolling - linter: ${{ matrix.linter }} - package-name: - robotiq_driver - robotiq_controllers - robotiq_description - + - uses: actions/checkout@v3 + - uses: ros-tooling/setup-ros@0.6.2 + - uses: ros-tooling/action-ros-lint@v0.1 + with: + distribution: rolling + linter: ${{ matrix.linter }} + package-name: robotiq_driver + robotiq_controllers + robotiq_description ament_lint_100: name: ament_${{ matrix.linter }} @@ -29,17 +27,15 @@ jobs: strategy: fail-fast: false matrix: - linter: [cpplint] + linter: [cpplint] steps: - - uses: actions/checkout@v3 - - uses: ros-tooling/setup-ros@0.6.2 - - uses: ros-tooling/action-ros-lint@v0.1 - with: - distribution: rolling - linter: cpplint - arguments: "--linelength=100 --filter=-whitespace/newline" - package-name: - robotiq_driver - robotiq_controllers - robotiq_description - ros2_robotiq_gripper + - uses: actions/checkout@v3 + - uses: ros-tooling/setup-ros@0.6.2 + - uses: ros-tooling/action-ros-lint@v0.1 + with: + distribution: rolling + linter: cpplint + arguments: "--linelength=100 --filter=-whitespace/newline" + package-name: robotiq_driver + robotiq_controllers + robotiq_description From 878859365914bdb28a5efe0455746b21e6c3f5e8 Mon Sep 17 00:00:00 2001 From: Giovanni Remigi Date: Tue, 5 Sep 2023 16:53:49 +0200 Subject: [PATCH 09/47] Removed ros-linter. Reformatted code as per Studio conventions. --- .clang-format | 76 ++++++-- .github/workflows/ci-ros-lint.yml | 2 +- .../robotiq_activation_controller.hpp | 12 +- .../src/robotiq_activation_controller.cpp | 58 +++--- robotiq_driver/include/robotiq_driver/crc.hpp | 2 +- .../include/robotiq_driver/default_driver.hpp | 11 +- .../robotiq_driver/default_driver_factory.hpp | 11 +- .../include/robotiq_driver/driver_factory.hpp | 2 +- .../robotiq_driver/hardware_interface.hpp | 27 +-- robotiq_driver/package.xml | 1 + robotiq_driver/src/crc.cpp | 7 +- robotiq_driver/src/default_driver.cpp | 174 ++++++++++------- robotiq_driver/src/default_driver_factory.cpp | 14 +- robotiq_driver/src/gripper_interface_test.cpp | 28 ++- robotiq_driver/src/hardware_interface.cpp | 176 ++++++++++-------- ...est_robotiq_gripper_hardware_interface.cpp | 15 +- 16 files changed, 370 insertions(+), 246 deletions(-) diff --git a/.clang-format b/.clang-format index 4a79087..3437820 100644 --- a/.clang-format +++ b/.clang-format @@ -1,20 +1,74 @@ --- -Language: Cpp BasedOnStyle: Google +ColumnLimit: 120 +MaxEmptyLinesToKeep: 1 +SortIncludes: false +Standard: Auto +IndentWidth: 2 +TabWidth: 2 +UseTab: Never AccessModifierOffset: -2 -AlignAfterOpenBracket: AlwaysBreak +ConstructorInitializerIndentWidth: 2 +NamespaceIndentation: None +ContinuationIndentWidth: 4 +IndentCaseLabels: true +IndentFunctionDeclarationAfterType: false + +AlignEscapedNewlinesLeft: false +AlignTrailingComments: true + +AllowAllParametersOfDeclarationOnNextLine: false +ExperimentalAutoDetectBinPacking: false +ObjCSpaceBeforeProtocolList: true +Cpp11BracedListStyle: false + +AllowShortBlocksOnASingleLine: true +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AllowShortFunctionsOnASingleLine: None +AllowShortCaseLabelsOnASingleLine: false + +AlwaysBreakTemplateDeclarations: true +AlwaysBreakBeforeMultilineStrings: false +BreakBeforeBinaryOperators: false +BreakBeforeTernaryOperators: false +BreakConstructorInitializersBeforeComma: true + +BinPackParameters: true +ConstructorInitializerAllOnOneLineOrOnePerLine: true +DerivePointerBinding: false +PointerBindsToType: true + +PenaltyExcessCharacter: 50 +PenaltyBreakBeforeFirstCallParameter: 30 +PenaltyBreakComment: 1000 +PenaltyBreakFirstLessLess: 10 +PenaltyBreakString: 100 +PenaltyReturnTypeOnItsOwnLine: 50 + +SpacesBeforeTrailingComments: 2 +SpacesInParentheses: false +SpacesInAngles: false +SpaceInEmptyParentheses: false +SpacesInCStyleCastParentheses: false +SpaceAfterCStyleCast: false +SpaceAfterControlStatementKeyword: true +SpaceBeforeAssignmentOperators: true + +# Configure each individual brace in BraceWrapping +BreakBeforeBraces: Custom + +# Control of individual brace wrapping cases BraceWrapping: + AfterCaseLabel: true AfterClass: true + AfterControlStatement: true + AfterEnum: true AfterFunction: true AfterNamespace: true AfterStruct: true - AfterEnum: true -BreakBeforeBraces: Custom -ColumnLimit: 100 -ConstructorInitializerIndentWidth: 0 -ContinuationIndentWidth: 2 -DerivePointerAlignment: false -PointerAlignment: Middle -ReflowComments: false -... + AfterUnion: true + BeforeCatch: true + BeforeElse: true + IndentBraces: false diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index e2eb79b..cfc2a96 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -35,7 +35,7 @@ jobs: with: distribution: rolling linter: cpplint - arguments: "--linelength=100 --filter=-whitespace/newline" + arguments: "--linelength=120 --filter=-whitespace/newline" package-name: robotiq_driver robotiq_controllers robotiq_description diff --git a/robotiq_controllers/include/robotiq_controllers/robotiq_activation_controller.hpp b/robotiq_controllers/include/robotiq_controllers/robotiq_activation_controller.hpp index f922400..d0dac29 100644 --- a/robotiq_controllers/include/robotiq_controllers/robotiq_activation_controller.hpp +++ b/robotiq_controllers/include/robotiq_controllers/robotiq_activation_controller.hpp @@ -40,19 +40,17 @@ class RobotiqActivationController : public controller_interface::ControllerInter controller_interface::InterfaceConfiguration state_interface_configuration() const override; - controller_interface::return_type update( - const rclcpp::Time & time, const rclcpp::Duration & period) override; + controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override; - CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; - CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; CallbackReturn on_init() override; private: - bool reactivateGripper( - std_srvs::srv::Trigger::Request::SharedPtr req, - std_srvs::srv::Trigger::Response::SharedPtr resp); + bool reactivateGripper(std_srvs::srv::Trigger::Request::SharedPtr req, + std_srvs::srv::Trigger::Response::SharedPtr resp); static constexpr double ASYNC_WAITING = 2.0; enum CommandInterfaces diff --git a/robotiq_controllers/src/robotiq_activation_controller.cpp b/robotiq_controllers/src/robotiq_activation_controller.cpp index 03c4fa8..aa63d21 100644 --- a/robotiq_controllers/src/robotiq_activation_controller.cpp +++ b/robotiq_controllers/src/robotiq_activation_controller.cpp @@ -30,8 +30,7 @@ namespace robotiq_controllers { -controller_interface::InterfaceConfiguration -RobotiqActivationController::command_interface_configuration() const +controller_interface::InterfaceConfiguration RobotiqActivationController::command_interface_configuration() const { controller_interface::InterfaceConfiguration config; config.type = controller_interface::interface_configuration_type::INDIVIDUAL; @@ -42,8 +41,7 @@ RobotiqActivationController::command_interface_configuration() const return config; } -controller_interface::InterfaceConfiguration -RobotiqActivationController::state_interface_configuration() const +controller_interface::InterfaceConfiguration RobotiqActivationController::state_interface_configuration() const { controller_interface::InterfaceConfiguration config; config.type = controller_interface::interface_configuration_type::INDIVIDUAL; @@ -51,62 +49,67 @@ RobotiqActivationController::state_interface_configuration() const return config; } -controller_interface::return_type RobotiqActivationController::update( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +controller_interface::return_type RobotiqActivationController::update(const rclcpp::Time& /*time*/, + const rclcpp::Duration& /*period*/) { return controller_interface::return_type::OK; } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -RobotiqActivationController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) +RobotiqActivationController::on_activate(const rclcpp_lifecycle::State& /*previous_state*/) { // Check command interfaces. - if (command_interfaces_.size() != 2) { - RCLCPP_ERROR( - get_node()->get_logger(), "Expected %d command interfaces, but got %zu.", 2, - command_interfaces_.size()); + if (command_interfaces_.size() != 2) + { + RCLCPP_ERROR(get_node()->get_logger(), "Expected %d command interfaces, but got %zu.", 2, + command_interfaces_.size()); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } - try { + try + { // Create service for re-activating the gripper. reactivate_gripper_srv_ = get_node()->create_service( - "~/reactivate_gripper", - [this]( - std_srvs::srv::Trigger::Request::SharedPtr req, - std_srvs::srv::Trigger::Response::SharedPtr resp) { this->reactivateGripper(req, resp); }); - } catch (...) { + "~/reactivate_gripper", + [this](std_srvs::srv::Trigger::Request::SharedPtr req, std_srvs::srv::Trigger::Response::SharedPtr resp) { + this->reactivateGripper(req, resp); + }); + } + catch (...) + { return LifecycleNodeInterface::CallbackReturn::ERROR; } return LifecycleNodeInterface::CallbackReturn::SUCCESS; } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -RobotiqActivationController::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) +RobotiqActivationController::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/) { - try { + try + { reactivate_gripper_srv_.reset(); - } catch (...) { + } + catch (...) + { return LifecycleNodeInterface::CallbackReturn::ERROR; } return LifecycleNodeInterface::CallbackReturn::SUCCESS; } -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -RobotiqActivationController::on_init() +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotiqActivationController::on_init() { return LifecycleNodeInterface::CallbackReturn::SUCCESS; } -bool RobotiqActivationController::reactivateGripper( - std_srvs::srv::Trigger::Request::SharedPtr /*req*/, - std_srvs::srv::Trigger::Response::SharedPtr resp) +bool RobotiqActivationController::reactivateGripper(std_srvs::srv::Trigger::Request::SharedPtr /*req*/, + std_srvs::srv::Trigger::Response::SharedPtr resp) { command_interfaces_[REACTIVATE_GRIPPER_RESPONSE].set_value(ASYNC_WAITING); command_interfaces_[REACTIVATE_GRIPPER_CMD].set_value(1.0); - while (command_interfaces_[REACTIVATE_GRIPPER_RESPONSE].get_value() == ASYNC_WAITING) { + while (command_interfaces_[REACTIVATE_GRIPPER_RESPONSE].get_value() == ASYNC_WAITING) + { std::this_thread::sleep_for(std::chrono::milliseconds(50)); } resp->success = command_interfaces_[REACTIVATE_GRIPPER_RESPONSE].get_value(); @@ -117,5 +120,4 @@ bool RobotiqActivationController::reactivateGripper( #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS( - robotiq_controllers::RobotiqActivationController, controller_interface::ControllerInterface) +PLUGINLIB_EXPORT_CLASS(robotiq_controllers::RobotiqActivationController, controller_interface::ControllerInterface) diff --git a/robotiq_driver/include/robotiq_driver/crc.hpp b/robotiq_driver/include/robotiq_driver/crc.hpp index ca19d21..a9aa70d 100644 --- a/robotiq_driver/include/robotiq_driver/crc.hpp +++ b/robotiq_driver/include/robotiq_driver/crc.hpp @@ -31,4 +31,4 @@ #include #include -uint16_t computeCRC(const std::vector & cmd); +uint16_t computeCRC(const std::vector& cmd); diff --git a/robotiq_driver/include/robotiq_driver/default_driver.hpp b/robotiq_driver/include/robotiq_driver/default_driver.hpp index c506a34..5ba4479 100644 --- a/robotiq_driver/include/robotiq_driver/default_driver.hpp +++ b/robotiq_driver/include/robotiq_driver/default_driver.hpp @@ -28,12 +28,10 @@ #pragma once -#include - #include +#include #include #include - /** * @brief This class is responsible for communicating with the gripper via a serial port, and maintaining a record of * the gripper's current state. @@ -44,7 +42,7 @@ namespace robotiq_driver class DefaultDriver : public Driver { public: - explicit DefaultDriver(const std::string & com_port = "/dev/ttyUSB0", uint8_t slave_id = 0x09); + explicit DefaultDriver(const std::string& com_port = "/dev/ttyUSB0", uint8_t slave_id = 0x09); bool connect() override; void disconnect() override; @@ -101,8 +99,7 @@ class DefaultDriver : public Driver private: std::vector create_read_command(uint16_t first_register, uint8_t num_registers); - std::vector create_write_command( - uint16_t first_register, const std::vector & data); + std::vector create_write_command(uint16_t first_register, const std::vector& data); /** * @brief read response from the gripper. @@ -118,7 +115,7 @@ class DefaultDriver : public Driver * @param cmd The command. * @throw serial::IOException on failure to successfully communicate with gripper port */ - void send_command(const std::vector & cmd); + void send_command(const std::vector& cmd); /** * @brief Read the current status of the gripper, and update member variables as appropriate. diff --git a/robotiq_driver/include/robotiq_driver/default_driver_factory.hpp b/robotiq_driver/include/robotiq_driver/default_driver_factory.hpp index a521e0e..e267b0b 100644 --- a/robotiq_driver/include/robotiq_driver/default_driver_factory.hpp +++ b/robotiq_driver/include/robotiq_driver/default_driver_factory.hpp @@ -28,11 +28,13 @@ #pragma once -#include -#include #include #include +#include + +#include + namespace robotiq_driver { /** @@ -48,11 +50,10 @@ class DefaultDriverFactory : public DriverFactory * @param info The hardware information. * @return A driver to interact with the hardware. */ - std::unique_ptr create(const hardware_interface::HardwareInfo & info) const; + std::unique_ptr create(const hardware_interface::HardwareInfo& info) const; protected: // Seam for testing. - virtual std::unique_ptr create_driver( - const hardware_interface::HardwareInfo & info) const; + virtual std::unique_ptr create_driver(const hardware_interface::HardwareInfo& info) const; }; } // namespace robotiq_driver diff --git a/robotiq_driver/include/robotiq_driver/driver_factory.hpp b/robotiq_driver/include/robotiq_driver/driver_factory.hpp index b380dd8..6584e4c 100644 --- a/robotiq_driver/include/robotiq_driver/driver_factory.hpp +++ b/robotiq_driver/include/robotiq_driver/driver_factory.hpp @@ -46,6 +46,6 @@ namespace robotiq_driver class DriverFactory { public: - virtual std::unique_ptr create(const hardware_interface::HardwareInfo & info) const = 0; + virtual std::unique_ptr create(const hardware_interface::HardwareInfo& info) const = 0; }; } // namespace robotiq_driver diff --git a/robotiq_driver/include/robotiq_driver/hardware_interface.hpp b/robotiq_driver/include/robotiq_driver/hardware_interface.hpp index 5eadd20..ab4e220 100644 --- a/robotiq_driver/include/robotiq_driver/hardware_interface.hpp +++ b/robotiq_driver/include/robotiq_driver/hardware_interface.hpp @@ -30,17 +30,20 @@ #include -#include +#include +#include + #include #include #include #include -#include -#include + #include #include -#include -#include + +#include +#include +#include #include #include @@ -71,7 +74,7 @@ class RobotiqGripperHardwareInterface : public hardware_interface::SystemInterfa * parsed or CallbackReturn::ERROR if any error happens or data are missing. */ ROBOTIQ_DRIVER_PUBLIC - CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; + CallbackReturn on_init(const hardware_interface::HardwareInfo& info) override; /** * Connect to the hardware. @@ -80,7 +83,7 @@ class RobotiqGripperHardwareInterface : public hardware_interface::SystemInterfa * parsed or CallbackReturn::ERROR if any error happens or data are missing. */ ROBOTIQ_DRIVER_PUBLIC - CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override; /** * This method exposes position and velocity of joints for reading. @@ -100,7 +103,7 @@ class RobotiqGripperHardwareInterface : public hardware_interface::SystemInterfa * @returns CallbackReturn::SUCCESS or CallbackReturn::ERROR. */ ROBOTIQ_DRIVER_PUBLIC - CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; /** * This method is invoked when the hardware is disconnected. @@ -108,21 +111,19 @@ class RobotiqGripperHardwareInterface : public hardware_interface::SystemInterfa * @returns CallbackReturn::SUCCESS or CallbackReturn::ERROR. */ ROBOTIQ_DRIVER_PUBLIC - CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; /** * Read data from the hardware. */ ROBOTIQ_DRIVER_PUBLIC - hardware_interface::return_type read( - const rclcpp::Time & time, const rclcpp::Duration & period) override; + hardware_interface::return_type read(const rclcpp::Time& time, const rclcpp::Duration& period) override; /** * Write data to hardware. */ ROBOTIQ_DRIVER_PUBLIC - hardware_interface::return_type write( - const rclcpp::Time & time, const rclcpp::Duration & period) override; + hardware_interface::return_type write(const rclcpp::Time& time, const rclcpp::Duration& period) override; private: // Interface to send binary data to the hardware using the serial port. diff --git a/robotiq_driver/package.xml b/robotiq_driver/package.xml index 4c98252..9b5db0f 100644 --- a/robotiq_driver/package.xml +++ b/robotiq_driver/package.xml @@ -12,6 +12,7 @@ ament_cmake hardware_interface + lifecycle_msgs pluginlib rclcpp rclcpp_lifecycle diff --git a/robotiq_driver/src/crc.cpp b/robotiq_driver/src/crc.cpp index f6a8563..bc3d867 100644 --- a/robotiq_driver/src/crc.cpp +++ b/robotiq_driver/src/crc.cpp @@ -28,6 +28,7 @@ #include "robotiq_driver/crc.hpp" +// clang-format off /* Table of CRC values for high–order byte */ static unsigned char kCRCHiTable[] = { 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, @@ -65,13 +66,15 @@ static unsigned char kCRCLoTable[] = { 0x9C, 0x5C, 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89, 0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C, 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83, 0x41, 0x81, 0x80, 0x40}; +// clang-format on -uint16_t computeCRC(const std::vector & cmd) +uint16_t computeCRC(const std::vector& cmd) { uint16_t crc_hi = 0x00FF; uint16_t crc_lo = 0x00FF; - for (auto byte : cmd) { + for (auto byte : cmd) + { std::size_t index = crc_lo ^ byte; crc_lo = crc_hi ^ kCRCHiTable[index]; crc_hi = kCRCLoTable[index]; diff --git a/robotiq_driver/src/default_driver.cpp b/robotiq_driver/src/default_driver.cpp index 090091d..fea1ae3 100644 --- a/robotiq_driver/src/default_driver.cpp +++ b/robotiq_driver/src/default_driver.cpp @@ -65,18 +65,25 @@ constexpr size_t kResponseHeaderSize = 3; constexpr size_t kGripperStatusIndex = 0; constexpr size_t kPositionIndex = 4; -static uint8_t getFirstByte(uint16_t val) { return (val & 0xFF00) >> 8; } +static uint8_t getFirstByte(uint16_t val) +{ + return (val & 0xFF00) >> 8; +} -static uint8_t getSecondByte(uint16_t val) { return val & 0x00FF; } +static uint8_t getSecondByte(uint16_t val) +{ + return val & 0x00FF; +} -DefaultDriver::DefaultDriver(const std::string & com_port, uint8_t slave_id) -: port_(com_port, kBaudRate, serial::Timeout::simpleTimeout(kTimeoutMilliseconds)), - slave_id_(slave_id), - read_command_(create_read_command(kFirstOutputRegister, kNumOutputRegisters)), - commanded_gripper_speed_(0x80), - commanded_gripper_force_(0x80) +DefaultDriver::DefaultDriver(const std::string& com_port, uint8_t slave_id) + : port_(com_port, kBaudRate, serial::Timeout::simpleTimeout(kTimeoutMilliseconds)) + , slave_id_(slave_id) + , read_command_(create_read_command(kFirstOutputRegister, kNumOutputRegisters)) + , commanded_gripper_speed_(0x80) + , commanded_gripper_force_(0x80) { - if (!port_.isOpen()) { + if (!port_.isOpen()) + { THROW(serial::IOException, "Failed to open gripper port."); } } @@ -87,30 +94,37 @@ bool DefaultDriver::connect() return port_.isOpen(); } -void DefaultDriver::disconnect() { port_.close(); } +void DefaultDriver::disconnect() +{ + port_.close(); +} void DefaultDriver::activate() { - const auto cmd = create_write_command( - kActionRequestRegister, {0x0100, 0x0000, 0x0000} // set rACT to 1, clear all - // other registers. + const auto cmd = create_write_command(kActionRequestRegister, { 0x0100, 0x0000, 0x0000 } // set rACT to 1, clear all + // other registers. ); - try { + try + { send_command(cmd); read_response(kWriteResponseSize); update_status(); - if (gripper_status_ == GripperStatus::COMPLETED) { + if (gripper_status_ == GripperStatus::COMPLETED) + { return; } - while (gripper_status_ == GripperStatus::IN_PROGRESS) { + while (gripper_status_ == GripperStatus::IN_PROGRESS) + { std::this_thread::sleep_for(std::chrono::milliseconds(1000)); update_status(); } - } catch (const serial::IOException & e) { + } + catch (const serial::IOException& e) + { // catch connection error and rethrow std::cerr << "Failed to activate gripper"; throw; @@ -119,11 +133,14 @@ void DefaultDriver::activate() void DefaultDriver::deactivate() { - const auto cmd = create_write_command(kActionRequestRegister, {0x0000, 0x0000, 0x0000}); - try { + const auto cmd = create_write_command(kActionRequestRegister, { 0x0000, 0x0000, 0x0000 }); + try + { send_command(cmd); read_response(kWriteResponseSize); - } catch (const serial::IOException & e) { + } + catch (const serial::IOException& e) + { // catch connection error and rethrow std::cerr << "Failed to activate gripper"; throw; @@ -136,17 +153,21 @@ void DefaultDriver::set_gripper_position(uint8_t pos) uint8_t gripper_options_1 = 0x00; uint8_t gripper_options_2 = 0x00; - const auto cmd = create_write_command( - kActionRequestRegister, - {uint16_t(action_register << 8 | gripper_options_1), uint16_t(gripper_options_2 << 8 | pos), - uint16_t(commanded_gripper_speed_ << 8 | commanded_gripper_force_)}); - try { + const auto cmd = + create_write_command(kActionRequestRegister, + { uint16_t(action_register << 8 | gripper_options_1), uint16_t(gripper_options_2 << 8 | pos), + uint16_t(commanded_gripper_speed_ << 8 | commanded_gripper_force_) }); + try + { send_command(cmd); read_response(kWriteResponseSize); - } catch (const serial::IOException & e) { + } + catch (const serial::IOException& e) + { // catch connection error and rethrow std::cerr << "Failed to set gripper position\n"; - if (port_.isOpen()) { + if (port_.isOpen()) + { std::cerr << "Error caught while reading or writing to device. Connection is open, " "continuing to attempt " "communication with gripper.\n ERROR: " @@ -159,9 +180,12 @@ void DefaultDriver::set_gripper_position(uint8_t pos) uint8_t DefaultDriver::get_gripper_position() { - try { + try + { update_status(); - } catch (const serial::IOException & e) { + } + catch (const serial::IOException& e) + { // catch connection error and rethrow std::cerr << "Failed to get gripper position\n"; throw; @@ -172,9 +196,12 @@ uint8_t DefaultDriver::get_gripper_position() bool DefaultDriver::gripper_is_moving() { - try { + try + { update_status(); - } catch (const serial::IOException & e) { + } + catch (const serial::IOException& e) + { // catch connection error and rethrow std::cerr << "Failed to get gripper position\n"; throw; @@ -183,41 +210,44 @@ bool DefaultDriver::gripper_is_moving() return object_detection_status_ == ObjectDetectionStatus::MOVING; } -std::vector DefaultDriver::create_read_command( - uint16_t first_register, uint8_t num_registers) +std::vector DefaultDriver::create_read_command(uint16_t first_register, uint8_t num_registers) { - std::vector cmd = { - slave_id_, - kReadFunctionCode, - getFirstByte(first_register), - getSecondByte(first_register), - getFirstByte(num_registers), - getSecondByte(num_registers)}; + std::vector cmd = { slave_id_, + kReadFunctionCode, + getFirstByte(first_register), + getSecondByte(first_register), + getFirstByte(num_registers), + getSecondByte(num_registers) }; auto crc = computeCRC(cmd); cmd.push_back(getFirstByte(crc)); cmd.push_back(getSecondByte(crc)); return cmd; } -void DefaultDriver::set_speed(uint8_t speed) { commanded_gripper_speed_ = speed; } +void DefaultDriver::set_speed(uint8_t speed) +{ + commanded_gripper_speed_ = speed; +} -void DefaultDriver::set_force(uint8_t force) { commanded_gripper_force_ = force; } +void DefaultDriver::set_force(uint8_t force) +{ + commanded_gripper_force_ = force; +} -std::vector DefaultDriver::create_write_command( - uint16_t first_register, const std::vector & data) +std::vector DefaultDriver::create_write_command(uint16_t first_register, const std::vector& data) { uint16_t num_registers = data.size(); uint8_t num_bytes = 2 * num_registers; - std::vector cmd = { - slave_id_, - kWriteFunctionCode, - getFirstByte(first_register), - getSecondByte(first_register), - getFirstByte(num_registers), - getSecondByte(num_registers), - num_bytes}; - for (auto d : data) { + std::vector cmd = { slave_id_, + kWriteFunctionCode, + getFirstByte(first_register), + getSecondByte(first_register), + getFirstByte(num_registers), + getSecondByte(num_registers), + num_bytes }; + for (auto d : data) + { cmd.push_back(getFirstByte(d)); cmd.push_back(getSecondByte(d)); } @@ -234,22 +264,24 @@ std::vector DefaultDriver::read_response(size_t num_bytes_requested) std::vector response; size_t num_bytes_read = port_.read(response, num_bytes_requested); - if (num_bytes_read != num_bytes_requested) { - const auto error_msg = "Requested " + std::to_string(num_bytes_requested) + - " bytes, but only got " + std::to_string(num_bytes_read); + if (num_bytes_read != num_bytes_requested) + { + const auto error_msg = + "Requested " + std::to_string(num_bytes_requested) + " bytes, but only got " + std::to_string(num_bytes_read); THROW(serial::IOException, error_msg.c_str()); } return response; } -void DefaultDriver::send_command(const std::vector & cmd) +void DefaultDriver::send_command(const std::vector& cmd) { size_t num_bytes_written = port_.write(cmd); port_.flush(); - if (num_bytes_written != cmd.size()) { - const auto error_msg = "Attempted to write " + std::to_string(cmd.size()) + - " bytes, but only wrote " + std::to_string(num_bytes_written); + if (num_bytes_written != cmd.size()) + { + const auto error_msg = "Attempted to write " + std::to_string(cmd.size()) + " bytes, but only wrote " + + std::to_string(num_bytes_written); THROW(serial::IOException, error_msg.c_str()); } } @@ -257,7 +289,8 @@ void DefaultDriver::send_command(const std::vector & cmd) void DefaultDriver::update_status() { // Tell the gripper that we want to read its status. - try { + try + { send_command(read_command_); const auto response = read_response(kReadResponseSize); @@ -266,15 +299,14 @@ void DefaultDriver::update_status() uint8_t gripper_status_byte = response[kResponseHeaderSize + kGripperStatusIndex]; // Activation status. - activation_status_ = - ((gripper_status_byte & 0x01) == 0x00) ? ActivationStatus::RESET : ActivationStatus::ACTIVE; + activation_status_ = ((gripper_status_byte & 0x01) == 0x00) ? ActivationStatus::RESET : ActivationStatus::ACTIVE; // Action status. - action_status_ = - ((gripper_status_byte & 0x08) == 0x00) ? ActionStatus::STOPPED : ActionStatus::MOVING; + action_status_ = ((gripper_status_byte & 0x08) == 0x00) ? ActionStatus::STOPPED : ActionStatus::MOVING; // Gripper status. - switch ((gripper_status_byte & 0x30) >> 4) { + switch ((gripper_status_byte & 0x30) >> 4) + { case 0x00: gripper_status_ = GripperStatus::RESET; break; @@ -287,7 +319,8 @@ void DefaultDriver::update_status() } // Object detection status. - switch ((gripper_status_byte & 0xC0) >> 6) { + switch ((gripper_status_byte & 0xC0) >> 6) + { case 0x00: object_detection_status_ = ObjectDetectionStatus::MOVING; break; @@ -304,9 +337,12 @@ void DefaultDriver::update_status() // Read the current gripper position. gripper_position_ = response[kResponseHeaderSize + kPositionIndex]; - } catch (const serial::IOException & e) { + } + catch (const serial::IOException& e) + { std::cerr << "Failed to update gripper status.\n"; - if (port_.isOpen()) { + if (port_.isOpen()) + { std::cerr << "Error caught while reading or writing to device. Connection is open, " "continuing to attempt " "communication with gripper.\n ERROR: " diff --git a/robotiq_driver/src/default_driver_factory.cpp b/robotiq_driver/src/default_driver_factory.cpp index ad82a4e..d4d2357 100644 --- a/robotiq_driver/src/default_driver_factory.cpp +++ b/robotiq_driver/src/default_driver_factory.cpp @@ -26,18 +26,19 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. -#include -#include -#include #include +#include + +#include + +#include namespace robotiq_driver { const auto kLogger = rclcpp::get_logger("DefaultDriverFactory"); -std::unique_ptr DefaultDriverFactory::create( - const hardware_interface::HardwareInfo & info) const +std::unique_ptr DefaultDriverFactory::create(const hardware_interface::HardwareInfo& info) const { double gripper_speed = stod(info.hardware_parameters.at("gripper_speed_multiplier")); double gripper_force = stod(info.hardware_parameters.at("gripper_force_multiplier")); @@ -53,8 +54,7 @@ std::unique_ptr DefaultDriverFactory::create( return driver; } -std::unique_ptr DefaultDriverFactory::create_driver( - const hardware_interface::HardwareInfo & info) const +std::unique_ptr DefaultDriverFactory::create_driver(const hardware_interface::HardwareInfo& info) const { std::string com_port = info.hardware_parameters.at("COM_port"); return std::make_unique(com_port); diff --git a/robotiq_driver/src/gripper_interface_test.cpp b/robotiq_driver/src/gripper_interface_test.cpp index c584ef5..bb19fe6 100644 --- a/robotiq_driver/src/gripper_interface_test.cpp +++ b/robotiq_driver/src/gripper_interface_test.cpp @@ -26,16 +26,18 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. -#include #include + #include +#include constexpr auto kComPort = "/dev/ttyUSB0"; constexpr auto kSlaveID = 0x09; int main() { - try { + try + { robotiq_driver::DefaultDriver gripper(kComPort, kSlaveID); std::cout << "Deactivating gripper...\n"; @@ -48,25 +50,29 @@ int main() std::cout << "Closing gripper...\n"; gripper.set_gripper_position(0xFF); - while (gripper.gripper_is_moving()) { + while (gripper.gripper_is_moving()) + { std::this_thread::sleep_for(std::chrono::milliseconds(500)); } std::cout << "Opening gripper...\n"; gripper.set_gripper_position(0x00); - while (gripper.gripper_is_moving()) { + while (gripper.gripper_is_moving()) + { std::this_thread::sleep_for(std::chrono::milliseconds(500)); } std::cout << "Half closing gripper...\n"; gripper.set_gripper_position(0x80); - while (gripper.gripper_is_moving()) { + while (gripper.gripper_is_moving()) + { std::this_thread::sleep_for(std::chrono::milliseconds(500)); } std::cout << "Opening gripper...\n"; gripper.set_gripper_position(0x00); - while (gripper.gripper_is_moving()) { + while (gripper.gripper_is_moving()) + { std::this_thread::sleep_for(std::chrono::milliseconds(500)); } @@ -75,7 +81,8 @@ int main() std::cout << "Closing gripper...\n"; gripper.set_gripper_position(0xFF); - while (gripper.gripper_is_moving()) { + while (gripper.gripper_is_moving()) + { std::this_thread::sleep_for(std::chrono::milliseconds(500)); } @@ -84,10 +91,13 @@ int main() std::cout << "Opening gripper...\n"; gripper.set_gripper_position(0x00); - while (gripper.gripper_is_moving()) { + while (gripper.gripper_is_moving()) + { std::this_thread::sleep_for(std::chrono::milliseconds(500)); } - } catch (const serial::IOException & e) { + } + catch (const serial::IOException& e) + { std::cout << "Failed to communicating with the Gripper. Please check the Gripper connection"; return 1; } diff --git a/robotiq_driver/src/hardware_interface.cpp b/robotiq_driver/src/hardware_interface.cpp index e29e3ef..772f6ee 100644 --- a/robotiq_driver/src/hardware_interface.cpp +++ b/robotiq_driver/src/hardware_interface.cpp @@ -26,17 +26,20 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. +#include +#include + +#include +#include + #include +#include + #include #include -#include -#include #include #include -#include -#include -#include #include constexpr uint8_t kGripperMinPos = 3; @@ -53,18 +56,17 @@ RobotiqGripperHardwareInterface::RobotiqGripperHardwareInterface() } // This constructor is use for testing only. -RobotiqGripperHardwareInterface::RobotiqGripperHardwareInterface( - std::unique_ptr driver_factory) -: driver_factory_{std::move(driver_factory)} +RobotiqGripperHardwareInterface::RobotiqGripperHardwareInterface(std::unique_ptr driver_factory) + : driver_factory_{ std::move(driver_factory) } { } -hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init( - const hardware_interface::HardwareInfo & info) +hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(const hardware_interface::HardwareInfo& info) { RCLCPP_DEBUG(kLogger, "on_init"); - if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { + if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) + { return CallbackReturn::ERROR; } @@ -77,45 +79,49 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init( reactivate_gripper_cmd_ = NO_NEW_CMD_; reactivate_gripper_async_cmd_.store(false); - const hardware_interface::ComponentInfo & joint = info_.joints[0]; + const hardware_interface::ComponentInfo& joint = info_.joints[0]; // There is one command interface: position. - if (joint.command_interfaces.size() != 1) { - RCLCPP_FATAL( - kLogger, "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), - joint.command_interfaces.size()); + if (joint.command_interfaces.size() != 1) + { + RCLCPP_FATAL(kLogger, "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), + joint.command_interfaces.size()); return CallbackReturn::ERROR; } - if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) { - RCLCPP_FATAL( - kLogger, "Joint '%s' has %s command interfaces found. '%s' expected.", joint.name.c_str(), - joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); + if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) + { + RCLCPP_FATAL(kLogger, "Joint '%s' has %s command interfaces found. '%s' expected.", joint.name.c_str(), + joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); return CallbackReturn::ERROR; } // There are two state interfaces: position and velocity. - if (joint.state_interfaces.size() != 2) { - RCLCPP_FATAL( - kLogger, "Joint '%s' has %zu state interface. 2 expected.", joint.name.c_str(), - joint.state_interfaces.size()); + if (joint.state_interfaces.size() != 2) + { + RCLCPP_FATAL(kLogger, "Joint '%s' has %zu state interface. 2 expected.", joint.name.c_str(), + joint.state_interfaces.size()); return CallbackReturn::ERROR; } - for (int i = 0; i < 2; ++i) { + for (int i = 0; i < 2; ++i) + { if (!(joint.state_interfaces[i].name == hardware_interface::HW_IF_POSITION || - joint.state_interfaces[i].name == hardware_interface::HW_IF_VELOCITY)) { - RCLCPP_FATAL( - kLogger, "Joint '%s' has %s state interface. Expected %s or %s.", joint.name.c_str(), - joint.state_interfaces[i].name.c_str(), hardware_interface::HW_IF_POSITION, - hardware_interface::HW_IF_VELOCITY); + joint.state_interfaces[i].name == hardware_interface::HW_IF_VELOCITY)) + { + RCLCPP_FATAL(kLogger, "Joint '%s' has %s state interface. Expected %s or %s.", joint.name.c_str(), + joint.state_interfaces[i].name.c_str(), hardware_interface::HW_IF_POSITION, + hardware_interface::HW_IF_VELOCITY); return CallbackReturn::ERROR; } } - try { + try + { driver_ = driver_factory_->create(info_); - } catch (const serial::IOException & e) { + } + catch (const serial::IOException& e) + { RCLCPP_FATAL(kLogger, "Failed to open gripper port."); return CallbackReturn::ERROR; } @@ -124,78 +130,83 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init( } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -RobotiqGripperHardwareInterface::on_configure(const rclcpp_lifecycle::State & previous_state) +RobotiqGripperHardwareInterface::on_configure(const rclcpp_lifecycle::State& previous_state) { RCLCPP_DEBUG(kLogger, "on_configure"); - try { - if ( - hardware_interface::SystemInterface::on_configure(previous_state) != - CallbackReturn::SUCCESS) { + try + { + if (hardware_interface::SystemInterface::on_configure(previous_state) != CallbackReturn::SUCCESS) + { return CallbackReturn::ERROR; } // Open the serial port and handshake. bool connected = driver_->connect(); - if (!connected) { + if (!connected) + { RCLCPP_ERROR(kLogger, "Cannot connect to the Robotiq gripper"); return CallbackReturn::ERROR; } - } catch (const std::exception & e) { + } + catch (const std::exception& e) + { RCLCPP_ERROR(kLogger, "Cannot configure the Robotiq gripper: %s", e.what()); return CallbackReturn::ERROR; } return CallbackReturn::SUCCESS; } -std::vector -RobotiqGripperHardwareInterface::export_state_interfaces() +std::vector RobotiqGripperHardwareInterface::export_state_interfaces() { RCLCPP_DEBUG(kLogger, "export_state_interfaces"); std::vector state_interfaces; - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[0].name, hardware_interface::HW_IF_POSITION, &gripper_position_)); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[0].name, hardware_interface::HW_IF_VELOCITY, &gripper_velocity_)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(info_.joints[0].name, hardware_interface::HW_IF_POSITION, &gripper_position_)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(info_.joints[0].name, hardware_interface::HW_IF_VELOCITY, &gripper_velocity_)); return state_interfaces; } -std::vector -RobotiqGripperHardwareInterface::export_command_interfaces() +std::vector RobotiqGripperHardwareInterface::export_command_interfaces() { RCLCPP_DEBUG(kLogger, "export_command_interfaces"); std::vector command_interfaces; command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[0].name, hardware_interface::HW_IF_POSITION, &gripper_position_command_)); + info_.joints[0].name, hardware_interface::HW_IF_POSITION, &gripper_position_command_)); + command_interfaces.emplace_back( + hardware_interface::CommandInterface("reactivate_gripper", "reactivate_gripper_cmd", &reactivate_gripper_cmd_)); command_interfaces.emplace_back(hardware_interface::CommandInterface( - "reactivate_gripper", "reactivate_gripper_cmd", &reactivate_gripper_cmd_)); - command_interfaces.emplace_back(hardware_interface::CommandInterface( - "reactivate_gripper", "reactivate_gripper_response", &reactivate_gripper_response_)); + "reactivate_gripper", "reactivate_gripper_response", &reactivate_gripper_response_)); return command_interfaces; } -hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) +hardware_interface::CallbackReturn +RobotiqGripperHardwareInterface::on_activate(const rclcpp_lifecycle::State& /*previous_state*/) { RCLCPP_DEBUG(kLogger, "on_activate"); // set some default values for joints - if (std::isnan(gripper_position_)) { + if (std::isnan(gripper_position_)) + { gripper_position_ = 0; gripper_velocity_ = 0; gripper_position_command_ = 0; } // Activate the gripper. - try { + try + { driver_->deactivate(); driver_->activate(); - } catch (const serial::IOException & e) { + } + catch (const serial::IOException& e) + { RCLCPP_FATAL(kLogger, "Failed to communicate with Gripper. Check Gripper connection."); return CallbackReturn::ERROR; } @@ -209,13 +220,17 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_activate( const auto io_interval = std::chrono::milliseconds(10); auto last_io = std::chrono::high_resolution_clock::now(); - while (communication_thread_is_running_.load()) { + while (communication_thread_is_running_.load()) + { const auto now = std::chrono::high_resolution_clock::now(); - if (now - last_io > io_interval) { - try { + if (now - last_io > io_interval) + { + try + { // Re-activate the gripper // (this can be used, for example, to re-run the auto-calibration). - if (reactivate_gripper_async_cmd_.load()) { + if (reactivate_gripper_async_cmd_.load()) + { this->driver_->deactivate(); this->driver_->activate(); reactivate_gripper_async_cmd_.store(false); @@ -229,9 +244,10 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_activate( gripper_current_state_.store(this->driver_->get_gripper_position()); last_io = now; - } catch (serial::IOException & e) { - RCLCPP_ERROR( - kLogger, "Check Robotiq Gripper connection and restart drivers. ERROR: %s", e.what()); + } + catch (serial::IOException& e) + { + RCLCPP_ERROR(kLogger, "Check Robotiq Gripper connection and restart drivers. ERROR: %s", e.what()); communication_thread_is_running_.store(false); } } @@ -241,17 +257,20 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_activate( return CallbackReturn::SUCCESS; } -hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) +hardware_interface::CallbackReturn +RobotiqGripperHardwareInterface::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/) { RCLCPP_DEBUG(kLogger, "on_deactivate"); communication_thread_is_running_.store(false); communication_thread_.join(); - try { + try + { driver_->deactivate(); - } catch (const std::exception & e) { + } + catch (const std::exception& e) + { RCLCPP_ERROR(kLogger, "Failed to deactivate gripper. Check Gripper connection"); return CallbackReturn::ERROR; } @@ -259,19 +278,20 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_deactivat return CallbackReturn::SUCCESS; } -hardware_interface::return_type RobotiqGripperHardwareInterface::read( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +hardware_interface::return_type RobotiqGripperHardwareInterface::read(const rclcpp::Time& /*time*/, + const rclcpp::Duration& /*period*/) { - gripper_position_ = - gripper_closed_pos_ * (gripper_current_state_.load() - kGripperMinPos) / kGripperRange; + gripper_position_ = gripper_closed_pos_ * (gripper_current_state_.load() - kGripperMinPos) / kGripperRange; - if (!std::isnan(reactivate_gripper_cmd_)) { + if (!std::isnan(reactivate_gripper_cmd_)) + { RCLCPP_INFO(kLogger, "Sending gripper reactivation request."); reactivate_gripper_async_cmd_.store(true); reactivate_gripper_cmd_ = NO_NEW_CMD_; } - if (reactivate_gripper_async_response_.load().has_value()) { + if (reactivate_gripper_async_response_.load().has_value()) + { reactivate_gripper_response_ = reactivate_gripper_async_response_.load().value(); reactivate_gripper_async_response_.store(std::nullopt); } @@ -279,11 +299,10 @@ hardware_interface::return_type RobotiqGripperHardwareInterface::read( return hardware_interface::return_type::OK; } -hardware_interface::return_type RobotiqGripperHardwareInterface::write( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +hardware_interface::return_type RobotiqGripperHardwareInterface::write(const rclcpp::Time& /*time*/, + const rclcpp::Duration& /*period*/) { - double gripper_pos = - (gripper_position_command_ / gripper_closed_pos_) * kGripperRange + kGripperMinPos; + double gripper_pos = (gripper_position_command_ / gripper_closed_pos_) * kGripperRange + kGripperMinPos; gripper_pos = std::max(std::min(gripper_pos, 255.0), 0.0); write_command_.store(uint8_t(gripper_pos)); @@ -294,5 +313,4 @@ hardware_interface::return_type RobotiqGripperHardwareInterface::write( #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS( - robotiq_driver::RobotiqGripperHardwareInterface, hardware_interface::SystemInterface) +PLUGINLIB_EXPORT_CLASS(robotiq_driver::RobotiqGripperHardwareInterface, hardware_interface::SystemInterface) diff --git a/robotiq_driver/tests/test_robotiq_gripper_hardware_interface.cpp b/robotiq_driver/tests/test_robotiq_gripper_hardware_interface.cpp index 2cea65d..c0d697d 100644 --- a/robotiq_driver/tests/test_robotiq_gripper_hardware_interface.cpp +++ b/robotiq_driver/tests/test_robotiq_gripper_hardware_interface.cpp @@ -29,18 +29,22 @@ #include #include -#include +#include +#include + #include #include #include #include + #include #include -#include -#include + #include #include +#include + namespace robotiq_driver::test { @@ -51,7 +55,7 @@ namespace robotiq_driver::test TEST(TestRobotiqGripperHardwareInterface, load_urdf) { std::string urdf_control_ = - R"( + R"( robotiq_driver/RobotiqGripperHardwareInterface @@ -70,8 +74,7 @@ TEST(TestRobotiqGripperHardwareInterface, load_urdf) )"; - auto urdf = - ros2_control_test_assets::urdf_head + urdf_control_ + ros2_control_test_assets::urdf_tail; + auto urdf = ros2_control_test_assets::urdf_head + urdf_control_ + ros2_control_test_assets::urdf_tail; hardware_interface::ResourceManager rm(urdf); // Check interfaces From d7cfc1ba2ae46ec4effcdea551c3d702a3c92edd Mon Sep 17 00:00:00 2001 From: Giovanni Remigi Date: Tue, 5 Sep 2023 16:58:01 +0200 Subject: [PATCH 10/47] Revert "Auxiliary commit to revert individual files from 878859365914bdb28a5efe0455746b21e6c3f5e8" This reverts commit f26c1f3e0fffd9cd1cc603adde5b3265962f96f8. --- .github/workflows/ci-ros-lint.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index cfc2a96..e2eb79b 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -35,7 +35,7 @@ jobs: with: distribution: rolling linter: cpplint - arguments: "--linelength=120 --filter=-whitespace/newline" + arguments: "--linelength=100 --filter=-whitespace/newline" package-name: robotiq_driver robotiq_controllers robotiq_description From 46b8b589a5a0a68621dfba7c83029f1ac1cb12b0 Mon Sep 17 00:00:00 2001 From: Giovanni Remigi Date: Tue, 5 Sep 2023 17:04:53 +0200 Subject: [PATCH 11/47] Linting packages. --- robotiq_driver/package.xml | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/robotiq_driver/package.xml b/robotiq_driver/package.xml index 9b5db0f..1d35780 100644 --- a/robotiq_driver/package.xml +++ b/robotiq_driver/package.xml @@ -21,6 +21,15 @@ gripper_controllers joint_state_broadcaster + + ament_clang_format + + ament_clang_tidy + + ament_cmake_copyright + + ament_cmake_lint_cmake + ament_lint_auto ament_lint_common From 92eefce7151f3c7c41897d2e977e77958aa9631a Mon Sep 17 00:00:00 2001 From: Giovanni Remigi Date: Tue, 5 Sep 2023 17:11:42 +0200 Subject: [PATCH 12/47] Remove ros linting clashing with .clang-format. --- .github/workflows/ci-ros-lint.yml | 41 ------------------------------- 1 file changed, 41 deletions(-) delete mode 100644 .github/workflows/ci-ros-lint.yml diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml deleted file mode 100644 index e2eb79b..0000000 --- a/.github/workflows/ci-ros-lint.yml +++ /dev/null @@ -1,41 +0,0 @@ -name: ROS Lint -on: - pull_request: - -jobs: - ament_lint: - name: ament_${{ matrix.linter }} - runs-on: ubuntu-20.04 - strategy: - fail-fast: false - matrix: - linter: [cppcheck, copyright, lint_cmake] - steps: - - uses: actions/checkout@v3 - - uses: ros-tooling/setup-ros@0.6.2 - - uses: ros-tooling/action-ros-lint@v0.1 - with: - distribution: rolling - linter: ${{ matrix.linter }} - package-name: robotiq_driver - robotiq_controllers - robotiq_description - - ament_lint_100: - name: ament_${{ matrix.linter }} - runs-on: ubuntu-20.04 - strategy: - fail-fast: false - matrix: - linter: [cpplint] - steps: - - uses: actions/checkout@v3 - - uses: ros-tooling/setup-ros@0.6.2 - - uses: ros-tooling/action-ros-lint@v0.1 - with: - distribution: rolling - linter: cpplint - arguments: "--linelength=100 --filter=-whitespace/newline" - package-name: robotiq_driver - robotiq_controllers - robotiq_description From 19569f8702c17c7eea0d7d9bfd9c5f767e31e0f0 Mon Sep 17 00:00:00 2001 From: Giovanni Remigi Date: Tue, 5 Sep 2023 21:57:29 +0200 Subject: [PATCH 13/47] Revert "Auxiliary commit to revert individual files from 92eefce7151f3c7c41897d2e977e77958aa9631a" This reverts commit 4bf404b2d78818853054658aefc81247ee7f69d7. --- .github/workflows/ci-ros-lint.yml | 41 +++++++++++++++++++++++++++++++ 1 file changed, 41 insertions(+) create mode 100644 .github/workflows/ci-ros-lint.yml diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml new file mode 100644 index 0000000..e2eb79b --- /dev/null +++ b/.github/workflows/ci-ros-lint.yml @@ -0,0 +1,41 @@ +name: ROS Lint +on: + pull_request: + +jobs: + ament_lint: + name: ament_${{ matrix.linter }} + runs-on: ubuntu-20.04 + strategy: + fail-fast: false + matrix: + linter: [cppcheck, copyright, lint_cmake] + steps: + - uses: actions/checkout@v3 + - uses: ros-tooling/setup-ros@0.6.2 + - uses: ros-tooling/action-ros-lint@v0.1 + with: + distribution: rolling + linter: ${{ matrix.linter }} + package-name: robotiq_driver + robotiq_controllers + robotiq_description + + ament_lint_100: + name: ament_${{ matrix.linter }} + runs-on: ubuntu-20.04 + strategy: + fail-fast: false + matrix: + linter: [cpplint] + steps: + - uses: actions/checkout@v3 + - uses: ros-tooling/setup-ros@0.6.2 + - uses: ros-tooling/action-ros-lint@v0.1 + with: + distribution: rolling + linter: cpplint + arguments: "--linelength=100 --filter=-whitespace/newline" + package-name: robotiq_driver + robotiq_controllers + robotiq_description From dad11145bac4c1576d30ff9ac4f43d92313077a4 Mon Sep 17 00:00:00 2001 From: Giovanni Remigi Date: Tue, 5 Sep 2023 23:07:07 +0200 Subject: [PATCH 14/47] Updated CMakeLists.txt --- robotiq_driver/CMakeLists.txt | 54 +++++++++++++++++++++-------------- 1 file changed, 33 insertions(+), 21 deletions(-) diff --git a/robotiq_driver/CMakeLists.txt b/robotiq_driver/CMakeLists.txt index 11fd7f3..d8e804e 100644 --- a/robotiq_driver/CMakeLists.txt +++ b/robotiq_driver/CMakeLists.txt @@ -1,5 +1,8 @@ -cmake_minimum_required(VERSION 3.8) -project(robotiq_driver) +cmake_minimum_required(VERSION 3.22.1) +project(robotiq_driver VERSION 0.1.0 LANGUAGES CXX) + +# This module provides installation directories as per the GNU coding standards. +include(GNUInstallDirs) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) @@ -21,23 +24,25 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS serial ) +# Robotiq driver library. + add_library( - ${PROJECT_NAME} + robotiq_driver SHARED src/crc.cpp src/hardware_interface.cpp src/default_driver.cpp src/default_driver_factory.cpp ) +target_link_libraries(robotiq_driver atomic) target_include_directories( - ${PROJECT_NAME} + robotiq_driver PUBLIC $ $ ) -target_link_libraries(${PROJECT_NAME} atomic) ament_target_dependencies( - ${PROJECT_NAME} + robotiq_driver ${THIS_PACKAGE_INCLUDE_DEPENDS} ) @@ -52,7 +57,7 @@ pluginlib_export_plugin_description_file(hardware_interface hardware_interface_p add_executable(gripper_interface_test src/gripper_interface_test.cpp) target_include_directories(gripper_interface_test PRIVATE include) ament_target_dependencies(gripper_interface_test serial) -target_link_libraries(gripper_interface_test ${PROJECT_NAME}) +target_link_libraries(gripper_interface_test robotiq_driver) ############################################################################### # EXPORTS @@ -62,7 +67,7 @@ target_link_libraries(gripper_interface_test ${PROJECT_NAME}) # Without this a client cannot find this library. # It can take an arbitrary list of targets named as EXPORT in an install call. ament_export_targets( - export_${PROJECT_NAME} + robotiq_driver_targets # Must match the EXPORT label below in the install section. ) # Help downstream packages to find transitive dependencies i.e. export all # dependencies required by a package to use this library. @@ -78,34 +83,31 @@ ament_export_include_directories( ) # Tell downstream packages our libraries to link against. ament_export_libraries( - ${PROJECT_NAME} + robotiq_driver ) ############################################################################### # INSTALL install( - TARGETS ${PROJECT_NAME} - EXPORT export_${PROJECT_NAME} - ARCHIVE DESTINATION lib/${PROJECT_NAME} - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin + TARGETS robotiq_driver + EXPORT robotiq_driver_targets + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} # lib + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} # lib + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} # bin + INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} # include ) install( TARGETS gripper_interface_test - DESTINATION lib/${PROJECT_NAME} -) -install( - DIRECTORY include/ - DESTINATION include + DESTINATION lib/robotiq_driver ) install( DIRECTORY config - DESTINATION share/${PROJECT_NAME} + DESTINATION share/robotiq_driver ) install( DIRECTORY launch - DESTINATION share/${PROJECT_NAME} + DESTINATION share/robotiq_driver ) ############################################################################### @@ -118,4 +120,14 @@ if(BUILD_TESTING) add_subdirectory(tests) endif() +############################################################################### +# LINTERS + +add_custom_target(format + COMMAND clang-format -i `git ls-files *.hpp *.cpp` + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}) +add_custom_target(tidy + COMMAND clang-tidy -p ${CMAKE_BINARY_DIR} `git ls-files *.cpp` + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}) + ament_package() From de63c83c1b074d7788790183efa0a810dcae5b91 Mon Sep 17 00:00:00 2001 From: Giovanni Remigi Date: Tue, 5 Sep 2023 23:23:03 +0200 Subject: [PATCH 15/47] Removed connection to the serial port in the contructor. --- robotiq_driver/src/default_driver.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/robotiq_driver/src/default_driver.cpp b/robotiq_driver/src/default_driver.cpp index fea1ae3..362a79d 100644 --- a/robotiq_driver/src/default_driver.cpp +++ b/robotiq_driver/src/default_driver.cpp @@ -82,10 +82,6 @@ DefaultDriver::DefaultDriver(const std::string& com_port, uint8_t slave_id) , commanded_gripper_speed_(0x80) , commanded_gripper_force_(0x80) { - if (!port_.isOpen()) - { - THROW(serial::IOException, "Failed to open gripper port."); - } } bool DefaultDriver::connect() From 312ab082fa8f80d75233fed4bfc312279920124c Mon Sep 17 00:00:00 2001 From: Giovanni Remigi Date: Wed, 6 Sep 2023 11:47:22 +0200 Subject: [PATCH 16/47] Refactoring for testing. --- robotiq_driver/CMakeLists.txt | 18 +- .../robotiq_driver/{crc.hpp => crc_utils.hpp} | 12 +- .../include/robotiq_driver/data_utils.hpp | 76 +++++++++ .../include/robotiq_driver/default_driver.hpp | 30 ++-- .../include/robotiq_driver/default_serial.hpp | 73 +++++++++ .../robotiq_driver/default_serial_factory.hpp | 58 +++++++ .../include/robotiq_driver/driver.hpp | 2 + .../robotiq_driver/driver_exception.hpp | 67 ++++++++ .../robotiq_driver/hardware_interface.hpp | 2 +- .../include/robotiq_driver/serial.hpp | 123 ++++++++++++++ .../include/robotiq_driver/serial_factory.hpp | 55 +++++++ ...ility_control.h => visibility_control.hpp} | 0 robotiq_driver/src/crc.cpp | 84 ---------- robotiq_driver/src/crc_utils.cpp | 131 +++++++++++++++ robotiq_driver/src/data_utils.cpp | 99 +++++++++++ robotiq_driver/src/default_driver.cpp | 155 ++++++++---------- robotiq_driver/src/default_driver_factory.cpp | 38 ++++- robotiq_driver/src/default_serial.cpp | 111 +++++++++++++ robotiq_driver/src/default_serial_factory.cpp | 80 +++++++++ robotiq_driver/src/gripper_interface_test.cpp | 52 +++--- robotiq_driver/src/hardware_interface.cpp | 2 +- robotiq_driver/tests/CMakeLists.txt | 24 +++ robotiq_driver/tests/mock/mock_driver.hpp | 53 ++++++ robotiq_driver/tests/mock/mock_serial.hpp | 53 ++++++ .../tests/test_default_driver_factory.cpp | 109 ++++++++++++ .../tests/test_default_serial_factory.cpp | 100 +++++++++++ 26 files changed, 1383 insertions(+), 224 deletions(-) rename robotiq_driver/include/robotiq_driver/{crc.hpp => crc_utils.hpp} (83%) create mode 100644 robotiq_driver/include/robotiq_driver/data_utils.hpp create mode 100644 robotiq_driver/include/robotiq_driver/default_serial.hpp create mode 100644 robotiq_driver/include/robotiq_driver/default_serial_factory.hpp create mode 100644 robotiq_driver/include/robotiq_driver/driver_exception.hpp create mode 100644 robotiq_driver/include/robotiq_driver/serial.hpp create mode 100644 robotiq_driver/include/robotiq_driver/serial_factory.hpp rename robotiq_driver/include/robotiq_driver/{visibility_control.h => visibility_control.hpp} (100%) delete mode 100644 robotiq_driver/src/crc.cpp create mode 100644 robotiq_driver/src/crc_utils.cpp create mode 100644 robotiq_driver/src/data_utils.cpp create mode 100644 robotiq_driver/src/default_serial.cpp create mode 100644 robotiq_driver/src/default_serial_factory.cpp create mode 100644 robotiq_driver/tests/mock/mock_driver.hpp create mode 100644 robotiq_driver/tests/mock/mock_serial.hpp create mode 100644 robotiq_driver/tests/test_default_driver_factory.cpp create mode 100644 robotiq_driver/tests/test_default_serial_factory.cpp diff --git a/robotiq_driver/CMakeLists.txt b/robotiq_driver/CMakeLists.txt index d8e804e..c6f20c2 100644 --- a/robotiq_driver/CMakeLists.txt +++ b/robotiq_driver/CMakeLists.txt @@ -29,10 +29,26 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS add_library( robotiq_driver SHARED - src/crc.cpp + include/robotiq_driver/crc_utils.hpp + include/robotiq_driver/data_utils.hpp + include/robotiq_driver/default_driver.hpp + include/robotiq_driver/default_driver_factory.hpp + include/robotiq_driver/default_serial.hpp + include/robotiq_driver/default_serial_factory.hpp + include/robotiq_driver/driver.hpp + include/robotiq_driver/driver_exception.hpp + include/robotiq_driver/driver_factory.hpp + include/robotiq_driver/hardware_interface.hpp + include/robotiq_driver/serial.hpp + include/robotiq_driver/serial_factory.hpp + include/robotiq_driver/visibility_control.hpp + src/crc_utils.cpp + src/data_utils.cpp src/hardware_interface.cpp src/default_driver.cpp src/default_driver_factory.cpp + src/default_serial.cpp + src/default_serial_factory.cpp ) target_link_libraries(robotiq_driver atomic) target_include_directories( diff --git a/robotiq_driver/include/robotiq_driver/crc.hpp b/robotiq_driver/include/robotiq_driver/crc_utils.hpp similarity index 83% rename from robotiq_driver/include/robotiq_driver/crc.hpp rename to robotiq_driver/include/robotiq_driver/crc_utils.hpp index a9aa70d..2d7f54c 100644 --- a/robotiq_driver/include/robotiq_driver/crc.hpp +++ b/robotiq_driver/include/robotiq_driver/crc_utils.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 PickNik, Inc. +// Copyright (c) 2023 PickNik, Inc. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: @@ -31,4 +31,12 @@ #include #include -uint16_t computeCRC(const std::vector& cmd); +namespace robotiq_driver::crc_utils +{ +/** + * @brief Compute the CRC for the CRC-16 MODBUS protocol. + * @param data The data to compute the CRC for. + * @return A 16-bits CRC. + */ +uint16_t compute_crc(const std::vector& data); +} // namespace robotiq_driver::crc_utils diff --git a/robotiq_driver/include/robotiq_driver/data_utils.hpp b/robotiq_driver/include/robotiq_driver/data_utils.hpp new file mode 100644 index 0000000..0d68269 --- /dev/null +++ b/robotiq_driver/include/robotiq_driver/data_utils.hpp @@ -0,0 +1,76 @@ +// Copyright (c) 2023 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#pragma once + +#include +#include +#include +#include + +/** + * Utility class to convert between commonly used data types. + */ +namespace robotiq_driver::data_utils +{ +/** + * Convert a sequence of uint8_t into a sequence of hex numbers. + * @param bytes The sequence of bytes. + * @return A string containing the sequence of hex numbers. + */ +std::string to_hex(const std::vector& bytes); + +/** + * Convert a sequence of uint16_t into a sequence of hex numbers. + * @param bytes The sequence of bytes. + * @return A string containing the sequence of hex numbers. + */ +std::string to_hex(const std::vector& bytes); + +/** + * Convert a byte to a binary representation for testing purposes. + * @param byte The byte to decode. + * @return The binary representation of the given byte. + */ +std::string to_binary_string(const uint8_t byte); + +/** + * Get the Most Significant Byte (MSB) of the given value. + * @param value A 16-bits value. + * @return The Most Significant Byte (MSB) of the given value. + */ +uint8_t get_msb(uint16_t value); + +/** + * Get the Least Significant Byte (LSB) of the given value. + * @param value A 16-bits value. + * @return The Least Significant Byte (LSB) of the given value. + */ +uint8_t get_lsb(uint16_t value); + +} // namespace robotiq_driver::data_utils diff --git a/robotiq_driver/include/robotiq_driver/default_driver.hpp b/robotiq_driver/include/robotiq_driver/default_driver.hpp index 5ba4479..c63ec7d 100644 --- a/robotiq_driver/include/robotiq_driver/default_driver.hpp +++ b/robotiq_driver/include/robotiq_driver/default_driver.hpp @@ -29,7 +29,9 @@ #pragma once #include -#include +#include + +#include #include #include /** @@ -42,37 +44,28 @@ namespace robotiq_driver class DefaultDriver : public Driver { public: - explicit DefaultDriver(const std::string& com_port = "/dev/ttyUSB0", uint8_t slave_id = 0x09); + explicit DefaultDriver(std::unique_ptr serial); bool connect() override; void disconnect() override; - /** - * @brief Activates the gripper. - * - * @throw serial::IOException on failure to successfully communicate with gripper port - */ + void set_slave_address(uint8_t slave_address) override; + + /** Activate the gripper with the specified operation mode and parameters. */ void activate() override; - /** - * @brief Deactivates the gripper. - * - * @throw serial::IOException on failure to successfully communicate with gripper port - */ + /** Deactivate the gripper. */ void deactivate() override; /** * @brief Commands the gripper to move to the desired position. - * * @param pos A value between 0x00 (fully open) and 0xFF (fully closed). */ void set_gripper_position(uint8_t pos) override; /** * @brief Return the current position of the gripper. - * * @throw serial::IOException on failure to successfully communicate with gripper port - * * @return uint8_t A value between 0x00 (fully open) and 0xFF (fully closed). */ uint8_t get_gripper_position() override; @@ -85,14 +78,12 @@ class DefaultDriver : public Driver /** * @brief Set the speed of the gripper. - * * @param speed A value between 0x00 (stopped) and 0xFF (full speed). */ void set_speed(uint8_t speed) override; /** * @brief Set how forcefully the gripper opens or closes. - * * @param force A value between 0x00 (no force) or 0xFF (maximum force). */ void set_force(uint8_t force) override; @@ -124,9 +115,8 @@ class DefaultDriver : public Driver */ void update_status(); - serial::Serial port_; - uint8_t slave_id_; - std::vector read_command_; + std::unique_ptr serial_ = nullptr; + uint8_t slave_address_; ActivationStatus activation_status_; ActionStatus action_status_; diff --git a/robotiq_driver/include/robotiq_driver/default_serial.hpp b/robotiq_driver/include/robotiq_driver/default_serial.hpp new file mode 100644 index 0000000..d3cece6 --- /dev/null +++ b/robotiq_driver/include/robotiq_driver/default_serial.hpp @@ -0,0 +1,73 @@ +// Copyright (c) 2023 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#pragma once + +#include +#include + +#include + +namespace serial +{ +class Serial; +} + +namespace robotiq_driver +{ +class DefaultSerial : public Serial +{ +public: + /** + * Creates a Serial object to send and receive bytes to and from the serial + * port. + */ + DefaultSerial(); + + void open() override; + + [[nodiscard]] bool is_open() const override; + + void close() override; + + [[nodiscard]] std::vector read(size_t size = 1) override; + void write(const std::vector& data) override; + + void set_port(const std::string& port) override; + [[nodiscard]] std::string get_port() const override; + + void set_timeout(std::chrono::milliseconds timeout_ms) override; + [[nodiscard]] std::chrono::milliseconds get_timeout() const override; + + void set_baudrate(uint32_t baudrate) override; + [[nodiscard]] uint32_t get_baudrate() const override; + +private: + std::unique_ptr serial_ = nullptr; +}; +} // namespace robotiq_driver diff --git a/robotiq_driver/include/robotiq_driver/default_serial_factory.hpp b/robotiq_driver/include/robotiq_driver/default_serial_factory.hpp new file mode 100644 index 0000000..7e7465e --- /dev/null +++ b/robotiq_driver/include/robotiq_driver/default_serial_factory.hpp @@ -0,0 +1,58 @@ +// Copyright (c) 2023 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#pragma once + +#include +#include +#include + +#include + +namespace robotiq_driver +{ +/** + * This class is used to create a default driver to interact with the hardware. + */ +class DefaultSerialFactory : public SerialFactory +{ +public: + DefaultSerialFactory() = default; + + /** + * @brief Create a serial interface. + * @param info The hardware information. + * @return A sarial interface to communicate with the hardware. + */ + std::unique_ptr create(const hardware_interface::HardwareInfo& info) const; + +protected: + // Seam for testing. + virtual std::unique_ptr create_serial() const; +}; +} // namespace robotiq_driver diff --git a/robotiq_driver/include/robotiq_driver/driver.hpp b/robotiq_driver/include/robotiq_driver/driver.hpp index 7e66525..9309b39 100644 --- a/robotiq_driver/include/robotiq_driver/driver.hpp +++ b/robotiq_driver/include/robotiq_driver/driver.hpp @@ -65,6 +65,8 @@ class Driver AT_REQUESTED_POSITION }; + virtual void set_slave_address(uint8_t slave_address) = 0; + /** Connect to the gripper serial connection. */ virtual bool connect() = 0; diff --git a/robotiq_driver/include/robotiq_driver/driver_exception.hpp b/robotiq_driver/include/robotiq_driver/driver_exception.hpp new file mode 100644 index 0000000..58273a1 --- /dev/null +++ b/robotiq_driver/include/robotiq_driver/driver_exception.hpp @@ -0,0 +1,67 @@ +/* + * Copyright (c) 2022, Giovanni Remigi + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#pragma once + +#include +#include +#include + +namespace robotiq_driver +{ +/** + * This is a custom exception thrown by the Driver. + */ +class DriverException : public std::exception +{ + std::string what_; + +public: + explicit DriverException(const std::string& description) + { + std::stringstream ss; + ss << "DriverException: " << description << "."; + what_ = ss.str(); + } + + DriverException(const DriverException& other) : what_(other.what_) + { + } + + ~DriverException() override = default; + + // Disable copy constructors + DriverException& operator=(const DriverException&) = delete; + + [[nodiscard]] const char* what() const throw() override + { + return what_.c_str(); + } +}; +} // namespace robotiq_driver diff --git a/robotiq_driver/include/robotiq_driver/hardware_interface.hpp b/robotiq_driver/include/robotiq_driver/hardware_interface.hpp index ab4e220..1a7fc9c 100644 --- a/robotiq_driver/include/robotiq_driver/hardware_interface.hpp +++ b/robotiq_driver/include/robotiq_driver/hardware_interface.hpp @@ -28,7 +28,7 @@ #pragma once -#include +#include #include #include diff --git a/robotiq_driver/include/robotiq_driver/serial.hpp b/robotiq_driver/include/robotiq_driver/serial.hpp new file mode 100644 index 0000000..6d665db --- /dev/null +++ b/robotiq_driver/include/robotiq_driver/serial.hpp @@ -0,0 +1,123 @@ +// Copyright (c) 2023 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#pragma once + +#include +#include +#include +#include + +namespace robotiq_driver +{ +/** + * The driver talks to the hardware through an implementation of the + * serial interface. + * We can mock the serial connection to check that a high level command + * (e.g activate) is converted into the correct sequence of bytes + * including CRC. + */ +class Serial +{ +public: + virtual ~Serial() = default; + + /** + * Opens the serial port as long as the port is set and the port isn't + * already open. + */ + virtual void open() = 0; + + /** + * Gets the open status of the serial port. + * @return Returns true if the port is open, false otherwise. + */ + [[nodiscard]] virtual bool is_open() const = 0; + + /** Closes the serial port. */ + virtual void close() = 0; + + /** + * Read a given amount of bytes from the serial port. + * @param size The number of bytes to read. + * @return The sequence of bytes. + * @throw serial::PortNotOpenedException + * @throw serial::SerialException + */ + [[nodiscard]] virtual std::vector read(size_t size) = 0; + + /** + * Write a sequence of bytes to the serial port. + * @param data A vector containing data to be written to the serial port. + * @throw serial::PortNotOpenedException + * @throw serial::SerialException + * @throw serial::IOException + */ + virtual void write(const std::vector& data) = 0; + + /** + * Sets the serial port identifier. + * @param port A const std::string reference containing the address of the + * serial port, which would be something like "COM1" on Windows and + * "/dev/ttyS0" on Linux. + * @throw std::invalid_argument + */ + virtual void set_port(const std::string& port) = 0; + + /** + * Gets the serial port identifier. + * @see SerialInterface::setPort + * @throw std::invalid_argument + */ + [[nodiscard]] virtual std::string get_port() const = 0; + + /** + * Set read timeout in milliseconds. + * @param timeout Read timeout in milliseconds. + */ + virtual void set_timeout(const std::chrono::milliseconds timeout) = 0; + + /** + * Get read timeout in milliseconds. + * @return Read timeout in milliseconds. + */ + [[nodiscard]] virtual std::chrono::milliseconds get_timeout() const = 0; + + /** + * Sets the baudrate for the serial port. + * @param baudrate An integer that sets the baud rate for the serial port. + */ + virtual void set_baudrate(uint32_t baudrate) = 0; + + /** + * Gets the baudrate for the serial port. + * @return An integer that sets the baud rate for the serial port. + */ + [[nodiscard]] virtual uint32_t get_baudrate() const = 0; +}; +} // namespace robotiq_driver diff --git a/robotiq_driver/include/robotiq_driver/serial_factory.hpp b/robotiq_driver/include/robotiq_driver/serial_factory.hpp new file mode 100644 index 0000000..bc14919 --- /dev/null +++ b/robotiq_driver/include/robotiq_driver/serial_factory.hpp @@ -0,0 +1,55 @@ +// Copyright (c) 2023 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#pragma once + +#include + +#include + +#include + +namespace robotiq_driver +{ +/** + * This factory is used to create and configure a Serial interface + * implementation that is used by the driver to interact with the hardware. + * A factory is used to keep the code cleaner but also to simplify testing. + * With a factory, we can test that the parameters read by the hardware + * interface are correctly parsed and passed down to the serial interface. + * A factory can also be mocked to return different implementation of the + * serial interface (or mocks) that do not require interaction with the real + * hardware. + */ +class SerialFactory +{ +public: + virtual std::unique_ptr create(const hardware_interface::HardwareInfo& info) const = 0; +}; + +} // namespace robotiq_driver diff --git a/robotiq_driver/include/robotiq_driver/visibility_control.h b/robotiq_driver/include/robotiq_driver/visibility_control.hpp similarity index 100% rename from robotiq_driver/include/robotiq_driver/visibility_control.h rename to robotiq_driver/include/robotiq_driver/visibility_control.hpp diff --git a/robotiq_driver/src/crc.cpp b/robotiq_driver/src/crc.cpp deleted file mode 100644 index bc3d867..0000000 --- a/robotiq_driver/src/crc.cpp +++ /dev/null @@ -1,84 +0,0 @@ -// Copyright (c) 2022 PickNik, Inc. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the {copyright_holder} nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. - -#include "robotiq_driver/crc.hpp" - -// clang-format off -/* Table of CRC values for high–order byte */ -static unsigned char kCRCHiTable[] = { - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, - 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, - 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, - 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, - 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, - 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, - 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, - 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, - 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40}; - -/* Table of CRC values for low–order byte */ -static unsigned char kCRCLoTable[] = { - 0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7, 0x05, 0xC5, 0xC4, 0x04, - 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 0x08, 0xC8, - 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, - 0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3, 0x11, 0xD1, 0xD0, 0x10, - 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32, 0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4, - 0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, - 0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED, 0xEC, 0x2C, - 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26, 0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, - 0xA0, 0x60, 0x61, 0xA1, 0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4, - 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68, - 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA, 0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, - 0xB4, 0x74, 0x75, 0xB5, 0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0, - 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, - 0x9C, 0x5C, 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, - 0x88, 0x48, 0x49, 0x89, 0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C, - 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83, 0x41, 0x81, 0x80, 0x40}; -// clang-format on - -uint16_t computeCRC(const std::vector& cmd) -{ - uint16_t crc_hi = 0x00FF; - uint16_t crc_lo = 0x00FF; - - for (auto byte : cmd) - { - std::size_t index = crc_lo ^ byte; - crc_lo = crc_hi ^ kCRCHiTable[index]; - crc_hi = kCRCLoTable[index]; - } - - return (crc_lo << 8) + crc_hi; -} diff --git a/robotiq_driver/src/crc_utils.cpp b/robotiq_driver/src/crc_utils.cpp new file mode 100644 index 0000000..08cc447 --- /dev/null +++ b/robotiq_driver/src/crc_utils.cpp @@ -0,0 +1,131 @@ +// Copyright (c) 2023 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include + +#include + +namespace robotiq_driver::crc_utils +{ +/** + * Following are the tables for the CRC-16 MODBUS protocol. This particular CRC + * is commonly used in the Modbus RTU serial communications protocol, which is + * a de facto standard communication protocol and an ISO standard used by many + * industrial devices. + */ + +/* Table of CRC values for high–order byte */ + +// clang-format off +constexpr std::array kCRCHiTable = { + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40 +}; + +/* Table of CRC values for low–order byte */ +constexpr std::array kCRCLoTable = { + 0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, + 0xC6, 0x06, 0x07, 0xC7, 0x05, 0xC5, 0xC4, 0x04, + 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E, + 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 0x08, 0xC8, + 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A, + 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, + 0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, + 0xD2, 0x12, 0x13, 0xD3, 0x11, 0xD1, 0xD0, 0x10, + 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32, + 0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4, + 0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, + 0xFA, 0x3A, 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, + 0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, + 0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED, 0xEC, 0x2C, + 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26, + 0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, + 0xA0, 0x60, 0x61, 0xA1, 0x63, 0xA3, 0xA2, 0x62, + 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4, + 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, 0x6E, 0xAE, + 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68, + 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA, + 0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, + 0xB4, 0x74, 0x75, 0xB5, 0x77, 0xB7, 0xB6, 0x76, + 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0, + 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, + 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, + 0x9C, 0x5C, 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, + 0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, + 0x88, 0x48, 0x49, 0x89, 0x4B, 0x8B, 0x8A, 0x4A, + 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C, + 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, + 0x82, 0x42, 0x43, 0x83, 0x41, 0x81, 0x80, 0x40 +}; +// clang-format on + +uint16_t compute_crc(const std::vector& cmd) +{ + uint16_t crc_hi = 0x00FF; + uint16_t crc_lo = 0x00FF; + + for (uint8_t byte : cmd) + { + uint8_t index = crc_lo ^ byte; + crc_lo = crc_hi ^ kCRCHiTable[index]; + crc_hi = kCRCLoTable[index]; + } + + return (crc_lo << 8) + crc_hi; +} +} // namespace robotiq_driver::crc_utils diff --git a/robotiq_driver/src/data_utils.cpp b/robotiq_driver/src/data_utils.cpp new file mode 100644 index 0000000..55b6bff --- /dev/null +++ b/robotiq_driver/src/data_utils.cpp @@ -0,0 +1,99 @@ +// Copyright (c) 2023 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include + +#include +#include +#include + +namespace robotiq_driver::data_utils +{ +constexpr std::array vChars = { + '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F' +}; + +std::string to_hex(const std::vector& bytes) +{ + std::string hex; + for (auto it = std::begin(bytes); it != std::end(bytes); ++it) + { + if (it != bytes.begin()) + { + hex += " "; + } + hex += ""; + uint8_t ch = *it; + hex += vChars[((ch >> 4) & 0xF)]; + hex += vChars[(ch & 0xF)]; + } + + return hex; +} + +std::string to_hex(const std::vector& bytes) +{ + std::string hex; + for (auto it = std::begin(bytes); it != std::end(bytes); ++it) + { + if (it != bytes.begin()) + { + hex += " "; + } + hex += ""; + uint16_t ch = *it; + hex += vChars[((ch >> 12) & 0xF)]; + hex += vChars[((ch >> 8) & 0xF)]; + hex += vChars[((ch >> 4) & 0xF)]; + hex += vChars[(ch & 0xF)]; + } + + return hex; +} + +std::string to_binary_string(const uint8_t byte) +{ + std::string result = "0x"; + for (int i = 7; i >= 0; --i) + { + result += ((byte >> i) & 1) ? '1' : '0'; + } + return result; +} + +uint8_t get_msb(uint16_t value) +{ + return static_cast(value >> 8) & 0xFF; +} + +uint8_t get_lsb(uint16_t value) +{ + return static_cast(value & 0xFF); +} + +} // namespace robotiq_driver::data_utils diff --git a/robotiq_driver/src/default_driver.cpp b/robotiq_driver/src/default_driver.cpp index 362a79d..3afc4fa 100644 --- a/robotiq_driver/src/default_driver.cpp +++ b/robotiq_driver/src/default_driver.cpp @@ -26,23 +26,28 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. -#include "robotiq_driver/default_driver.hpp" - #include #include #include #include -#include "robotiq_driver/crc.hpp" +#include + +#include "robotiq_driver/data_utils.hpp" +#include "robotiq_driver/default_driver.hpp" +#include +#include + +#include namespace robotiq_driver { -constexpr int kBaudRate = 115200; -constexpr auto kTimeoutMilliseconds = 1000; +const auto kLogger = rclcpp::get_logger("DefaultDriver"); constexpr uint8_t kReadFunctionCode = 0x03; constexpr uint16_t kFirstOutputRegister = 0x07D0; constexpr uint16_t kNumOutputRegisters = 0x0006; + // The response to a read request consists of: // slave ID (1 byte) // function code (1 byte) @@ -53,6 +58,7 @@ constexpr int kReadResponseSize = 2 * kNumOutputRegisters + 5; constexpr uint8_t kWriteFunctionCode = 0x10; constexpr uint16_t kActionRequestRegister = 0x03E8; + // The response to a write command consists of: // slave ID (1 byte) // function code (1 byte) @@ -65,41 +71,33 @@ constexpr size_t kResponseHeaderSize = 3; constexpr size_t kGripperStatusIndex = 0; constexpr size_t kPositionIndex = 4; -static uint8_t getFirstByte(uint16_t val) +DefaultDriver::DefaultDriver(std::unique_ptr serial) + : serial_{ std::move(serial) }, commanded_gripper_speed_(0x80), commanded_gripper_force_(0x80) { - return (val & 0xFF00) >> 8; } -static uint8_t getSecondByte(uint16_t val) -{ - return val & 0x00FF; -} - -DefaultDriver::DefaultDriver(const std::string& com_port, uint8_t slave_id) - : port_(com_port, kBaudRate, serial::Timeout::simpleTimeout(kTimeoutMilliseconds)) - , slave_id_(slave_id) - , read_command_(create_read_command(kFirstOutputRegister, kNumOutputRegisters)) - , commanded_gripper_speed_(0x80) - , commanded_gripper_force_(0x80) +bool DefaultDriver::connect() { + serial_->open(); + return serial_->is_open(); } -bool DefaultDriver::connect() +void DefaultDriver::disconnect() { - port_.open(); - return port_.isOpen(); + serial_->close(); } -void DefaultDriver::disconnect() +void DefaultDriver::set_slave_address(uint8_t slave_address) { - port_.close(); + slave_address_ = slave_address; } void DefaultDriver::activate() { - const auto cmd = create_write_command(kActionRequestRegister, { 0x0100, 0x0000, 0x0000 } // set rACT to 1, clear all - // other registers. - ); + RCLCPP_INFO(kLogger, "Activate..."); + + // set rACT to 1, clear all ther registers. + const auto cmd = create_write_command(kActionRequestRegister, { 0x0100, 0x0000, 0x0000 }); try { @@ -119,27 +117,25 @@ void DefaultDriver::activate() update_status(); } } - catch (const serial::IOException& e) + catch (const std::exception& e) { - // catch connection error and rethrow - std::cerr << "Failed to activate gripper"; - throw; + throw DriverException{ std::string{ "Failed to activate the gripper: " } + e.what() }; } } void DefaultDriver::deactivate() { + RCLCPP_INFO(kLogger, "Dectivate..."); + const auto cmd = create_write_command(kActionRequestRegister, { 0x0000, 0x0000, 0x0000 }); try { send_command(cmd); read_response(kWriteResponseSize); } - catch (const serial::IOException& e) + catch (const std::exception& e) { - // catch connection error and rethrow - std::cerr << "Failed to activate gripper"; - throw; + throw DriverException{ std::string{ "Failed to deactivate the gripper: " } + e.what() }; } } @@ -158,19 +154,9 @@ void DefaultDriver::set_gripper_position(uint8_t pos) send_command(cmd); read_response(kWriteResponseSize); } - catch (const serial::IOException& e) + catch (const std::exception& e) { - // catch connection error and rethrow - std::cerr << "Failed to set gripper position\n"; - if (port_.isOpen()) - { - std::cerr << "Error caught while reading or writing to device. Connection is open, " - "continuing to attempt " - "communication with gripper.\n ERROR: " - << e.what(); - return; - } - throw; + throw DriverException{ std::string{ "Failed to set gripper position: " } + e.what() }; } } @@ -180,11 +166,9 @@ uint8_t DefaultDriver::get_gripper_position() { update_status(); } - catch (const serial::IOException& e) + catch (const std::exception& e) { - // catch connection error and rethrow - std::cerr << "Failed to get gripper position\n"; - throw; + throw DriverException{ std::string{ "Failed to get gripper position: " } + e.what() }; } return gripper_position_; @@ -196,11 +180,9 @@ bool DefaultDriver::gripper_is_moving() { update_status(); } - catch (const serial::IOException& e) + catch (const std::exception& e) { - // catch connection error and rethrow - std::cerr << "Failed to get gripper position\n"; - throw; + throw DriverException{ std::string{ "Failed to get gripper status: " } + e.what() }; } return object_detection_status_ == ObjectDetectionStatus::MOVING; @@ -208,15 +190,15 @@ bool DefaultDriver::gripper_is_moving() std::vector DefaultDriver::create_read_command(uint16_t first_register, uint8_t num_registers) { - std::vector cmd = { slave_id_, + std::vector cmd = { slave_address_, kReadFunctionCode, - getFirstByte(first_register), - getSecondByte(first_register), - getFirstByte(num_registers), - getSecondByte(num_registers) }; - auto crc = computeCRC(cmd); - cmd.push_back(getFirstByte(crc)); - cmd.push_back(getSecondByte(crc)); + data_utils::get_msb(first_register), + data_utils::get_lsb(first_register), + data_utils::get_msb(num_registers), + data_utils::get_lsb(num_registers) }; + auto crc = crc_utils::compute_crc(cmd); + cmd.push_back(data_utils::get_msb(crc)); + cmd.push_back(data_utils::get_lsb(crc)); return cmd; } @@ -235,22 +217,22 @@ std::vector DefaultDriver::create_write_command(uint16_t first_register uint16_t num_registers = data.size(); uint8_t num_bytes = 2 * num_registers; - std::vector cmd = { slave_id_, + std::vector cmd = { slave_address_, kWriteFunctionCode, - getFirstByte(first_register), - getSecondByte(first_register), - getFirstByte(num_registers), - getSecondByte(num_registers), + data_utils::get_msb(first_register), + data_utils::get_lsb(first_register), + data_utils::get_msb(num_registers), + data_utils::get_lsb(num_registers), num_bytes }; for (auto d : data) { - cmd.push_back(getFirstByte(d)); - cmd.push_back(getSecondByte(d)); + cmd.push_back(data_utils::get_msb(d)); + cmd.push_back(data_utils::get_lsb(d)); } - auto crc = computeCRC(cmd); - cmd.push_back(getFirstByte(crc)); - cmd.push_back(getSecondByte(crc)); + auto crc = crc_utils::compute_crc(cmd); + cmd.push_back(data_utils::get_msb(crc)); + cmd.push_back(data_utils::get_lsb(crc)); return cmd; } @@ -258,28 +240,22 @@ std::vector DefaultDriver::create_write_command(uint16_t first_register std::vector DefaultDriver::read_response(size_t num_bytes_requested) { std::vector response; - size_t num_bytes_read = port_.read(response, num_bytes_requested); + response.reserve(num_bytes_requested); - if (num_bytes_read != num_bytes_requested) + try { - const auto error_msg = - "Requested " + std::to_string(num_bytes_requested) + " bytes, but only got " + std::to_string(num_bytes_read); - THROW(serial::IOException, error_msg.c_str()); + response = serial_->read(num_bytes_requested); + } + catch (const serial::IOException& e) + { + throw DriverException{ std::string{ "Failed to read reponse: " } + e.what() }; } - return response; } void DefaultDriver::send_command(const std::vector& cmd) { - size_t num_bytes_written = port_.write(cmd); - port_.flush(); - if (num_bytes_written != cmd.size()) - { - const auto error_msg = "Attempted to write " + std::to_string(cmd.size()) + " bytes, but only wrote " + - std::to_string(num_bytes_written); - THROW(serial::IOException, error_msg.c_str()); - } + serial_->write(cmd); } void DefaultDriver::update_status() @@ -287,7 +263,8 @@ void DefaultDriver::update_status() // Tell the gripper that we want to read its status. try { - send_command(read_command_); + const auto read_command = create_read_command(kFirstOutputRegister, kNumOutputRegisters); + send_command(read_command); const auto response = read_response(kReadResponseSize); @@ -334,10 +311,10 @@ void DefaultDriver::update_status() // Read the current gripper position. gripper_position_ = response[kResponseHeaderSize + kPositionIndex]; } - catch (const serial::IOException& e) + catch (const std::exception& e) { std::cerr << "Failed to update gripper status.\n"; - if (port_.isOpen()) + if (serial_->is_open()) { std::cerr << "Error caught while reading or writing to device. Connection is open, " "continuing to attempt " diff --git a/robotiq_driver/src/default_driver_factory.cpp b/robotiq_driver/src/default_driver_factory.cpp index d4d2357..4f2c2a9 100644 --- a/robotiq_driver/src/default_driver_factory.cpp +++ b/robotiq_driver/src/default_driver_factory.cpp @@ -28,6 +28,8 @@ #include #include +#include +#include #include @@ -35,19 +37,41 @@ namespace robotiq_driver { +constexpr auto kSlaveAddressParamName = "slave_address"; +constexpr uint8_t kSlaveAddressParamDefault = 0x09; + +constexpr auto kGripperSpeedMultiplierParamName = "gripper_speed_multiplier"; +constexpr double kGripperSpeedMultiplierParamDefault = 1.0; + +constexpr auto kGripperForceMultiplierParamName = "gripper_force_multiplier"; +constexpr double kGripperForceMultiplierParamDefault = 1.0; const auto kLogger = rclcpp::get_logger("DefaultDriverFactory"); std::unique_ptr DefaultDriverFactory::create(const hardware_interface::HardwareInfo& info) const { - double gripper_speed = stod(info.hardware_parameters.at("gripper_speed_multiplier")); - double gripper_force = stod(info.hardware_parameters.at("gripper_force_multiplier")); + RCLCPP_INFO(kLogger, "Reading %s...", kSlaveAddressParamName); + // Convert base-16 address stored as a string (for example, "0x9") into an integer + const uint8_t slave_address = + info.hardware_parameters.count(kSlaveAddressParamName) ? + static_cast(std::stoul(info.hardware_parameters.at(kSlaveAddressParamName), nullptr, 16)) : + kSlaveAddressParamDefault; + RCLCPP_INFO(kLogger, "%s: %d", kSlaveAddressParamName, slave_address); + + RCLCPP_INFO(kLogger, "Reading %s...", kGripperSpeedMultiplierParamName); + double gripper_speed = info.hardware_parameters.count(kGripperSpeedMultiplierParamName) ? + std::clamp(stod(info.hardware_parameters.at(kGripperSpeedMultiplierParamName)), 0.0, 1.0) : + kGripperSpeedMultiplierParamDefault; + RCLCPP_INFO(kLogger, "%s: %fs", kGripperSpeedMultiplierParamName, gripper_speed); - // Speed and force must lie between 0.0 and 1.0. - gripper_speed = std::min(1.0, std::max(0.0, gripper_speed)); - gripper_force = std::min(1.0, std::max(0.0, gripper_force)); + RCLCPP_INFO(kLogger, "Reading %s...", kGripperForceMultiplierParamName); + double gripper_force = info.hardware_parameters.count(kGripperForceMultiplierParamName) ? + std::clamp(stod(info.hardware_parameters.at(kGripperForceMultiplierParamName)), 0.0, 1.0) : + kGripperForceMultiplierParamDefault; + RCLCPP_INFO(kLogger, "%s: %fs", kGripperForceMultiplierParamName, gripper_force); auto driver = create_driver(info); + driver->set_slave_address(slave_address); driver->set_speed(gripper_speed * 0xFF); driver->set_force(gripper_force * 0xFF); @@ -56,7 +80,7 @@ std::unique_ptr DefaultDriverFactory::create(const hardware_interface::H std::unique_ptr DefaultDriverFactory::create_driver(const hardware_interface::HardwareInfo& info) const { - std::string com_port = info.hardware_parameters.at("COM_port"); - return std::make_unique(com_port); + auto serial = DefaultSerialFactory().create(info); + return std::make_unique(std::move(serial)); } } // namespace robotiq_driver diff --git a/robotiq_driver/src/default_serial.cpp b/robotiq_driver/src/default_serial.cpp new file mode 100644 index 0000000..ba493d6 --- /dev/null +++ b/robotiq_driver/src/default_serial.cpp @@ -0,0 +1,111 @@ +// Copyright (c) 2023 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include + +#include + +namespace robotiq_driver +{ + +DefaultSerial::DefaultSerial() : serial_{ std::make_unique() } +{ +} + +void DefaultSerial::open() +{ + serial_->open(); +} + +bool DefaultSerial::is_open() const +{ + return serial_->isOpen(); +} + +void DefaultSerial::close() +{ + serial_->close(); +} + +std::vector DefaultSerial::read(size_t size) +{ + std::vector data; + size_t bytes_read = serial_->read(data, size); + if (bytes_read != size) + { + const auto error_msg = "Requested " + std::to_string(size) + " bytes, but got " + std::to_string(bytes_read); + THROW(serial::IOException, error_msg.c_str()); + } + return data; +} + +void DefaultSerial::write(const std::vector& data) +{ + std::size_t num_bytes_written = serial_->write(data); + serial_->flush(); + if (num_bytes_written != data.size()) + { + const auto error_msg = + "Attempted to write " + std::to_string(data.size()) + " bytes, but wrote " + std::to_string(num_bytes_written); + THROW(serial::IOException, error_msg.c_str()); + } +} + +void DefaultSerial::set_port(const std::string& port) +{ + serial_->setPort(port); +} + +std::string DefaultSerial::get_port() const +{ + return serial_->getPort(); +} + +void DefaultSerial::set_timeout(std::chrono::milliseconds timeout) +{ + serial::Timeout simple_timeout = serial::Timeout::simpleTimeout(static_cast(timeout.count())); + serial_->setTimeout(simple_timeout); +} + +std::chrono::milliseconds DefaultSerial::get_timeout() const +{ + uint32_t timeout = serial_->getTimeout().read_timeout_constant; + return std::chrono::milliseconds{ timeout }; +} + +void DefaultSerial::set_baudrate(uint32_t baudrate) +{ + serial_->setBaudrate(baudrate); +} + +uint32_t DefaultSerial::get_baudrate() const +{ + return serial_->getBaudrate(); +} + +} // namespace robotiq_driver diff --git a/robotiq_driver/src/default_serial_factory.cpp b/robotiq_driver/src/default_serial_factory.cpp new file mode 100644 index 0000000..598805e --- /dev/null +++ b/robotiq_driver/src/default_serial_factory.cpp @@ -0,0 +1,80 @@ +// Copyright (c) 2023 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include +#include + +#include + +namespace robotiq_driver +{ + +const auto kLogger = rclcpp::get_logger("DefaultSerialFactory"); + +constexpr auto kUsbPortParamName = "COM_port"; +constexpr auto kUsbPortParamDefault = "/dev/ttyUSB0"; + +constexpr auto kBaudrateParamName = "baudrate"; +constexpr auto kBaudrateAddressParamDefault = 115200; + +constexpr auto kTimeoutParamName = "timeout"; +constexpr auto kTimeoutParamDefault = 0.5; + +std::unique_ptr DefaultSerialFactory::create(const hardware_interface::HardwareInfo& info) const +{ + RCLCPP_INFO(kLogger, "Reading %s...", kUsbPortParamName); + std::string usb_port = info.hardware_parameters.count(kUsbPortParamName) ? + info.hardware_parameters.at(kUsbPortParamName) : + kUsbPortParamDefault; + RCLCPP_INFO(kLogger, "%s: %s", kUsbPortParamName, usb_port.c_str()); + + RCLCPP_INFO(kLogger, "Reading %s...", kBaudrateParamName); + uint32_t baudrate = info.hardware_parameters.count(kBaudrateParamName) ? + static_cast(std::stoul(info.hardware_parameters.at(kBaudrateParamName))) : + kBaudrateAddressParamDefault; + RCLCPP_INFO(kLogger, "%s: %dbps", kBaudrateParamName, baudrate); + + RCLCPP_INFO(kLogger, "Reading %s...", kTimeoutParamName); + double timeout = info.hardware_parameters.count(kTimeoutParamName) ? + std::stod(info.hardware_parameters.at(kTimeoutParamName)) : + kTimeoutParamDefault; + RCLCPP_INFO(kLogger, "%s: %fs", kTimeoutParamName, timeout); + + auto serial = create_serial(); + serial->set_port(usb_port); + serial->set_baudrate(baudrate); + serial->set_timeout(std::chrono::duration_cast(std::chrono::duration(timeout))); + return serial; +} + +std::unique_ptr DefaultSerialFactory::create_serial() const +{ + return std::make_unique(); +} + +} // namespace robotiq_driver diff --git a/robotiq_driver/src/gripper_interface_test.cpp b/robotiq_driver/src/gripper_interface_test.cpp index bb19fe6..e666e11 100644 --- a/robotiq_driver/src/gripper_interface_test.cpp +++ b/robotiq_driver/src/gripper_interface_test.cpp @@ -27,76 +27,90 @@ // POSSIBILITY OF SUCH DAMAGE. #include +#include +#include #include #include +#include constexpr auto kComPort = "/dev/ttyUSB0"; -constexpr auto kSlaveID = 0x09; +constexpr auto kBaudRate = 115200; +constexpr auto kTimeout = 0.5; + +constexpr auto kSlaveAddress = 0x09; + +using namespace robotiq_driver; int main() { try { - robotiq_driver::DefaultDriver gripper(kComPort, kSlaveID); + auto serial = std::make_unique(); + serial->set_port(kComPort); + serial->set_baudrate(kBaudRate); + serial->set_timeout(std::chrono::duration_cast(std::chrono::duration(kTimeout))); + + auto driver = std::make_unique(std::move(serial)); + driver->set_slave_address(kSlaveAddress); std::cout << "Deactivating gripper...\n"; - gripper.deactivate(); + driver->deactivate(); std::cout << "Activating gripper...\n"; - gripper.activate(); + driver->activate(); std::cout << "Gripper successfully activated.\n"; std::cout << "Closing gripper...\n"; - gripper.set_gripper_position(0xFF); - while (gripper.gripper_is_moving()) + driver->set_gripper_position(0xFF); + while (driver->gripper_is_moving()) { std::this_thread::sleep_for(std::chrono::milliseconds(500)); } std::cout << "Opening gripper...\n"; - gripper.set_gripper_position(0x00); - while (gripper.gripper_is_moving()) + driver->set_gripper_position(0x00); + while (driver->gripper_is_moving()) { std::this_thread::sleep_for(std::chrono::milliseconds(500)); } std::cout << "Half closing gripper...\n"; - gripper.set_gripper_position(0x80); - while (gripper.gripper_is_moving()) + driver->set_gripper_position(0x80); + while (driver->gripper_is_moving()) { std::this_thread::sleep_for(std::chrono::milliseconds(500)); } std::cout << "Opening gripper...\n"; - gripper.set_gripper_position(0x00); - while (gripper.gripper_is_moving()) + driver->set_gripper_position(0x00); + while (driver->gripper_is_moving()) { std::this_thread::sleep_for(std::chrono::milliseconds(500)); } std::cout << "Decreasing gripper speed...\n"; - gripper.set_speed(0x0F); + driver->set_speed(0x0F); std::cout << "Closing gripper...\n"; - gripper.set_gripper_position(0xFF); - while (gripper.gripper_is_moving()) + driver->set_gripper_position(0xFF); + while (driver->gripper_is_moving()) { std::this_thread::sleep_for(std::chrono::milliseconds(500)); } std::cout << "Increasing gripper speed...\n"; - gripper.set_speed(0xFF); + driver->set_speed(0xFF); std::cout << "Opening gripper...\n"; - gripper.set_gripper_position(0x00); - while (gripper.gripper_is_moving()) + driver->set_gripper_position(0x00); + while (driver->gripper_is_moving()) { std::this_thread::sleep_for(std::chrono::milliseconds(500)); } } - catch (const serial::IOException& e) + catch (...) { std::cout << "Failed to communicating with the Gripper. Please check the Gripper connection"; return 1; diff --git a/robotiq_driver/src/hardware_interface.cpp b/robotiq_driver/src/hardware_interface.cpp index 772f6ee..be1eac9 100644 --- a/robotiq_driver/src/hardware_interface.cpp +++ b/robotiq_driver/src/hardware_interface.cpp @@ -122,7 +122,7 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(cons } catch (const serial::IOException& e) { - RCLCPP_FATAL(kLogger, "Failed to open gripper port."); + RCLCPP_FATAL(kLogger, "Failed to create a driver: %s", e.what()); return CallbackReturn::ERROR; } diff --git a/robotiq_driver/tests/CMakeLists.txt b/robotiq_driver/tests/CMakeLists.txt index e2181dc..f80e3c4 100644 --- a/robotiq_driver/tests/CMakeLists.txt +++ b/robotiq_driver/tests/CMakeLists.txt @@ -21,3 +21,27 @@ target_include_directories(test_robotiq_gripper_hardware_interface PRIVATE ${CMAKE_CURRENT_SOURCE_DIR} ) target_link_libraries(test_robotiq_gripper_hardware_interface robotiq_driver) + +############################################################################### +# test_default_serial_factory + +ament_add_gmock(test_default_serial_factory + test_default_serial_factory.cpp + mock/mock_serial.hpp +) +target_include_directories(test_default_serial_factory + PRIVATE ${CMAKE_CURRENT_SOURCE_DIR} +) +target_link_libraries(test_default_serial_factory robotiq_driver) + +############################################################################### +# test_default_driver_factory + +ament_add_gmock(test_default_driver_factory + test_default_driver_factory.cpp + mock/mock_driver.hpp +) +target_include_directories(test_default_driver_factory + PRIVATE ${CMAKE_CURRENT_SOURCE_DIR} +) +target_link_libraries(test_default_driver_factory robotiq_driver) diff --git a/robotiq_driver/tests/mock/mock_driver.hpp b/robotiq_driver/tests/mock/mock_driver.hpp new file mode 100644 index 0000000..db64935 --- /dev/null +++ b/robotiq_driver/tests/mock/mock_driver.hpp @@ -0,0 +1,53 @@ +// Copyright (c) 2022 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#pragma once + +#include + +#include + +#include + +namespace robotiq_driver::test +{ +class MockDriver : public robotiq_driver::Driver +{ +public: + MOCK_METHOD(bool, connect, (), (override)); + MOCK_METHOD(void, disconnect, (), (override)); + MOCK_METHOD(void, activate, (), (override)); + MOCK_METHOD(void, deactivate, (), (override)); + MOCK_METHOD(void, set_slave_address, (uint8_t slave_address), (override)); + MOCK_METHOD(void, set_gripper_position, (uint8_t position), (override)); + MOCK_METHOD(uint8_t, get_gripper_position, (), (override)); + MOCK_METHOD(bool, gripper_is_moving, (), (override)); + MOCK_METHOD(void, set_speed, (uint8_t speed), (override)); + MOCK_METHOD(void, set_force, (uint8_t force), (override)); +}; +} // namespace robotiq_driver::test diff --git a/robotiq_driver/tests/mock/mock_serial.hpp b/robotiq_driver/tests/mock/mock_serial.hpp new file mode 100644 index 0000000..6b1dcff --- /dev/null +++ b/robotiq_driver/tests/mock/mock_serial.hpp @@ -0,0 +1,53 @@ +/* + * Copyright (c) 2022, Giovanni Remigi + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#pragma once + +#include "robotiq_driver/serial.hpp" + +#include + +namespace robotiq_driver::test +{ +class MockSerial : public robotiq_driver::Serial +{ +public: + MOCK_METHOD(void, open, (), (override)); + MOCK_METHOD(bool, is_open, (), (override, const)); + MOCK_METHOD(void, close, (), (override)); + MOCK_METHOD(std::vector, read, (size_t size), (override)); + MOCK_METHOD(void, write, (const std::vector& buffer), (override)); + MOCK_METHOD(void, set_port, (const std::string& port), (override)); + MOCK_METHOD(std::string, get_port, (), (override, const)); + MOCK_METHOD(void, set_timeout, (std::chrono::milliseconds timeout), (override)); + MOCK_METHOD(std::chrono::milliseconds, get_timeout, (), (override, const)); + MOCK_METHOD(void, set_baudrate, (uint32_t baudrate), (override)); + MOCK_METHOD(uint32_t, get_baudrate, (), (override, const)); +}; +} // namespace robotiq_driver::test diff --git a/robotiq_driver/tests/test_default_driver_factory.cpp b/robotiq_driver/tests/test_default_driver_factory.cpp new file mode 100644 index 0000000..78c1923 --- /dev/null +++ b/robotiq_driver/tests/test_default_driver_factory.cpp @@ -0,0 +1,109 @@ +/* + * Copyright (c) 2022, Giovanni Remigi + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#include + +#include + +#include + +#include + +namespace robotiq_driver::test +{ +// This factory will populate the injected mock with data read form the HardwareInfo. +class TestDriverFactory : public DefaultDriverFactory +{ +public: + explicit TestDriverFactory(std::unique_ptr driver) : driver_{ std::move(driver) } + { + } + +protected: + std::unique_ptr create_driver([[maybe_unused]] const hardware_interface::HardwareInfo& info) const override + { + return std::move(driver_); + } + +private: + mutable std::unique_ptr driver_; +}; + +/** + * Here we test the driver factory with default parameters. + */ +TEST(TestDefaultDriverFactory, create_with_default_parameters) +{ + hardware_interface::HardwareInfo info; + + auto driver = std::make_unique(); + + // This line is only required when running the test inside the IDE. + testing::Mock::AllowLeak(driver.get()); + + EXPECT_CALL(*driver, set_slave_address(0x9)); + EXPECT_CALL(*driver, set_speed(0xFF)); + EXPECT_CALL(*driver, set_force(0xFF)); + EXPECT_CALL(*driver, connect()).Times(0); + EXPECT_CALL(*driver, disconnect()).Times(0); + EXPECT_CALL(*driver, activate()).Times(0); + EXPECT_CALL(*driver, deactivate()).Times(0); + + TestDriverFactory driver_factory{ std::move(driver) }; + auto created_driver = driver_factory.create(info); +} + +/** + * Here we test the driver factory with given parameters. + */ +TEST(TestDefaultDriverFactory, create_with_given_parameters) +{ + hardware_interface::HardwareInfo info; + + info.hardware_parameters.emplace("slave_address", "1"); + info.hardware_parameters.emplace("gripper_speed_multiplier", "0.5"); + info.hardware_parameters.emplace("gripper_force_multiplier", "0.5"); + + auto driver = std::make_unique(); + + // This line is only required when running the test inside the IDE. + testing::Mock::AllowLeak(driver.get()); + + EXPECT_CALL(*driver, set_slave_address(0x1)); + EXPECT_CALL(*driver, set_speed(0x7F)); + EXPECT_CALL(*driver, set_force(0x7F)); + EXPECT_CALL(*driver, connect()).Times(0); + EXPECT_CALL(*driver, disconnect()).Times(0); + EXPECT_CALL(*driver, activate()).Times(0); + EXPECT_CALL(*driver, deactivate()).Times(0); + + TestDriverFactory driver_factory{ std::move(driver) }; + auto created_driver = driver_factory.create(info); +} +} // namespace robotiq_driver::test diff --git a/robotiq_driver/tests/test_default_serial_factory.cpp b/robotiq_driver/tests/test_default_serial_factory.cpp new file mode 100644 index 0000000..f7c7d5a --- /dev/null +++ b/robotiq_driver/tests/test_default_serial_factory.cpp @@ -0,0 +1,100 @@ +/* + * Copyright (c) 2022, Giovanni Remigi + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#include + +#include + +#include + +#include + +namespace robotiq_driver::test +{ +// This factory will populate the injected mock with data read form the HardwareInfo. +class StubSerialFactory : public DefaultSerialFactory +{ +public: + explicit StubSerialFactory(std::unique_ptr serial) : serial_{ std::move(serial) } + { + } + +protected: + std::unique_ptr create_serial() const override + { + return std::move(serial_); + } + +private: + mutable std::unique_ptr serial_; +}; + +/** + * Here we test the serial factory with default parameters. + */ +TEST(TestDefaultSerialFactory, create_with_default_parameters) +{ + hardware_interface::HardwareInfo info; + + auto serial = std::make_unique(); + + // This line is only required when running the test inside the IDE. + testing::Mock::AllowLeak(serial.get()); + + EXPECT_CALL(*serial, set_port("/dev/ttyUSB0")); + EXPECT_CALL(*serial, set_baudrate(115200)); + EXPECT_CALL(*serial, set_timeout(std::chrono::milliseconds{ 500 })); + + StubSerialFactory serial_factory(std::move(serial)); + auto created_serial = serial_factory.create(info); +} + +/** + * Here we test the serial factory with default parameters. + */ +TEST(TestDefaultSerialFactory, create_with_given_parameters) +{ + hardware_interface::HardwareInfo info; + info.hardware_parameters.emplace("COM_port", "/dev/ttyUSB1"); + info.hardware_parameters.emplace("baudrate", "9600"); + info.hardware_parameters.emplace("timeout", "0.1"); + + auto serial = std::make_unique(); + + // This line is only required when running the test inside the IDE. + testing::Mock::AllowLeak(serial.get()); + + EXPECT_CALL(*serial, set_port("/dev/ttyUSB1")); + EXPECT_CALL(*serial, set_baudrate(9600)); + EXPECT_CALL(*serial, set_timeout(std::chrono::milliseconds(100))); + + StubSerialFactory serial_factory(std::move(serial)); + auto created_serial = serial_factory.create(info); +} +} // namespace robotiq_driver::test From c1dc4593a644b41900faf1386f40865fd8689191 Mon Sep 17 00:00:00 2001 From: Giovanni Remigi Date: Wed, 6 Sep 2023 11:50:22 +0200 Subject: [PATCH 17/47] Pre-commits. --- robotiq_driver/src/default_driver.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/robotiq_driver/src/default_driver.cpp b/robotiq_driver/src/default_driver.cpp index 3afc4fa..3ca296c 100644 --- a/robotiq_driver/src/default_driver.cpp +++ b/robotiq_driver/src/default_driver.cpp @@ -96,7 +96,7 @@ void DefaultDriver::activate() { RCLCPP_INFO(kLogger, "Activate..."); - // set rACT to 1, clear all ther registers. + // set rACT to 1, clear all other registers. const auto cmd = create_write_command(kActionRequestRegister, { 0x0100, 0x0000, 0x0000 }); try @@ -125,7 +125,7 @@ void DefaultDriver::activate() void DefaultDriver::deactivate() { - RCLCPP_INFO(kLogger, "Dectivate..."); + RCLCPP_INFO(kLogger, "Deactivate..."); const auto cmd = create_write_command(kActionRequestRegister, { 0x0000, 0x0000, 0x0000 }); try @@ -248,7 +248,7 @@ std::vector DefaultDriver::read_response(size_t num_bytes_requested) } catch (const serial::IOException& e) { - throw DriverException{ std::string{ "Failed to read reponse: " } + e.what() }; + throw DriverException{ std::string{ "Failed to read response: " } + e.what() }; } return response; } From 4c3eafcfaf93925aaa61e77c43a7b8d0a085f213 Mon Sep 17 00:00:00 2001 From: Giovanni Remigi Date: Wed, 6 Sep 2023 11:53:48 +0200 Subject: [PATCH 18/47] Added copyright. --- .../robotiq_driver/driver_exception.hpp | 55 +++++++++---------- robotiq_driver/tests/mock/mock_serial.hpp | 55 +++++++++---------- .../tests/test_default_driver_factory.cpp | 55 +++++++++---------- .../tests/test_default_serial_factory.cpp | 55 +++++++++---------- 4 files changed, 108 insertions(+), 112 deletions(-) diff --git a/robotiq_driver/include/robotiq_driver/driver_exception.hpp b/robotiq_driver/include/robotiq_driver/driver_exception.hpp index 58273a1..291c544 100644 --- a/robotiq_driver/include/robotiq_driver/driver_exception.hpp +++ b/robotiq_driver/include/robotiq_driver/driver_exception.hpp @@ -1,31 +1,30 @@ -/* - * Copyright (c) 2022, Giovanni Remigi - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - */ +// Copyright (c) 2023 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. #pragma once diff --git a/robotiq_driver/tests/mock/mock_serial.hpp b/robotiq_driver/tests/mock/mock_serial.hpp index 6b1dcff..766fea1 100644 --- a/robotiq_driver/tests/mock/mock_serial.hpp +++ b/robotiq_driver/tests/mock/mock_serial.hpp @@ -1,31 +1,30 @@ -/* - * Copyright (c) 2022, Giovanni Remigi - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - */ +// Copyright (c) 2023 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. #pragma once diff --git a/robotiq_driver/tests/test_default_driver_factory.cpp b/robotiq_driver/tests/test_default_driver_factory.cpp index 78c1923..b0a9913 100644 --- a/robotiq_driver/tests/test_default_driver_factory.cpp +++ b/robotiq_driver/tests/test_default_driver_factory.cpp @@ -1,31 +1,30 @@ -/* - * Copyright (c) 2022, Giovanni Remigi - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - */ +// Copyright (c) 2023 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. #include diff --git a/robotiq_driver/tests/test_default_serial_factory.cpp b/robotiq_driver/tests/test_default_serial_factory.cpp index f7c7d5a..2a34257 100644 --- a/robotiq_driver/tests/test_default_serial_factory.cpp +++ b/robotiq_driver/tests/test_default_serial_factory.cpp @@ -1,31 +1,30 @@ -/* - * Copyright (c) 2022, Giovanni Remigi - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - */ +// Copyright (c) 2023 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. #include From e1089ad10252cc36e195253dc4c2779f00f6fe88 Mon Sep 17 00:00:00 2001 From: Giovanni Remigi Date: Wed, 6 Sep 2023 11:57:56 +0200 Subject: [PATCH 19/47] Increased max line size to 120. --- .github/workflows/ci-ros-lint.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index e2eb79b..29bafaf 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -21,7 +21,7 @@ jobs: robotiq_controllers robotiq_description - ament_lint_100: + ament_lint_120: name: ament_${{ matrix.linter }} runs-on: ubuntu-20.04 strategy: @@ -35,7 +35,7 @@ jobs: with: distribution: rolling linter: cpplint - arguments: "--linelength=100 --filter=-whitespace/newline" + arguments: "--linelength=120 --filter=-whitespace/newline" package-name: robotiq_driver robotiq_controllers robotiq_description From d283f18d1ac12dbac54201b91911c2dd4d5d0e0c Mon Sep 17 00:00:00 2001 From: Giovanni Remigi Date: Wed, 6 Sep 2023 12:10:26 +0200 Subject: [PATCH 20/47] Fixed ROS lints. --- .github/workflows/ci-ros-lint.yml | 4 ++-- .../include/robotiq_driver/default_driver.hpp | 7 ++++--- .../robotiq_driver/default_driver_factory.hpp | 4 ++-- .../include/robotiq_driver/default_serial.hpp | 7 +++++-- .../robotiq_driver/default_serial_factory.hpp | 4 ++-- .../include/robotiq_driver/driver_factory.hpp | 4 +++- .../robotiq_driver/hardware_interface.hpp | 12 +++++------ .../include/robotiq_driver/serial_factory.hpp | 4 ++-- .../robotiq_driver/visibility_control.hpp | 6 +++--- robotiq_driver/src/crc_utils.cpp | 4 ++-- robotiq_driver/src/data_utils.cpp | 4 ++-- robotiq_driver/src/default_driver.cpp | 4 +--- robotiq_driver/src/default_driver_factory.cpp | 4 ++-- robotiq_driver/src/default_serial.cpp | 4 ++-- robotiq_driver/src/gripper_interface_test.cpp | 9 +++++---- robotiq_driver/src/hardware_interface.cpp | 20 +++++++++---------- robotiq_driver/tests/mock/mock_driver.hpp | 4 ++-- robotiq_driver/tests/mock/mock_serial.hpp | 7 +++++-- ...est_robotiq_gripper_hardware_interface.cpp | 4 ++-- 19 files changed, 61 insertions(+), 55 deletions(-) diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index 29bafaf..30d2d6d 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -21,7 +21,7 @@ jobs: robotiq_controllers robotiq_description - ament_lint_120: + ament_lint_121: name: ament_${{ matrix.linter }} runs-on: ubuntu-20.04 strategy: @@ -35,7 +35,7 @@ jobs: with: distribution: rolling linter: cpplint - arguments: "--linelength=120 --filter=-whitespace/newline" + arguments: "--linelength=121 --filter=-whitespace/newline" package-name: robotiq_driver robotiq_controllers robotiq_description diff --git a/robotiq_driver/include/robotiq_driver/default_driver.hpp b/robotiq_driver/include/robotiq_driver/default_driver.hpp index c63ec7d..57461f4 100644 --- a/robotiq_driver/include/robotiq_driver/default_driver.hpp +++ b/robotiq_driver/include/robotiq_driver/default_driver.hpp @@ -28,12 +28,13 @@ #pragma once -#include -#include - #include #include #include + +#include +#include + /** * @brief This class is responsible for communicating with the gripper via a serial port, and maintaining a record of * the gripper's current state. diff --git a/robotiq_driver/include/robotiq_driver/default_driver_factory.hpp b/robotiq_driver/include/robotiq_driver/default_driver_factory.hpp index e267b0b..142ce80 100644 --- a/robotiq_driver/include/robotiq_driver/default_driver_factory.hpp +++ b/robotiq_driver/include/robotiq_driver/default_driver_factory.hpp @@ -28,13 +28,13 @@ #pragma once +#include + #include #include #include -#include - namespace robotiq_driver { /** diff --git a/robotiq_driver/include/robotiq_driver/default_serial.hpp b/robotiq_driver/include/robotiq_driver/default_serial.hpp index d3cece6..9f60022 100644 --- a/robotiq_driver/include/robotiq_driver/default_serial.hpp +++ b/robotiq_driver/include/robotiq_driver/default_serial.hpp @@ -28,10 +28,13 @@ #pragma once -#include +#include + #include +#include +#include -#include +#include namespace serial { diff --git a/robotiq_driver/include/robotiq_driver/default_serial_factory.hpp b/robotiq_driver/include/robotiq_driver/default_serial_factory.hpp index 7e7465e..e635674 100644 --- a/robotiq_driver/include/robotiq_driver/default_serial_factory.hpp +++ b/robotiq_driver/include/robotiq_driver/default_serial_factory.hpp @@ -28,12 +28,12 @@ #pragma once +#include + #include #include #include -#include - namespace robotiq_driver { /** diff --git a/robotiq_driver/include/robotiq_driver/driver_factory.hpp b/robotiq_driver/include/robotiq_driver/driver_factory.hpp index 6584e4c..d06d96c 100644 --- a/robotiq_driver/include/robotiq_driver/driver_factory.hpp +++ b/robotiq_driver/include/robotiq_driver/driver_factory.hpp @@ -28,8 +28,10 @@ #pragma once -#include #include + +#include + #include namespace robotiq_driver diff --git a/robotiq_driver/include/robotiq_driver/hardware_interface.hpp b/robotiq_driver/include/robotiq_driver/hardware_interface.hpp index 1a7fc9c..b2ef65e 100644 --- a/robotiq_driver/include/robotiq_driver/hardware_interface.hpp +++ b/robotiq_driver/include/robotiq_driver/hardware_interface.hpp @@ -28,6 +28,12 @@ #pragma once +#include +#include +#include +#include +#include + #include #include @@ -41,12 +47,6 @@ #include #include -#include -#include -#include -#include -#include - namespace robotiq_driver { class RobotiqGripperHardwareInterface : public hardware_interface::SystemInterface diff --git a/robotiq_driver/include/robotiq_driver/serial_factory.hpp b/robotiq_driver/include/robotiq_driver/serial_factory.hpp index bc14919..8236215 100644 --- a/robotiq_driver/include/robotiq_driver/serial_factory.hpp +++ b/robotiq_driver/include/robotiq_driver/serial_factory.hpp @@ -28,12 +28,12 @@ #pragma once +#include + #include #include -#include - namespace robotiq_driver { /** diff --git a/robotiq_driver/include/robotiq_driver/visibility_control.hpp b/robotiq_driver/include/robotiq_driver/visibility_control.hpp index df5a56e..19918d3 100644 --- a/robotiq_driver/include/robotiq_driver/visibility_control.hpp +++ b/robotiq_driver/include/robotiq_driver/visibility_control.hpp @@ -19,8 +19,8 @@ * library cannot have, but the consuming code must have inorder to link. */ -#ifndef ROBOTIQ_DRIVER__VISIBILITY_CONTROL_H_ -#define ROBOTIQ_DRIVER__VISIBILITY_CONTROL_H_ +#ifndef ROBOTIQ_DRIVER__VISIBILITY_CONTROL_HPP_ +#define ROBOTIQ_DRIVER__VISIBILITY_CONTROL_HPP_ // This logic was borrowed (then namespaced) from the examples on the gcc wiki: // https://gcc.gnu.org/wiki/Visibility @@ -53,4 +53,4 @@ #define ROBOTIQ_DRIVER_PUBLIC_TYPE #endif -#endif // ROBOTIQ_DRIVER__VISIBILITY_CONTROL_H_ +#endif // ROBOTIQ_DRIVER__VISIBILITY_CONTROL_HPP_ diff --git a/robotiq_driver/src/crc_utils.cpp b/robotiq_driver/src/crc_utils.cpp index 08cc447..8cba7f8 100644 --- a/robotiq_driver/src/crc_utils.cpp +++ b/robotiq_driver/src/crc_utils.cpp @@ -26,10 +26,10 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. -#include - #include +#include + namespace robotiq_driver::crc_utils { /** diff --git a/robotiq_driver/src/data_utils.cpp b/robotiq_driver/src/data_utils.cpp index 55b6bff..b6c1770 100644 --- a/robotiq_driver/src/data_utils.cpp +++ b/robotiq_driver/src/data_utils.cpp @@ -26,12 +26,12 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. -#include - #include #include #include +#include + namespace robotiq_driver::data_utils { constexpr std::array vChars = { diff --git a/robotiq_driver/src/default_driver.cpp b/robotiq_driver/src/default_driver.cpp index 3ca296c..5fc132f 100644 --- a/robotiq_driver/src/default_driver.cpp +++ b/robotiq_driver/src/default_driver.cpp @@ -31,8 +31,6 @@ #include #include -#include - #include "robotiq_driver/data_utils.hpp" #include "robotiq_driver/default_driver.hpp" #include @@ -246,7 +244,7 @@ std::vector DefaultDriver::read_response(size_t num_bytes_requested) { response = serial_->read(num_bytes_requested); } - catch (const serial::IOException& e) + catch (const std::exception& e) { throw DriverException{ std::string{ "Failed to read response: " } + e.what() }; } diff --git a/robotiq_driver/src/default_driver_factory.cpp b/robotiq_driver/src/default_driver_factory.cpp index 4f2c2a9..3add121 100644 --- a/robotiq_driver/src/default_driver_factory.cpp +++ b/robotiq_driver/src/default_driver_factory.cpp @@ -26,6 +26,8 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. +#include + #include #include #include @@ -33,8 +35,6 @@ #include -#include - namespace robotiq_driver { constexpr auto kSlaveAddressParamName = "slave_address"; diff --git a/robotiq_driver/src/default_serial.cpp b/robotiq_driver/src/default_serial.cpp index ba493d6..0f3e976 100644 --- a/robotiq_driver/src/default_serial.cpp +++ b/robotiq_driver/src/default_serial.cpp @@ -26,10 +26,10 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. -#include - #include +#include + namespace robotiq_driver { diff --git a/robotiq_driver/src/gripper_interface_test.cpp b/robotiq_driver/src/gripper_interface_test.cpp index e666e11..60f9cce 100644 --- a/robotiq_driver/src/gripper_interface_test.cpp +++ b/robotiq_driver/src/gripper_interface_test.cpp @@ -26,21 +26,22 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. -#include -#include - #include #include #include #include +#include +#include + constexpr auto kComPort = "/dev/ttyUSB0"; constexpr auto kBaudRate = 115200; constexpr auto kTimeout = 0.5; constexpr auto kSlaveAddress = 0x09; -using namespace robotiq_driver; +using robotiq_driver::DefaultDriver; +using robotiq_driver::DefaultSerial; int main() { diff --git a/robotiq_driver/src/hardware_interface.cpp b/robotiq_driver/src/hardware_interface.cpp index be1eac9..d058495 100644 --- a/robotiq_driver/src/hardware_interface.cpp +++ b/robotiq_driver/src/hardware_interface.cpp @@ -26,22 +26,20 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. +#include +#include +#include +#include +#include + #include #include #include #include -#include - #include -#include -#include -#include -#include -#include - constexpr uint8_t kGripperMinPos = 3; constexpr uint8_t kGripperMaxPos = 230; constexpr uint8_t kGripperRange = kGripperMaxPos - kGripperMinPos; @@ -120,7 +118,7 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(cons { driver_ = driver_factory_->create(info_); } - catch (const serial::IOException& e) + catch (const std::exception& e) { RCLCPP_FATAL(kLogger, "Failed to create a driver: %s", e.what()); return CallbackReturn::ERROR; @@ -205,7 +203,7 @@ RobotiqGripperHardwareInterface::on_activate(const rclcpp_lifecycle::State& /*pr driver_->deactivate(); driver_->activate(); } - catch (const serial::IOException& e) + catch (const std::exception& e) { RCLCPP_FATAL(kLogger, "Failed to communicate with Gripper. Check Gripper connection."); return CallbackReturn::ERROR; @@ -245,7 +243,7 @@ RobotiqGripperHardwareInterface::on_activate(const rclcpp_lifecycle::State& /*pr last_io = now; } - catch (serial::IOException& e) + catch (std::exception& e) { RCLCPP_ERROR(kLogger, "Check Robotiq Gripper connection and restart drivers. ERROR: %s", e.what()); communication_thread_is_running_.store(false); diff --git a/robotiq_driver/tests/mock/mock_driver.hpp b/robotiq_driver/tests/mock/mock_driver.hpp index db64935..41f730a 100644 --- a/robotiq_driver/tests/mock/mock_driver.hpp +++ b/robotiq_driver/tests/mock/mock_driver.hpp @@ -28,10 +28,10 @@ #pragma once -#include - #include +#include + #include namespace robotiq_driver::test diff --git a/robotiq_driver/tests/mock/mock_serial.hpp b/robotiq_driver/tests/mock/mock_serial.hpp index 766fea1..b7b8980 100644 --- a/robotiq_driver/tests/mock/mock_serial.hpp +++ b/robotiq_driver/tests/mock/mock_serial.hpp @@ -28,10 +28,13 @@ #pragma once -#include "robotiq_driver/serial.hpp" - #include +#include +#include + +#include "robotiq_driver/serial.hpp" + namespace robotiq_driver::test { class MockSerial : public robotiq_driver::Serial diff --git a/robotiq_driver/tests/test_robotiq_gripper_hardware_interface.cpp b/robotiq_driver/tests/test_robotiq_gripper_hardware_interface.cpp index c0d697d..7b058fd 100644 --- a/robotiq_driver/tests/test_robotiq_gripper_hardware_interface.cpp +++ b/robotiq_driver/tests/test_robotiq_gripper_hardware_interface.cpp @@ -29,6 +29,8 @@ #include #include +#include + #include #include @@ -43,8 +45,6 @@ #include #include -#include - namespace robotiq_driver::test { From 2239dd97b1bdc8773757f1b904a576e2f574c1f2 Mon Sep 17 00:00:00 2001 From: Giovanni Remigi Date: Wed, 6 Sep 2023 12:19:04 +0200 Subject: [PATCH 21/47] Try to exclude tests from linting. --- .github/workflows/ci-ros-lint.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index 30d2d6d..4c8a80a 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -17,6 +17,7 @@ jobs: with: distribution: rolling linter: ${{ matrix.linter }} + arguments: "--exclude=tests/" package-name: robotiq_driver robotiq_controllers robotiq_description From 011df4b48e9faf54d902815dddb39417aa879307 Mon Sep 17 00:00:00 2001 From: Giovanni Remigi Date: Wed, 6 Sep 2023 12:22:02 +0200 Subject: [PATCH 22/47] Removed cppcheck linter because it has problem recognizing macros in GTests. --- .github/workflows/ci-ros-lint.yml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index 4c8a80a..84f85ef 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -9,7 +9,7 @@ jobs: strategy: fail-fast: false matrix: - linter: [cppcheck, copyright, lint_cmake] + linter: [copyright, lint_cmake] steps: - uses: actions/checkout@v3 - uses: ros-tooling/setup-ros@0.6.2 @@ -17,7 +17,6 @@ jobs: with: distribution: rolling linter: ${{ matrix.linter }} - arguments: "--exclude=tests/" package-name: robotiq_driver robotiq_controllers robotiq_description From 7414c37db2c70a388b542ecda94951151e5e2cb8 Mon Sep 17 00:00:00 2001 From: Giovanni Remigi Date: Wed, 6 Sep 2023 12:28:11 +0200 Subject: [PATCH 23/47] Trying to add ros2_control_test_assets to test CMakeLists.txt --- robotiq_driver/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/robotiq_driver/CMakeLists.txt b/robotiq_driver/CMakeLists.txt index c6f20c2..c89e37d 100644 --- a/robotiq_driver/CMakeLists.txt +++ b/robotiq_driver/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(serial REQUIRED) +find_package(ros2_control_test_assets) set(THIS_PACKAGE_INCLUDE_DEPENDS ament_cmake From 56ddcd4c35d7ac5ac85972730e2525e0e4b4a35c Mon Sep 17 00:00:00 2001 From: Giovanni Remigi Date: Wed, 6 Sep 2023 12:38:17 +0200 Subject: [PATCH 24/47] Tryint to build everything on Ubuntu 22.04. --- .github/workflows/ci-format.yml | 20 ++++++------- .github/workflows/ci-ros-lint.yml | 4 +-- .github/workflows/prerelease-check.yml | 30 +++++++++---------- .../reusable-industrial-ci-with-cache.yml | 27 ++++++++--------- 4 files changed, 40 insertions(+), 41 deletions(-) diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index 3d60336..797a757 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -13,14 +13,14 @@ on: jobs: pre-commit: name: Format - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - - uses: actions/checkout@v3 - - uses: actions/setup-python@v4.4.0 - with: - python-version: '3.10' - - name: Install system hooks - run: sudo apt install -qq clang-format-14 cppcheck - - uses: pre-commit/action@v3.0.0 - with: - extra_args: --all-files --hook-stage manual + - uses: actions/checkout@v3 + - uses: actions/setup-python@v4.4.0 + with: + python-version: "3.10" + - name: Install system hooks + run: sudo apt install -qq clang-format-14 cppcheck + - uses: pre-commit/action@v3.0.0 + with: + extra_args: --all-files --hook-stage manual diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index 84f85ef..d038c28 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -5,7 +5,7 @@ on: jobs: ament_lint: name: ament_${{ matrix.linter }} - runs-on: ubuntu-20.04 + runs-on: ubuntu-22.04 strategy: fail-fast: false matrix: @@ -23,7 +23,7 @@ jobs: ament_lint_121: name: ament_${{ matrix.linter }} - runs-on: ubuntu-20.04 + runs-on: ubuntu-22.04 strategy: fail-fast: false matrix: diff --git a/.github/workflows/prerelease-check.yml b/.github/workflows/prerelease-check.yml index 9052985..82695cf 100644 --- a/.github/workflows/prerelease-check.yml +++ b/.github/workflows/prerelease-check.yml @@ -4,31 +4,31 @@ on: workflow_dispatch: inputs: ros_distro: - description: 'Chose ROS distribution' + description: "Chose ROS distribution" required: true - default: 'rolling' + default: "rolling" type: choice options: - - foxy - - galactic - - humble - - iron - - rolling + - foxy + - galactic + - humble + - iron + - rolling branch: - description: 'Chose branch for distro' + description: "Chose branch for distro" required: true - default: 'master' + default: "master" type: choice options: - - foxy - - galactic - - humble - - iron - - master + - foxy + - galactic + - humble + - iron + - master jobs: pre_release: - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - uses: actions/checkout@v3 with: diff --git a/.github/workflows/reusable-industrial-ci-with-cache.yml b/.github/workflows/reusable-industrial-ci-with-cache.yml index 490b680..905a4bf 100644 --- a/.github/workflows/reusable-industrial-ci-with-cache.yml +++ b/.github/workflows/reusable-industrial-ci-with-cache.yml @@ -6,51 +6,50 @@ on: workflow_call: inputs: ref_for_scheduled_build: - description: 'Reference on which the repo should be checkout for scheduled build. Usually is this name of a branch or a tag.' - default: '' + description: "Reference on which the repo should be checkout for scheduled build. Usually is this name of a branch or a tag." + default: "" required: false type: string upstream_workspace: - description: 'UPSTREAM_WORKSPACE variable for industrial_ci. Usually path to local .repos file.' + description: "UPSTREAM_WORKSPACE variable for industrial_ci. Usually path to local .repos file." required: true type: string ros_distro: - description: 'ROS_DISTRO variable for industrial_ci' + description: "ROS_DISTRO variable for industrial_ci" required: true type: string ros_repo: description: 'ROS_REPO to run for industrial_ci. Possible values: "main", "testing"' - default: 'main' + default: "main" required: false type: string os_code_name: - description: 'OS_CODE_NAME variable for industrial_ci' - default: '' + description: "OS_CODE_NAME variable for industrial_ci" + default: "" required: false type: string before_install_upstream_dependencies: - description: 'BEFORE_INSTALL_UPSTREAM_DEPENDENCIES variable for industrial_ci' - default: '' + description: "BEFORE_INSTALL_UPSTREAM_DEPENDENCIES variable for industrial_ci" + default: "" required: false type: string ccache_dir: description: 'Local path to store cache (from "github.workspace"). For standard industrial_ci configuration do not have to be changed' - default: '.ccache' + default: ".ccache" required: false type: string basedir: description: 'Local path to workspace base directory to cache (from "github.workspace"). For standard industrial_ci configuration do not have to be changed' - default: '.work' + default: ".work" required: false type: string - jobs: reusable_industrial_ci_with_cache: name: ${{ inputs.ros_distro }} ${{ inputs.ros_repo }} ${{ inputs.os_code_name }} - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 env: CCACHE_DIR: ${{ github.workspace }}/${{ inputs.ccache_dir }} BASEDIR: ${{ github.workspace }}/${{ inputs.basedir }} @@ -80,7 +79,7 @@ jobs: restore-keys: | ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }} ccache-${{ env.CACHE_PREFIX }} - - uses: 'ros-industrial/industrial_ci@master' + - uses: "ros-industrial/industrial_ci@master" env: UPSTREAM_WORKSPACE: ${{ inputs.upstream_workspace }} ROS_DISTRO: ${{ inputs.ros_distro }} From 51e98f8f225f16d7012761f941b533a496c5b9f3 Mon Sep 17 00:00:00 2001 From: Giovanni Remigi Date: Wed, 6 Sep 2023 22:25:31 +0200 Subject: [PATCH 25/47] Moved gripper_interface_test command into its own package. --- robotiq_driver/CMakeLists.txt | 24 +++-- robotiq_hardware_tests/CMakeLists.txt | 39 +++++++++ robotiq_hardware_tests/package.xml | 33 +++++++ .../src/command_line_utility.cpp | 87 +++++++++++++++++++ .../src/command_line_utility.hpp | 73 ++++++++++++++++ .../src/gripper_interface_test.cpp | 3 +- 6 files changed, 243 insertions(+), 16 deletions(-) create mode 100644 robotiq_hardware_tests/CMakeLists.txt create mode 100644 robotiq_hardware_tests/package.xml create mode 100644 robotiq_hardware_tests/src/command_line_utility.cpp create mode 100644 robotiq_hardware_tests/src/command_line_utility.hpp rename {robotiq_driver => robotiq_hardware_tests}/src/gripper_interface_test.cpp (99%) diff --git a/robotiq_driver/CMakeLists.txt b/robotiq_driver/CMakeLists.txt index c89e37d..36aaa37 100644 --- a/robotiq_driver/CMakeLists.txt +++ b/robotiq_driver/CMakeLists.txt @@ -1,4 +1,6 @@ -cmake_minimum_required(VERSION 3.22.1) +# See https://docs.ros.org/en/humble/How-To-Guides/Ament-CMake-Documentation.html + +cmake_minimum_required(VERSION 3.8) project(robotiq_driver VERSION 0.1.0 LANGUAGES CXX) # This module provides installation directories as per the GNU coding standards. @@ -14,7 +16,6 @@ find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(serial REQUIRED) -find_package(ros2_control_test_assets) set(THIS_PACKAGE_INCLUDE_DEPENDS ament_cmake @@ -68,14 +69,6 @@ ament_target_dependencies( pluginlib_export_plugin_description_file(hardware_interface hardware_interface_plugin.xml) -############################################################################### -# ADDITIONAL OPTIONAL EXECUTABLES - -add_executable(gripper_interface_test src/gripper_interface_test.cpp) -target_include_directories(gripper_interface_test PRIVATE include) -ament_target_dependencies(gripper_interface_test serial) -target_link_libraries(gripper_interface_test robotiq_driver) - ############################################################################### # EXPORTS @@ -106,6 +99,13 @@ ament_export_libraries( ############################################################################### # INSTALL +# Install all files of the include folder into the give destination. +install( + DIRECTORY include/ + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} # include +) + +# Install our library. install( TARGETS robotiq_driver EXPORT robotiq_driver_targets @@ -114,10 +114,6 @@ install( RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} # bin INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} # include ) -install( - TARGETS gripper_interface_test - DESTINATION lib/robotiq_driver -) install( DIRECTORY config DESTINATION share/robotiq_driver diff --git a/robotiq_hardware_tests/CMakeLists.txt b/robotiq_hardware_tests/CMakeLists.txt new file mode 100644 index 0000000..752b731 --- /dev/null +++ b/robotiq_hardware_tests/CMakeLists.txt @@ -0,0 +1,39 @@ +# See https://docs.ros.org/en/foxy/How-To-Guides/Ament-CMake-Documentation.html + +cmake_minimum_required(VERSION 3.8) +project(robotiq_hardware_tests VERSION 0.0.1 LANGUAGES CXX) + +# This module provides installation directories as per the GNU coding standards. +include(GNUInstallDirs) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(serial REQUIRED) +find_package(robotiq_driver REQUIRED) + +set(THIS_PACKAGE_INCLUDE_DEPENDS + ament_cmake + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + serial +) + +add_executable(full_test + src/gripper_interface_test.cpp + src/command_line_utility.hpp + src/command_line_utility.cpp +) +target_include_directories(full_test + PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(full_test robotiq_driver::robotiq_driver) +ament_target_dependencies(full_test robotiq_driver) + +ament_package() diff --git a/robotiq_hardware_tests/package.xml b/robotiq_hardware_tests/package.xml new file mode 100644 index 0000000..3ab47ff --- /dev/null +++ b/robotiq_hardware_tests/package.xml @@ -0,0 +1,33 @@ + + + + robotiq_hardware_tests + 0.0.1 + ROS2 driver for the Robotiq gripper. + Giovanni Remigi + BSD + Giovanni Remigi + + ament_cmake + + launch + launch_ros + robotiq_driver + + + ament_clang_format + + ament_clang_tidy + + ament_cmake_copyright + + ament_cmake_lint_cmake + + ament_lint_auto + + ament_lint_common + + + ament_cmake + + diff --git a/robotiq_hardware_tests/src/command_line_utility.cpp b/robotiq_hardware_tests/src/command_line_utility.cpp new file mode 100644 index 0000000..2d72482 --- /dev/null +++ b/robotiq_hardware_tests/src/command_line_utility.cpp @@ -0,0 +1,87 @@ +// Copyright (c) 2023 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include + +#include "command_line_utility.hpp" + +void CommandLineUtility::registerHandler(const std::string& parameter, ParameterHandler handler, bool isMandatory) +{ + handlers[parameter] = handler; + if (isMandatory) + { + mandatoryParams.insert(parameter); + } +} + +bool CommandLineUtility::parse(int argc, char* argv[]) +{ + for (int i = 1; i < argc; i++) + { + auto it = handlers.find(argv[i]); + if (it != handlers.end()) + { + receivedParams.insert(it->first); + + if (std::holds_alternative(it->second)) + { + auto& handler = std::get(it->second); + i++; + if (i < argc) + { + handler(argv[i]); + } + else + { + std::cerr << it->first << " requires a value.\n"; + } + } + else if (std::holds_alternative(it->second)) + { + auto& handler = std::get(it->second); + handler(); + } + } + else + { + std::cerr << "Unknown argument: " << argv[i] << "\n"; + return false; + } + } + + for (const auto& param : mandatoryParams) + { + if (receivedParams.find(param) == receivedParams.end()) + { + std::cerr << "Missing mandatory argument: " << param << "\n"; + return false; + } + } + + return true; +} diff --git a/robotiq_hardware_tests/src/command_line_utility.hpp b/robotiq_hardware_tests/src/command_line_utility.hpp new file mode 100644 index 0000000..edb5666 --- /dev/null +++ b/robotiq_hardware_tests/src/command_line_utility.hpp @@ -0,0 +1,73 @@ +// Copyright (c) 2023 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#pragma once + +#include +#include +#include +#include +#include + +/** + * Class to parse the command line. + */ +class CommandLineUtility +{ + using LambdaWithValue = std::function; + using LambdaWithoutValue = std::function; + using ParameterHandler = std::variant; + +public: + /** + * Assign to each command parameter a lambda function to handle it. + * @param parameter The parameter to handle. + * @param handler The lambda function to handle the parameter value. + * @param isMandatory True if the parameter is mandatory, else otherwise. + */ + void registerHandler(const std::string& parameter, ParameterHandler handler, bool isMandatory = false); + + /** + * Parse the command line and read all parameters. + * @param argc The number of tokens in the command line. + * @param argv The list of tokens. + * @return True if the parsing is successful. + */ + bool parse(int argc, char* argv[]); + +private: + // Map that associates a lambda function to each parameter to process the expected value. + std::map handlers; + + // Store all mandatory parameters. + std::set mandatoryParams; + + // Store the parameters which are parsed in the command line. If a parameter is mandatory, + // but cannot be found in the received parameters, then print an error. + std::set receivedParams; +}; diff --git a/robotiq_driver/src/gripper_interface_test.cpp b/robotiq_hardware_tests/src/gripper_interface_test.cpp similarity index 99% rename from robotiq_driver/src/gripper_interface_test.cpp rename to robotiq_hardware_tests/src/gripper_interface_test.cpp index 60f9cce..f0ea7c2 100644 --- a/robotiq_driver/src/gripper_interface_test.cpp +++ b/robotiq_hardware_tests/src/gripper_interface_test.cpp @@ -36,8 +36,7 @@ constexpr auto kComPort = "/dev/ttyUSB0"; constexpr auto kBaudRate = 115200; -constexpr auto kTimeout = 0.5; - +constexpr auto kTimeout = 1; constexpr auto kSlaveAddress = 0x09; using robotiq_driver::DefaultDriver; From dfe6f7948327393e8a6e642ebfca16343318d898 Mon Sep 17 00:00:00 2001 From: Giovanni Remigi Date: Wed, 6 Sep 2023 22:35:13 +0200 Subject: [PATCH 26/47] Some refactoring. --- robotiq_driver/src/default_driver.cpp | 20 +++++++++---------- .../src/gripper_interface_test.cpp | 4 ++-- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/robotiq_driver/src/default_driver.cpp b/robotiq_driver/src/default_driver.cpp index 5fc132f..34def8b 100644 --- a/robotiq_driver/src/default_driver.cpp +++ b/robotiq_driver/src/default_driver.cpp @@ -186,6 +186,16 @@ bool DefaultDriver::gripper_is_moving() return object_detection_status_ == ObjectDetectionStatus::MOVING; } +void DefaultDriver::set_speed(uint8_t speed) +{ + commanded_gripper_speed_ = speed; +} + +void DefaultDriver::set_force(uint8_t force) +{ + commanded_gripper_force_ = force; +} + std::vector DefaultDriver::create_read_command(uint16_t first_register, uint8_t num_registers) { std::vector cmd = { slave_address_, @@ -200,16 +210,6 @@ std::vector DefaultDriver::create_read_command(uint16_t first_register, return cmd; } -void DefaultDriver::set_speed(uint8_t speed) -{ - commanded_gripper_speed_ = speed; -} - -void DefaultDriver::set_force(uint8_t force) -{ - commanded_gripper_force_ = force; -} - std::vector DefaultDriver::create_write_command(uint16_t first_register, const std::vector& data) { uint16_t num_registers = data.size(); diff --git a/robotiq_hardware_tests/src/gripper_interface_test.cpp b/robotiq_hardware_tests/src/gripper_interface_test.cpp index f0ea7c2..a4264ba 100644 --- a/robotiq_hardware_tests/src/gripper_interface_test.cpp +++ b/robotiq_hardware_tests/src/gripper_interface_test.cpp @@ -110,9 +110,9 @@ int main() std::this_thread::sleep_for(std::chrono::milliseconds(500)); } } - catch (...) + catch (std::exception e) { - std::cout << "Failed to communicating with the Gripper. Please check the Gripper connection"; + std::cout << "Failed to communicating with the gripper: " << e.what(); return 1; } From 21dc2c71eebd400138c8ff308d62e36d117bea4d Mon Sep 17 00:00:00 2001 From: Giovanni Remigi Date: Wed, 6 Sep 2023 22:35:57 +0200 Subject: [PATCH 27/47] Some refactoring. --- robotiq_hardware_tests/src/gripper_interface_test.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robotiq_hardware_tests/src/gripper_interface_test.cpp b/robotiq_hardware_tests/src/gripper_interface_test.cpp index a4264ba..7c41b66 100644 --- a/robotiq_hardware_tests/src/gripper_interface_test.cpp +++ b/robotiq_hardware_tests/src/gripper_interface_test.cpp @@ -112,7 +112,7 @@ int main() } catch (std::exception e) { - std::cout << "Failed to communicating with the gripper: " << e.what(); + std::cout << "Failed to communicating with the gripper: " << e.what() << std::endl; return 1; } From 6ccf69544ff09019b035a3b1b658043a01c44286 Mon Sep 17 00:00:00 2001 From: Giovanni Remigi Date: Wed, 6 Sep 2023 22:36:38 +0200 Subject: [PATCH 28/47] Some refactoring. --- robotiq_hardware_tests/src/gripper_interface_test.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robotiq_hardware_tests/src/gripper_interface_test.cpp b/robotiq_hardware_tests/src/gripper_interface_test.cpp index 7c41b66..b62ac5a 100644 --- a/robotiq_hardware_tests/src/gripper_interface_test.cpp +++ b/robotiq_hardware_tests/src/gripper_interface_test.cpp @@ -110,7 +110,7 @@ int main() std::this_thread::sleep_for(std::chrono::milliseconds(500)); } } - catch (std::exception e) + catch (const std::exception& e) { std::cout << "Failed to communicating with the gripper: " << e.what() << std::endl; return 1; From 9691e54e6429ddcb0ac1d964e3caf8cbf149fcb5 Mon Sep 17 00:00:00 2001 From: Giovanni Remigi Date: Wed, 6 Sep 2023 22:43:03 +0200 Subject: [PATCH 29/47] Test command refactored. --- .../src/gripper_interface_test.cpp | 85 +++++++++++++++---- 1 file changed, 70 insertions(+), 15 deletions(-) diff --git a/robotiq_hardware_tests/src/gripper_interface_test.cpp b/robotiq_hardware_tests/src/gripper_interface_test.cpp index b62ac5a..54d18e9 100644 --- a/robotiq_hardware_tests/src/gripper_interface_test.cpp +++ b/robotiq_hardware_tests/src/gripper_interface_test.cpp @@ -34,6 +34,8 @@ #include #include +#include "command_line_utility.hpp" + constexpr auto kComPort = "/dev/ttyUSB0"; constexpr auto kBaudRate = 115200; constexpr auto kTimeout = 1; @@ -42,55 +44,108 @@ constexpr auto kSlaveAddress = 0x09; using robotiq_driver::DefaultDriver; using robotiq_driver::DefaultSerial; -int main() +int main(int argc, char* argv[]) { + CommandLineUtility cli; + + std::string port = kComPort; + cli.registerHandler( + "--port", [&port](const char* value) { port = value; }, false); + + int baudrate = kBaudRate; + cli.registerHandler( + "--baudrate", [&baudrate](const char* value) { baudrate = std::stoi(value); }, false); + + double timeout = kTimeout; + cli.registerHandler( + "--timeout", [&timeout](const char* value) { timeout = std::stod(value); }, false); + + int slave_address = kSlaveAddress; + cli.registerHandler( + "--slave-address", [&slave_address](const char* value) { slave_address = std::stoi(value); }, false); + + cli.registerHandler("-h", [&]() { + std::cout << "Usage: ./set_relative_pressure [OPTIONS]\n" + << "Options:\n" + << " --port VALUE Set the com port (default " << kComPort << ")\n" + << " --baudrate VALUE Set the baudrate (default " << kBaudRate << "bps)\n" + << " --timeout VALUE Set the read/write timeout (default " << kTimeout << "ms)\n" + << " --slave-address VALUE Set the slave address (default " << kSlaveAddress << ")\n" + << " -h Show this help message\n"; + exit(0); + }); + + if (!cli.parse(argc, argv)) + { + return 1; + } + try { auto serial = std::make_unique(); - serial->set_port(kComPort); - serial->set_baudrate(kBaudRate); - serial->set_timeout(std::chrono::duration_cast(std::chrono::duration(kTimeout))); + serial->set_port(port); + serial->set_baudrate(baudrate); + serial->set_timeout(std::chrono::duration_cast(std::chrono::duration(timeout))); auto driver = std::make_unique(std::move(serial)); - driver->set_slave_address(kSlaveAddress); + driver->set_slave_address(slave_address); + + std::cout << "Using the following parameters: " << std::endl; + std::cout << " - port: " << port << std::endl; + std::cout << " - baudrate: " << baudrate << "bps" << std::endl; + std::cout << " - read/write timeout: " << timeout << "s" << std::endl; + std::cout << " - slave address: " << slave_address << std::endl; + + const bool connected = driver->connect(); + if (!connected) + { + std::cout << "The gripper is not connected" << std::endl; + return 1; + } + + std::cout << "The gripper is connected." << std::endl; + std::cout << "Deactivating the gripper..." << std::endl; + ; - std::cout << "Deactivating gripper...\n"; driver->deactivate(); - std::cout << "Activating gripper...\n"; + std::cout << "The gripper is deactivated." << std::endl; + std::cout << "Activating gripper..." << std::endl; + ; + driver->activate(); - std::cout << "Gripper successfully activated.\n"; + std::cout << "The gripper is activated." << std::endl; + std::cout << "Closing the gripper..." << std::endl; - std::cout << "Closing gripper...\n"; driver->set_gripper_position(0xFF); while (driver->gripper_is_moving()) { std::this_thread::sleep_for(std::chrono::milliseconds(500)); } - std::cout << "Opening gripper...\n"; + std::cout << "Opening the gripper..." << std::endl; driver->set_gripper_position(0x00); while (driver->gripper_is_moving()) { std::this_thread::sleep_for(std::chrono::milliseconds(500)); } - std::cout << "Half closing gripper...\n"; + std::cout << "Half closing the gripper..." << std::endl; driver->set_gripper_position(0x80); while (driver->gripper_is_moving()) { std::this_thread::sleep_for(std::chrono::milliseconds(500)); } - std::cout << "Opening gripper...\n"; + std::cout << "Opening gripper..." << std::endl; driver->set_gripper_position(0x00); while (driver->gripper_is_moving()) { std::this_thread::sleep_for(std::chrono::milliseconds(500)); } - std::cout << "Decreasing gripper speed...\n"; + std::cout << "Decreasing gripper speed..." << std::endl; driver->set_speed(0x0F); std::cout << "Closing gripper...\n"; @@ -100,10 +155,10 @@ int main() std::this_thread::sleep_for(std::chrono::milliseconds(500)); } - std::cout << "Increasing gripper speed...\n"; + std::cout << "Increasing gripper speed..." << std::endl; driver->set_speed(0xFF); - std::cout << "Opening gripper...\n"; + std::cout << "Opening gripper..." << std::endl; driver->set_gripper_position(0x00); while (driver->gripper_is_moving()) { From 685fe55f5b27a0df572decbedb6e6f8b663f367b Mon Sep 17 00:00:00 2001 From: Giovanni Remigi Date: Wed, 6 Sep 2023 23:21:55 +0200 Subject: [PATCH 30/47] Ongoing refactoring. --- robotiq_bringup/CMakeLists.txt | 16 ++ .../launch/robotiq_bringup.launch.py | 156 ++++++++++++++++++ robotiq_bringup/package.xml | 21 +++ robotiq_description/CMakeLists.txt | 5 +- .../config/robotiq_controllers.yaml | 0 .../launch/robotiq_control.launch.py | 0 robotiq_description/package.xml | 3 + robotiq_driver/CMakeLists.txt | 8 - 8 files changed, 199 insertions(+), 10 deletions(-) create mode 100644 robotiq_bringup/CMakeLists.txt create mode 100644 robotiq_bringup/launch/robotiq_bringup.launch.py create mode 100644 robotiq_bringup/package.xml rename {robotiq_driver => robotiq_description}/config/robotiq_controllers.yaml (100%) rename {robotiq_driver => robotiq_description}/launch/robotiq_control.launch.py (100%) diff --git a/robotiq_bringup/CMakeLists.txt b/robotiq_bringup/CMakeLists.txt new file mode 100644 index 0000000..fcd9d35 --- /dev/null +++ b/robotiq_bringup/CMakeLists.txt @@ -0,0 +1,16 @@ +# See https://docs.ros.org/en/foxy/How-To-Guides/Ament-CMake-Documentation.html + +cmake_minimum_required(VERSION 3.22.1) +project(robotiq_bringup VERSION 0.1.0 LANGUAGES CXX) +include(GNUInstallDirs) + +find_package(ament_cmake REQUIRED) + +# Install directories +install( + DIRECTORY + launch + DESTINATION share/robotiq_bringup +) + +ament_package() diff --git a/robotiq_bringup/launch/robotiq_bringup.launch.py b/robotiq_bringup/launch/robotiq_bringup.launch.py new file mode 100644 index 0000000..ce8037f --- /dev/null +++ b/robotiq_bringup/launch/robotiq_bringup.launch.py @@ -0,0 +1,156 @@ +# Copyright (c) 2023 PickNik, Inc. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, + OpaqueFunction, + RegisterEventHandler, +) +from launch.conditions import IfCondition +from launch.event_handlers import OnProcessExit +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import ( + LaunchConfiguration, + PathJoinSubstitution, +) +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +import xacro + + +def launch_setup(context, *args, **kwargs): + # Declare all parameters. + description_package_param = LaunchConfiguration("description_package") + description_file_param = LaunchConfiguration("description_file") + controllers_config_file_param = LaunchConfiguration("controllers_file") + launch_rviz = LaunchConfiguration("launch_rviz") + + # Extract all parameters' values. + description_file = PathJoinSubstitution( + [FindPackageShare(description_package_param), "urdf", description_file_param] + ).perform(context) + rviz_config_file = PathJoinSubstitution( + [FindPackageShare(description_package_param), "rviz", "view_urdf.rviz"] + ).perform(context) + controllers_config_file = PathJoinSubstitution( + [ + FindPackageShare(description_package_param), + "config", + controllers_config_file_param, + ] + ).perform(context) + + robot_description_content = xacro.process_file(description_file).toxml() + + # The Controller Manager (CM) connects the controllers’ and hardware-abstraction sides of the ros2_control + # framework. It also serves as the entry-point for users through ROS services. + # https://control.ros.org/master/doc/getting_started/getting_started.html#architecture + controller_manager = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[ + {"robot_description": robot_description_content}, + controllers_config_file, + ], + output={ + "stdout": "screen", + "stderr": "screen", + }, + ) + + # This is a controller for the Robotiq gripper. + robotiq_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["robotiq_controller", "-c", "/controller_manager"], + ) + + # robot_state_publisher uses the URDF specified by the parameter robot_description and the joint positions + # from the topic /joint_states to calculate the forward kinematics of the robot and publish the results via + # tf. + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + parameters=[{"robot_description": robot_description_content}], + output="screen", + ) + + # Starts up the RViz2 application. + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + arguments=["-d", rviz_config_file], + condition=IfCondition(launch_rviz), + ) + + nodes_to_start = [ + controller_manager, + robotiq_controller_spawner, + robot_state_publisher_node, + rviz_node + ] + return nodes_to_start + + +def generate_launch_description(): + """ + A Python launch file is meant to help implement the markup based frontends like YAML and XML, and so it is + declarative in nature rather than imperative. For this reason, it is not possible to directly access the content of + LaunchConfiguration parameters, which are asyncio futures. To access the content of a LaunchConfiguration, we must + provide a context by wrapping the initialization method into an OpaqueFunction. See more info: + https://answers.ros.org/question/397123/how-to-access-the-runtime-value-of-a-launchconfiguration-instance-within-custom-launch-code-injected-via-an-opaquefunction-in-ros2 + https://github.com/Serafadam/interbotix_ros_manipulators/blob/xsarm_control_galactic/interbotix_ros_xsarms/interbotix_xsarm_control/launch/xsarm_control.launch.py + """ + declared_arguments = [ + DeclareLaunchArgument( + "description_package", + default_value="robotiq_description", + description="Package containing all robot configuration files.", + ), + DeclareLaunchArgument( + "description_file", + default_value="robotiq_2f_85_gripper.urdf.xacro", + description="URDF/XACRO description file for the robot.", + ), + DeclareLaunchArgument( + "controllers_file", + default_value="controllers.yaml", + description="YAML file with the controllers configuration.", + ), + DeclareLaunchArgument( + "launch_rviz", default_value="true", description="Launch RViz?" + ), + OpaqueFunction(function=launch_setup), + ] + + return LaunchDescription(declared_arguments) diff --git a/robotiq_bringup/package.xml b/robotiq_bringup/package.xml new file mode 100644 index 0000000..52d5410 --- /dev/null +++ b/robotiq_bringup/package.xml @@ -0,0 +1,21 @@ + + + + robotiq_bringup + 0.0.1 + + Bringup package for the Robotiq gripper for testing purposes. + + Giovanni Remigi + BSD + + ament_cmake + + robotiq_description + robotiq_driver + robotiq_controllers + + + ament_cmake + + diff --git a/robotiq_description/CMakeLists.txt b/robotiq_description/CMakeLists.txt index 6ba826b..e908b35 100644 --- a/robotiq_description/CMakeLists.txt +++ b/robotiq_description/CMakeLists.txt @@ -10,10 +10,11 @@ find_package(ament_cmake REQUIRED) install( DIRECTORY - urdf + config launch - rviz meshes + rviz + urdf DESTINATION share/${PROJECT_NAME} ) diff --git a/robotiq_driver/config/robotiq_controllers.yaml b/robotiq_description/config/robotiq_controllers.yaml similarity index 100% rename from robotiq_driver/config/robotiq_controllers.yaml rename to robotiq_description/config/robotiq_controllers.yaml diff --git a/robotiq_driver/launch/robotiq_control.launch.py b/robotiq_description/launch/robotiq_control.launch.py similarity index 100% rename from robotiq_driver/launch/robotiq_control.launch.py rename to robotiq_description/launch/robotiq_control.launch.py diff --git a/robotiq_description/package.xml b/robotiq_description/package.xml index 18611bc..42866b6 100644 --- a/robotiq_description/package.xml +++ b/robotiq_description/package.xml @@ -18,6 +18,9 @@ rviz2 urdf xacro + ros2_control + ros2_controllers + robotiq_driver ament_lint_auto ament_lint_common diff --git a/robotiq_driver/CMakeLists.txt b/robotiq_driver/CMakeLists.txt index 36aaa37..ff81ac9 100644 --- a/robotiq_driver/CMakeLists.txt +++ b/robotiq_driver/CMakeLists.txt @@ -114,14 +114,6 @@ install( RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} # bin INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} # include ) -install( - DIRECTORY config - DESTINATION share/robotiq_driver -) -install( - DIRECTORY launch - DESTINATION share/robotiq_driver -) ############################################################################### # TESTS From 5a5a62de60a9afa01c9e4074cda5fd2e68de9594 Mon Sep 17 00:00:00 2001 From: Giovanni Remigi Date: Wed, 6 Sep 2023 23:30:17 +0200 Subject: [PATCH 31/47] Ongoing refactoring. --- robotiq_bringup/package.xml | 3 +++ robotiq_description/package.xml | 3 +++ robotiq_driver/package.xml | 3 --- 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/robotiq_bringup/package.xml b/robotiq_bringup/package.xml index 52d5410..8f472fa 100644 --- a/robotiq_bringup/package.xml +++ b/robotiq_bringup/package.xml @@ -15,6 +15,9 @@ robotiq_driver robotiq_controllers + gripper_controllers + joint_state_broadcaster + ament_cmake diff --git a/robotiq_description/package.xml b/robotiq_description/package.xml index 42866b6..98a7348 100644 --- a/robotiq_description/package.xml +++ b/robotiq_description/package.xml @@ -22,6 +22,9 @@ ros2_controllers robotiq_driver + gripper_controllers + joint_state_broadcaster + ament_lint_auto ament_lint_common diff --git a/robotiq_driver/package.xml b/robotiq_driver/package.xml index 1d35780..b80ad68 100644 --- a/robotiq_driver/package.xml +++ b/robotiq_driver/package.xml @@ -18,9 +18,6 @@ rclcpp_lifecycle serial - gripper_controllers - joint_state_broadcaster - ament_clang_format From 91a08d052050070e2e226b9c8ffeaf03759e16fc Mon Sep 17 00:00:00 2001 From: Giovanni Remigi Date: Wed, 6 Sep 2023 23:50:19 +0200 Subject: [PATCH 32/47] Removed bringup package. --- robotiq_bringup/CMakeLists.txt | 16 -- .../launch/robotiq_bringup.launch.py | 156 ------------------ robotiq_bringup/package.xml | 24 --- robotiq_description/package.xml | 9 +- robotiq_driver/package.xml | 1 - 5 files changed, 6 insertions(+), 200 deletions(-) delete mode 100644 robotiq_bringup/CMakeLists.txt delete mode 100644 robotiq_bringup/launch/robotiq_bringup.launch.py delete mode 100644 robotiq_bringup/package.xml diff --git a/robotiq_bringup/CMakeLists.txt b/robotiq_bringup/CMakeLists.txt deleted file mode 100644 index fcd9d35..0000000 --- a/robotiq_bringup/CMakeLists.txt +++ /dev/null @@ -1,16 +0,0 @@ -# See https://docs.ros.org/en/foxy/How-To-Guides/Ament-CMake-Documentation.html - -cmake_minimum_required(VERSION 3.22.1) -project(robotiq_bringup VERSION 0.1.0 LANGUAGES CXX) -include(GNUInstallDirs) - -find_package(ament_cmake REQUIRED) - -# Install directories -install( - DIRECTORY - launch - DESTINATION share/robotiq_bringup -) - -ament_package() diff --git a/robotiq_bringup/launch/robotiq_bringup.launch.py b/robotiq_bringup/launch/robotiq_bringup.launch.py deleted file mode 100644 index ce8037f..0000000 --- a/robotiq_bringup/launch/robotiq_bringup.launch.py +++ /dev/null @@ -1,156 +0,0 @@ -# Copyright (c) 2023 PickNik, Inc. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# -# * Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in the -# documentation and/or other materials provided with the distribution. -# -# * Neither the name of the {copyright_holder} nor the names of its -# contributors may be used to endorse or promote products derived from -# this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - IncludeLaunchDescription, - OpaqueFunction, - RegisterEventHandler, -) -from launch.conditions import IfCondition -from launch.event_handlers import OnProcessExit -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import ( - LaunchConfiguration, - PathJoinSubstitution, -) -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - -import xacro - - -def launch_setup(context, *args, **kwargs): - # Declare all parameters. - description_package_param = LaunchConfiguration("description_package") - description_file_param = LaunchConfiguration("description_file") - controllers_config_file_param = LaunchConfiguration("controllers_file") - launch_rviz = LaunchConfiguration("launch_rviz") - - # Extract all parameters' values. - description_file = PathJoinSubstitution( - [FindPackageShare(description_package_param), "urdf", description_file_param] - ).perform(context) - rviz_config_file = PathJoinSubstitution( - [FindPackageShare(description_package_param), "rviz", "view_urdf.rviz"] - ).perform(context) - controllers_config_file = PathJoinSubstitution( - [ - FindPackageShare(description_package_param), - "config", - controllers_config_file_param, - ] - ).perform(context) - - robot_description_content = xacro.process_file(description_file).toxml() - - # The Controller Manager (CM) connects the controllers’ and hardware-abstraction sides of the ros2_control - # framework. It also serves as the entry-point for users through ROS services. - # https://control.ros.org/master/doc/getting_started/getting_started.html#architecture - controller_manager = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[ - {"robot_description": robot_description_content}, - controllers_config_file, - ], - output={ - "stdout": "screen", - "stderr": "screen", - }, - ) - - # This is a controller for the Robotiq gripper. - robotiq_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["robotiq_controller", "-c", "/controller_manager"], - ) - - # robot_state_publisher uses the URDF specified by the parameter robot_description and the joint positions - # from the topic /joint_states to calculate the forward kinematics of the robot and publish the results via - # tf. - robot_state_publisher_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - name="robot_state_publisher", - parameters=[{"robot_description": robot_description_content}], - output="screen", - ) - - # Starts up the RViz2 application. - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - arguments=["-d", rviz_config_file], - condition=IfCondition(launch_rviz), - ) - - nodes_to_start = [ - controller_manager, - robotiq_controller_spawner, - robot_state_publisher_node, - rviz_node - ] - return nodes_to_start - - -def generate_launch_description(): - """ - A Python launch file is meant to help implement the markup based frontends like YAML and XML, and so it is - declarative in nature rather than imperative. For this reason, it is not possible to directly access the content of - LaunchConfiguration parameters, which are asyncio futures. To access the content of a LaunchConfiguration, we must - provide a context by wrapping the initialization method into an OpaqueFunction. See more info: - https://answers.ros.org/question/397123/how-to-access-the-runtime-value-of-a-launchconfiguration-instance-within-custom-launch-code-injected-via-an-opaquefunction-in-ros2 - https://github.com/Serafadam/interbotix_ros_manipulators/blob/xsarm_control_galactic/interbotix_ros_xsarms/interbotix_xsarm_control/launch/xsarm_control.launch.py - """ - declared_arguments = [ - DeclareLaunchArgument( - "description_package", - default_value="robotiq_description", - description="Package containing all robot configuration files.", - ), - DeclareLaunchArgument( - "description_file", - default_value="robotiq_2f_85_gripper.urdf.xacro", - description="URDF/XACRO description file for the robot.", - ), - DeclareLaunchArgument( - "controllers_file", - default_value="controllers.yaml", - description="YAML file with the controllers configuration.", - ), - DeclareLaunchArgument( - "launch_rviz", default_value="true", description="Launch RViz?" - ), - OpaqueFunction(function=launch_setup), - ] - - return LaunchDescription(declared_arguments) diff --git a/robotiq_bringup/package.xml b/robotiq_bringup/package.xml deleted file mode 100644 index 8f472fa..0000000 --- a/robotiq_bringup/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - - robotiq_bringup - 0.0.1 - - Bringup package for the Robotiq gripper for testing purposes. - - Giovanni Remigi - BSD - - ament_cmake - - robotiq_description - robotiq_driver - robotiq_controllers - - gripper_controllers - joint_state_broadcaster - - - ament_cmake - - diff --git a/robotiq_description/package.xml b/robotiq_description/package.xml index 98a7348..8992468 100644 --- a/robotiq_description/package.xml +++ b/robotiq_description/package.xml @@ -18,9 +18,12 @@ rviz2 urdf xacro - ros2_control - ros2_controllers - robotiq_driver + + ros2_control + robotiq_driver + + ros2_controllers + robotiq_controllers gripper_controllers joint_state_broadcaster diff --git a/robotiq_driver/package.xml b/robotiq_driver/package.xml index b80ad68..4c5733d 100644 --- a/robotiq_driver/package.xml +++ b/robotiq_driver/package.xml @@ -32,7 +32,6 @@ ament_lint_common - ament_cmake_gtest ament_cmake_gmock ros2_control_test_assets From 03e37010bfa52bb48439dfaf966a82f936771b4f Mon Sep 17 00:00:00 2001 From: Giovanni Remigi Date: Thu, 7 Sep 2023 11:15:02 +0200 Subject: [PATCH 33/47] Added fake driver for testing. config folder moved from robotiq_driver to robotiq_description. --- .../launch/robotiq_control.launch.py | 13 ++- robotiq_description/package.xml | 3 - .../urdf/robotiq_gripper.ros2_control.xacro | 4 + robotiq_driver/CMakeLists.txt | 2 + .../robotiq_driver/fake/fake_driver.hpp | 70 ++++++++++++++ robotiq_driver/src/default_driver_factory.cpp | 20 +++- robotiq_driver/src/fake/fake_driver.cpp | 95 +++++++++++++++++++ 7 files changed, 199 insertions(+), 8 deletions(-) create mode 100644 robotiq_driver/include/robotiq_driver/fake/fake_driver.hpp create mode 100644 robotiq_driver/src/fake/fake_driver.cpp diff --git a/robotiq_description/launch/robotiq_control.launch.py b/robotiq_description/launch/robotiq_control.launch.py index 2e0d196..497edbe 100644 --- a/robotiq_description/launch/robotiq_control.launch.py +++ b/robotiq_description/launch/robotiq_control.launch.py @@ -33,6 +33,7 @@ LaunchConfiguration, PathJoinSubstitution, ) +from launch.conditions import IfCondition import launch_ros import os @@ -67,6 +68,13 @@ def generate_launch_description(): description="Absolute path to rviz config file", ) ) + args.append( + launch.actions.DeclareLaunchArgument( + name="launch_rviz", + default_value="false", + description="Launch RViz?" + ) + ) robot_description_content = Command( [ @@ -85,7 +93,7 @@ def generate_launch_description(): update_rate_config_file = PathJoinSubstitution( [ - pkg_share, + description_pkg_share, "config", "robotiq_update_rate.yaml", ] @@ -93,7 +101,7 @@ def generate_launch_description(): controllers_file = "robotiq_controllers.yaml" initial_joint_controllers = PathJoinSubstitution( - [pkg_share, "config", controllers_file] + [description_pkg_share, "config", controllers_file] ) control_node = launch_ros.actions.Node( @@ -118,6 +126,7 @@ def generate_launch_description(): name="rviz2", output="log", arguments=["-d", LaunchConfiguration("rvizconfig")], + condition=IfCondition(LaunchConfiguration("launch_rviz")), ) joint_state_broadcaster_spawner = launch_ros.actions.Node( diff --git a/robotiq_description/package.xml b/robotiq_description/package.xml index 8992468..2cb93b0 100644 --- a/robotiq_description/package.xml +++ b/robotiq_description/package.xml @@ -20,10 +20,7 @@ xacro ros2_control - robotiq_driver - ros2_controllers - robotiq_controllers gripper_controllers joint_state_broadcaster diff --git a/robotiq_description/urdf/robotiq_gripper.ros2_control.xacro b/robotiq_description/urdf/robotiq_gripper.ros2_control.xacro index dd271ba..5440c35 100644 --- a/robotiq_description/urdf/robotiq_gripper.ros2_control.xacro +++ b/robotiq_description/urdf/robotiq_gripper.ros2_control.xacro @@ -13,6 +13,10 @@ + + + false + isaac_ros2_control/IsaacSystem /isaac_joint_commands diff --git a/robotiq_driver/CMakeLists.txt b/robotiq_driver/CMakeLists.txt index ff81ac9..ee4724e 100644 --- a/robotiq_driver/CMakeLists.txt +++ b/robotiq_driver/CMakeLists.txt @@ -31,6 +31,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS add_library( robotiq_driver SHARED + include/robotiq_driver/fake/fake_driver.hpp include/robotiq_driver/crc_utils.hpp include/robotiq_driver/data_utils.hpp include/robotiq_driver/default_driver.hpp @@ -49,6 +50,7 @@ add_library( src/hardware_interface.cpp src/default_driver.cpp src/default_driver_factory.cpp + src/fake/fake_driver.cpp src/default_serial.cpp src/default_serial_factory.cpp ) diff --git a/robotiq_driver/include/robotiq_driver/fake/fake_driver.hpp b/robotiq_driver/include/robotiq_driver/fake/fake_driver.hpp new file mode 100644 index 0000000..ddb5b95 --- /dev/null +++ b/robotiq_driver/include/robotiq_driver/fake/fake_driver.hpp @@ -0,0 +1,70 @@ +// Copyright (c) 2023 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#pragma once + +#include + +namespace robotiq_driver +{ +/** + * This is a fake driver that can be used for testing interactions with the + * hardware interface or the controller without being connected to the real + * hardware. At the moment the fake driver is very basic but it can be + * improved to behave as close as possible to the real hardware. + * To use this driver you have to enable the following parameter in your + * hardware interface configuration in the robot URDF. + * + * + * true + */ +class FakeDriver : public Driver +{ +public: + void set_slave_address(uint8_t slave_address) override; + bool connect() override; + void disconnect() override; + void activate() override; + void deactivate() override; + void set_gripper_position(uint8_t position) override; + uint8_t get_gripper_position() override; + bool gripper_is_moving() override; + void set_speed(uint8_t speed) override; + void set_force(uint8_t force) override; + +private: + uint8_t slave_address_ = 0x00; + bool connected_ = false; + bool activated_ = false; + uint8_t position_ = 0; + bool gripper_is_moving_ = false; + uint8_t speed_ = 0; + uint8_t force_ = 0; +}; + +} // namespace robotiq_driver diff --git a/robotiq_driver/src/default_driver_factory.cpp b/robotiq_driver/src/default_driver_factory.cpp index 3add121..05d66b5 100644 --- a/robotiq_driver/src/default_driver_factory.cpp +++ b/robotiq_driver/src/default_driver_factory.cpp @@ -32,11 +32,14 @@ #include #include #include +#include #include namespace robotiq_driver { +const auto kLogger = rclcpp::get_logger("DefaultDriverFactory"); + constexpr auto kSlaveAddressParamName = "slave_address"; constexpr uint8_t kSlaveAddressParamDefault = 0x09; @@ -46,7 +49,8 @@ constexpr double kGripperSpeedMultiplierParamDefault = 1.0; constexpr auto kGripperForceMultiplierParamName = "gripper_force_multiplier"; constexpr double kGripperForceMultiplierParamDefault = 1.0; -const auto kLogger = rclcpp::get_logger("DefaultDriverFactory"); +constexpr auto kUseDummyParamName = "use_dummy"; +constexpr auto kUseDummyParamDefault = "false"; std::unique_ptr DefaultDriverFactory::create(const hardware_interface::HardwareInfo& info) const { @@ -80,7 +84,17 @@ std::unique_ptr DefaultDriverFactory::create(const hardware_interface::H std::unique_ptr DefaultDriverFactory::create_driver(const hardware_interface::HardwareInfo& info) const { - auto serial = DefaultSerialFactory().create(info); - return std::make_unique(std::move(serial)); + // We give the user an option to startup a dummy gripper for testing purposes. + if (info.hardware_parameters.count(kUseDummyParamName) && + info.hardware_parameters.at(kUseDummyParamName) != kUseDummyParamDefault) + { + RCLCPP_INFO(kLogger, "You are connected to a dummy driver, not a real hardware."); + return std::make_unique(); + } + else + { + auto serial = DefaultSerialFactory().create(info); + return std::make_unique(std::move(serial)); + } } } // namespace robotiq_driver diff --git a/robotiq_driver/src/fake/fake_driver.cpp b/robotiq_driver/src/fake/fake_driver.cpp new file mode 100644 index 0000000..1dc87e9 --- /dev/null +++ b/robotiq_driver/src/fake/fake_driver.cpp @@ -0,0 +1,95 @@ +// Copyright (c) 2023 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include + +#include + +namespace robotiq_driver +{ +const auto kLogger = rclcpp::get_logger("FakeDriver"); + +void FakeDriver::set_slave_address(uint8_t slave_address) +{ + slave_address_ = slave_address; + RCLCPP_INFO(kLogger, "slave_address set to: %d", slave_address); +} + +bool FakeDriver::connect() +{ + connected_ = true; + RCLCPP_INFO(kLogger, "Gripper connected."); + return true; +} + +void FakeDriver::disconnect() +{ + RCLCPP_INFO(kLogger, "Gripper disconnected."); + connected_ = false; +} + +void FakeDriver::activate() +{ + RCLCPP_INFO(kLogger, "Gripper activated."); + activated_ = true; +} + +void FakeDriver::deactivate() +{ + RCLCPP_INFO(kLogger, "Gripper deactivated."); + activated_ = false; +} + +void FakeDriver::set_gripper_position(uint8_t position) +{ + position_ = position; +} + +uint8_t FakeDriver::get_gripper_position() +{ + return position_; +} + +bool FakeDriver::gripper_is_moving() +{ + return gripper_is_moving_; +} + +void FakeDriver::set_speed(uint8_t speed) +{ + RCLCPP_INFO(kLogger, "Set gripper speed."); + speed_ = speed; +} + +void FakeDriver::set_force(uint8_t force) +{ + RCLCPP_INFO(kLogger, "Set gripper force."); + force_ = force; +} + +} // namespace robotiq_driver From bcd5fe689d553a36ccfdaa1f734b3c61f1850cd5 Mon Sep 17 00:00:00 2001 From: Giovanni Remigi Date: Fri, 8 Sep 2023 19:09:25 +0200 Subject: [PATCH 34/47] Hardware interface and driver refactoring. --- .../include/robotiq_driver/default_driver.hpp | 24 +- .../robotiq_driver/hardware_interface.hpp | 4 + robotiq_driver/src/default_driver.cpp | 236 +++++++++--------- robotiq_driver/src/hardware_interface.cpp | 99 ++++---- 4 files changed, 181 insertions(+), 182 deletions(-) diff --git a/robotiq_driver/include/robotiq_driver/default_driver.hpp b/robotiq_driver/include/robotiq_driver/default_driver.hpp index 57461f4..8465dc8 100644 --- a/robotiq_driver/include/robotiq_driver/default_driver.hpp +++ b/robotiq_driver/include/robotiq_driver/default_driver.hpp @@ -90,24 +90,18 @@ class DefaultDriver : public Driver void set_force(uint8_t force) override; private: - std::vector create_read_command(uint16_t first_register, uint8_t num_registers); - std::vector create_write_command(uint16_t first_register, const std::vector& data); - /** - * @brief read response from the gripper. - * - * @param num_bytes Number of bytes to be read from device port. - * @throw serial::IOException on failure to successfully communicate with gripper port + * With this command we send a request and wait for a response of given size. + * Behind the scene, if the response is not received, the software makes an attempt + * to resend the command up to 5 times before returning an empty response. + * @param request The command request. + * @param response_size The response expected size. + * @return The response or an empty vector if an en error occurred. */ - std::vector read_response(size_t num_bytes); + std::vector send(const std::vector& request, size_t response_size) const; - /** - * @brief Send a command to the gripper. - * - * @param cmd The command. - * @throw serial::IOException on failure to successfully communicate with gripper port - */ - void send_command(const std::vector& cmd); + std::vector create_read_command(uint16_t first_register, uint8_t num_registers); + std::vector create_write_command(uint16_t first_register, const std::vector& data); /** * @brief Read the current status of the gripper, and update member variables as appropriate. diff --git a/robotiq_driver/include/robotiq_driver/hardware_interface.hpp b/robotiq_driver/include/robotiq_driver/hardware_interface.hpp index b2ef65e..f9a18b2 100644 --- a/robotiq_driver/include/robotiq_driver/hardware_interface.hpp +++ b/robotiq_driver/include/robotiq_driver/hardware_interface.hpp @@ -60,6 +60,9 @@ class RobotiqGripperHardwareInterface : public hardware_interface::SystemInterfa ROBOTIQ_DRIVER_PUBLIC RobotiqGripperHardwareInterface(); + ROBOTIQ_DRIVER_PUBLIC + ~RobotiqGripperHardwareInterface(); + /** * Constructor with a driver factory. This method is used for testing. * @param driver_factory The driver that interact with the hardware. @@ -135,6 +138,7 @@ class RobotiqGripperHardwareInterface : public hardware_interface::SystemInterfa // We use a thread to read/write to the driver so that we dont block the hardware_interface read/write. std::thread communication_thread_; std::atomic communication_thread_is_running_; + void background_task(); double gripper_closed_pos_; diff --git a/robotiq_driver/src/default_driver.cpp b/robotiq_driver/src/default_driver.cpp index 34def8b..3064c52 100644 --- a/robotiq_driver/src/default_driver.cpp +++ b/robotiq_driver/src/default_driver.cpp @@ -26,6 +26,8 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. +#include + #include #include #include @@ -69,11 +71,45 @@ constexpr size_t kResponseHeaderSize = 3; constexpr size_t kGripperStatusIndex = 0; constexpr size_t kPositionIndex = 4; +// If the gripper connection is not stable we may want to try sending the command again. +constexpr auto kMaxRetries = 5; + DefaultDriver::DefaultDriver(std::unique_ptr serial) : serial_{ std::move(serial) }, commanded_gripper_speed_(0x80), commanded_gripper_force_(0x80) { } +std::vector DefaultDriver::send(const std::vector& request, size_t response_size) const +{ + std::vector response; + response.reserve(response_size); + + int retry_count = 0; + while (retry_count < kMaxRetries) + { + try + { + serial_->write(request); + response = serial_->read(response_size); + break; + } + catch (const serial::IOException& e) + { + RCLCPP_WARN(kLogger, "Resending the command because the previous attempt (%d of %d) failed: %s", retry_count + 1, + kMaxRetries, e.what()); + retry_count++; + } + } + + if (retry_count == kMaxRetries) + { + RCLCPP_ERROR(kLogger, "Reached maximum retries. Operation failed."); + return {}; + } + + return response; +} + bool DefaultDriver::connect() { serial_->open(); @@ -95,15 +131,16 @@ void DefaultDriver::activate() RCLCPP_INFO(kLogger, "Activate..."); // set rACT to 1, clear all other registers. - const auto cmd = create_write_command(kActionRequestRegister, { 0x0100, 0x0000, 0x0000 }); + const auto request = create_write_command(kActionRequestRegister, { 0x0100, 0x0000, 0x0000 }); + auto response = send(request, kWriteResponseSize); + if (response.empty()) + { + throw DriverException{ "Failed to activate the gripper." }; + } try { - send_command(cmd); - read_response(kWriteResponseSize); - update_status(); - if (gripper_status_ == GripperStatus::COMPLETED) { return; @@ -125,15 +162,11 @@ void DefaultDriver::deactivate() { RCLCPP_INFO(kLogger, "Deactivate..."); - const auto cmd = create_write_command(kActionRequestRegister, { 0x0000, 0x0000, 0x0000 }); - try + const auto request = create_write_command(kActionRequestRegister, { 0x0000, 0x0000, 0x0000 }); + auto response = send(request, kWriteResponseSize); + if (response.empty()) { - send_command(cmd); - read_response(kWriteResponseSize); - } - catch (const std::exception& e) - { - throw DriverException{ std::string{ "Failed to deactivate the gripper: " } + e.what() }; + throw DriverException{ "Failed to deactivate the gripper." }; } } @@ -143,18 +176,15 @@ void DefaultDriver::set_gripper_position(uint8_t pos) uint8_t gripper_options_1 = 0x00; uint8_t gripper_options_2 = 0x00; - const auto cmd = + const auto request = create_write_command(kActionRequestRegister, { uint16_t(action_register << 8 | gripper_options_1), uint16_t(gripper_options_2 << 8 | pos), uint16_t(commanded_gripper_speed_ << 8 | commanded_gripper_force_) }); - try - { - send_command(cmd); - read_response(kWriteResponseSize); - } - catch (const std::exception& e) + + auto response = send(request, kWriteResponseSize); + if (response.empty()) { - throw DriverException{ std::string{ "Failed to set gripper position: " } + e.what() }; + throw DriverException{ "Failed to set gripper position." }; } } @@ -198,16 +228,16 @@ void DefaultDriver::set_force(uint8_t force) std::vector DefaultDriver::create_read_command(uint16_t first_register, uint8_t num_registers) { - std::vector cmd = { slave_address_, - kReadFunctionCode, - data_utils::get_msb(first_register), - data_utils::get_lsb(first_register), - data_utils::get_msb(num_registers), - data_utils::get_lsb(num_registers) }; - auto crc = crc_utils::compute_crc(cmd); - cmd.push_back(data_utils::get_msb(crc)); - cmd.push_back(data_utils::get_lsb(crc)); - return cmd; + std::vector request = { slave_address_, + kReadFunctionCode, + data_utils::get_msb(first_register), + data_utils::get_lsb(first_register), + data_utils::get_msb(num_registers), + data_utils::get_lsb(num_registers) }; + auto crc = crc_utils::compute_crc(request); + request.push_back(data_utils::get_msb(crc)); + request.push_back(data_utils::get_lsb(crc)); + return request; } std::vector DefaultDriver::create_write_command(uint16_t first_register, const std::vector& data) @@ -215,112 +245,76 @@ std::vector DefaultDriver::create_write_command(uint16_t first_register uint16_t num_registers = data.size(); uint8_t num_bytes = 2 * num_registers; - std::vector cmd = { slave_address_, - kWriteFunctionCode, - data_utils::get_msb(first_register), - data_utils::get_lsb(first_register), - data_utils::get_msb(num_registers), - data_utils::get_lsb(num_registers), - num_bytes }; + std::vector request = { slave_address_, + kWriteFunctionCode, + data_utils::get_msb(first_register), + data_utils::get_lsb(first_register), + data_utils::get_msb(num_registers), + data_utils::get_lsb(num_registers), + num_bytes }; for (auto d : data) { - cmd.push_back(data_utils::get_msb(d)); - cmd.push_back(data_utils::get_lsb(d)); + request.push_back(data_utils::get_msb(d)); + request.push_back(data_utils::get_lsb(d)); } - auto crc = crc_utils::compute_crc(cmd); - cmd.push_back(data_utils::get_msb(crc)); - cmd.push_back(data_utils::get_lsb(crc)); + auto crc = crc_utils::compute_crc(request); + request.push_back(data_utils::get_msb(crc)); + request.push_back(data_utils::get_lsb(crc)); - return cmd; -} - -std::vector DefaultDriver::read_response(size_t num_bytes_requested) -{ - std::vector response; - response.reserve(num_bytes_requested); - - try - { - response = serial_->read(num_bytes_requested); - } - catch (const std::exception& e) - { - throw DriverException{ std::string{ "Failed to read response: " } + e.what() }; - } - return response; -} - -void DefaultDriver::send_command(const std::vector& cmd) -{ - serial_->write(cmd); + return request; } void DefaultDriver::update_status() { - // Tell the gripper that we want to read its status. - try + const auto request = create_read_command(kFirstOutputRegister, kNumOutputRegisters); + auto response = send(request, kReadResponseSize); + if (response.empty()) { - const auto read_command = create_read_command(kFirstOutputRegister, kNumOutputRegisters); - send_command(read_command); - - const auto response = read_response(kReadResponseSize); - - // Process the response. - uint8_t gripper_status_byte = response[kResponseHeaderSize + kGripperStatusIndex]; - - // Activation status. - activation_status_ = ((gripper_status_byte & 0x01) == 0x00) ? ActivationStatus::RESET : ActivationStatus::ACTIVE; + throw DriverException{ "Failed to read the gripper status." }; + } - // Action status. - action_status_ = ((gripper_status_byte & 0x08) == 0x00) ? ActionStatus::STOPPED : ActionStatus::MOVING; + // Process the response. + uint8_t gripper_status_byte = response[kResponseHeaderSize + kGripperStatusIndex]; - // Gripper status. - switch ((gripper_status_byte & 0x30) >> 4) - { - case 0x00: - gripper_status_ = GripperStatus::RESET; - break; - case 0x01: - gripper_status_ = GripperStatus::IN_PROGRESS; - break; - case 0x03: - gripper_status_ = GripperStatus::COMPLETED; - break; - } + // Activation status. + activation_status_ = ((gripper_status_byte & 0x01) == 0x00) ? ActivationStatus::RESET : ActivationStatus::ACTIVE; - // Object detection status. - switch ((gripper_status_byte & 0xC0) >> 6) - { - case 0x00: - object_detection_status_ = ObjectDetectionStatus::MOVING; - break; - case 0x01: - object_detection_status_ = ObjectDetectionStatus::OBJECT_DETECTED_OPENING; - break; - case 0x02: - object_detection_status_ = ObjectDetectionStatus::OBJECT_DETECTED_CLOSING; - break; - case 0x03: - object_detection_status_ = ObjectDetectionStatus::AT_REQUESTED_POSITION; - break; - } + // Action status. + action_status_ = ((gripper_status_byte & 0x08) == 0x00) ? ActionStatus::STOPPED : ActionStatus::MOVING; - // Read the current gripper position. - gripper_position_ = response[kResponseHeaderSize + kPositionIndex]; + // Gripper status. + switch ((gripper_status_byte & 0x30) >> 4) + { + case 0x00: + gripper_status_ = GripperStatus::RESET; + break; + case 0x01: + gripper_status_ = GripperStatus::IN_PROGRESS; + break; + case 0x03: + gripper_status_ = GripperStatus::COMPLETED; + break; } - catch (const std::exception& e) + + // Object detection status. + switch ((gripper_status_byte & 0xC0) >> 6) { - std::cerr << "Failed to update gripper status.\n"; - if (serial_->is_open()) - { - std::cerr << "Error caught while reading or writing to device. Connection is open, " - "continuing to attempt " - "communication with gripper.\n ERROR: " - << e.what(); - return; - } - throw; + case 0x00: + object_detection_status_ = ObjectDetectionStatus::MOVING; + break; + case 0x01: + object_detection_status_ = ObjectDetectionStatus::OBJECT_DETECTED_OPENING; + break; + case 0x02: + object_detection_status_ = ObjectDetectionStatus::OBJECT_DETECTED_CLOSING; + break; + case 0x03: + object_detection_status_ = ObjectDetectionStatus::AT_REQUESTED_POSITION; + break; } + + // Read the current gripper position. + gripper_position_ = response[kResponseHeaderSize + kPositionIndex]; } } // namespace robotiq_driver diff --git a/robotiq_driver/src/hardware_interface.cpp b/robotiq_driver/src/hardware_interface.cpp index d058495..2a293cb 100644 --- a/robotiq_driver/src/hardware_interface.cpp +++ b/robotiq_driver/src/hardware_interface.cpp @@ -40,11 +40,13 @@ #include +const auto kLogger = rclcpp::get_logger("RobotiqGripperHardwareInterface"); + constexpr uint8_t kGripperMinPos = 3; constexpr uint8_t kGripperMaxPos = 230; constexpr uint8_t kGripperRange = kGripperMaxPos - kGripperMinPos; -const auto kLogger = rclcpp::get_logger("RobotiqGripperHardwareInterface"); +constexpr auto kGripperCommsLoopPeriod = std::chrono::milliseconds{ 10 }; namespace robotiq_driver { @@ -53,6 +55,15 @@ RobotiqGripperHardwareInterface::RobotiqGripperHardwareInterface() driver_factory_ = std::make_unique(); } +RobotiqGripperHardwareInterface::~RobotiqGripperHardwareInterface() +{ + communication_thread_is_running_.store(false); + if (communication_thread_.joinable()) + { + communication_thread_.join(); + } +} + // This constructor is use for testing only. RobotiqGripperHardwareInterface::RobotiqGripperHardwareInterface(std::unique_ptr driver_factory) : driver_factory_{ std::move(driver_factory) } @@ -202,56 +213,17 @@ RobotiqGripperHardwareInterface::on_activate(const rclcpp_lifecycle::State& /*pr { driver_->deactivate(); driver_->activate(); + + communication_thread_is_running_.store(true); + communication_thread_ = std::thread([this] { this->background_task(); }); } catch (const std::exception& e) { - RCLCPP_FATAL(kLogger, "Failed to communicate with Gripper. Check Gripper connection."); + RCLCPP_FATAL(kLogger, "Failed to communicate with the Robotiq gripper: %s", e.what()); return CallbackReturn::ERROR; } RCLCPP_INFO(kLogger, "Robotiq Gripper successfully activated!"); - - communication_thread_is_running_.store(true); - - communication_thread_ = std::thread([this] { - // Read from and write to the gripper at 100 Hz. - const auto io_interval = std::chrono::milliseconds(10); - auto last_io = std::chrono::high_resolution_clock::now(); - - while (communication_thread_is_running_.load()) - { - const auto now = std::chrono::high_resolution_clock::now(); - if (now - last_io > io_interval) - { - try - { - // Re-activate the gripper - // (this can be used, for example, to re-run the auto-calibration). - if (reactivate_gripper_async_cmd_.load()) - { - this->driver_->deactivate(); - this->driver_->activate(); - reactivate_gripper_async_cmd_.store(false); - reactivate_gripper_async_response_.store(true); - } - - // Write the latest command to the gripper. - this->driver_->set_gripper_position(write_command_.load()); - - // Read the state of the gripper. - gripper_current_state_.store(this->driver_->get_gripper_position()); - - last_io = now; - } - catch (std::exception& e) - { - RCLCPP_ERROR(kLogger, "Check Robotiq Gripper connection and restart drivers. ERROR: %s", e.what()); - communication_thread_is_running_.store(false); - } - } - } - }); - return CallbackReturn::SUCCESS; } @@ -262,6 +234,10 @@ RobotiqGripperHardwareInterface::on_deactivate(const rclcpp_lifecycle::State& /* communication_thread_is_running_.store(false); communication_thread_.join(); + if (communication_thread_.joinable()) + { + communication_thread_.join(); + } try { @@ -269,10 +245,10 @@ RobotiqGripperHardwareInterface::on_deactivate(const rclcpp_lifecycle::State& /* } catch (const std::exception& e) { - RCLCPP_ERROR(kLogger, "Failed to deactivate gripper. Check Gripper connection"); + RCLCPP_ERROR(kLogger, "Failed to deactivate the Robotiq gripper: %s", e.what()); return CallbackReturn::ERROR; } - + RCLCPP_INFO(kLogger, "Robotiq Gripper successfully deactivated!"); return CallbackReturn::SUCCESS; } @@ -307,6 +283,37 @@ hardware_interface::return_type RobotiqGripperHardwareInterface::write(const rcl return hardware_interface::return_type::OK; } +void RobotiqGripperHardwareInterface::background_task() +{ + while (communication_thread_is_running_.load()) + { + try + { + // Re-activate the gripper + // (this can be used, for example, to re-run the auto-calibration). + if (reactivate_gripper_async_cmd_.load()) + { + this->driver_->deactivate(); + this->driver_->activate(); + reactivate_gripper_async_cmd_.store(false); + reactivate_gripper_async_response_.store(true); + } + + // Write the latest command to the gripper. + this->driver_->set_gripper_position(write_command_.load()); + + // Read the state of the gripper. + gripper_current_state_.store(this->driver_->get_gripper_position()); + } + catch (std::exception& e) + { + RCLCPP_ERROR(kLogger, "Error: %s", e.what()); + } + + std::this_thread::sleep_for(kGripperCommsLoopPeriod); + } +} + } // namespace robotiq_driver #include "pluginlib/class_list_macros.hpp" From 7301c648e72494ba5d392aed576880bc480618ec Mon Sep 17 00:00:00 2001 From: Giovanni Remigi Date: Fri, 8 Sep 2023 19:20:22 +0200 Subject: [PATCH 35/47] Driver refactoring. --- robotiq_driver/src/default_driver.cpp | 40 ++++++--------------------- 1 file changed, 8 insertions(+), 32 deletions(-) diff --git a/robotiq_driver/src/default_driver.cpp b/robotiq_driver/src/default_driver.cpp index 3064c52..0824e89 100644 --- a/robotiq_driver/src/default_driver.cpp +++ b/robotiq_driver/src/default_driver.cpp @@ -138,23 +138,15 @@ void DefaultDriver::activate() throw DriverException{ "Failed to activate the gripper." }; } - try + update_status(); + if (gripper_status_ == GripperStatus::COMPLETED) { - update_status(); - if (gripper_status_ == GripperStatus::COMPLETED) - { - return; - } - - while (gripper_status_ == GripperStatus::IN_PROGRESS) - { - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - update_status(); - } + return; } - catch (const std::exception& e) + while (gripper_status_ == GripperStatus::IN_PROGRESS) { - throw DriverException{ std::string{ "Failed to activate the gripper: " } + e.what() }; + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + update_status(); } } @@ -190,29 +182,13 @@ void DefaultDriver::set_gripper_position(uint8_t pos) uint8_t DefaultDriver::get_gripper_position() { - try - { - update_status(); - } - catch (const std::exception& e) - { - throw DriverException{ std::string{ "Failed to get gripper position: " } + e.what() }; - } - + update_status(); return gripper_position_; } bool DefaultDriver::gripper_is_moving() { - try - { - update_status(); - } - catch (const std::exception& e) - { - throw DriverException{ std::string{ "Failed to get gripper status: " } + e.what() }; - } - + update_status(); return object_detection_status_ == ObjectDetectionStatus::MOVING; } From d719ed290b0c5f08ff34a20c94087e91f2fdcca1 Mon Sep 17 00:00:00 2001 From: Giovanni Remigi Date: Fri, 8 Sep 2023 19:23:34 +0200 Subject: [PATCH 36/47] Fixed pre-commits. --- robotiq_description/launch/robotiq_control.launch.py | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/robotiq_description/launch/robotiq_control.launch.py b/robotiq_description/launch/robotiq_control.launch.py index 497edbe..d9430ee 100644 --- a/robotiq_description/launch/robotiq_control.launch.py +++ b/robotiq_description/launch/robotiq_control.launch.py @@ -49,10 +49,6 @@ def generate_launch_description(): description_pkg_share, "rviz", "view_urdf.rviz" ) - pkg_share = launch_ros.substitutions.FindPackageShare( - package="robotiq_driver" - ).find("robotiq_driver") - args = [] args.append( launch.actions.DeclareLaunchArgument( @@ -70,9 +66,7 @@ def generate_launch_description(): ) args.append( launch.actions.DeclareLaunchArgument( - name="launch_rviz", - default_value="false", - description="Launch RViz?" + name="launch_rviz", default_value="false", description="Launch RViz?" ) ) From a434edfc9447f8bb039a24c0e0d551e493d86236 Mon Sep 17 00:00:00 2001 From: Giovanni Date: Fri, 8 Sep 2023 22:52:28 +0200 Subject: [PATCH 37/47] Update .github/workflows/prerelease-check.yml Co-authored-by: Alex Moriarty --- .github/workflows/prerelease-check.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/prerelease-check.yml b/.github/workflows/prerelease-check.yml index 82695cf..7d24735 100644 --- a/.github/workflows/prerelease-check.yml +++ b/.github/workflows/prerelease-check.yml @@ -17,7 +17,7 @@ on: branch: description: "Chose branch for distro" required: true - default: "master" + default: "main" type: choice options: - foxy From fb01131a66271d4bfd3ce6eee60b95c1a3817645 Mon Sep 17 00:00:00 2001 From: Giovanni Date: Fri, 8 Sep 2023 22:52:36 +0200 Subject: [PATCH 38/47] Update .github/workflows/prerelease-check.yml Co-authored-by: Alex Moriarty --- .github/workflows/prerelease-check.yml | 1 - 1 file changed, 1 deletion(-) diff --git a/.github/workflows/prerelease-check.yml b/.github/workflows/prerelease-check.yml index 7d24735..38319d6 100644 --- a/.github/workflows/prerelease-check.yml +++ b/.github/workflows/prerelease-check.yml @@ -9,7 +9,6 @@ on: default: "rolling" type: choice options: - - foxy - galactic - humble - iron From a5a9468b243cde5910d026b55872c7ea8936ff8d Mon Sep 17 00:00:00 2001 From: Giovanni Date: Fri, 8 Sep 2023 22:52:51 +0200 Subject: [PATCH 39/47] Update .github/workflows/prerelease-check.yml Co-authored-by: Alex Moriarty --- .github/workflows/prerelease-check.yml | 1 - 1 file changed, 1 deletion(-) diff --git a/.github/workflows/prerelease-check.yml b/.github/workflows/prerelease-check.yml index 38319d6..2ade759 100644 --- a/.github/workflows/prerelease-check.yml +++ b/.github/workflows/prerelease-check.yml @@ -9,7 +9,6 @@ on: default: "rolling" type: choice options: - - galactic - humble - iron - rolling From 21d1b4f28addde25f6ad7bba2ffa1058866b2844 Mon Sep 17 00:00:00 2001 From: Giovanni Date: Fri, 8 Sep 2023 22:53:01 +0200 Subject: [PATCH 40/47] Update .github/workflows/prerelease-check.yml Co-authored-by: Alex Moriarty --- .github/workflows/prerelease-check.yml | 1 - 1 file changed, 1 deletion(-) diff --git a/.github/workflows/prerelease-check.yml b/.github/workflows/prerelease-check.yml index 2ade759..6cfde0a 100644 --- a/.github/workflows/prerelease-check.yml +++ b/.github/workflows/prerelease-check.yml @@ -18,7 +18,6 @@ on: default: "main" type: choice options: - - foxy - galactic - humble - iron From c5819d850d6335b677b097a5b05ef6cc59e2b389 Mon Sep 17 00:00:00 2001 From: Giovanni Date: Fri, 8 Sep 2023 22:53:10 +0200 Subject: [PATCH 41/47] Update .github/workflows/prerelease-check.yml Co-authored-by: Alex Moriarty --- .github/workflows/prerelease-check.yml | 1 - 1 file changed, 1 deletion(-) diff --git a/.github/workflows/prerelease-check.yml b/.github/workflows/prerelease-check.yml index 6cfde0a..343abeb 100644 --- a/.github/workflows/prerelease-check.yml +++ b/.github/workflows/prerelease-check.yml @@ -18,7 +18,6 @@ on: default: "main" type: choice options: - - galactic - humble - iron - master From 1658cdfdaee522c863d3406734a14b2825ef3660 Mon Sep 17 00:00:00 2001 From: Giovanni Remigi Date: Fri, 8 Sep 2023 23:15:31 +0200 Subject: [PATCH 42/47] Adding ros2_control_test_assets to CMakeLists.txt --- robotiq_driver/tests/CMakeLists.txt | 3 +++ 1 file changed, 3 insertions(+) diff --git a/robotiq_driver/tests/CMakeLists.txt b/robotiq_driver/tests/CMakeLists.txt index f80e3c4..8c682aa 100644 --- a/robotiq_driver/tests/CMakeLists.txt +++ b/robotiq_driver/tests/CMakeLists.txt @@ -21,6 +21,9 @@ target_include_directories(test_robotiq_gripper_hardware_interface PRIVATE ${CMAKE_CURRENT_SOURCE_DIR} ) target_link_libraries(test_robotiq_gripper_hardware_interface robotiq_driver) +ament_target_dependencies(test_robotiq_gripper_hardware_interface + ros2_control_test_assets +) ############################################################################### # test_default_serial_factory From 9f0a10039c15e1b6f06b6e55bb69c96273be579e Mon Sep 17 00:00:00 2001 From: Giovanni Remigi Date: Fri, 8 Sep 2023 23:38:01 +0200 Subject: [PATCH 43/47] Removed unecessary dependencies. --- robotiq_hardware_tests/CMakeLists.txt | 3 --- 1 file changed, 3 deletions(-) diff --git a/robotiq_hardware_tests/CMakeLists.txt b/robotiq_hardware_tests/CMakeLists.txt index 752b731..6bfb054 100644 --- a/robotiq_hardware_tests/CMakeLists.txt +++ b/robotiq_hardware_tests/CMakeLists.txt @@ -31,9 +31,6 @@ add_executable(full_test src/command_line_utility.hpp src/command_line_utility.cpp ) -target_include_directories(full_test - PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) -target_link_libraries(full_test robotiq_driver::robotiq_driver) ament_target_dependencies(full_test robotiq_driver) ament_package() From 660eec5cce89a7b1d0a1239dbba582aacd62d2ac Mon Sep 17 00:00:00 2001 From: Giovanni Remigi Date: Thu, 14 Sep 2023 16:20:39 +0200 Subject: [PATCH 44/47] Added CRC-16/MODBUS test. --- robotiq_driver/tests/CMakeLists.txt | 11 +++++++ robotiq_driver/tests/test_crc_utils.cpp | 43 +++++++++++++++++++++++++ 2 files changed, 54 insertions(+) create mode 100644 robotiq_driver/tests/test_crc_utils.cpp diff --git a/robotiq_driver/tests/CMakeLists.txt b/robotiq_driver/tests/CMakeLists.txt index 8c682aa..ce9618d 100644 --- a/robotiq_driver/tests/CMakeLists.txt +++ b/robotiq_driver/tests/CMakeLists.txt @@ -48,3 +48,14 @@ target_include_directories(test_default_driver_factory PRIVATE ${CMAKE_CURRENT_SOURCE_DIR} ) target_link_libraries(test_default_driver_factory robotiq_driver) + +############################################################################### +# test_crc_utils + +ament_add_gmock(test_crc_utils + test_crc_utils.cpp +) +target_include_directories(test_crc_utils + PRIVATE ${CMAKE_CURRENT_SOURCE_DIR} +) +target_link_libraries(test_crc_utils robotiq_driver) diff --git a/robotiq_driver/tests/test_crc_utils.cpp b/robotiq_driver/tests/test_crc_utils.cpp new file mode 100644 index 0000000..6c19697 --- /dev/null +++ b/robotiq_driver/tests/test_crc_utils.cpp @@ -0,0 +1,43 @@ +// Copyright 2023 Giovanni Remigi +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Giovanni Remigi nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WAQRRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include +#include + +namespace robotiq_driver::test +{ +TEST(TestCrcUtils, calculate_crc) +{ + ASSERT_EQ(crc_utils::compute_crc({ 0xA1, 0xB2, 0xC3, 0xD4, 0xE5, 0xF6 }), 0x97DA); + ASSERT_EQ(crc_utils::compute_crc({ 0xE2, 0x12, 0xF1, 0xFF, 0x00, 0xD2 }), 0x2D0B); + ASSERT_EQ(crc_utils::compute_crc({ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF }), 0x0194); + ASSERT_EQ(crc_utils::compute_crc({ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }), 0x001B); + ASSERT_EQ(crc_utils::compute_crc({ 0x31, 0x32, 0x33, 0x34, 0x35, 0x36, 0x37, 0x38, 0x39 }), 0x374B); + ASSERT_EQ(crc_utils::compute_crc({ 0x80, 0x00, 0x00, 0x03 }), 0x69E5); +} +} // namespace robotiq_driver::test From 0f41ce9e287eecdd740846dd4885c076e9742066 Mon Sep 17 00:00:00 2001 From: Giovanni Remigi Date: Thu, 14 Sep 2023 16:41:50 +0200 Subject: [PATCH 45/47] Unit tests. --- robotiq_driver/src/data_utils.cpp | 2 +- robotiq_driver/tests/CMakeLists.txt | 11 +++++ robotiq_driver/tests/test_crc_utils.cpp | 6 +-- robotiq_driver/tests/test_data_utils.cpp | 58 ++++++++++++++++++++++++ 4 files changed, 73 insertions(+), 4 deletions(-) create mode 100644 robotiq_driver/tests/test_data_utils.cpp diff --git a/robotiq_driver/src/data_utils.cpp b/robotiq_driver/src/data_utils.cpp index b6c1770..3f24af8 100644 --- a/robotiq_driver/src/data_utils.cpp +++ b/robotiq_driver/src/data_utils.cpp @@ -78,7 +78,7 @@ std::string to_hex(const std::vector& bytes) std::string to_binary_string(const uint8_t byte) { - std::string result = "0x"; + std::string result = ""; for (int i = 7; i >= 0; --i) { result += ((byte >> i) & 1) ? '1' : '0'; diff --git a/robotiq_driver/tests/CMakeLists.txt b/robotiq_driver/tests/CMakeLists.txt index ce9618d..357e1d8 100644 --- a/robotiq_driver/tests/CMakeLists.txt +++ b/robotiq_driver/tests/CMakeLists.txt @@ -59,3 +59,14 @@ target_include_directories(test_crc_utils PRIVATE ${CMAKE_CURRENT_SOURCE_DIR} ) target_link_libraries(test_crc_utils robotiq_driver) + +############################################################################### +# test_data_utils + +ament_add_gmock(test_data_utils + test_data_utils.cpp +) +target_include_directories(test_data_utils + PRIVATE ${CMAKE_CURRENT_SOURCE_DIR} +) +target_link_libraries(test_data_utils robotiq_driver) diff --git a/robotiq_driver/tests/test_crc_utils.cpp b/robotiq_driver/tests/test_crc_utils.cpp index 6c19697..31bb900 100644 --- a/robotiq_driver/tests/test_crc_utils.cpp +++ b/robotiq_driver/tests/test_crc_utils.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Giovanni Remigi +// Copyright (c) 2022 PickNik, Inc. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: @@ -10,12 +10,12 @@ // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // -// * Neither the name of the Giovanni Remigi nor the names of its +// * Neither the name of the {copyright_holder} nor the names of its // contributors may be used to endorse or promote products derived from // this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WAQRRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR diff --git a/robotiq_driver/tests/test_data_utils.cpp b/robotiq_driver/tests/test_data_utils.cpp new file mode 100644 index 0000000..61312c4 --- /dev/null +++ b/robotiq_driver/tests/test_data_utils.cpp @@ -0,0 +1,58 @@ +// Copyright (c) 2022 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +#include +#include + +namespace robotiq_driver::test +{ +TEST(TestDataUtils, uint8_t_to_hex) +{ + ASSERT_EQ(data_utils::to_hex(std::vector{ 255, 121, 56, 33, 125, 60 }), "FF 79 38 21 7D 3C"); +} + +TEST(TestDataUtils, uint16_t_to_hex) +{ + ASSERT_EQ(data_utils::to_hex(std::vector{ 1169, 58544, 14917, 42884, 36112, 16512, 33207, 62584, 30418 }), + "0491 E4B0 3A45 A784 8D10 4080 81B7 F478 76D2"); +} + +TEST(TestDataUtils, to_binary_string) +{ + ASSERT_EQ(data_utils::to_binary_string(155), "10011011"); +} + +TEST(TestDataUtils, get_msb) +{ + ASSERT_EQ(data_utils::get_msb(0x14A2), 0x14); +} + +TEST(TestDataUtils, get_lsb) +{ + ASSERT_EQ(data_utils::get_lsb(0x14A2), 0xA2); +} +} // namespace robotiq_driver::test From f5d7b7a9e1beb32e71e37905c51a126e240e2a04 Mon Sep 17 00:00:00 2001 From: Giovanni Date: Thu, 28 Sep 2023 18:29:48 +0200 Subject: [PATCH 46/47] Update robotiq_driver/CMakeLists.txt Co-authored-by: Alex Moriarty --- robotiq_driver/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robotiq_driver/CMakeLists.txt b/robotiq_driver/CMakeLists.txt index ee4724e..df9f8a6 100644 --- a/robotiq_driver/CMakeLists.txt +++ b/robotiq_driver/CMakeLists.txt @@ -1,7 +1,7 @@ # See https://docs.ros.org/en/humble/How-To-Guides/Ament-CMake-Documentation.html cmake_minimum_required(VERSION 3.8) -project(robotiq_driver VERSION 0.1.0 LANGUAGES CXX) +project(robotiq_driver LANGUAGES CXX) # This module provides installation directories as per the GNU coding standards. include(GNUInstallDirs) From 4c3fd184cad81804d1a9adfa1015354f1d686272 Mon Sep 17 00:00:00 2001 From: Giovanni Remigi Date: Fri, 27 Oct 2023 21:19:28 +0200 Subject: [PATCH 47/47] Removed version from CMakeLists.txt file. --- robotiq_hardware_tests/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robotiq_hardware_tests/CMakeLists.txt b/robotiq_hardware_tests/CMakeLists.txt index 6bfb054..668cf31 100644 --- a/robotiq_hardware_tests/CMakeLists.txt +++ b/robotiq_hardware_tests/CMakeLists.txt @@ -1,7 +1,7 @@ # See https://docs.ros.org/en/foxy/How-To-Guides/Ament-CMake-Documentation.html cmake_minimum_required(VERSION 3.8) -project(robotiq_hardware_tests VERSION 0.0.1 LANGUAGES CXX) +project(robotiq_hardware_tests LANGUAGES CXX) # This module provides installation directories as per the GNU coding standards. include(GNUInstallDirs)