From da32fbf46a600243ce794b36c2b21528fb24033d Mon Sep 17 00:00:00 2001 From: Chandima Fernando Date: Tue, 12 Mar 2024 18:15:56 +0000 Subject: [PATCH 01/32] feat: add docker for serial lib --- docker/ur-hande-control/Dockerfile | 29 +++++++++++++++++++++++++++++ 1 file changed, 29 insertions(+) create mode 100644 docker/ur-hande-control/Dockerfile diff --git a/docker/ur-hande-control/Dockerfile b/docker/ur-hande-control/Dockerfile new file mode 100644 index 00000000..6244b51d --- /dev/null +++ b/docker/ur-hande-control/Dockerfile @@ -0,0 +1,29 @@ +FROM osrf/ros:humble-desktop-full + + +RUN apt-get update \ + && apt-get -y install \ + python3-pip \ + ros-${ROS_DISTRO}-rclpy \ + ros-${ROS_DISTRO}-ur \ + build-essential \ + python3-colcon-common-extensions \ + # + # Clean up + && apt-get autoremove -y \ + && apt-get clean -y \ + && rm -rf /var/lib/apt/lists/* +RUN pip3 install rosdep && \ + rosdep update + +RUN mkdir /root/ws/src -p + +WORKDIR /root/ws/src + +RUN git clone https://github.com/PickNikRobotics/serial-release.git -b release/humble/serial + +WORKDIR /root/ws/ + +RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash && \ + colcon build --symlink-install --packages-select serial" + From f19c7045af849b98e66bc33977204be586f4ec92 Mon Sep 17 00:00:00 2001 From: Chandima Fernando Date: Mon, 18 Mar 2024 18:50:14 +0000 Subject: [PATCH 02/32] feat: gripper control --- src/robotiq_driver/CMakeLists.txt | 121 ++++++++++++++++++++++++++++++ 1 file changed, 121 insertions(+) create mode 100644 src/robotiq_driver/CMakeLists.txt diff --git a/src/robotiq_driver/CMakeLists.txt b/src/robotiq_driver/CMakeLists.txt new file mode 100644 index 00000000..28297ec1 --- /dev/null +++ b/src/robotiq_driver/CMakeLists.txt @@ -0,0 +1,121 @@ +cmake_minimum_required(VERSION 3.8) +project(robotiq_driver) + +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) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(serial REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(pdf_beamtime_interfaces REQUIRED) + +set(THIS_PACKAGE_INCLUDE_DEPENDS + ament_cmake + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + serial + pdf_beamtime_interfaces +) + +add_library( + ${PROJECT_NAME} + SHARED + src/crc.cpp + src/hardware_interface.cpp + src/robotiq_gripper_interface.cpp +) + +# Generate interfaces for actions and services +rosidl_generate_interfaces(${PROJECT_NAME}_lib + "srv/GriiperControlMsg.srv" +) + +target_include_directories( + ${PROJECT_NAME} + PUBLIC + $ + $ +) +ament_target_dependencies( + ${PROJECT_NAME} + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) + +pluginlib_export_plugin_description_file(hardware_interface hardware_interface_plugin.xml) + +add_executable(gripper_service src/gripper_service.cpp) +# add_executable(gripper_cmds src/test_interface.cpp) + +#add_executable(gripper_interface_test src/gripper_interface_test.cpp) +target_include_directories(gripper_service PRIVATE include) +ament_target_dependencies(gripper_service serial pdf_beamtime_interfaces) +target_link_libraries(gripper_service ${PROJECT_NAME}) + +# target_include_directories(gripper_cmds PRIVATE include) +# ament_target_dependencies(gripper_cmds serial pdf_beamtime_interfaces) +# target_link_libraries(gripper_cmds ${PROJECT_NAME}) + + +# INSTALL +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib/${PROJECT_NAME} + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) +install( + TARGETS gripper_service + # gripper_cmds + + DESTINATION lib/${PROJECT_NAME} +) +install( + DIRECTORY include/ + DESTINATION include +) +install( + DIRECTORY config + DESTINATION share/${PROJECT_NAME} +) +install( + DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) + +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) + ament_lint_auto_find_test_dependencies() +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 2894bd045aaa0f00f0836a53bb45129c39957b45 Mon Sep 17 00:00:00 2001 From: Chandima Fernando Date: Mon, 18 Mar 2024 18:50:30 +0000 Subject: [PATCH 03/32] feat: gripper control --- docker/docker-compose.yml | 5 + src/pdf_beamtime_interfaces/CMakeLists.txt | 1 + .../srv/GriiperControlMsg.srv | 3 + .../hardware_interface_plugin.xml | 7 + .../include/robotiq_driver/crc.hpp | 6 + .../robotiq_driver/gripper_service.hpp | 24 ++ .../robotiq_driver/hardware_interface.hpp | 102 +++++ .../robotiq_gripper_interface.hpp | 162 ++++++++ .../robotiq_driver/visibility_control.h | 56 +++ src/robotiq_driver/package.xml | 28 ++ src/robotiq_driver/src/crc.cpp | 77 ++++ src/robotiq_driver/src/gripper_service.cpp | 112 ++++++ src/robotiq_driver/src/hardware_interface.cpp | 277 ++++++++++++++ .../src/robotiq_gripper_interface.cpp | 348 ++++++++++++++++++ src/robotiq_driver/srv/GriiperControlMsg.srv | 3 + src/serial-release | 1 + 16 files changed, 1212 insertions(+) create mode 100644 src/pdf_beamtime_interfaces/srv/GriiperControlMsg.srv create mode 100644 src/robotiq_driver/hardware_interface_plugin.xml create mode 100644 src/robotiq_driver/include/robotiq_driver/crc.hpp create mode 100644 src/robotiq_driver/include/robotiq_driver/gripper_service.hpp create mode 100644 src/robotiq_driver/include/robotiq_driver/hardware_interface.hpp create mode 100644 src/robotiq_driver/include/robotiq_driver/robotiq_gripper_interface.hpp create mode 100644 src/robotiq_driver/include/robotiq_driver/visibility_control.h create mode 100644 src/robotiq_driver/package.xml create mode 100644 src/robotiq_driver/src/crc.cpp create mode 100644 src/robotiq_driver/src/gripper_service.cpp create mode 100644 src/robotiq_driver/src/hardware_interface.cpp create mode 100644 src/robotiq_driver/src/robotiq_gripper_interface.cpp create mode 100644 src/robotiq_driver/srv/GriiperControlMsg.srv create mode 160000 src/serial-release diff --git a/docker/docker-compose.yml b/docker/docker-compose.yml index 4a9bf08b..b76ed0e2 100644 --- a/docker/docker-compose.yml +++ b/docker/docker-compose.yml @@ -53,6 +53,11 @@ services: depends_on: - urdriver + urhandecontrol: + image: ur-hande-control:latest + networks: + - ursim_net + networks: ursim_net: ipam: diff --git a/src/pdf_beamtime_interfaces/CMakeLists.txt b/src/pdf_beamtime_interfaces/CMakeLists.txt index 195d0ffa..9d513bc9 100644 --- a/src/pdf_beamtime_interfaces/CMakeLists.txt +++ b/src/pdf_beamtime_interfaces/CMakeLists.txt @@ -17,6 +17,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/BoxObstacleMsg.srv" "srv/CylinderObstacleMsg.srv" "srv/BlueskyInterruptMsg.srv" + # "srv/GriiperControlMsg.srv" ) diff --git a/src/pdf_beamtime_interfaces/srv/GriiperControlMsg.srv b/src/pdf_beamtime_interfaces/srv/GriiperControlMsg.srv new file mode 100644 index 00000000..3c32b01f --- /dev/null +++ b/src/pdf_beamtime_interfaces/srv/GriiperControlMsg.srv @@ -0,0 +1,3 @@ +string command +--- +string results diff --git a/src/robotiq_driver/hardware_interface_plugin.xml b/src/robotiq_driver/hardware_interface_plugin.xml new file mode 100644 index 00000000..6d0a84ed --- /dev/null +++ b/src/robotiq_driver/hardware_interface_plugin.xml @@ -0,0 +1,7 @@ + + + + ROS2 controller for the Robotiq gripper. + + + diff --git a/src/robotiq_driver/include/robotiq_driver/crc.hpp b/src/robotiq_driver/include/robotiq_driver/crc.hpp new file mode 100644 index 00000000..869fcaed --- /dev/null +++ b/src/robotiq_driver/include/robotiq_driver/crc.hpp @@ -0,0 +1,6 @@ +#pragma once + +#include +#include + +uint16_t computeCRC(const std::vector & cmd); diff --git a/src/robotiq_driver/include/robotiq_driver/gripper_service.hpp b/src/robotiq_driver/include/robotiq_driver/gripper_service.hpp new file mode 100644 index 00000000..bc72b569 --- /dev/null +++ b/src/robotiq_driver/include/robotiq_driver/gripper_service.hpp @@ -0,0 +1,24 @@ +/*Copyright 2023 Brookhaven National Laboratory +BSD 3 Clause License. See LICENSE.txt for details.*/ +#pragma once + +#include +#include +#include "rclcpp/rclcpp.hpp" + +#include +// #include "pdf_beamtime_interface/srv/gripper_cmd.hpp" + +#include + +class GripperService +{ + +private: + const char * kComPort = "/tmp/ttyUR"; + const int kSlaveID = 0x09; + RobotiqGripperInterface gripper_; + +public: + GripperService(); +}; diff --git a/src/robotiq_driver/include/robotiq_driver/hardware_interface.hpp b/src/robotiq_driver/include/robotiq_driver/hardware_interface.hpp new file mode 100644 index 00000000..6c3353f3 --- /dev/null +++ b/src/robotiq_driver/include/robotiq_driver/hardware_interface.hpp @@ -0,0 +1,102 @@ +// 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 "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/visibility_control.h" +#include "robotiq_driver/robotiq_gripper_interface.hpp" + +namespace robotiq_driver +{ +class RobotiqGripperHardwareInterface : public hardware_interface::SystemInterface +{ +public: + RCLCPP_SHARED_PTR_DEFINITIONS(RobotiqGripperHardwareInterface) + + ROBOTIQ_DRIVER_PUBLIC + RobotiqGripperHardwareInterface(); + + ROBOTIQ_DRIVER_PUBLIC + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; + + ROBOTIQ_DRIVER_PUBLIC + std::vector export_state_interfaces() override; + + ROBOTIQ_DRIVER_PUBLIC + std::vector export_command_interfaces() override; + + ROBOTIQ_DRIVER_PUBLIC + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + + ROBOTIQ_DRIVER_PUBLIC + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + + ROBOTIQ_DRIVER_PUBLIC + hardware_interface::return_type read( + const rclcpp::Time & time, + const rclcpp::Duration & period) override; + + 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_; + std::unique_ptr gripper_interface_; + + double gripper_closed_pos_; + std::string com_port_; + + std::thread command_interface_; + std::atomic command_interface_is_running_; + std::atomic write_command_; + std::atomic gripper_current_state_; + + double reactivate_gripper_cmd_; + std::atomic reactivate_gripper_async_cmd_; + double reactivate_gripper_response_; + std::atomic> reactivate_gripper_async_response_; +}; + +} // namespace robotiq_driver diff --git a/src/robotiq_driver/include/robotiq_driver/robotiq_gripper_interface.hpp b/src/robotiq_driver/include/robotiq_driver/robotiq_gripper_interface.hpp new file mode 100644 index 00000000..59232056 --- /dev/null +++ b/src/robotiq_driver/include/robotiq_driver/robotiq_gripper_interface.hpp @@ -0,0 +1,162 @@ +// 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 + +/** + * @brief This class is responsible for communicating with the gripper via a serial port, and maintaining a record of + * the gripper's current state. + * + */ +class RobotiqGripperInterface +{ +public: + RobotiqGripperInterface(const std::string& com_port = "/tmp/ttyUR", uint8_t slave_id = 0x09); + + /** + * @brief Activates the gripper. + * + * @throw serial::IOException on failure to successfully communicate with gripper port + */ + void activateGripper(); + + /** + * @brief Deactivates the gripper. + * + * @throw serial::IOException on failure to successfully communicate with gripper port + */ + void deactivateGripper(); + + /** + * @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); + + /** + * @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 getGripperPosition(); + + /** + * @brief Returns true if the gripper is currently moving, false otherwise. + * + */ + bool gripperIsMoving(); + + /** + * @brief Set the speed of the gripper. + * + * @param speed A value between 0x00 (stopped) and 0xFF (full speed). + */ + void setSpeed(uint8_t speed); + + /** + * @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 + }; + +private: + std::vector createReadCommand(uint16_t first_register, uint8_t num_registers); + std::vector createWriteCommand(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 + */ + std::vector readResponse(size_t num_bytes); + + /** + * @brief Send a command to the gripper. + * + * @param cmd The command. + * @throw serial::IOException on failure to successfully communicate with gripper port + */ + void sendCommand(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(); + + serial::Serial port_; + uint8_t slave_id_; + std::vector read_command_; + + ActivationStatus activation_status_; + ActionStatus action_status_; + GripperStatus gripper_status_; + ObjectDetectionStatus object_detection_status_; + + uint8_t gripper_position_; + uint8_t commanded_gripper_speed_; + uint8_t commanded_gripper_force_; +}; diff --git a/src/robotiq_driver/include/robotiq_driver/visibility_control.h b/src/robotiq_driver/include/robotiq_driver/visibility_control.h new file mode 100644 index 00000000..df5a56e4 --- /dev/null +++ b/src/robotiq_driver/include/robotiq_driver/visibility_control.h @@ -0,0 +1,56 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef ROBOTIQ_DRIVER__VISIBILITY_CONTROL_H_ +#define ROBOTIQ_DRIVER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define ROBOTIQ_DRIVER_EXPORT __attribute__((dllexport)) +#define ROBOTIQ_DRIVER_IMPORT __attribute__((dllimport)) +#else +#define ROBOTIQ_DRIVER_EXPORT __declspec(dllexport) +#define ROBOTIQ_DRIVER_IMPORT __declspec(dllimport) +#endif +#ifdef ROBOTIQ_DRIVER_BUILDING_DLL +#define ROBOTIQ_DRIVER_PUBLIC ROBOTIQ_DRIVER_EXPORT +#else +#define ROBOTIQ_DRIVER_PUBLIC ROBOTIQ_DRIVER_IMPORT +#endif +#define ROBOTIQ_DRIVER_PUBLIC_TYPE ROBOTIQ_DRIVER_PUBLIC +#define ROBOTIQ_DRIVER_LOCAL +#else +#define ROBOTIQ_DRIVER_EXPORT __attribute__((visibility("default"))) +#define ROBOTIQ_DRIVER_IMPORT +#if __GNUC__ >= 4 +#define ROBOTIQ_DRIVER_PUBLIC __attribute__((visibility("default"))) +#define ROBOTIQ_DRIVER_LOCAL __attribute__((visibility("hidden"))) +#else +#define ROBOTIQ_DRIVER_PUBLIC +#define ROBOTIQ_DRIVER_LOCAL +#endif +#define ROBOTIQ_DRIVER_PUBLIC_TYPE +#endif + +#endif // ROBOTIQ_DRIVER__VISIBILITY_CONTROL_H_ diff --git a/src/robotiq_driver/package.xml b/src/robotiq_driver/package.xml new file mode 100644 index 00000000..be87ec74 --- /dev/null +++ b/src/robotiq_driver/package.xml @@ -0,0 +1,28 @@ + + + + robotiq_driver + 0.0.0 + ROS2 driver package for the Robotiq gripper. + Cory Crean + BSD + + ament_cmake + + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + serial + rosidl_default_generators + pdf_beamtime_interfaces + + ament_lint_auto + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + + diff --git a/src/robotiq_driver/src/crc.cpp b/src/robotiq_driver/src/crc.cpp new file mode 100644 index 00000000..6ae6bf66 --- /dev/null +++ b/src/robotiq_driver/src/crc.cpp @@ -0,0 +1,77 @@ +#include "robotiq_driver/crc.hpp" + +/* 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 +}; + +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/src/robotiq_driver/src/gripper_service.cpp b/src/robotiq_driver/src/gripper_service.cpp new file mode 100644 index 00000000..6fde6a71 --- /dev/null +++ b/src/robotiq_driver/src/gripper_service.cpp @@ -0,0 +1,112 @@ +// constexpr auto kComPort = "/tmp/ttyUR"; +// constexpr auto kSlaveID = 0x09; + +#include + + +GripperService::GripperService() +{ + RobotiqGripperInterface gripper(kComPort, kSlaveID); +} + +void gripper_controller( + const std::shared_ptr request, + std::shared_ptr response) +{ + + // This function sends the gripper control command + bool status = false; + try { + char cmd = request->cmd; + + switch (cmd) { + case 'A': + // Activate the gripper + gripper.deactivateGripper(); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + gripper.activateGripper(); + RCLCPP_INFO(rclcpp::get_logger("gripper_controller"), "Activation successful"); + + break; + + case 'D': + // Deactivate the gripper + gripper.deactivateGripper(); + RCLCPP_INFO(rclcpp::get_logger("gripper_controller"), "Deactivated"); + + break; + + case 'M': + { + // Closes the gripper to the percentage set by request->grip + uint8_t val = request->grip * 2.55; // convert the scales from 01-100 to 0-255 + // std::cout << "######### request and val : " << std::endl ; + // std::cout << static_cast(request->grip) << std::endl ; + // std::cout << static_cast(val) << std::endl ; + gripper.setGripperPosition(val); + RCLCPP_INFO(rclcpp::get_logger("gripper_controller"), "Gripper Open"); + } + break; + + case 'O': + /* Open the grippper fully */ + gripper.setGripperPosition(0x00); + RCLCPP_INFO(rclcpp::get_logger("gripper_controller"), "Gripper Open"); + break; + + case 'C': + /* Close the grippper fully */ + gripper.setGripperPosition(0xFF); + RCLCPP_INFO(rclcpp::get_logger("gripper_controller"), "Gripper Open"); + break; + + // case 'S': + // /* Close the grippper fully */ + // gripper.setGripperPosition(0xFF); + // break; + + default: + break; + } + + status = true; + } catch (const std::exception & e) { + std::cerr << e.what() << '\n'; + status = false; + } + + // Send the response back + response->status = status; + +} + +int main(int argc, char ** argv) +{ + + rclcpp::init(argc, argv); + + std::shared_ptr node = rclcpp::Node::make_shared("gripper_service"); // CHANGE + + RCLCPP_INFO(rclcpp::get_logger("gripper_controller"), "Activate the gripper ..."); + // Clear the registers + gripper.deactivateGripper(); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + // Activate the gripper + gripper.activateGripper(); + + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + gripper.setSpeed(0x0F); + + RCLCPP_INFO(rclcpp::get_logger("gripper_controller"), "Activation successful"); + + rclcpp::Service::SharedPtr service = + node->create_service("gripper_service", &gripper_controller); // CHANGE + + RCLCPP_INFO(rclcpp::get_logger("gripper_controller"), "Ready to recieve gripper commands."); // CHANGE + + rclcpp::spin(node); + rclcpp::shutdown(); + + return 0; + +} diff --git a/src/robotiq_driver/src/hardware_interface.cpp b/src/robotiq_driver/src/hardware_interface.cpp new file mode 100644 index 00000000..0beda015 --- /dev/null +++ b/src/robotiq_driver/src/hardware_interface.cpp @@ -0,0 +1,277 @@ +// 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/hardware_interface.hpp" + +#include +#include +#include +#include +#include + +#include "hardware_interface/actuator_interface.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" +#include + +constexpr uint8_t kGripperMinPos = 3; +constexpr uint8_t kGripperMaxPos = 230; +constexpr uint8_t kGripperRange = kGripperMaxPos - kGripperMinPos; + +const auto kLogger = rclcpp::get_logger("RobotiqGripperHardwareInterface"); + +namespace robotiq_driver +{ +RobotiqGripperHardwareInterface::RobotiqGripperHardwareInterface() +{ +} + +hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init( + const hardware_interface::HardwareInfo & info) +{ + 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(); + gripper_position_command_ = std::numeric_limits::quiet_NaN(); + reactivate_gripper_cmd_ = NO_NEW_CMD_; + reactivate_gripper_async_cmd_.store(false); + + 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()); + 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); + 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()); + return CallbackReturn::ERROR; + } + + 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); + return CallbackReturn::ERROR; + } + } + + 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); + } catch (const serial::IOException & e) { + RCLCPP_FATAL(kLogger, "Failed to open gripper port."); + return CallbackReturn::ERROR; + } + + return CallbackReturn::SUCCESS; +} + +std::vector RobotiqGripperHardwareInterface:: +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_)); + + return state_interfaces; +} + +std::vector RobotiqGripperHardwareInterface:: +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_)); + 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_)); + + return command_interfaces; +} + +hardware_interface::CallbackReturn +RobotiqGripperHardwareInterface::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) +{ + // set some default values for joints + if (std::isnan(gripper_position_)) { + gripper_position_ = 0; + gripper_velocity_ = 0; + gripper_position_command_ = 0; + } + + // Activate the gripper. + try { + gripper_interface_->deactivateGripper(); + gripper_interface_->activateGripper(); + } catch (const serial::IOException & e) { + RCLCPP_FATAL(kLogger, "Failed to communicate with Gripper. Check Gripper connection."); + return CallbackReturn::ERROR; + } + + RCLCPP_INFO(kLogger, "Robotiq Gripper successfully activated!"); + + command_interface_is_running_.store(true); + + command_interface_ = 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()) { + 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->gripper_interface_->deactivateGripper(); + this->gripper_interface_->activateGripper(); + 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()); + + // Read the state of the gripper. + gripper_current_state_.store(this->gripper_interface_->getGripperPosition()); + + last_io = now; + } catch (serial::IOException & e) { + RCLCPP_ERROR(kLogger, "Check Robotiq Gripper connection and restart drivers."); + command_interface_is_running_.store(false); + } + } + } + }); + + return CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn +RobotiqGripperHardwareInterface::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) +{ + command_interface_is_running_.store(false); + command_interface_.join(); + + try { + gripper_interface_->deactivateGripper(); + } catch (const std::exception & e) { + RCLCPP_ERROR(kLogger, "Failed to deactivate gripper. Check Gripper connection"); + return CallbackReturn::ERROR; + } + + return CallbackReturn::SUCCESS; +} + +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; + + 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()) { + reactivate_gripper_response_ = reactivate_gripper_async_response_.load().value(); + reactivate_gripper_async_response_.store(std::nullopt); + } + + return hardware_interface::return_type::OK; +} + +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; + gripper_pos = std::max(std::min(gripper_pos, 255.0), 0.0); + write_command_.store(uint8_t(gripper_pos)); + + return hardware_interface::return_type::OK; +} + +} // namespace robotiq_driver + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + robotiq_driver::RobotiqGripperHardwareInterface, + hardware_interface::SystemInterface) diff --git a/src/robotiq_driver/src/robotiq_gripper_interface.cpp b/src/robotiq_driver/src/robotiq_gripper_interface.cpp new file mode 100644 index 00000000..9c7a957c --- /dev/null +++ b/src/robotiq_driver/src/robotiq_gripper_interface.cpp @@ -0,0 +1,348 @@ +// 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/robotiq_gripper_interface.hpp" +#include "robotiq_driver/crc.hpp" + +#include +#include +#include +#include + +constexpr int kBaudRate = 115200; +constexpr auto kTimeoutMilliseconds = 1000; + +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) +// number of data bytes (1 byte) +// data bytes (2 bytes per register) +// CRC (2 bytes) +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) +// address of the first register that was written (2 bytes) +// number of registers written (2 bytes) +// CRC (2 bytes) +constexpr int kWriteResponseSize = 8; + +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 getSecondByte(uint16_t val) +{ + return val & 0x00FF; +} + +RobotiqGripperInterface::RobotiqGripperInterface(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)) + , commanded_gripper_speed_(0x80) + , commanded_gripper_force_(0x80) +{ + if (!port_.isOpen()) { + THROW(serial::IOException, "Failed to open gripper port."); + } +} + +void RobotiqGripperInterface::activateGripper() +{ + const auto cmd = createWriteCommand( + kActionRequestRegister, {0x0100, 0x0000, 0x0000} // set rACT to 1, clear all + // other registers. + ); + + // std::stringstream ss; + // for (std::size_t i = 0; i < cmd.size(); i++) + // { + // if (i != 0) + // { + // ss << ", "; + // } + // ss << "0x" << std::hex << static_cast(cmd[i]); + // } + // std::cout << ss.str() < RobotiqGripperInterface::createReadCommand( + 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)}; + auto crc = computeCRC(cmd); + cmd.push_back(getFirstByte(crc)); + cmd.push_back(getSecondByte(crc)); + return cmd; +} + +void RobotiqGripperInterface::setSpeed(uint8_t speed) +{ + commanded_gripper_speed_ = speed; +} + +void RobotiqGripperInterface::setForce(uint8_t force) +{ + commanded_gripper_force_ = force; +} + +std::vector RobotiqGripperInterface::createWriteCommand( + uint16_t first_register, + const std::vector & data) +{ + + // std::stringstream ss; + // for (std::size_t i = 0; i < data.size(); i++) + // { + // if (i != 0) + // { + // ss << ", "; + // } + // ss << "0x" << std::hex << static_cast(data[i]); + // } + // std::cout << ss.str() < 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)); + } + + auto crc = computeCRC(cmd); + cmd.push_back(getFirstByte(crc)); + cmd.push_back(getSecondByte(crc)); + + // std::cout << " The final command cmd: " << std::endl ; + // std::stringstream ss3; + // for (std::size_t i = 0; i < cmd.size(); i++) + // { + // if (i != 0) + // { + // ss3 << ", "; + // } + // ss3 << "0x" << std::hex << static_cast(cmd[i]); + // } + // std::cout << ss3.str() < RobotiqGripperInterface::readResponse(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); + THROW(serial::IOException, error_msg.c_str()); + } + + return response; +} + +void RobotiqGripperInterface::sendCommand(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()); + } +} + +void RobotiqGripperInterface::updateStatus() +{ + // Tell the gripper that we want to read its status. + try { + sendCommand(read_command_); + + const auto response = readResponse(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; + + // Action status. + action_status_ = + ((gripper_status_byte & 0x08) == 0x00) ? ActionStatus::STOPPED : ActionStatus::MOVING; + + // 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; + } + + // 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; + } + + // Read the current gripper position. + gripper_position_ = response[kResponseHeaderSize + kPositionIndex]; + } catch (const serial::IOException & e) { + std::cerr << "Failed to update gripper status.\n"; + throw; + } +} diff --git a/src/robotiq_driver/srv/GriiperControlMsg.srv b/src/robotiq_driver/srv/GriiperControlMsg.srv new file mode 100644 index 00000000..3c32b01f --- /dev/null +++ b/src/robotiq_driver/srv/GriiperControlMsg.srv @@ -0,0 +1,3 @@ +string command +--- +string results diff --git a/src/serial-release b/src/serial-release new file mode 160000 index 00000000..8edf5917 --- /dev/null +++ b/src/serial-release @@ -0,0 +1 @@ +Subproject commit 8edf5917d226c33efaf1916ef0fa74f1b70e53d5 From 243b821bea3219098560940330030e3ed5726b17 Mon Sep 17 00:00:00 2001 From: Chandima Fernando Date: Tue, 19 Mar 2024 18:29:21 +0000 Subject: [PATCH 04/32] fix: gripper bugs --- .vscode/settings.json | 75 ++++++++++++- src/hello_moveit/src/hello_moveit.cpp | 2 +- src/pdf_beamtime_interfaces/CMakeLists.txt | 2 +- .../srv/GriiperControlMsg.srv | 3 - .../srv/GripperControlMsg.srv | 4 + src/robotiq_driver/CMakeLists.txt | 7 +- .../robotiq_driver/gripper_service.hpp | 25 ++++- src/robotiq_driver/src/gripper_service.cpp | 101 ++++++++++-------- src/robotiq_driver/srv/GriiperControlMsg.srv | 3 - 9 files changed, 157 insertions(+), 65 deletions(-) delete mode 100644 src/pdf_beamtime_interfaces/srv/GriiperControlMsg.srv create mode 100644 src/pdf_beamtime_interfaces/srv/GripperControlMsg.srv delete mode 100644 src/robotiq_driver/srv/GriiperControlMsg.srv diff --git a/.vscode/settings.json b/.vscode/settings.json index edd644ad..6736fc0d 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -7,7 +7,80 @@ "*.repos": "yaml", "*.world": "xml", "*.xacro": "xml", - "functional": "cpp" + "functional": "cpp", + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "csignal": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "any": "cpp", + "array": "cpp", + "atomic": "cpp", + "strstream": "cpp", + "bit": "cpp", + "*.tcc": "cpp", + "bitset": "cpp", + "chrono": "cpp", + "codecvt": "cpp", + "compare": "cpp", + "complex": "cpp", + "concepts": "cpp", + "condition_variable": "cpp", + "cstdint": "cpp", + "deque": "cpp", + "forward_list": "cpp", + "list": "cpp", + "map": "cpp", + "set": "cpp", + "string": "cpp", + "unordered_map": "cpp", + "unordered_set": "cpp", + "vector": "cpp", + "exception": "cpp", + "algorithm": "cpp", + "iterator": "cpp", + "memory": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "optional": "cpp", + "random": "cpp", + "ratio": "cpp", + "string_view": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "fstream": "cpp", + "future": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "mutex": "cpp", + "new": "cpp", + "numbers": "cpp", + "ostream": "cpp", + "semaphore": "cpp", + "shared_mutex": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "stop_token": "cpp", + "streambuf": "cpp", + "thread": "cpp", + "cfenv": "cpp", + "cinttypes": "cpp", + "typeindex": "cpp", + "typeinfo": "cpp", + "variant": "cpp" }, "python.analysis.extraPaths": [ "/opt/ros/humble/lib/python3.10/site-packages/" diff --git a/src/hello_moveit/src/hello_moveit.cpp b/src/hello_moveit/src/hello_moveit.cpp index a7e5ea7a..46ef02ae 100644 --- a/src/hello_moveit/src/hello_moveit.cpp +++ b/src/hello_moveit/src/hello_moveit.cpp @@ -55,7 +55,7 @@ int main(int argc, char * argv[]) // Create the MoveIt MoveGroup Interface RCLCPP_INFO(logger, "assembling move_group_interface"); using moveit::planning_interface::MoveGroupInterface; - auto move_group_interface = MoveGroupInterface(node, "ur_manipulator"); + auto move_group_interface = MoveGroupInterface(node, "ur_arm"); // Set a target Pose auto const target_pose = [] { diff --git a/src/pdf_beamtime_interfaces/CMakeLists.txt b/src/pdf_beamtime_interfaces/CMakeLists.txt index 9d513bc9..64bf3e47 100644 --- a/src/pdf_beamtime_interfaces/CMakeLists.txt +++ b/src/pdf_beamtime_interfaces/CMakeLists.txt @@ -17,7 +17,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/BoxObstacleMsg.srv" "srv/CylinderObstacleMsg.srv" "srv/BlueskyInterruptMsg.srv" - # "srv/GriiperControlMsg.srv" + "srv/GripperControlMsg.srv" ) diff --git a/src/pdf_beamtime_interfaces/srv/GriiperControlMsg.srv b/src/pdf_beamtime_interfaces/srv/GriiperControlMsg.srv deleted file mode 100644 index 3c32b01f..00000000 --- a/src/pdf_beamtime_interfaces/srv/GriiperControlMsg.srv +++ /dev/null @@ -1,3 +0,0 @@ -string command ---- -string results diff --git a/src/pdf_beamtime_interfaces/srv/GripperControlMsg.srv b/src/pdf_beamtime_interfaces/srv/GripperControlMsg.srv new file mode 100644 index 00000000..cdeb7b18 --- /dev/null +++ b/src/pdf_beamtime_interfaces/srv/GripperControlMsg.srv @@ -0,0 +1,4 @@ +string command +int32 grip # 0-100, percentage of open/close of the gripper +--- +int32 results diff --git a/src/robotiq_driver/CMakeLists.txt b/src/robotiq_driver/CMakeLists.txt index 28297ec1..76df0de2 100644 --- a/src/robotiq_driver/CMakeLists.txt +++ b/src/robotiq_driver/CMakeLists.txt @@ -5,7 +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) @@ -34,17 +33,13 @@ add_library( src/robotiq_gripper_interface.cpp ) -# Generate interfaces for actions and services -rosidl_generate_interfaces(${PROJECT_NAME}_lib - "srv/GriiperControlMsg.srv" -) - target_include_directories( ${PROJECT_NAME} PUBLIC $ $ ) + ament_target_dependencies( ${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS} diff --git a/src/robotiq_driver/include/robotiq_driver/gripper_service.hpp b/src/robotiq_driver/include/robotiq_driver/gripper_service.hpp index bc72b569..b1ba3317 100644 --- a/src/robotiq_driver/include/robotiq_driver/gripper_service.hpp +++ b/src/robotiq_driver/include/robotiq_driver/gripper_service.hpp @@ -6,9 +6,9 @@ BSD 3 Clause License. See LICENSE.txt for details.*/ #include #include "rclcpp/rclcpp.hpp" -#include -// #include "pdf_beamtime_interface/srv/gripper_cmd.hpp" +#include "pdf_beamtime_interfaces/srv/gripper_control_msg.hpp" +// #include "robotiq_driver/robotiq_gripper_interface.hpp" #include class GripperService @@ -17,8 +17,27 @@ class GripperService private: const char * kComPort = "/tmp/ttyUR"; const int kSlaveID = 0x09; + + rclcpp::Node::SharedPtr node_; RobotiqGripperInterface gripper_; +/// @brief Gripper states + enum class Gripper_State {OPEN, CLOSE, PARTIAL, MOVING, ACTIVE, DEACTIVE}; + + std::map gripper_command_map_ = { + {"ACTIVE", Gripper_State::ACTIVE}, + {"DEACTIVE", Gripper_State::DEACTIVE}, + {"OPEN", Gripper_State::OPEN}, + {"CLOSE", Gripper_State::CLOSE}, + {"PARTIAL", Gripper_State::PARTIAL} + }; + + void gripper_controller( + const std::shared_ptr request, + std::shared_ptr response); + public: - GripperService(); + explicit GripperService(); + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr getNodeBaseInterface(); + }; diff --git a/src/robotiq_driver/src/gripper_service.cpp b/src/robotiq_driver/src/gripper_service.cpp index 6fde6a71..e197b327 100644 --- a/src/robotiq_driver/src/gripper_service.cpp +++ b/src/robotiq_driver/src/gripper_service.cpp @@ -1,82 +1,104 @@ -// constexpr auto kComPort = "/tmp/ttyUR"; -// constexpr auto kSlaveID = 0x09; - #include +using namespace std::placeholders; GripperService::GripperService() +: node_(std::make_shared("gripper_server_node")), + gripper_(kComPort, kSlaveID) { - RobotiqGripperInterface gripper(kComPort, kSlaveID); + RCLCPP_INFO(rclcpp::get_logger("gripper_controller"), "Activate the gripper ..."); + // Clear the registers + gripper_.deactivateGripper(); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + // Activate the gripper + gripper_.activateGripper(); + + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + gripper_.setSpeed(0x0F); + + RCLCPP_INFO(rclcpp::get_logger("gripper_controller"), "Activation successful"); + + rclcpp::Service::SharedPtr service = + node_->create_service( + "gripper_service", + std::bind( + &GripperService::gripper_controller, this, _1, _2)); // CHANGE + + RCLCPP_INFO(rclcpp::get_logger("gripper_controller"), "Ready to receive gripper commands."); // CHANGE + } -void gripper_controller( - const std::shared_ptr request, - std::shared_ptr response) +rclcpp::node_interfaces::NodeBaseInterface::SharedPtr GripperService::getNodeBaseInterface() +// Expose the node base interface so that the node can be added to a component manager. { + return node_->get_node_base_interface(); +} +void GripperService::gripper_controller( + const std::shared_ptr request, + std::shared_ptr response) +{ // This function sends the gripper control command - bool status = false; + // moveit::core::MoveItErrorCode gripper_results = moveit::core::MoveItErrorCode::FAILURE; + + int status = 0; try { - char cmd = request->cmd; + // conver the request command string to the mapping enum + Gripper_State gripper_command_enum = gripper_command_map_[request->command]; - switch (cmd) { - case 'A': + switch (gripper_command_enum) { + case Gripper_State::ACTIVE: // Activate the gripper - gripper.deactivateGripper(); + gripper_.deactivateGripper(); std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - gripper.activateGripper(); + gripper_.activateGripper(); RCLCPP_INFO(rclcpp::get_logger("gripper_controller"), "Activation successful"); break; - case 'D': + case Gripper_State::DEACTIVE: // Deactivate the gripper - gripper.deactivateGripper(); + gripper_.deactivateGripper(); RCLCPP_INFO(rclcpp::get_logger("gripper_controller"), "Deactivated"); break; - case 'M': + case Gripper_State::PARTIAL: { // Closes the gripper to the percentage set by request->grip uint8_t val = request->grip * 2.55; // convert the scales from 01-100 to 0-255 // std::cout << "######### request and val : " << std::endl ; // std::cout << static_cast(request->grip) << std::endl ; // std::cout << static_cast(val) << std::endl ; - gripper.setGripperPosition(val); + gripper_.setGripperPosition(val); RCLCPP_INFO(rclcpp::get_logger("gripper_controller"), "Gripper Open"); } break; - case 'O': + case Gripper_State::OPEN: /* Open the grippper fully */ - gripper.setGripperPosition(0x00); + gripper_.setGripperPosition(0x00); RCLCPP_INFO(rclcpp::get_logger("gripper_controller"), "Gripper Open"); break; - case 'C': + case Gripper_State::CLOSE: /* Close the grippper fully */ - gripper.setGripperPosition(0xFF); + gripper_.setGripperPosition(0xFF); RCLCPP_INFO(rclcpp::get_logger("gripper_controller"), "Gripper Open"); break; - // case 'S': - // /* Close the grippper fully */ - // gripper.setGripperPosition(0xFF); - // break; - default: break; } - status = true; + status = 1; } catch (const std::exception & e) { std::cerr << e.what() << '\n'; - status = false; + status = 0; } // Send the response back - response->status = status; + response->results = status; } @@ -85,26 +107,11 @@ int main(int argc, char ** argv) rclcpp::init(argc, argv); - std::shared_ptr node = rclcpp::Node::make_shared("gripper_service"); // CHANGE - - RCLCPP_INFO(rclcpp::get_logger("gripper_controller"), "Activate the gripper ..."); - // Clear the registers - gripper.deactivateGripper(); - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - // Activate the gripper - gripper.activateGripper(); - - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - gripper.setSpeed(0x0F); - - RCLCPP_INFO(rclcpp::get_logger("gripper_controller"), "Activation successful"); - - rclcpp::Service::SharedPtr service = - node->create_service("gripper_service", &gripper_controller); // CHANGE + auto gripper_server = std::make_shared(); - RCLCPP_INFO(rclcpp::get_logger("gripper_controller"), "Ready to recieve gripper commands."); // CHANGE + // std::shared_ptr node = rclcpp::Node::make_shared("gripper_service"); // CHANGE - rclcpp::spin(node); + rclcpp::spin(gripper_server->getNodeBaseInterface()); rclcpp::shutdown(); return 0; diff --git a/src/robotiq_driver/srv/GriiperControlMsg.srv b/src/robotiq_driver/srv/GriiperControlMsg.srv deleted file mode 100644 index 3c32b01f..00000000 --- a/src/robotiq_driver/srv/GriiperControlMsg.srv +++ /dev/null @@ -1,3 +0,0 @@ -string command ---- -string results From 41836187489d680630359b9316bb3432760d7aba Mon Sep 17 00:00:00 2001 From: Chandima Fernando Date: Tue, 19 Mar 2024 18:32:08 +0000 Subject: [PATCH 05/32] fix: test path for hello moveit --- docker/erobs_hello_moveit/Dockerfile | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/docker/erobs_hello_moveit/Dockerfile b/docker/erobs_hello_moveit/Dockerfile index 8052c497..4e82cbfa 100644 --- a/docker/erobs_hello_moveit/Dockerfile +++ b/docker/erobs_hello_moveit/Dockerfile @@ -24,7 +24,9 @@ RUN pip3 install numpy rosdep && \ ARG GITHUB_TOKEN RUN mkdir /root/ws/src -p WORKDIR /root/ws/src -RUN git clone https://maffettone:${GITHUB_TOKEN}@github.com/maffettone/erobs.git -b humble +# RUN git clone https://maffettone:${GITHUB_TOKEN}@github.com/maffettone/erobs.git -b humble +RUN git clone https://github.com/ChandimaFernando/erobs.git -b humble_hande_control + WORKDIR /root/ws/ RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash && \ rosdep install -r --from-paths . --ignore-src --rosdistro $ROS_DISTRO -y" From 4e69eb68b45092380569675b6f5d8c40f71ba927 Mon Sep 17 00:00:00 2001 From: Chandima Fernando Date: Wed, 20 Mar 2024 13:40:04 +0000 Subject: [PATCH 06/32] fix: cmake compile issue --- src/robotiq_driver/CMakeLists.txt | 33 +----------- .../robotiq_driver/gripper_service.hpp | 19 +++---- src/robotiq_driver/src/gripper_service.cpp | 50 +++++++------------ .../src/robotiq_gripper_interface.cpp | 36 ------------- 4 files changed, 30 insertions(+), 108 deletions(-) diff --git a/src/robotiq_driver/CMakeLists.txt b/src/robotiq_driver/CMakeLists.txt index 76df0de2..7823d1a7 100644 --- a/src/robotiq_driver/CMakeLists.txt +++ b/src/robotiq_driver/CMakeLists.txt @@ -48,18 +48,11 @@ ament_target_dependencies( pluginlib_export_plugin_description_file(hardware_interface hardware_interface_plugin.xml) add_executable(gripper_service src/gripper_service.cpp) -# add_executable(gripper_cmds src/test_interface.cpp) -#add_executable(gripper_interface_test src/gripper_interface_test.cpp) target_include_directories(gripper_service PRIVATE include) ament_target_dependencies(gripper_service serial pdf_beamtime_interfaces) target_link_libraries(gripper_service ${PROJECT_NAME}) -# target_include_directories(gripper_cmds PRIVATE include) -# ament_target_dependencies(gripper_cmds serial pdf_beamtime_interfaces) -# target_link_libraries(gripper_cmds ${PROJECT_NAME}) - - # INSTALL install( TARGETS ${PROJECT_NAME} @@ -68,24 +61,16 @@ install( LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) + install( TARGETS gripper_service - # gripper_cmds - DESTINATION lib/${PROJECT_NAME} ) + install( DIRECTORY include/ DESTINATION include ) -install( - DIRECTORY config - DESTINATION share/${PROJECT_NAME} -) -install( - DIRECTORY launch - DESTINATION share/${PROJECT_NAME} -) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) @@ -99,18 +84,4 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() 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() diff --git a/src/robotiq_driver/include/robotiq_driver/gripper_service.hpp b/src/robotiq_driver/include/robotiq_driver/gripper_service.hpp index b1ba3317..eb5c75b9 100644 --- a/src/robotiq_driver/include/robotiq_driver/gripper_service.hpp +++ b/src/robotiq_driver/include/robotiq_driver/gripper_service.hpp @@ -15,23 +15,25 @@ class GripperService { private: + /// @brief your serial port goes here const char * kComPort = "/tmp/ttyUR"; const int kSlaveID = 0x09; rclcpp::Node::SharedPtr node_; RobotiqGripperInterface gripper_; -/// @brief Gripper states - enum class Gripper_State {OPEN, CLOSE, PARTIAL, MOVING, ACTIVE, DEACTIVE}; +/// @brief Gripper commands as enums for ease of use + enum class Gripper_Command {OPEN, CLOSE, PARTIAL, ACTIVE, DEACTIVE}; - std::map gripper_command_map_ = { - {"ACTIVE", Gripper_State::ACTIVE}, - {"DEACTIVE", Gripper_State::DEACTIVE}, - {"OPEN", Gripper_State::OPEN}, - {"CLOSE", Gripper_State::CLOSE}, - {"PARTIAL", Gripper_State::PARTIAL} + std::map gripper_command_map_ = { + {"ACTIVE", Gripper_Command::ACTIVE}, + {"DEACTIVE", Gripper_Command::DEACTIVE}, + {"OPEN", Gripper_Command::OPEN}, + {"CLOSE", Gripper_Command::CLOSE}, + {"PARTIAL", Gripper_Command::PARTIAL} }; + /// @brief callback function for the gripper service void gripper_controller( const std::shared_ptr request, std::shared_ptr response); @@ -39,5 +41,4 @@ class GripperService public: explicit GripperService(); rclcpp::node_interfaces::NodeBaseInterface::SharedPtr getNodeBaseInterface(); - }; diff --git a/src/robotiq_driver/src/gripper_service.cpp b/src/robotiq_driver/src/gripper_service.cpp index e197b327..f9e179c2 100644 --- a/src/robotiq_driver/src/gripper_service.cpp +++ b/src/robotiq_driver/src/gripper_service.cpp @@ -6,7 +6,7 @@ GripperService::GripperService() : node_(std::make_shared("gripper_server_node")), gripper_(kComPort, kSlaveID) { - RCLCPP_INFO(rclcpp::get_logger("gripper_controller"), "Activate the gripper ..."); + RCLCPP_INFO(node_->get_logger(), "Activate the gripper ..."); // Clear the registers gripper_.deactivateGripper(); std::this_thread::sleep_for(std::chrono::milliseconds(1000)); @@ -16,16 +16,14 @@ GripperService::GripperService() std::this_thread::sleep_for(std::chrono::milliseconds(1000)); gripper_.setSpeed(0x0F); - RCLCPP_INFO(rclcpp::get_logger("gripper_controller"), "Activation successful"); + RCLCPP_INFO(node_->get_logger(), "Activation is successful"); rclcpp::Service::SharedPtr service = node_->create_service( "gripper_service", std::bind( - &GripperService::gripper_controller, this, _1, _2)); // CHANGE - - RCLCPP_INFO(rclcpp::get_logger("gripper_controller"), "Ready to receive gripper commands."); // CHANGE - + &GripperService::gripper_controller, this, _1, _2)); + RCLCPP_INFO(node_->get_logger(), "Ready to receive gripper commands."); } rclcpp::node_interfaces::NodeBaseInterface::SharedPtr GripperService::getNodeBaseInterface() @@ -38,82 +36,70 @@ void GripperService::gripper_controller( const std::shared_ptr request, std::shared_ptr response) { - // This function sends the gripper control command - // moveit::core::MoveItErrorCode gripper_results = moveit::core::MoveItErrorCode::FAILURE; - int status = 0; try { // conver the request command string to the mapping enum - Gripper_State gripper_command_enum = gripper_command_map_[request->command]; + Gripper_Command gripper_command_enum = gripper_command_map_[request->command]; switch (gripper_command_enum) { - case Gripper_State::ACTIVE: + case Gripper_Command::ACTIVE: // Activate the gripper gripper_.deactivateGripper(); std::this_thread::sleep_for(std::chrono::milliseconds(1000)); gripper_.activateGripper(); - RCLCPP_INFO(rclcpp::get_logger("gripper_controller"), "Activation successful"); + RCLCPP_INFO(node_->get_logger(), "Activation is successful"); break; - case Gripper_State::DEACTIVE: + case Gripper_Command::DEACTIVE: // Deactivate the gripper gripper_.deactivateGripper(); - RCLCPP_INFO(rclcpp::get_logger("gripper_controller"), "Deactivated"); + RCLCPP_INFO(node_->get_logger(), "Gripper is Deactivated"); break; - case Gripper_State::PARTIAL: + case Gripper_Command::PARTIAL: { // Closes the gripper to the percentage set by request->grip uint8_t val = request->grip * 2.55; // convert the scales from 01-100 to 0-255 - // std::cout << "######### request and val : " << std::endl ; - // std::cout << static_cast(request->grip) << std::endl ; - // std::cout << static_cast(val) << std::endl ; gripper_.setGripperPosition(val); - RCLCPP_INFO(rclcpp::get_logger("gripper_controller"), "Gripper Open"); + RCLCPP_INFO(node_->get_logger(), "Gripper is Open"); } break; - case Gripper_State::OPEN: - /* Open the grippper fully */ + case Gripper_Command::OPEN: + /* Open the gripper fully */ gripper_.setGripperPosition(0x00); - RCLCPP_INFO(rclcpp::get_logger("gripper_controller"), "Gripper Open"); + RCLCPP_INFO(node_->get_logger(), "Gripper is Open"); break; - case Gripper_State::CLOSE: - /* Close the grippper fully */ + case Gripper_Command::CLOSE: + /* Close the gripper fully */ gripper_.setGripperPosition(0xFF); - RCLCPP_INFO(rclcpp::get_logger("gripper_controller"), "Gripper Open"); + RCLCPP_INFO(node_->get_logger(), "Gripper is Close"); break; default: break; } - status = 1; } catch (const std::exception & e) { - std::cerr << e.what() << '\n'; + RCLCPP_ERROR(node_->get_logger(), e.what()); status = 0; } // Send the response back response->results = status; - } int main(int argc, char ** argv) { - rclcpp::init(argc, argv); auto gripper_server = std::make_shared(); - // std::shared_ptr node = rclcpp::Node::make_shared("gripper_service"); // CHANGE - rclcpp::spin(gripper_server->getNodeBaseInterface()); rclcpp::shutdown(); return 0; - } diff --git a/src/robotiq_driver/src/robotiq_gripper_interface.cpp b/src/robotiq_driver/src/robotiq_gripper_interface.cpp index 9c7a957c..335d52b7 100644 --- a/src/robotiq_driver/src/robotiq_gripper_interface.cpp +++ b/src/robotiq_driver/src/robotiq_gripper_interface.cpp @@ -91,17 +91,6 @@ void RobotiqGripperInterface::activateGripper() // other registers. ); - // std::stringstream ss; - // for (std::size_t i = 0; i < cmd.size(); i++) - // { - // if (i != 0) - // { - // ss << ", "; - // } - // ss << "0x" << std::hex << static_cast(cmd[i]); - // } - // std::cout << ss.str() < RobotiqGripperInterface::createWriteCommand( const std::vector & data) { - // std::stringstream ss; - // for (std::size_t i = 0; i < data.size(); i++) - // { - // if (i != 0) - // { - // ss << ", "; - // } - // ss << "0x" << std::hex << static_cast(data[i]); - // } - // std::cout << ss.str() < RobotiqGripperInterface::createWriteCommand( cmd.push_back(getFirstByte(crc)); cmd.push_back(getSecondByte(crc)); - // std::cout << " The final command cmd: " << std::endl ; - // std::stringstream ss3; - // for (std::size_t i = 0; i < cmd.size(); i++) - // { - // if (i != 0) - // { - // ss3 << ", "; - // } - // ss3 << "0x" << std::hex << static_cast(cmd[i]); - // } - // std::cout << ss3.str() < Date: Wed, 20 Mar 2024 09:41:31 -0400 Subject: [PATCH 07/32] fix: point to correct repo in docker --- docker/erobs_hello_moveit/Dockerfile | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/docker/erobs_hello_moveit/Dockerfile b/docker/erobs_hello_moveit/Dockerfile index 4e82cbfa..bc5645cb 100644 --- a/docker/erobs_hello_moveit/Dockerfile +++ b/docker/erobs_hello_moveit/Dockerfile @@ -24,8 +24,7 @@ RUN pip3 install numpy rosdep && \ ARG GITHUB_TOKEN RUN mkdir /root/ws/src -p WORKDIR /root/ws/src -# RUN git clone https://maffettone:${GITHUB_TOKEN}@github.com/maffettone/erobs.git -b humble -RUN git clone https://github.com/ChandimaFernando/erobs.git -b humble_hande_control +RUN git clone https://maffettone:${GITHUB_TOKEN}@github.com/maffettone/erobs.git -b humble WORKDIR /root/ws/ RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash && \ From 9bd76b8d562575e488fb25cae9b00b4ea9f9af56 Mon Sep 17 00:00:00 2001 From: Chandima Fernando Date: Wed, 20 Mar 2024 09:54:34 -0400 Subject: [PATCH 08/32] refactor: add copyright --- .vscode/settings.json | 76 +------------------ docker/docker-compose.yml | 5 -- docker/erobs_hello_moveit/Dockerfile | 1 - .../include/robotiq_driver/crc.hpp | 27 +++++++ src/robotiq_driver/src/gripper_service.cpp | 2 + 5 files changed, 30 insertions(+), 81 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 6736fc0d..0537a16c 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -6,81 +6,7 @@ "files.associations": { "*.repos": "yaml", "*.world": "xml", - "*.xacro": "xml", - "functional": "cpp", - "cctype": "cpp", - "clocale": "cpp", - "cmath": "cpp", - "csignal": "cpp", - "cstdarg": "cpp", - "cstddef": "cpp", - "cstdio": "cpp", - "cstdlib": "cpp", - "cstring": "cpp", - "ctime": "cpp", - "cwchar": "cpp", - "cwctype": "cpp", - "any": "cpp", - "array": "cpp", - "atomic": "cpp", - "strstream": "cpp", - "bit": "cpp", - "*.tcc": "cpp", - "bitset": "cpp", - "chrono": "cpp", - "codecvt": "cpp", - "compare": "cpp", - "complex": "cpp", - "concepts": "cpp", - "condition_variable": "cpp", - "cstdint": "cpp", - "deque": "cpp", - "forward_list": "cpp", - "list": "cpp", - "map": "cpp", - "set": "cpp", - "string": "cpp", - "unordered_map": "cpp", - "unordered_set": "cpp", - "vector": "cpp", - "exception": "cpp", - "algorithm": "cpp", - "iterator": "cpp", - "memory": "cpp", - "memory_resource": "cpp", - "numeric": "cpp", - "optional": "cpp", - "random": "cpp", - "ratio": "cpp", - "string_view": "cpp", - "system_error": "cpp", - "tuple": "cpp", - "type_traits": "cpp", - "utility": "cpp", - "fstream": "cpp", - "future": "cpp", - "initializer_list": "cpp", - "iomanip": "cpp", - "iosfwd": "cpp", - "iostream": "cpp", - "istream": "cpp", - "limits": "cpp", - "mutex": "cpp", - "new": "cpp", - "numbers": "cpp", - "ostream": "cpp", - "semaphore": "cpp", - "shared_mutex": "cpp", - "sstream": "cpp", - "stdexcept": "cpp", - "stop_token": "cpp", - "streambuf": "cpp", - "thread": "cpp", - "cfenv": "cpp", - "cinttypes": "cpp", - "typeindex": "cpp", - "typeinfo": "cpp", - "variant": "cpp" + "*.xacro": "xml" }, "python.analysis.extraPaths": [ "/opt/ros/humble/lib/python3.10/site-packages/" diff --git a/docker/docker-compose.yml b/docker/docker-compose.yml index b76ed0e2..4a9bf08b 100644 --- a/docker/docker-compose.yml +++ b/docker/docker-compose.yml @@ -53,11 +53,6 @@ services: depends_on: - urdriver - urhandecontrol: - image: ur-hande-control:latest - networks: - - ursim_net - networks: ursim_net: ipam: diff --git a/docker/erobs_hello_moveit/Dockerfile b/docker/erobs_hello_moveit/Dockerfile index bc5645cb..8052c497 100644 --- a/docker/erobs_hello_moveit/Dockerfile +++ b/docker/erobs_hello_moveit/Dockerfile @@ -25,7 +25,6 @@ ARG GITHUB_TOKEN RUN mkdir /root/ws/src -p WORKDIR /root/ws/src RUN git clone https://maffettone:${GITHUB_TOKEN}@github.com/maffettone/erobs.git -b humble - WORKDIR /root/ws/ RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash && \ rosdep install -r --from-paths . --ignore-src --rosdistro $ROS_DISTRO -y" diff --git a/src/robotiq_driver/include/robotiq_driver/crc.hpp b/src/robotiq_driver/include/robotiq_driver/crc.hpp index 869fcaed..91e10273 100644 --- a/src/robotiq_driver/include/robotiq_driver/crc.hpp +++ b/src/robotiq_driver/include/robotiq_driver/crc.hpp @@ -1,3 +1,30 @@ +// 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 diff --git a/src/robotiq_driver/src/gripper_service.cpp b/src/robotiq_driver/src/gripper_service.cpp index f9e179c2..68e5996a 100644 --- a/src/robotiq_driver/src/gripper_service.cpp +++ b/src/robotiq_driver/src/gripper_service.cpp @@ -1,3 +1,5 @@ +/*Copyright 2023 Brookhaven National Laboratory +BSD 3 Clause License. See LICENSE.txt for details.*/ #include using namespace std::placeholders; From c3fbb997402de56b358dc4619cdf26b8d05a37d2 Mon Sep 17 00:00:00 2001 From: Chandima Fernando Date: Wed, 20 Mar 2024 09:56:14 -0400 Subject: [PATCH 09/32] refactor: correct settings.json --- .vscode/settings.json | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 0537a16c..edd644ad 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -6,7 +6,8 @@ "files.associations": { "*.repos": "yaml", "*.world": "xml", - "*.xacro": "xml" + "*.xacro": "xml", + "functional": "cpp" }, "python.analysis.extraPaths": [ "/opt/ros/humble/lib/python3.10/site-packages/" From 993ff98b58006f2b1e57331525a27c48a2a18bba Mon Sep 17 00:00:00 2001 From: Chandima Fernando Date: Thu, 21 Mar 2024 10:19:30 -0400 Subject: [PATCH 10/32] fix: move serial to ra separate repo --- docker/ur-hande-control/Dockerfile | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/docker/ur-hande-control/Dockerfile b/docker/ur-hande-control/Dockerfile index 6244b51d..0bea345a 100644 --- a/docker/ur-hande-control/Dockerfile +++ b/docker/ur-hande-control/Dockerfile @@ -20,10 +20,19 @@ RUN mkdir /root/ws/src -p WORKDIR /root/ws/src -RUN git clone https://github.com/PickNikRobotics/serial-release.git -b release/humble/serial + +RUN git clone https://github.com/PickNikRobotics/serial-release.git -b release/humble/serial serial +WORKDIR /root/ws/src/serial +RUN git init --separate-git-dir=../serial-release.git +RUN git fetch --all + +WORKDIR /root/ws/src/ +# RUN git clone https://github.com/PickNikRobotics/serial-release.git -b release/humble/serial +RUN git clone hhttps://github.com/ChandimaFernando/erobs.git -b humble_hande_control WORKDIR /root/ws/ RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash && \ - colcon build --symlink-install --packages-select serial" + colcon build --symlink-install --packages-select serial robotiq_driver" +CMD ["/bin/sh", "-c", ". /opt/ros/${ROS_DISTRO}/setup.sh && ros2 run robotiq_driver gripper_service"] From 3d1d9822f0d1214fc5c85793519cbda125b9aabe Mon Sep 17 00:00:00 2001 From: Chandima Fernando Date: Thu, 21 Mar 2024 16:13:16 +0000 Subject: [PATCH 11/32] fix: remove submodule --- docker/ur-hande-control/Dockerfile | 15 ++++----------- .../include/robotiq_driver/gripper_service.hpp | 1 - src/serial-release | 1 - 3 files changed, 4 insertions(+), 13 deletions(-) delete mode 160000 src/serial-release diff --git a/docker/ur-hande-control/Dockerfile b/docker/ur-hande-control/Dockerfile index 0bea345a..10c0e396 100644 --- a/docker/ur-hande-control/Dockerfile +++ b/docker/ur-hande-control/Dockerfile @@ -20,19 +20,12 @@ RUN mkdir /root/ws/src -p WORKDIR /root/ws/src - -RUN git clone https://github.com/PickNikRobotics/serial-release.git -b release/humble/serial serial -WORKDIR /root/ws/src/serial -RUN git init --separate-git-dir=../serial-release.git -RUN git fetch --all - -WORKDIR /root/ws/src/ -# RUN git clone https://github.com/PickNikRobotics/serial-release.git -b release/humble/serial -RUN git clone hhttps://github.com/ChandimaFernando/erobs.git -b humble_hande_control +RUN git clone https://github.com/PickNikRobotics/serial-release.git -b release/humble/serial +RUN git clone https://github.com/ChandimaFernando/erobs.git -b humble_hande_control WORKDIR /root/ws/ RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash && \ - colcon build --symlink-install --packages-select serial robotiq_driver" + colcon build --symlink-install --packages-select pdf_beamtime_interfaces serial robotiq_driver" -CMD ["/bin/sh", "-c", ". /opt/ros/${ROS_DISTRO}/setup.sh && ros2 run robotiq_driver gripper_service"] +CMD ["/bin/sh", "-c", ". /opt/ros/${ROS_DISTRO}/setup.sh && . /root/ws/install/setup.sh && ros2 run robotiq_driver gripper_service"] diff --git a/src/robotiq_driver/include/robotiq_driver/gripper_service.hpp b/src/robotiq_driver/include/robotiq_driver/gripper_service.hpp index eb5c75b9..2cb86b13 100644 --- a/src/robotiq_driver/include/robotiq_driver/gripper_service.hpp +++ b/src/robotiq_driver/include/robotiq_driver/gripper_service.hpp @@ -8,7 +8,6 @@ BSD 3 Clause License. See LICENSE.txt for details.*/ #include "pdf_beamtime_interfaces/srv/gripper_control_msg.hpp" -// #include "robotiq_driver/robotiq_gripper_interface.hpp" #include class GripperService diff --git a/src/serial-release b/src/serial-release deleted file mode 160000 index 8edf5917..00000000 --- a/src/serial-release +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 8edf5917d226c33efaf1916ef0fa74f1b70e53d5 From c8859ba2f870c30023aa5f34edf5f42e8f68d47b Mon Sep 17 00:00:00 2001 From: Chandima Fernando Date: Thu, 21 Mar 2024 18:36:52 +0000 Subject: [PATCH 12/32] fix: simplify gripper service --- docker/ur-hande-control/Dockerfile | 6 +- src/gripper_service/CMakeLists.txt | 56 ++++ src/gripper_service/LICENSE | 202 ++++++++++++ .../gripper_service}/gripper_service.hpp | 0 src/gripper_service/package.xml | 18 + .../src/gripper_service.cpp | 0 src/robotiq_driver/CMakeLists.txt | 87 ----- .../hardware_interface_plugin.xml | 7 - .../include/robotiq_driver/crc.hpp | 33 -- .../robotiq_driver/hardware_interface.hpp | 102 ------ .../robotiq_gripper_interface.hpp | 162 --------- .../robotiq_driver/visibility_control.h | 56 ---- src/robotiq_driver/package.xml | 28 -- src/robotiq_driver/src/crc.cpp | 77 ----- src/robotiq_driver/src/hardware_interface.cpp | 277 ---------------- .../src/robotiq_gripper_interface.cpp | 312 ------------------ 16 files changed, 280 insertions(+), 1143 deletions(-) create mode 100644 src/gripper_service/CMakeLists.txt create mode 100644 src/gripper_service/LICENSE rename src/{robotiq_driver/include/robotiq_driver => gripper_service/include/gripper_service}/gripper_service.hpp (100%) create mode 100644 src/gripper_service/package.xml rename src/{robotiq_driver => gripper_service}/src/gripper_service.cpp (100%) delete mode 100644 src/robotiq_driver/CMakeLists.txt delete mode 100644 src/robotiq_driver/hardware_interface_plugin.xml delete mode 100644 src/robotiq_driver/include/robotiq_driver/crc.hpp delete mode 100644 src/robotiq_driver/include/robotiq_driver/hardware_interface.hpp delete mode 100644 src/robotiq_driver/include/robotiq_driver/robotiq_gripper_interface.hpp delete mode 100644 src/robotiq_driver/include/robotiq_driver/visibility_control.h delete mode 100644 src/robotiq_driver/package.xml delete mode 100644 src/robotiq_driver/src/crc.cpp delete mode 100644 src/robotiq_driver/src/hardware_interface.cpp delete mode 100644 src/robotiq_driver/src/robotiq_gripper_interface.cpp diff --git a/docker/ur-hande-control/Dockerfile b/docker/ur-hande-control/Dockerfile index 10c0e396..6cda67b8 100644 --- a/docker/ur-hande-control/Dockerfile +++ b/docker/ur-hande-control/Dockerfile @@ -21,11 +21,13 @@ RUN mkdir /root/ws/src -p WORKDIR /root/ws/src RUN git clone https://github.com/PickNikRobotics/serial-release.git -b release/humble/serial -RUN git clone https://github.com/ChandimaFernando/erobs.git -b humble_hande_control +RUN git clone https://github.com/PickNikRobotics/ros2_robotiq_gripper.git -b cmake-external-project-serial +# RUN git clone https://github.com/ChandimaFernando/erobs.git -b humble_hande_control WORKDIR /root/ws/ RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash && \ colcon build --symlink-install --packages-select pdf_beamtime_interfaces serial robotiq_driver" -CMD ["/bin/sh", "-c", ". /opt/ros/${ROS_DISTRO}/setup.sh && . /root/ws/install/setup.sh && ros2 run robotiq_driver gripper_service"] +CMD [ "/bin/bash" ] +# CMD ["/bin/sh", "-c", ". /opt/ros/${ROS_DISTRO}/setup.sh && . /root/ws/install/setup.sh && ros2 run robotiq_driver gripper_service"] diff --git a/src/gripper_service/CMakeLists.txt b/src/gripper_service/CMakeLists.txt new file mode 100644 index 00000000..09613dbc --- /dev/null +++ b/src/gripper_service/CMakeLists.txt @@ -0,0 +1,56 @@ +cmake_minimum_required(VERSION 3.8) +project(gripper_service) + +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(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(pdf_beamtime_interfaces REQUIRED) +find_package(robotiq_driver REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +set(dependencies + rclcpp + rclcpp_components + pdf_beamtime_interfaces + robotiq_driver +) + +include_directories(include) # This makes sure the libraries are visible + +add_executable(gripper_service src/gripper_service.cpp) +ament_target_dependencies(gripper_service ${dependencies}) + +install(TARGETS + gripper_service + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY DESTINATION share/${PROJECT_NAME}) + +install( + DIRECTORY include/ + DESTINATION include +) + +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) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/gripper_service/LICENSE b/src/gripper_service/LICENSE new file mode 100644 index 00000000..d6456956 --- /dev/null +++ b/src/gripper_service/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/src/robotiq_driver/include/robotiq_driver/gripper_service.hpp b/src/gripper_service/include/gripper_service/gripper_service.hpp similarity index 100% rename from src/robotiq_driver/include/robotiq_driver/gripper_service.hpp rename to src/gripper_service/include/gripper_service/gripper_service.hpp diff --git a/src/gripper_service/package.xml b/src/gripper_service/package.xml new file mode 100644 index 00000000..e2e5b156 --- /dev/null +++ b/src/gripper_service/package.xml @@ -0,0 +1,18 @@ + + + + gripper_service + 0.0.0 + TODO: Package description + wfernando1 + Apache-2.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/robotiq_driver/src/gripper_service.cpp b/src/gripper_service/src/gripper_service.cpp similarity index 100% rename from src/robotiq_driver/src/gripper_service.cpp rename to src/gripper_service/src/gripper_service.cpp diff --git a/src/robotiq_driver/CMakeLists.txt b/src/robotiq_driver/CMakeLists.txt deleted file mode 100644 index 7823d1a7..00000000 --- a/src/robotiq_driver/CMakeLists.txt +++ /dev/null @@ -1,87 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(robotiq_driver) - -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) -find_package(rclcpp REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) -find_package(serial REQUIRED) -find_package(rosidl_default_generators REQUIRED) -find_package(pdf_beamtime_interfaces REQUIRED) - -set(THIS_PACKAGE_INCLUDE_DEPENDS - ament_cmake - hardware_interface - pluginlib - rclcpp - rclcpp_lifecycle - serial - pdf_beamtime_interfaces -) - -add_library( - ${PROJECT_NAME} - SHARED - src/crc.cpp - src/hardware_interface.cpp - src/robotiq_gripper_interface.cpp -) - -target_include_directories( - ${PROJECT_NAME} - PUBLIC - $ - $ -) - -ament_target_dependencies( - ${PROJECT_NAME} - ${THIS_PACKAGE_INCLUDE_DEPENDS} -) - -pluginlib_export_plugin_description_file(hardware_interface hardware_interface_plugin.xml) - -add_executable(gripper_service src/gripper_service.cpp) - -target_include_directories(gripper_service PRIVATE include) -ament_target_dependencies(gripper_service serial pdf_beamtime_interfaces) -target_link_libraries(gripper_service ${PROJECT_NAME}) - -# INSTALL -install( - TARGETS ${PROJECT_NAME} - EXPORT export_${PROJECT_NAME} - ARCHIVE DESTINATION lib/${PROJECT_NAME} - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) - -install( - TARGETS gripper_service - DESTINATION lib/${PROJECT_NAME} -) - -install( - DIRECTORY include/ - DESTINATION include -) - -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) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/src/robotiq_driver/hardware_interface_plugin.xml b/src/robotiq_driver/hardware_interface_plugin.xml deleted file mode 100644 index 6d0a84ed..00000000 --- a/src/robotiq_driver/hardware_interface_plugin.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - ROS2 controller for the Robotiq gripper. - - - diff --git a/src/robotiq_driver/include/robotiq_driver/crc.hpp b/src/robotiq_driver/include/robotiq_driver/crc.hpp deleted file mode 100644 index 91e10273..00000000 --- a/src/robotiq_driver/include/robotiq_driver/crc.hpp +++ /dev/null @@ -1,33 +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. -#pragma once - -#include -#include - -uint16_t computeCRC(const std::vector & cmd); diff --git a/src/robotiq_driver/include/robotiq_driver/hardware_interface.hpp b/src/robotiq_driver/include/robotiq_driver/hardware_interface.hpp deleted file mode 100644 index 6c3353f3..00000000 --- a/src/robotiq_driver/include/robotiq_driver/hardware_interface.hpp +++ /dev/null @@ -1,102 +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. - -#pragma once - -#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/visibility_control.h" -#include "robotiq_driver/robotiq_gripper_interface.hpp" - -namespace robotiq_driver -{ -class RobotiqGripperHardwareInterface : public hardware_interface::SystemInterface -{ -public: - RCLCPP_SHARED_PTR_DEFINITIONS(RobotiqGripperHardwareInterface) - - ROBOTIQ_DRIVER_PUBLIC - RobotiqGripperHardwareInterface(); - - ROBOTIQ_DRIVER_PUBLIC - CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; - - ROBOTIQ_DRIVER_PUBLIC - std::vector export_state_interfaces() override; - - ROBOTIQ_DRIVER_PUBLIC - std::vector export_command_interfaces() override; - - ROBOTIQ_DRIVER_PUBLIC - CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; - - ROBOTIQ_DRIVER_PUBLIC - CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; - - ROBOTIQ_DRIVER_PUBLIC - hardware_interface::return_type read( - const rclcpp::Time & time, - const rclcpp::Duration & period) override; - - 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_; - std::unique_ptr gripper_interface_; - - double gripper_closed_pos_; - std::string com_port_; - - std::thread command_interface_; - std::atomic command_interface_is_running_; - std::atomic write_command_; - std::atomic gripper_current_state_; - - double reactivate_gripper_cmd_; - std::atomic reactivate_gripper_async_cmd_; - double reactivate_gripper_response_; - std::atomic> reactivate_gripper_async_response_; -}; - -} // namespace robotiq_driver diff --git a/src/robotiq_driver/include/robotiq_driver/robotiq_gripper_interface.hpp b/src/robotiq_driver/include/robotiq_driver/robotiq_gripper_interface.hpp deleted file mode 100644 index 59232056..00000000 --- a/src/robotiq_driver/include/robotiq_driver/robotiq_gripper_interface.hpp +++ /dev/null @@ -1,162 +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. - -#pragma once - -#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. - * - */ -class RobotiqGripperInterface -{ -public: - RobotiqGripperInterface(const std::string& com_port = "/tmp/ttyUR", uint8_t slave_id = 0x09); - - /** - * @brief Activates the gripper. - * - * @throw serial::IOException on failure to successfully communicate with gripper port - */ - void activateGripper(); - - /** - * @brief Deactivates the gripper. - * - * @throw serial::IOException on failure to successfully communicate with gripper port - */ - void deactivateGripper(); - - /** - * @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); - - /** - * @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 getGripperPosition(); - - /** - * @brief Returns true if the gripper is currently moving, false otherwise. - * - */ - bool gripperIsMoving(); - - /** - * @brief Set the speed of the gripper. - * - * @param speed A value between 0x00 (stopped) and 0xFF (full speed). - */ - void setSpeed(uint8_t speed); - - /** - * @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 - }; - -private: - std::vector createReadCommand(uint16_t first_register, uint8_t num_registers); - std::vector createWriteCommand(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 - */ - std::vector readResponse(size_t num_bytes); - - /** - * @brief Send a command to the gripper. - * - * @param cmd The command. - * @throw serial::IOException on failure to successfully communicate with gripper port - */ - void sendCommand(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(); - - serial::Serial port_; - uint8_t slave_id_; - std::vector read_command_; - - ActivationStatus activation_status_; - ActionStatus action_status_; - GripperStatus gripper_status_; - ObjectDetectionStatus object_detection_status_; - - uint8_t gripper_position_; - uint8_t commanded_gripper_speed_; - uint8_t commanded_gripper_force_; -}; diff --git a/src/robotiq_driver/include/robotiq_driver/visibility_control.h b/src/robotiq_driver/include/robotiq_driver/visibility_control.h deleted file mode 100644 index df5a56e4..00000000 --- a/src/robotiq_driver/include/robotiq_driver/visibility_control.h +++ /dev/null @@ -1,56 +0,0 @@ -// Copyright 2017 Open Source Robotics Foundation, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/* This header must be included by all rclcpp headers which declare symbols - * which are defined in the rclcpp library. When not building the rclcpp - * library, i.e. when using the headers in other package's code, the contents - * of this header change the visibility of certain symbols which the rclcpp - * library cannot have, but the consuming code must have inorder to link. - */ - -#ifndef ROBOTIQ_DRIVER__VISIBILITY_CONTROL_H_ -#define ROBOTIQ_DRIVER__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ -#ifdef __GNUC__ -#define ROBOTIQ_DRIVER_EXPORT __attribute__((dllexport)) -#define ROBOTIQ_DRIVER_IMPORT __attribute__((dllimport)) -#else -#define ROBOTIQ_DRIVER_EXPORT __declspec(dllexport) -#define ROBOTIQ_DRIVER_IMPORT __declspec(dllimport) -#endif -#ifdef ROBOTIQ_DRIVER_BUILDING_DLL -#define ROBOTIQ_DRIVER_PUBLIC ROBOTIQ_DRIVER_EXPORT -#else -#define ROBOTIQ_DRIVER_PUBLIC ROBOTIQ_DRIVER_IMPORT -#endif -#define ROBOTIQ_DRIVER_PUBLIC_TYPE ROBOTIQ_DRIVER_PUBLIC -#define ROBOTIQ_DRIVER_LOCAL -#else -#define ROBOTIQ_DRIVER_EXPORT __attribute__((visibility("default"))) -#define ROBOTIQ_DRIVER_IMPORT -#if __GNUC__ >= 4 -#define ROBOTIQ_DRIVER_PUBLIC __attribute__((visibility("default"))) -#define ROBOTIQ_DRIVER_LOCAL __attribute__((visibility("hidden"))) -#else -#define ROBOTIQ_DRIVER_PUBLIC -#define ROBOTIQ_DRIVER_LOCAL -#endif -#define ROBOTIQ_DRIVER_PUBLIC_TYPE -#endif - -#endif // ROBOTIQ_DRIVER__VISIBILITY_CONTROL_H_ diff --git a/src/robotiq_driver/package.xml b/src/robotiq_driver/package.xml deleted file mode 100644 index be87ec74..00000000 --- a/src/robotiq_driver/package.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - - robotiq_driver - 0.0.0 - ROS2 driver package for the Robotiq gripper. - Cory Crean - BSD - - ament_cmake - - hardware_interface - pluginlib - rclcpp - rclcpp_lifecycle - serial - rosidl_default_generators - pdf_beamtime_interfaces - - ament_lint_auto - ament_lint_common - - rosidl_interface_packages - - - ament_cmake - - diff --git a/src/robotiq_driver/src/crc.cpp b/src/robotiq_driver/src/crc.cpp deleted file mode 100644 index 6ae6bf66..00000000 --- a/src/robotiq_driver/src/crc.cpp +++ /dev/null @@ -1,77 +0,0 @@ -#include "robotiq_driver/crc.hpp" - -/* 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 -}; - -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/src/robotiq_driver/src/hardware_interface.cpp b/src/robotiq_driver/src/hardware_interface.cpp deleted file mode 100644 index 0beda015..00000000 --- a/src/robotiq_driver/src/hardware_interface.cpp +++ /dev/null @@ -1,277 +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/hardware_interface.hpp" - -#include -#include -#include -#include -#include - -#include "hardware_interface/actuator_interface.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/rclcpp.hpp" -#include - -constexpr uint8_t kGripperMinPos = 3; -constexpr uint8_t kGripperMaxPos = 230; -constexpr uint8_t kGripperRange = kGripperMaxPos - kGripperMinPos; - -const auto kLogger = rclcpp::get_logger("RobotiqGripperHardwareInterface"); - -namespace robotiq_driver -{ -RobotiqGripperHardwareInterface::RobotiqGripperHardwareInterface() -{ -} - -hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init( - const hardware_interface::HardwareInfo & info) -{ - 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(); - gripper_position_command_ = std::numeric_limits::quiet_NaN(); - reactivate_gripper_cmd_ = NO_NEW_CMD_; - reactivate_gripper_async_cmd_.store(false); - - 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()); - 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); - 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()); - return CallbackReturn::ERROR; - } - - 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); - return CallbackReturn::ERROR; - } - } - - 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); - } catch (const serial::IOException & e) { - RCLCPP_FATAL(kLogger, "Failed to open gripper port."); - return CallbackReturn::ERROR; - } - - return CallbackReturn::SUCCESS; -} - -std::vector RobotiqGripperHardwareInterface:: -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_)); - - return state_interfaces; -} - -std::vector RobotiqGripperHardwareInterface:: -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_)); - 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_)); - - return command_interfaces; -} - -hardware_interface::CallbackReturn -RobotiqGripperHardwareInterface::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) -{ - // set some default values for joints - if (std::isnan(gripper_position_)) { - gripper_position_ = 0; - gripper_velocity_ = 0; - gripper_position_command_ = 0; - } - - // Activate the gripper. - try { - gripper_interface_->deactivateGripper(); - gripper_interface_->activateGripper(); - } catch (const serial::IOException & e) { - RCLCPP_FATAL(kLogger, "Failed to communicate with Gripper. Check Gripper connection."); - return CallbackReturn::ERROR; - } - - RCLCPP_INFO(kLogger, "Robotiq Gripper successfully activated!"); - - command_interface_is_running_.store(true); - - command_interface_ = 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()) { - 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->gripper_interface_->deactivateGripper(); - this->gripper_interface_->activateGripper(); - 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()); - - // Read the state of the gripper. - gripper_current_state_.store(this->gripper_interface_->getGripperPosition()); - - last_io = now; - } catch (serial::IOException & e) { - RCLCPP_ERROR(kLogger, "Check Robotiq Gripper connection and restart drivers."); - command_interface_is_running_.store(false); - } - } - } - }); - - return CallbackReturn::SUCCESS; -} - -hardware_interface::CallbackReturn -RobotiqGripperHardwareInterface::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) -{ - command_interface_is_running_.store(false); - command_interface_.join(); - - try { - gripper_interface_->deactivateGripper(); - } catch (const std::exception & e) { - RCLCPP_ERROR(kLogger, "Failed to deactivate gripper. Check Gripper connection"); - return CallbackReturn::ERROR; - } - - return CallbackReturn::SUCCESS; -} - -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; - - 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()) { - reactivate_gripper_response_ = reactivate_gripper_async_response_.load().value(); - reactivate_gripper_async_response_.store(std::nullopt); - } - - return hardware_interface::return_type::OK; -} - -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; - gripper_pos = std::max(std::min(gripper_pos, 255.0), 0.0); - write_command_.store(uint8_t(gripper_pos)); - - return hardware_interface::return_type::OK; -} - -} // namespace robotiq_driver - -#include "pluginlib/class_list_macros.hpp" - -PLUGINLIB_EXPORT_CLASS( - robotiq_driver::RobotiqGripperHardwareInterface, - hardware_interface::SystemInterface) diff --git a/src/robotiq_driver/src/robotiq_gripper_interface.cpp b/src/robotiq_driver/src/robotiq_gripper_interface.cpp deleted file mode 100644 index 335d52b7..00000000 --- a/src/robotiq_driver/src/robotiq_gripper_interface.cpp +++ /dev/null @@ -1,312 +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/robotiq_gripper_interface.hpp" -#include "robotiq_driver/crc.hpp" - -#include -#include -#include -#include - -constexpr int kBaudRate = 115200; -constexpr auto kTimeoutMilliseconds = 1000; - -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) -// number of data bytes (1 byte) -// data bytes (2 bytes per register) -// CRC (2 bytes) -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) -// address of the first register that was written (2 bytes) -// number of registers written (2 bytes) -// CRC (2 bytes) -constexpr int kWriteResponseSize = 8; - -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 getSecondByte(uint16_t val) -{ - return val & 0x00FF; -} - -RobotiqGripperInterface::RobotiqGripperInterface(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)) - , commanded_gripper_speed_(0x80) - , commanded_gripper_force_(0x80) -{ - if (!port_.isOpen()) { - THROW(serial::IOException, "Failed to open gripper port."); - } -} - -void RobotiqGripperInterface::activateGripper() -{ - const auto cmd = createWriteCommand( - kActionRequestRegister, {0x0100, 0x0000, 0x0000} // set rACT to 1, clear all - // other registers. - ); - - try { - sendCommand(cmd); - readResponse(kWriteResponseSize); - - updateStatus(); - - if (gripper_status_ == GripperStatus::COMPLETED) { - return; - } - - while (gripper_status_ == GripperStatus::IN_PROGRESS) { - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - updateStatus(); - } - } catch (const serial::IOException & e) { - // catch connection error and rethrow - std::cerr << "Failed to activate gripper"; - throw; - } -} - -void RobotiqGripperInterface::deactivateGripper() -{ - const auto cmd = createWriteCommand(kActionRequestRegister, {0x0000, 0x0000, 0x0000}); - try { - sendCommand(cmd); - readResponse(kWriteResponseSize); - } catch (const serial::IOException & e) { - // catch connection error and rethrow - std::cerr << "Failed to activate gripper"; - throw; - } -} - -void RobotiqGripperInterface::setGripperPosition(uint8_t pos) -{ - uint8_t action_register = 0x09; - uint8_t gripper_options_1 = 0x00; - uint8_t gripper_options_2 = 0x00; - - const auto cmd = - createWriteCommand( - 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); - } catch (const serial::IOException & e) { - // catch connection error and rethrow - std::cerr << "Failed to set gripper position\n"; - throw; - } -} - -uint8_t RobotiqGripperInterface::getGripperPosition() -{ - try { - updateStatus(); - } catch (const serial::IOException & e) { - // catch connection error and rethrow - std::cerr << "Failed to get gripper position\n"; - throw; - } - - return gripper_position_; -} - -bool RobotiqGripperInterface::gripperIsMoving() -{ - try { - updateStatus(); - } catch (const serial::IOException & e) { - // catch connection error and rethrow - std::cerr << "Failed to get gripper position\n"; - throw; - } - - return object_detection_status_ == ObjectDetectionStatus::MOVING; -} - -std::vector RobotiqGripperInterface::createReadCommand( - 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)}; - auto crc = computeCRC(cmd); - cmd.push_back(getFirstByte(crc)); - cmd.push_back(getSecondByte(crc)); - return cmd; -} - -void RobotiqGripperInterface::setSpeed(uint8_t speed) -{ - commanded_gripper_speed_ = speed; -} - -void RobotiqGripperInterface::setForce(uint8_t force) -{ - commanded_gripper_force_ = force; -} - -std::vector RobotiqGripperInterface::createWriteCommand( - 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) { - cmd.push_back(getFirstByte(d)); - cmd.push_back(getSecondByte(d)); - } - - auto crc = computeCRC(cmd); - cmd.push_back(getFirstByte(crc)); - cmd.push_back(getSecondByte(crc)); - - return cmd; -} - -std::vector RobotiqGripperInterface::readResponse(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); - THROW(serial::IOException, error_msg.c_str()); - } - - return response; -} - -void RobotiqGripperInterface::sendCommand(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()); - } -} - -void RobotiqGripperInterface::updateStatus() -{ - // Tell the gripper that we want to read its status. - try { - sendCommand(read_command_); - - const auto response = readResponse(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; - - // Action status. - action_status_ = - ((gripper_status_byte & 0x08) == 0x00) ? ActionStatus::STOPPED : ActionStatus::MOVING; - - // 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; - } - - // 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; - } - - // Read the current gripper position. - gripper_position_ = response[kResponseHeaderSize + kPositionIndex]; - } catch (const serial::IOException & e) { - std::cerr << "Failed to update gripper status.\n"; - throw; - } -} From 6af1fee0a6d75bc63bc3c64cda59f3afa4128e18 Mon Sep 17 00:00:00 2001 From: Chandima Fernando Date: Thu, 21 Mar 2024 14:39:51 -0400 Subject: [PATCH 13/32] fix: add missing links to yml --- docker/ur-hande-control/Dockerfile | 4 ++-- src/gripper_service/package.xml | 4 ++++ 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/docker/ur-hande-control/Dockerfile b/docker/ur-hande-control/Dockerfile index 6cda67b8..fd69a402 100644 --- a/docker/ur-hande-control/Dockerfile +++ b/docker/ur-hande-control/Dockerfile @@ -22,12 +22,12 @@ WORKDIR /root/ws/src RUN git clone https://github.com/PickNikRobotics/serial-release.git -b release/humble/serial RUN git clone https://github.com/PickNikRobotics/ros2_robotiq_gripper.git -b cmake-external-project-serial -# RUN git clone https://github.com/ChandimaFernando/erobs.git -b humble_hande_control +RUN git clone https://github.com/ChandimaFernando/erobs.git -b humble_hande_control WORKDIR /root/ws/ RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash && \ - colcon build --symlink-install --packages-select pdf_beamtime_interfaces serial robotiq_driver" + colcon build --symlink-install --packages-select pdf_beamtime_interfaces serial robotiq_driver gripper_service" CMD [ "/bin/bash" ] # CMD ["/bin/sh", "-c", ". /opt/ros/${ROS_DISTRO}/setup.sh && . /root/ws/install/setup.sh && ros2 run robotiq_driver gripper_service"] diff --git a/src/gripper_service/package.xml b/src/gripper_service/package.xml index e2e5b156..8e79e3eb 100644 --- a/src/gripper_service/package.xml +++ b/src/gripper_service/package.xml @@ -8,6 +8,10 @@ Apache-2.0 ament_cmake + rclcpp + rclcpp_components + pdf_beamtime_interfaces + robotiq_driver ament_lint_auto ament_lint_common From 3156f6e8275e7958526702af253fff2fb9aaa310 Mon Sep 17 00:00:00 2001 From: Chandima Fernando Date: Thu, 21 Mar 2024 14:45:02 -0400 Subject: [PATCH 14/32] FIX: PATH CORRECTION --- docker/ur-hande-control/Dockerfile | 6 +++++- src/gripper_service/src/gripper_service.cpp | 2 +- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/docker/ur-hande-control/Dockerfile b/docker/ur-hande-control/Dockerfile index fd69a402..9152dfb7 100644 --- a/docker/ur-hande-control/Dockerfile +++ b/docker/ur-hande-control/Dockerfile @@ -27,7 +27,11 @@ RUN git clone https://github.com/ChandimaFernando/erobs.git -b humble_hande_cont WORKDIR /root/ws/ RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash && \ - colcon build --symlink-install --packages-select pdf_beamtime_interfaces serial robotiq_driver gripper_service" + colcon build --symlink-install --packages-select pdf_beamtime_interfaces serial robotiq_driver" + +RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash && \ + source /root/ws/install/setup.bash && \ + colcon build --symlink-install --packages-select gripper_service" CMD [ "/bin/bash" ] # CMD ["/bin/sh", "-c", ". /opt/ros/${ROS_DISTRO}/setup.sh && . /root/ws/install/setup.sh && ros2 run robotiq_driver gripper_service"] diff --git a/src/gripper_service/src/gripper_service.cpp b/src/gripper_service/src/gripper_service.cpp index 68e5996a..180033a3 100644 --- a/src/gripper_service/src/gripper_service.cpp +++ b/src/gripper_service/src/gripper_service.cpp @@ -1,6 +1,6 @@ /*Copyright 2023 Brookhaven National Laboratory BSD 3 Clause License. See LICENSE.txt for details.*/ -#include +#include using namespace std::placeholders; From 6f45c9115530bcf39f54776f98c49458273ba223 Mon Sep 17 00:00:00 2001 From: Chandima Fernando Date: Thu, 21 Mar 2024 14:49:44 -0400 Subject: [PATCH 15/32] fix: correct runtime cmd for dockerfile --- docker/ur-hande-control/Dockerfile | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/docker/ur-hande-control/Dockerfile b/docker/ur-hande-control/Dockerfile index 9152dfb7..e0676dce 100644 --- a/docker/ur-hande-control/Dockerfile +++ b/docker/ur-hande-control/Dockerfile @@ -33,5 +33,4 @@ RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash && \ source /root/ws/install/setup.bash && \ colcon build --symlink-install --packages-select gripper_service" -CMD [ "/bin/bash" ] -# CMD ["/bin/sh", "-c", ". /opt/ros/${ROS_DISTRO}/setup.sh && . /root/ws/install/setup.sh && ros2 run robotiq_driver gripper_service"] +CMD ["/bin/sh", "-c", ". /opt/ros/${ROS_DISTRO}/setup.sh && . /root/ws/install/setup.sh && ros2 run gripper_service gripper_service"] From 6ecc3a708f30bb783513c68cc8697f715b90ed19 Mon Sep 17 00:00:00 2001 From: Chandima Fernando Date: Thu, 21 Mar 2024 14:51:57 -0400 Subject: [PATCH 16/32] refactor: remove extra files --- src/gripper_service/LICENSE | 202 ------------------------------------ 1 file changed, 202 deletions(-) delete mode 100644 src/gripper_service/LICENSE diff --git a/src/gripper_service/LICENSE b/src/gripper_service/LICENSE deleted file mode 100644 index d6456956..00000000 --- a/src/gripper_service/LICENSE +++ /dev/null @@ -1,202 +0,0 @@ - - Apache License - Version 2.0, January 2004 - http://www.apache.org/licenses/ - - TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION - - 1. Definitions. - - "License" shall mean the terms and conditions for use, reproduction, - and distribution as defined by Sections 1 through 9 of this document. - - "Licensor" shall mean the copyright owner or entity authorized by - the copyright owner that is granting the License. - - "Legal Entity" shall mean the union of the acting entity and all - other entities that control, are controlled by, or are under common - control with that entity. For the purposes of this definition, - "control" means (i) the power, direct or indirect, to cause the - direction or management of such entity, whether by contract or - otherwise, or (ii) ownership of fifty percent (50%) or more of the - outstanding shares, or (iii) beneficial ownership of such entity. - - "You" (or "Your") shall mean an individual or Legal Entity - exercising permissions granted by this License. - - "Source" form shall mean the preferred form for making modifications, - including but not limited to software source code, documentation - source, and configuration files. - - "Object" form shall mean any form resulting from mechanical - transformation or translation of a Source form, including but - not limited to compiled object code, generated documentation, - and conversions to other media types. - - "Work" shall mean the work of authorship, whether in Source or - Object form, made available under the License, as indicated by a - copyright notice that is included in or attached to the work - (an example is provided in the Appendix below). - - "Derivative Works" shall mean any work, whether in Source or Object - form, that is based on (or derived from) the Work and for which the - editorial revisions, annotations, elaborations, or other modifications - represent, as a whole, an original work of authorship. For the purposes - of this License, Derivative Works shall not include works that remain - separable from, or merely link (or bind by name) to the interfaces of, - the Work and Derivative Works thereof. - - "Contribution" shall mean any work of authorship, including - the original version of the Work and any modifications or additions - to that Work or Derivative Works thereof, that is intentionally - submitted to Licensor for inclusion in the Work by the copyright owner - or by an individual or Legal Entity authorized to submit on behalf of - the copyright owner. For the purposes of this definition, "submitted" - means any form of electronic, verbal, or written communication sent - to the Licensor or its representatives, including but not limited to - communication on electronic mailing lists, source code control systems, - and issue tracking systems that are managed by, or on behalf of, the - Licensor for the purpose of discussing and improving the Work, but - excluding communication that is conspicuously marked or otherwise - designated in writing by the copyright owner as "Not a Contribution." - - "Contributor" shall mean Licensor and any individual or Legal Entity - on behalf of whom a Contribution has been received by Licensor and - subsequently incorporated within the Work. - - 2. Grant of Copyright License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - copyright license to reproduce, prepare Derivative Works of, - publicly display, publicly perform, sublicense, and distribute the - Work and such Derivative Works in Source or Object form. - - 3. Grant of Patent License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - (except as stated in this section) patent license to make, have made, - use, offer to sell, sell, import, and otherwise transfer the Work, - where such license applies only to those patent claims licensable - by such Contributor that are necessarily infringed by their - Contribution(s) alone or by combination of their Contribution(s) - with the Work to which such Contribution(s) was submitted. If You - institute patent litigation against any entity (including a - cross-claim or counterclaim in a lawsuit) alleging that the Work - or a Contribution incorporated within the Work constitutes direct - or contributory patent infringement, then any patent licenses - granted to You under this License for that Work shall terminate - as of the date such litigation is filed. - - 4. Redistribution. You may reproduce and distribute copies of the - Work or Derivative Works thereof in any medium, with or without - modifications, and in Source or Object form, provided that You - meet the following conditions: - - (a) You must give any other recipients of the Work or - Derivative Works a copy of this License; and - - (b) You must cause any modified files to carry prominent notices - stating that You changed the files; and - - (c) You must retain, in the Source form of any Derivative Works - that You distribute, all copyright, patent, trademark, and - attribution notices from the Source form of the Work, - excluding those notices that do not pertain to any part of - the Derivative Works; and - - (d) If the Work includes a "NOTICE" text file as part of its - distribution, then any Derivative Works that You distribute must - include a readable copy of the attribution notices contained - within such NOTICE file, excluding those notices that do not - pertain to any part of the Derivative Works, in at least one - of the following places: within a NOTICE text file distributed - as part of the Derivative Works; within the Source form or - documentation, if provided along with the Derivative Works; or, - within a display generated by the Derivative Works, if and - wherever such third-party notices normally appear. The contents - of the NOTICE file are for informational purposes only and - do not modify the License. You may add Your own attribution - notices within Derivative Works that You distribute, alongside - or as an addendum to the NOTICE text from the Work, provided - that such additional attribution notices cannot be construed - as modifying the License. - - You may add Your own copyright statement to Your modifications and - may provide additional or different license terms and conditions - for use, reproduction, or distribution of Your modifications, or - for any such Derivative Works as a whole, provided Your use, - reproduction, and distribution of the Work otherwise complies with - the conditions stated in this License. - - 5. Submission of Contributions. Unless You explicitly state otherwise, - any Contribution intentionally submitted for inclusion in the Work - by You to the Licensor shall be under the terms and conditions of - this License, without any additional terms or conditions. - Notwithstanding the above, nothing herein shall supersede or modify - the terms of any separate license agreement you may have executed - with Licensor regarding such Contributions. - - 6. Trademarks. This License does not grant permission to use the trade - names, trademarks, service marks, or product names of the Licensor, - except as required for reasonable and customary use in describing the - origin of the Work and reproducing the content of the NOTICE file. - - 7. Disclaimer of Warranty. Unless required by applicable law or - agreed to in writing, Licensor provides the Work (and each - Contributor provides its Contributions) on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or - implied, including, without limitation, any warranties or conditions - of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A - PARTICULAR PURPOSE. You are solely responsible for determining the - appropriateness of using or redistributing the Work and assume any - risks associated with Your exercise of permissions under this License. - - 8. Limitation of Liability. In no event and under no legal theory, - whether in tort (including negligence), contract, or otherwise, - unless required by applicable law (such as deliberate and grossly - negligent acts) or agreed to in writing, shall any Contributor be - liable to You for damages, including any direct, indirect, special, - incidental, or consequential damages of any character arising as a - result of this License or out of the use or inability to use the - Work (including but not limited to damages for loss of goodwill, - work stoppage, computer failure or malfunction, or any and all - other commercial damages or losses), even if such Contributor - has been advised of the possibility of such damages. - - 9. Accepting Warranty or Additional Liability. While redistributing - the Work or Derivative Works thereof, You may choose to offer, - and charge a fee for, acceptance of support, warranty, indemnity, - or other liability obligations and/or rights consistent with this - License. However, in accepting such obligations, You may act only - on Your own behalf and on Your sole responsibility, not on behalf - of any other Contributor, and only if You agree to indemnify, - defend, and hold each Contributor harmless for any liability - incurred by, or claims asserted against, such Contributor by reason - of your accepting any such warranty or additional liability. - - END OF TERMS AND CONDITIONS - - APPENDIX: How to apply the Apache License to your work. - - To apply the Apache License to your work, attach the following - boilerplate notice, with the fields enclosed by brackets "[]" - replaced with your own identifying information. (Don't include - the brackets!) The text should be enclosed in the appropriate - comment syntax for the file format. We also recommend that a - file or class name and description of purpose be included on the - same "printed page" as the copyright notice for easier - identification within third-party archives. - - Copyright [yyyy] [name of copyright owner] - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. From d0d86d5471965fedbbbcb133ab6ba3c4f5743c33 Mon Sep 17 00:00:00 2001 From: Chandima Fernando Date: Mon, 25 Mar 2024 10:37:05 -0400 Subject: [PATCH 17/32] fix: change node structure --- .../gripper_service/gripper_service.hpp | 5 ++- src/gripper_service/src/gripper_service.cpp | 32 ++++++++----------- src/ros2.repos | 8 +++++ 3 files changed, 23 insertions(+), 22 deletions(-) diff --git a/src/gripper_service/include/gripper_service/gripper_service.hpp b/src/gripper_service/include/gripper_service/gripper_service.hpp index 2cb86b13..bbc86de7 100644 --- a/src/gripper_service/include/gripper_service/gripper_service.hpp +++ b/src/gripper_service/include/gripper_service/gripper_service.hpp @@ -10,7 +10,7 @@ BSD 3 Clause License. See LICENSE.txt for details.*/ #include -class GripperService +class GripperService : public rclcpp::Node { private: @@ -18,7 +18,7 @@ class GripperService const char * kComPort = "/tmp/ttyUR"; const int kSlaveID = 0x09; - rclcpp::Node::SharedPtr node_; + // rclcpp::Node::SharedPtr node_; RobotiqGripperInterface gripper_; /// @brief Gripper commands as enums for ease of use @@ -39,5 +39,4 @@ class GripperService public: explicit GripperService(); - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr getNodeBaseInterface(); }; diff --git a/src/gripper_service/src/gripper_service.cpp b/src/gripper_service/src/gripper_service.cpp index 180033a3..04a88b98 100644 --- a/src/gripper_service/src/gripper_service.cpp +++ b/src/gripper_service/src/gripper_service.cpp @@ -5,10 +5,10 @@ BSD 3 Clause License. See LICENSE.txt for details.*/ using namespace std::placeholders; GripperService::GripperService() -: node_(std::make_shared("gripper_server_node")), +: Node("gripper_server_node"), gripper_(kComPort, kSlaveID) { - RCLCPP_INFO(node_->get_logger(), "Activate the gripper ..."); + RCLCPP_INFO(this->get_logger(), "Activate the gripper ..."); // Clear the registers gripper_.deactivateGripper(); std::this_thread::sleep_for(std::chrono::milliseconds(1000)); @@ -18,20 +18,14 @@ GripperService::GripperService() std::this_thread::sleep_for(std::chrono::milliseconds(1000)); gripper_.setSpeed(0x0F); - RCLCPP_INFO(node_->get_logger(), "Activation is successful"); + RCLCPP_INFO(this->get_logger(), "Activation is successful"); rclcpp::Service::SharedPtr service = - node_->create_service( + this->create_service( "gripper_service", std::bind( &GripperService::gripper_controller, this, _1, _2)); - RCLCPP_INFO(node_->get_logger(), "Ready to receive gripper commands."); -} - -rclcpp::node_interfaces::NodeBaseInterface::SharedPtr GripperService::getNodeBaseInterface() -// Expose the node base interface so that the node can be added to a component manager. -{ - return node_->get_node_base_interface(); + RCLCPP_INFO(this->get_logger(), "Ready to receive gripper commands."); } void GripperService::gripper_controller( @@ -49,14 +43,14 @@ void GripperService::gripper_controller( gripper_.deactivateGripper(); std::this_thread::sleep_for(std::chrono::milliseconds(1000)); gripper_.activateGripper(); - RCLCPP_INFO(node_->get_logger(), "Activation is successful"); + RCLCPP_INFO(this->get_logger(), "Activation is successful"); break; case Gripper_Command::DEACTIVE: // Deactivate the gripper gripper_.deactivateGripper(); - RCLCPP_INFO(node_->get_logger(), "Gripper is Deactivated"); + RCLCPP_INFO(this->get_logger(), "Gripper is Deactivated"); break; @@ -65,20 +59,20 @@ void GripperService::gripper_controller( // Closes the gripper to the percentage set by request->grip uint8_t val = request->grip * 2.55; // convert the scales from 01-100 to 0-255 gripper_.setGripperPosition(val); - RCLCPP_INFO(node_->get_logger(), "Gripper is Open"); + RCLCPP_INFO(this->get_logger(), "Gripper is Open"); } break; case Gripper_Command::OPEN: /* Open the gripper fully */ gripper_.setGripperPosition(0x00); - RCLCPP_INFO(node_->get_logger(), "Gripper is Open"); + RCLCPP_INFO(this->get_logger(), "Gripper is Open"); break; case Gripper_Command::CLOSE: /* Close the gripper fully */ gripper_.setGripperPosition(0xFF); - RCLCPP_INFO(node_->get_logger(), "Gripper is Close"); + RCLCPP_INFO(this->get_logger(), "Gripper is Close"); break; default: @@ -86,7 +80,7 @@ void GripperService::gripper_controller( } status = 1; } catch (const std::exception & e) { - RCLCPP_ERROR(node_->get_logger(), e.what()); + RCLCPP_ERROR(this->get_logger(), e.what()); status = 0; } @@ -98,9 +92,9 @@ int main(int argc, char ** argv) { rclcpp::init(argc, argv); - auto gripper_server = std::make_shared(); + auto gripper_server_node = std::make_shared(); - rclcpp::spin(gripper_server->getNodeBaseInterface()); + rclcpp::spin(gripper_server_node); rclcpp::shutdown(); return 0; diff --git a/src/ros2.repos b/src/ros2.repos index 87890a30..d7249b1a 100644 --- a/src/ros2.repos +++ b/src/ros2.repos @@ -8,3 +8,11 @@ repositories: type: git url: https://github.com/ros2/examples.git version: humble + gripper: + type: git + url: https://github.com/PickNikRobotics/ros2_robotiq_gripper.git + version: cmake-external-project-serial + serial: + type: git + url: https://github.com/PickNikRobotics/serial-release.git + version: release/humble/serial From 1acebbc7c62c4cec544a6d1507cf7896e6de14a3 Mon Sep 17 00:00:00 2001 From: Chandima Fernando Date: Mon, 25 Mar 2024 17:22:01 -0400 Subject: [PATCH 18/32] feat: build one docker img for all containers --- docker/erobs-common-img/Dockerfile | 55 +++++++++++++++++++ docker/erobs-common-img/README.md | 37 +++++++++++++ .../gripper_service/gripper_service.hpp | 9 ++- src/gripper_service/src/gripper_service.cpp | 23 +++----- 4 files changed, 107 insertions(+), 17 deletions(-) create mode 100644 docker/erobs-common-img/Dockerfile create mode 100644 docker/erobs-common-img/README.md diff --git a/docker/erobs-common-img/Dockerfile b/docker/erobs-common-img/Dockerfile new file mode 100644 index 00000000..edcb7f87 --- /dev/null +++ b/docker/erobs-common-img/Dockerfile @@ -0,0 +1,55 @@ +FROM osrf/ros:humble-desktop-full + +# Set display +ENV DISPLAY :1 + +RUN apt-get update \ + && apt-get -y install \ + python3-pip \ + ros-${ROS_DISTRO}-rclpy \ + ros-${ROS_DISTRO}-ur \ + build-essential \ + python3-colcon-common-extensions +RUN apt-get install -y x11vnc xvfb +# Clean up +RUN apt-get autoremove -y \ + && apt-get clean -y \ + && rm -rf /var/lib/apt/lists/* +RUN pip3 install rosdep && \ + rosdep update + +RUN mkdir /root/ws/src -p + +WORKDIR /root/ws/src + +# TODO @ChandimaFernando. Revert back after tests. RUN git clone https://github.com/maffettone/erobs.git -b humble +RUN git clone https://github.com/ChandimaFernando/erobs.git -b humble_hande_control +RUN git clone https://github.com/PickNikRobotics/serial-release.git -b release/humble/serial +RUN git clone https://github.com/PickNikRobotics/ros2_robotiq_gripper.git -b cmake-external-project-serial + +WORKDIR /root/ws/ + +RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash && \ + colcon build --symlink-install --packages-select ur3e_hande_robot_description && \ + colcon build --symlink-install --packages-select ur3e_hande_moveit_config" + +RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash && \ + colcon build --symlink-install --packages-select pdf_beamtime_interfaces serial robotiq_driver pdf_beamtime" + +RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash && \ + source /root/ws/install/setup.bash && \ + colcon build --symlink-install --packages-select gripper_service" + +WORKDIR /root/ws/ + +ENV ROBOT_IP=192.168.56.101 +ENV REVERSE_IP=192.168.56.102 +ENV UR_TYPE="ur3e" +ENV LAUNCH_RVIZ="false" +ENV DESCRIPTION_PKG="ur3e_hande_robot_description" +ENV DESCRIPTION_FILE="ur_with_hande.xacro" +ENV CONFIG_PKG="ur3e_hande_moveit_config" +ENV CONFIG_FILE="ur.srdf" + +# VNC viewer +EXPOSE 5901 diff --git a/docker/erobs-common-img/README.md b/docker/erobs-common-img/README.md new file mode 100644 index 00000000..71eef287 --- /dev/null +++ b/docker/erobs-common-img/README.md @@ -0,0 +1,37 @@ +# This file contains commands to launch multiple containers of the same docker image. +(TODO ChandimaFernando : Change the repo link to nsls-II repo.) + +export ROBOT_IP=10.67.218.141 +export UR_TYPE="ur3e" +export LAUNCH_RVIZ="false" +export DESCRIPTION_PKG="ur3e_hande_robot_description" +export DESCRIPTION_FILE="ur_with_hande.xacro" +export CONFIG_PKG="ur3e_hande_moveit_config" +export CONFIG_FILE="ur.srdf" +export ROS_DISTRO=humble + +# Enables the connection between the UR robot and the VM +podman run -it --network host --ipc=host --pid=host \ + --env ROBOT_IP=$ROBOT_IP \ + --env UR_TYPE=$UR_TYPE \ + --env ROS_DISTRO=$ROS_DISTRO \ + ghcr.io/chandimafernando/erobs-common-img:latest \ + /bin/sh -c "printenv && . /opt/ros/${ROS_DISTRO}/setup.sh && . /root/ws/install/setup.sh && ros2 launch ur_robot_driver ur_control.launch.py ur_type:=${UR_TYPE} robot_ip:=${ROBOT_IP} launch_rviz:=false tool_voltage:=24" + +# Runs gripper service to enable the gripper +podman run -it --network host --ipc=host --pid=host \ + ghcr.io/chandimafernando/erobs-common-img:latest \ + /bin/bash -c ". /root/ws/install/setup.sh && \ +ros2 run gripper_service gripper_service" + +# Launches move_group +podman run -it --network host --ipc=host --pid=host \ + --env ROBOT_IP=$ROBOT_IP \ + --env UR_TYPE=$UR_TYPE \ + --env ROS_DISTRO=$ROS_DISTRO \ + --env DESCRIPTION_PKG=$DESCRIPTION_PKG \ + --env DESCRIPTION_FILE=$DESCRIPTION_FILE \ + --env CONFIG_PKG=$CONFIG_PKG \ + --env CONFIG_FILE=$CONFIG_FILE \ + ghcr.io/chandimafernando/erobs-common-img:latest \ + /bin/sh -c "printenv && . /opt/ros/${ROS_DISTRO}/setup.sh && . /root/ws/install/setup.sh && ros2 launch ur_moveit_config ur_moveit.launch.py ur_type:=${UR_TYPE} launch_rviz:=${LAUNCH_RVIZ} description_package:=${DESCRIPTION_PKG} launch_servo:=false description_file:=${DESCRIPTION_FILE} moveit_config_package:=${CONFIG_PKG} moveit_config_file:=${CONFIG_FILE}" diff --git a/src/gripper_service/include/gripper_service/gripper_service.hpp b/src/gripper_service/include/gripper_service/gripper_service.hpp index bbc86de7..4b4b1402 100644 --- a/src/gripper_service/include/gripper_service/gripper_service.hpp +++ b/src/gripper_service/include/gripper_service/gripper_service.hpp @@ -4,7 +4,11 @@ BSD 3 Clause License. See LICENSE.txt for details.*/ #include #include -#include "rclcpp/rclcpp.hpp" +#include +#include +#include + +#include #include "pdf_beamtime_interfaces/srv/gripper_control_msg.hpp" @@ -12,7 +16,6 @@ BSD 3 Clause License. See LICENSE.txt for details.*/ class GripperService : public rclcpp::Node { - private: /// @brief your serial port goes here const char * kComPort = "/tmp/ttyUR"; @@ -38,5 +41,5 @@ class GripperService : public rclcpp::Node std::shared_ptr response); public: - explicit GripperService(); + GripperService(); }; diff --git a/src/gripper_service/src/gripper_service.cpp b/src/gripper_service/src/gripper_service.cpp index 04a88b98..9d07e170 100644 --- a/src/gripper_service/src/gripper_service.cpp +++ b/src/gripper_service/src/gripper_service.cpp @@ -11,11 +11,9 @@ GripperService::GripperService() RCLCPP_INFO(this->get_logger(), "Activate the gripper ..."); // Clear the registers gripper_.deactivateGripper(); - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + // Activate the gripper gripper_.activateGripper(); - - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); gripper_.setSpeed(0x0F); RCLCPP_INFO(this->get_logger(), "Activation is successful"); @@ -34,14 +32,13 @@ void GripperService::gripper_controller( { int status = 0; try { - // conver the request command string to the mapping enum + // conver the request command string to the mapping enum Gripper_Command gripper_command_enum = gripper_command_map_[request->command]; switch (gripper_command_enum) { case Gripper_Command::ACTIVE: // Activate the gripper gripper_.deactivateGripper(); - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); gripper_.activateGripper(); RCLCPP_INFO(this->get_logger(), "Activation is successful"); @@ -55,22 +52,20 @@ void GripperService::gripper_controller( break; case Gripper_Command::PARTIAL: - { - // Closes the gripper to the percentage set by request->grip - uint8_t val = request->grip * 2.55; // convert the scales from 01-100 to 0-255 - gripper_.setGripperPosition(val); - RCLCPP_INFO(this->get_logger(), "Gripper is Open"); - } + // Closes the gripper to the percentage set by request->grip + uint8_t val = request->grip * 2.55; // convert the scales from 01-100 to 0-255 + gripper_.setGripperPosition(val); + RCLCPP_INFO(this->get_logger(), "Gripper is Open"); break; case Gripper_Command::OPEN: - /* Open the gripper fully */ + // Open the gripper fully gripper_.setGripperPosition(0x00); RCLCPP_INFO(this->get_logger(), "Gripper is Open"); break; case Gripper_Command::CLOSE: - /* Close the gripper fully */ + // Close the gripper fully gripper_.setGripperPosition(0xFF); RCLCPP_INFO(this->get_logger(), "Gripper is Close"); break; @@ -84,7 +79,7 @@ void GripperService::gripper_controller( status = 0; } - // Send the response back + // Send the response back response->results = status; } From 9231df037eda314513033e5062d9719e2aee2630 Mon Sep 17 00:00:00 2001 From: Chandima Fernando Date: Mon, 25 Mar 2024 17:27:02 -0400 Subject: [PATCH 19/32] fix: gripper scope error --- src/gripper_service/src/gripper_service.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/gripper_service/src/gripper_service.cpp b/src/gripper_service/src/gripper_service.cpp index 9d07e170..19c1d1b6 100644 --- a/src/gripper_service/src/gripper_service.cpp +++ b/src/gripper_service/src/gripper_service.cpp @@ -52,10 +52,12 @@ void GripperService::gripper_controller( break; case Gripper_Command::PARTIAL: - // Closes the gripper to the percentage set by request->grip - uint8_t val = request->grip * 2.55; // convert the scales from 01-100 to 0-255 - gripper_.setGripperPosition(val); - RCLCPP_INFO(this->get_logger(), "Gripper is Open"); + { + // Closes the gripper to the percentage set by request->grip + uint8_t val = request->grip * 2.55; // convert the scales from 01-100 to 0-255 + gripper_.setGripperPosition(val); + RCLCPP_INFO(this->get_logger(), "Gripper is Open"); + } break; case Gripper_Command::OPEN: From adb286b25a3a8aa0d49fa72beb12fc7a42e47418 Mon Sep 17 00:00:00 2001 From: Chandima Fernando Date: Tue, 26 Mar 2024 10:08:58 -0400 Subject: [PATCH 20/32] fix: add ignore paths for lint --- .github/workflows/ros.yaml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.github/workflows/ros.yaml b/.github/workflows/ros.yaml index 89e9b457..749c7b59 100644 --- a/.github/workflows/ros.yaml +++ b/.github/workflows/ros.yaml @@ -3,6 +3,9 @@ name: ROS on: pull_request: push: + paths-ignore: + - './src/serial/**' + - './src/gripper/**' workflow_dispatch: jobs: From f97c3a390aa3a72c125a2fd041d9241647af732a Mon Sep 17 00:00:00 2001 From: Chandima Fernando Date: Tue, 26 Mar 2024 11:07:22 -0400 Subject: [PATCH 21/32] fix: remove setup.sh --- .github/actions/lint/run.sh | 2 +- .github/workflows/ros.yaml | 3 --- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/.github/actions/lint/run.sh b/.github/actions/lint/run.sh index 9d94eff7..1aa85f81 100755 --- a/.github/actions/lint/run.sh +++ b/.github/actions/lint/run.sh @@ -1,5 +1,5 @@ #!/bin/bash set -e -./setup.sh +# ./setup.sh ament_${LINTER} src/ diff --git a/.github/workflows/ros.yaml b/.github/workflows/ros.yaml index 749c7b59..89e9b457 100644 --- a/.github/workflows/ros.yaml +++ b/.github/workflows/ros.yaml @@ -3,9 +3,6 @@ name: ROS on: pull_request: push: - paths-ignore: - - './src/serial/**' - - './src/gripper/**' workflow_dispatch: jobs: From efc9bf6409658cd520c93c5d84dab0bb73644ce2 Mon Sep 17 00:00:00 2001 From: Chandima Fernando Date: Tue, 26 Mar 2024 11:11:43 -0400 Subject: [PATCH 22/32] fix: lint errors --- src/gripper_service/src/gripper_service.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/gripper_service/src/gripper_service.cpp b/src/gripper_service/src/gripper_service.cpp index 19c1d1b6..708604fb 100644 --- a/src/gripper_service/src/gripper_service.cpp +++ b/src/gripper_service/src/gripper_service.cpp @@ -54,7 +54,7 @@ void GripperService::gripper_controller( case Gripper_Command::PARTIAL: { // Closes the gripper to the percentage set by request->grip - uint8_t val = request->grip * 2.55; // convert the scales from 01-100 to 0-255 + uint8_t val = request->grip * 2.55; // convert the scales from 01-100 to 0-255 gripper_.setGripperPosition(val); RCLCPP_INFO(this->get_logger(), "Gripper is Open"); } From a79af471ee5ef3f7ce0bcc0e10980890f5e26eec Mon Sep 17 00:00:00 2001 From: Chandima Fernando Date: Wed, 27 Mar 2024 21:44:28 -0400 Subject: [PATCH 23/32] fix: point to build.sh --- build.sh | 10 +++++++++- docker/erobs-common-img/Dockerfile | 30 +++++++++++++++++++----------- 2 files changed, 28 insertions(+), 12 deletions(-) diff --git a/build.sh b/build.sh index 7bf6bd8b..2d67eab5 100755 --- a/build.sh +++ b/build.sh @@ -3,8 +3,16 @@ set -e # Set the default build type BUILD_TYPE=RelWithDebInfo -colcon build \ + +source /opt/ros/${ROS_DISTRO}/setup.bash && colcon build \ --merge-install \ --symlink-install \ --cmake-args "-DCMAKE_BUILD_TYPE=$BUILD_TYPE" "-DCMAKE_EXPORT_COMPILE_COMMANDS=On" \ + --packages-select ur3e_hande_robot_description serial robotiq_driver pdf_beamtime_interfaces \ -Wall -Wextra -Wpedantic + +colcon build \ + --merge-install \ + --symlink-install \ + --cmake-args "-DCMAKE_BUILD_TYPE=$BUILD_TYPE" "-DCMAKE_EXPORT_COMPILE_COMMANDS=On" \ + -Wall -Wextra -Wpedantic \ No newline at end of file diff --git a/docker/erobs-common-img/Dockerfile b/docker/erobs-common-img/Dockerfile index edcb7f87..9511f63e 100644 --- a/docker/erobs-common-img/Dockerfile +++ b/docker/erobs-common-img/Dockerfile @@ -24,21 +24,29 @@ WORKDIR /root/ws/src # TODO @ChandimaFernando. Revert back after tests. RUN git clone https://github.com/maffettone/erobs.git -b humble RUN git clone https://github.com/ChandimaFernando/erobs.git -b humble_hande_control -RUN git clone https://github.com/PickNikRobotics/serial-release.git -b release/humble/serial -RUN git clone https://github.com/PickNikRobotics/ros2_robotiq_gripper.git -b cmake-external-project-serial -WORKDIR /root/ws/ +WORKDIR /root/ws/src/erobs +RUN /bin/bash -c ". /root/ws/src/erobs/setup.sh" + +# WORKDIR /root/ws/src/ +WORKDIR /root/ws/src/erobs/ + +RUN /bin/bash -c "ls /root/ws/src/erobs/" +RUN /bin/bash -c "ls /root/ws/src/erobs/src" + +RUN /bin/bash -c "cat /root/ws/src/erobs/build.sh" +RUN /bin/bash -c ". /root/ws/src/erobs/build.sh" -RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash && \ - colcon build --symlink-install --packages-select ur3e_hande_robot_description && \ - colcon build --symlink-install --packages-select ur3e_hande_moveit_config" +# RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash && \ +# colcon build --symlink-install --packages-select ur3e_hande_robot_description && \ +# colcon build --symlink-install --packages-select ur3e_hande_moveit_config" -RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash && \ - colcon build --symlink-install --packages-select pdf_beamtime_interfaces serial robotiq_driver pdf_beamtime" +# RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash && \ +# colcon build --symlink-install --packages-select pdf_beamtime_interfaces serial robotiq_driver pdf_beamtime" -RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash && \ - source /root/ws/install/setup.bash && \ - colcon build --symlink-install --packages-select gripper_service" +# RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash && \ +# source /root/ws/install/setup.bash && \ +# colcon build --symlink-install --packages-select gripper_service" WORKDIR /root/ws/ From 295887a33101a729c669b3f7cd70c516c9293c76 Mon Sep 17 00:00:00 2001 From: Chandima Fernando Date: Wed, 27 Mar 2024 23:57:39 -0400 Subject: [PATCH 24/32] fix: cmake args temp fix --- build.sh | 4 ++-- docker/erobs-common-img/Dockerfile | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/build.sh b/build.sh index 2d67eab5..921d1361 100755 --- a/build.sh +++ b/build.sh @@ -7,9 +7,9 @@ BUILD_TYPE=RelWithDebInfo source /opt/ros/${ROS_DISTRO}/setup.bash && colcon build \ --merge-install \ --symlink-install \ - --cmake-args "-DCMAKE_BUILD_TYPE=$BUILD_TYPE" "-DCMAKE_EXPORT_COMPILE_COMMANDS=On" \ --packages-select ur3e_hande_robot_description serial robotiq_driver pdf_beamtime_interfaces \ - -Wall -Wextra -Wpedantic + --cmake-args "-DCMAKE_BUILD_TYPE=$BUILD_TYPE" "-DCMAKE_EXPORT_COMPILE_COMMANDS=On" \ + --cmake-args "-DCMAKE_CXX_FLAGS=-Wall -Wextra -Wpedantic" colcon build \ --merge-install \ diff --git a/docker/erobs-common-img/Dockerfile b/docker/erobs-common-img/Dockerfile index 9511f63e..8b9187d7 100644 --- a/docker/erobs-common-img/Dockerfile +++ b/docker/erobs-common-img/Dockerfile @@ -20,6 +20,7 @@ RUN pip3 install rosdep && \ RUN mkdir /root/ws/src -p +WORKDIR /root WORKDIR /root/ws/src # TODO @ChandimaFernando. Revert back after tests. RUN git clone https://github.com/maffettone/erobs.git -b humble @@ -28,10 +29,9 @@ RUN git clone https://github.com/ChandimaFernando/erobs.git -b humble_hande_cont WORKDIR /root/ws/src/erobs RUN /bin/bash -c ". /root/ws/src/erobs/setup.sh" -# WORKDIR /root/ws/src/ WORKDIR /root/ws/src/erobs/ -RUN /bin/bash -c "ls /root/ws/src/erobs/" +# RUN /bin/bash -c "ls /root/ws/src/erobs/" RUN /bin/bash -c "ls /root/ws/src/erobs/src" RUN /bin/bash -c "cat /root/ws/src/erobs/build.sh" From e4c7420a7e7290557df6e749e8b0dc62880dceb6 Mon Sep 17 00:00:00 2001 From: Chandima Fernando Date: Thu, 28 Mar 2024 00:28:01 -0400 Subject: [PATCH 25/32] fix: cleaned up Dockerfile --- docker/erobs-common-img/Dockerfile | 19 ------------------- 1 file changed, 19 deletions(-) diff --git a/docker/erobs-common-img/Dockerfile b/docker/erobs-common-img/Dockerfile index 8b9187d7..7fc10284 100644 --- a/docker/erobs-common-img/Dockerfile +++ b/docker/erobs-common-img/Dockerfile @@ -20,7 +20,6 @@ RUN pip3 install rosdep && \ RUN mkdir /root/ws/src -p -WORKDIR /root WORKDIR /root/ws/src # TODO @ChandimaFernando. Revert back after tests. RUN git clone https://github.com/maffettone/erobs.git -b humble @@ -28,26 +27,8 @@ RUN git clone https://github.com/ChandimaFernando/erobs.git -b humble_hande_cont WORKDIR /root/ws/src/erobs RUN /bin/bash -c ". /root/ws/src/erobs/setup.sh" - -WORKDIR /root/ws/src/erobs/ - -# RUN /bin/bash -c "ls /root/ws/src/erobs/" -RUN /bin/bash -c "ls /root/ws/src/erobs/src" - -RUN /bin/bash -c "cat /root/ws/src/erobs/build.sh" RUN /bin/bash -c ". /root/ws/src/erobs/build.sh" -# RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash && \ -# colcon build --symlink-install --packages-select ur3e_hande_robot_description && \ -# colcon build --symlink-install --packages-select ur3e_hande_moveit_config" - -# RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash && \ -# colcon build --symlink-install --packages-select pdf_beamtime_interfaces serial robotiq_driver pdf_beamtime" - -# RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash && \ -# source /root/ws/install/setup.bash && \ -# colcon build --symlink-install --packages-select gripper_service" - WORKDIR /root/ws/ ENV ROBOT_IP=192.168.56.101 From 914fcc18de4db4ae029bbe6c29ba532ab9eb1fb0 Mon Sep 17 00:00:00 2001 From: Chandima Fernando Date: Thu, 28 Mar 2024 00:54:13 -0400 Subject: [PATCH 26/32] fix: correct build location --- docker/erobs-common-img/Dockerfile | 8 +++++--- docker/erobs-common-img/README.md | 8 ++++---- setup.sh | 2 +- 3 files changed, 10 insertions(+), 8 deletions(-) diff --git a/docker/erobs-common-img/Dockerfile b/docker/erobs-common-img/Dockerfile index 7fc10284..6d8e0baa 100644 --- a/docker/erobs-common-img/Dockerfile +++ b/docker/erobs-common-img/Dockerfile @@ -25,9 +25,11 @@ WORKDIR /root/ws/src # TODO @ChandimaFernando. Revert back after tests. RUN git clone https://github.com/maffettone/erobs.git -b humble RUN git clone https://github.com/ChandimaFernando/erobs.git -b humble_hande_control -WORKDIR /root/ws/src/erobs -RUN /bin/bash -c ". /root/ws/src/erobs/setup.sh" -RUN /bin/bash -c ". /root/ws/src/erobs/build.sh" +# WORKDIR /root/ws/src/erobs +RUN /bin/bash -c "ls ./erobs" + +RUN /bin/bash -c ". ./erobs/setup.sh" +RUN /bin/bash -c ". ./erobs/build.sh" WORKDIR /root/ws/ diff --git a/docker/erobs-common-img/README.md b/docker/erobs-common-img/README.md index 71eef287..91aca7c6 100644 --- a/docker/erobs-common-img/README.md +++ b/docker/erobs-common-img/README.md @@ -10,7 +10,7 @@ export CONFIG_PKG="ur3e_hande_moveit_config" export CONFIG_FILE="ur.srdf" export ROS_DISTRO=humble -# Enables the connection between the UR robot and the VM +# Enable the connection between the UR robot and the VM podman run -it --network host --ipc=host --pid=host \ --env ROBOT_IP=$ROBOT_IP \ --env UR_TYPE=$UR_TYPE \ @@ -18,13 +18,13 @@ podman run -it --network host --ipc=host --pid=host \ ghcr.io/chandimafernando/erobs-common-img:latest \ /bin/sh -c "printenv && . /opt/ros/${ROS_DISTRO}/setup.sh && . /root/ws/install/setup.sh && ros2 launch ur_robot_driver ur_control.launch.py ur_type:=${UR_TYPE} robot_ip:=${ROBOT_IP} launch_rviz:=false tool_voltage:=24" -# Runs gripper service to enable the gripper +# Run gripper service to enable the gripper podman run -it --network host --ipc=host --pid=host \ ghcr.io/chandimafernando/erobs-common-img:latest \ /bin/bash -c ". /root/ws/install/setup.sh && \ -ros2 run gripper_service gripper_service" + ros2 run gripper_service gripper_service" -# Launches move_group +# Launch move_group podman run -it --network host --ipc=host --pid=host \ --env ROBOT_IP=$ROBOT_IP \ --env UR_TYPE=$UR_TYPE \ diff --git a/setup.sh b/setup.sh index bb31f70c..a89ea6bc 100755 --- a/setup.sh +++ b/setup.sh @@ -1,7 +1,7 @@ #!/bin/bash set -e -vcs import < src/ros2.repos src +vcs import < /root/ws/src/erobs/src/ros2.repos /root/ws/src/erobs/src sudo apt-get update rosdep update rosdep install --from-paths src --ignore-src -y From 53177594f77645ce99c56ab76fbd1a4e3ed061b3 Mon Sep 17 00:00:00 2001 From: Chandima Fernando Date: Thu, 28 Mar 2024 00:59:07 -0400 Subject: [PATCH 27/32] fix: more path corrections --- docker/erobs-common-img/Dockerfile | 2 ++ setup.sh | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/docker/erobs-common-img/Dockerfile b/docker/erobs-common-img/Dockerfile index 6d8e0baa..4d6825be 100644 --- a/docker/erobs-common-img/Dockerfile +++ b/docker/erobs-common-img/Dockerfile @@ -20,6 +20,7 @@ RUN pip3 install rosdep && \ RUN mkdir /root/ws/src -p +WORKDIR /root/ws WORKDIR /root/ws/src # TODO @ChandimaFernando. Revert back after tests. RUN git clone https://github.com/maffettone/erobs.git -b humble @@ -28,6 +29,7 @@ RUN git clone https://github.com/ChandimaFernando/erobs.git -b humble_hande_cont # WORKDIR /root/ws/src/erobs RUN /bin/bash -c "ls ./erobs" +RUN /bin/bash -c "cat ./erobs/setup.sh" RUN /bin/bash -c ". ./erobs/setup.sh" RUN /bin/bash -c ". ./erobs/build.sh" diff --git a/setup.sh b/setup.sh index a89ea6bc..f685cdf1 100755 --- a/setup.sh +++ b/setup.sh @@ -4,4 +4,4 @@ set -e vcs import < /root/ws/src/erobs/src/ros2.repos /root/ws/src/erobs/src sudo apt-get update rosdep update -rosdep install --from-paths src --ignore-src -y +rosdep install --from-paths /root/ws/src --ignore-src -y From e50afb143b5a733fd9712905442285b291af0593 Mon Sep 17 00:00:00 2001 From: Chandima Fernando Date: Thu, 28 Mar 2024 01:12:51 -0400 Subject: [PATCH 28/32] fix: correct and clean dockerfile --- docker/erobs-common-img/Dockerfile | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/docker/erobs-common-img/Dockerfile b/docker/erobs-common-img/Dockerfile index 4d6825be..5503db15 100644 --- a/docker/erobs-common-img/Dockerfile +++ b/docker/erobs-common-img/Dockerfile @@ -20,18 +20,15 @@ RUN pip3 install rosdep && \ RUN mkdir /root/ws/src -p -WORKDIR /root/ws WORKDIR /root/ws/src # TODO @ChandimaFernando. Revert back after tests. RUN git clone https://github.com/maffettone/erobs.git -b humble RUN git clone https://github.com/ChandimaFernando/erobs.git -b humble_hande_control -# WORKDIR /root/ws/src/erobs -RUN /bin/bash -c "ls ./erobs" +WORKDIR /root/ws/ -RUN /bin/bash -c "cat ./erobs/setup.sh" -RUN /bin/bash -c ". ./erobs/setup.sh" -RUN /bin/bash -c ". ./erobs/build.sh" +RUN /bin/bash -c ". ./src/erobs/setup.sh" +RUN /bin/bash -c ". ./src/erobs/build.sh" WORKDIR /root/ws/ From be77ef51c7707cbda785d3a82a71f96ec5b16b16 Mon Sep 17 00:00:00 2001 From: Chandima Fernando Date: Thu, 28 Mar 2024 01:38:46 -0400 Subject: [PATCH 29/32] fix: correct tests --- docker/erobs-common-img/Dockerfile | 11 +++++------ setup.sh | 4 ++-- 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/docker/erobs-common-img/Dockerfile b/docker/erobs-common-img/Dockerfile index 5503db15..515e8f06 100644 --- a/docker/erobs-common-img/Dockerfile +++ b/docker/erobs-common-img/Dockerfile @@ -18,19 +18,18 @@ RUN apt-get autoremove -y \ RUN pip3 install rosdep && \ rosdep update -RUN mkdir /root/ws/src -p +RUN mkdir /root/ws -p -WORKDIR /root/ws/src +WORKDIR /root/ws # TODO @ChandimaFernando. Revert back after tests. RUN git clone https://github.com/maffettone/erobs.git -b humble RUN git clone https://github.com/ChandimaFernando/erobs.git -b humble_hande_control -WORKDIR /root/ws/ - -RUN /bin/bash -c ". ./src/erobs/setup.sh" -RUN /bin/bash -c ". ./src/erobs/build.sh" +WORKDIR /root/ws/erobs/ +RUN /bin/bash -c ". ./setup.sh" WORKDIR /root/ws/ +RUN /bin/bash -c ". ./erobs/build.sh" ENV ROBOT_IP=192.168.56.101 ENV REVERSE_IP=192.168.56.102 diff --git a/setup.sh b/setup.sh index f685cdf1..bb31f70c 100755 --- a/setup.sh +++ b/setup.sh @@ -1,7 +1,7 @@ #!/bin/bash set -e -vcs import < /root/ws/src/erobs/src/ros2.repos /root/ws/src/erobs/src +vcs import < src/ros2.repos src sudo apt-get update rosdep update -rosdep install --from-paths /root/ws/src --ignore-src -y +rosdep install --from-paths src --ignore-src -y From 87deab4ab6f7d65c82ff0721249648d2d3b5367a Mon Sep 17 00:00:00 2001 From: Chandima Fernando Date: Thu, 28 Mar 2024 01:54:13 -0400 Subject: [PATCH 30/32] refactor: remove unnecessary images --- docker/ur-hande-control/Dockerfile | 36 ------------------------------ 1 file changed, 36 deletions(-) delete mode 100644 docker/ur-hande-control/Dockerfile diff --git a/docker/ur-hande-control/Dockerfile b/docker/ur-hande-control/Dockerfile deleted file mode 100644 index e0676dce..00000000 --- a/docker/ur-hande-control/Dockerfile +++ /dev/null @@ -1,36 +0,0 @@ -FROM osrf/ros:humble-desktop-full - - -RUN apt-get update \ - && apt-get -y install \ - python3-pip \ - ros-${ROS_DISTRO}-rclpy \ - ros-${ROS_DISTRO}-ur \ - build-essential \ - python3-colcon-common-extensions \ - # - # Clean up - && apt-get autoremove -y \ - && apt-get clean -y \ - && rm -rf /var/lib/apt/lists/* -RUN pip3 install rosdep && \ - rosdep update - -RUN mkdir /root/ws/src -p - -WORKDIR /root/ws/src - -RUN git clone https://github.com/PickNikRobotics/serial-release.git -b release/humble/serial -RUN git clone https://github.com/PickNikRobotics/ros2_robotiq_gripper.git -b cmake-external-project-serial -RUN git clone https://github.com/ChandimaFernando/erobs.git -b humble_hande_control - -WORKDIR /root/ws/ - -RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash && \ - colcon build --symlink-install --packages-select pdf_beamtime_interfaces serial robotiq_driver" - -RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash && \ - source /root/ws/install/setup.bash && \ - colcon build --symlink-install --packages-select gripper_service" - -CMD ["/bin/sh", "-c", ". /opt/ros/${ROS_DISTRO}/setup.sh && . /root/ws/install/setup.sh && ros2 run gripper_service gripper_service"] From 944c003d0b5ac1f6969b8d52e8924f4c9509551e Mon Sep 17 00:00:00 2001 From: Chandima Fernando Date: Thu, 28 Mar 2024 10:06:37 -0400 Subject: [PATCH 31/32] refactor: remove ros examples --- src/ros2.repos | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/ros2.repos b/src/ros2.repos index d7249b1a..56d01aba 100644 --- a/src/ros2.repos +++ b/src/ros2.repos @@ -4,10 +4,6 @@ # Example: ############ repositories: - examples: - type: git - url: https://github.com/ros2/examples.git - version: humble gripper: type: git url: https://github.com/PickNikRobotics/ros2_robotiq_gripper.git From e786a511064534e440ddd9eb5bcacaa54973c9d4 Mon Sep 17 00:00:00 2001 From: "Dr. Phil Maffettone" <43007690+maffettone@users.noreply.github.com> Date: Thu, 28 Mar 2024 07:13:28 -0700 Subject: [PATCH 32/32] Apply suggestions from code review doc: Some lint and readme additions --- .github/actions/lint/run.sh | 1 - build.sh | 2 +- docker/erobs-common-img/Dockerfile | 3 +-- docker/erobs-common-img/README.md | 4 +++- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/.github/actions/lint/run.sh b/.github/actions/lint/run.sh index 1aa85f81..4b2b2408 100755 --- a/.github/actions/lint/run.sh +++ b/.github/actions/lint/run.sh @@ -1,5 +1,4 @@ #!/bin/bash set -e -# ./setup.sh ament_${LINTER} src/ diff --git a/build.sh b/build.sh index 921d1361..d04f4747 100755 --- a/build.sh +++ b/build.sh @@ -15,4 +15,4 @@ colcon build \ --merge-install \ --symlink-install \ --cmake-args "-DCMAKE_BUILD_TYPE=$BUILD_TYPE" "-DCMAKE_EXPORT_COMPILE_COMMANDS=On" \ - -Wall -Wextra -Wpedantic \ No newline at end of file + -Wall -Wextra -Wpedantic diff --git a/docker/erobs-common-img/Dockerfile b/docker/erobs-common-img/Dockerfile index 515e8f06..a12f3323 100644 --- a/docker/erobs-common-img/Dockerfile +++ b/docker/erobs-common-img/Dockerfile @@ -22,8 +22,7 @@ RUN mkdir /root/ws -p WORKDIR /root/ws -# TODO @ChandimaFernando. Revert back after tests. RUN git clone https://github.com/maffettone/erobs.git -b humble -RUN git clone https://github.com/ChandimaFernando/erobs.git -b humble_hande_control +RUN git clone https://github.com/maffettone/erobs.git -b humble WORKDIR /root/ws/erobs/ RUN /bin/bash -c ". ./setup.sh" diff --git a/docker/erobs-common-img/README.md b/docker/erobs-common-img/README.md index 91aca7c6..2fca5d5b 100644 --- a/docker/erobs-common-img/README.md +++ b/docker/erobs-common-img/README.md @@ -1,5 +1,7 @@ # This file contains commands to launch multiple containers of the same docker image. -(TODO ChandimaFernando : Change the repo link to nsls-II repo.) +First the image will need to be built. In this case a copy is currently stored at +[this content registry](ghcr.io/chandimafernando/erobs-common-img:latest). +(TODO ChandimaFernando : Change the repo link to nsls-II repo, and add CI/CD for build on version tag.) export ROBOT_IP=10.67.218.141 export UR_TYPE="ur3e"