Skip to content

Commit

Permalink
Refactoring for testing.
Browse files Browse the repository at this point in the history
  • Loading branch information
kineticsystem committed Sep 6, 2023
1 parent de63c83 commit 312ab08
Show file tree
Hide file tree
Showing 26 changed files with 1,383 additions and 224 deletions.
18 changes: 17 additions & 1 deletion robotiq_driver/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand Down
Original file line number Diff line number Diff line change
@@ -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:
Expand Down Expand Up @@ -31,4 +31,12 @@
#include <cstdint>
#include <vector>

uint16_t computeCRC(const std::vector<uint8_t>& 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<uint8_t>& data);
} // namespace robotiq_driver::crc_utils
76 changes: 76 additions & 0 deletions robotiq_driver/include/robotiq_driver/data_utils.hpp
Original file line number Diff line number Diff line change
@@ -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 <array>
#include <cstdint>
#include <string>
#include <vector>

/**
* 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<uint8_t>& 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<uint16_t>& 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
30 changes: 10 additions & 20 deletions robotiq_driver/include/robotiq_driver/default_driver.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,9 @@
#pragma once

#include <robotiq_driver/driver.hpp>
#include <serial/serial.h>
#include <robotiq_driver/serial.hpp>

#include <memory>
#include <string>
#include <vector>
/**
Expand All @@ -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> 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;
Expand All @@ -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;
Expand Down Expand Up @@ -124,9 +115,8 @@ class DefaultDriver : public Driver
*/
void update_status();

serial::Serial port_;
uint8_t slave_id_;
std::vector<uint8_t> read_command_;
std::unique_ptr<Serial> serial_ = nullptr;
uint8_t slave_address_;

ActivationStatus activation_status_;
ActionStatus action_status_;
Expand Down
73 changes: 73 additions & 0 deletions robotiq_driver/include/robotiq_driver/default_serial.hpp
Original file line number Diff line number Diff line change
@@ -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 <robotiq_driver/serial.hpp>
#include <memory>

#include <serial/serial.h>

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<uint8_t> read(size_t size = 1) override;
void write(const std::vector<uint8_t>& 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::Serial> serial_ = nullptr;
};
} // namespace robotiq_driver
58 changes: 58 additions & 0 deletions robotiq_driver/include/robotiq_driver/default_serial_factory.hpp
Original file line number Diff line number Diff line change
@@ -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 <robotiq_driver/serial_factory.hpp>
#include <robotiq_driver/default_serial_factory.hpp>
#include <hardware_interface/hardware_info.hpp>

#include <memory>

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<Serial> create(const hardware_interface::HardwareInfo& info) const;

protected:
// Seam for testing.
virtual std::unique_ptr<Serial> create_serial() const;
};
} // namespace robotiq_driver
2 changes: 2 additions & 0 deletions robotiq_driver/include/robotiq_driver/driver.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down
Loading

0 comments on commit 312ab08

Please sign in to comment.