feat: Robotic hande control drivers and services (#28)

* feat: add docker for serial lib

* feat: gripper control

* feat: gripper control

* fix: gripper bugs

* fix: test path for hello moveit

* fix: cmake compile issue

* fix: point to correct repo in docker

* refactor: add copyright

* refactor: correct settings.json

* fix: move serial to ra separate repo

* fix: remove submodule

* fix: simplify gripper service

* fix: add missing links to yml

* FIX: PATH CORRECTION

* fix: correct runtime cmd for dockerfile

* refactor: remove extra files

* fix: change node structure

* feat: build one docker img for all containers

* fix: gripper scope error

* fix: add ignore paths for lint

* fix: remove setup.sh

* fix: lint errors

* fix: point to build.sh

* fix: cmake args temp fix

* fix: cleaned up Dockerfile

* fix: correct build location

* fix: more path corrections

* fix: correct and clean dockerfile

* fix: correct tests

* refactor: remove unnecessary images

* refactor: remove ros examples

* Apply suggestions from code review

doc: Some lint and readme additions

---------

Co-authored-by: Dr. Phil Maffettone <43007690+maffettone@users.noreply.github.com>

.github/actions/lint/run.sh

@@ -1,5 +1,4 @@
 #!/bin/bash
 set -e
 
-./setup.sh
 ament_${LINTER} src/

build.sh

@@ -3,6 +3,14 @@ set -e
 
 # Set the default build type
 BUILD_TYPE=RelWithDebInfo
+
+source /opt/ros/${ROS_DISTRO}/setup.bash && colcon build \
+        --merge-install \
+        --symlink-install \
+        --packages-select ur3e_hande_robot_description serial robotiq_driver pdf_beamtime_interfaces \
+        --cmake-args "-DCMAKE_BUILD_TYPE=$BUILD_TYPE" "-DCMAKE_EXPORT_COMPILE_COMMANDS=On" \
+        --cmake-args "-DCMAKE_CXX_FLAGS=-Wall -Wextra -Wpedantic"
+
 colcon build \
         --merge-install \
         --symlink-install \

docker/erobs-common-img/Dockerfile

@@ -0,0 +1,43 @@
+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 -p
+
+WORKDIR /root/ws
+
+RUN git clone https://github.com/maffettone/erobs.git -b humble
+
+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
+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

docker/erobs-common-img/README.md

@@ -0,0 +1,39 @@
+# This file contains commands to launch multiple containers of the same docker image.
+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"
+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
+
+# 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 \
+    --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"
+
+# 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"
+
+# Launch 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}"

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(<dependency> 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()

src/gripper_service/include/gripper_service/gripper_service.hpp

@@ -0,0 +1,45 @@
+/*Copyright 2023 Brookhaven National Laboratory
+BSD 3 Clause License. See LICENSE.txt for details.*/
+#pragma once
+
+#include <iostream>
+#include <thread>
+#include <memory>
+#include <string>
+#include <map>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include "pdf_beamtime_interfaces/srv/gripper_control_msg.hpp"
+
+#include <robotiq_driver/robotiq_gripper_interface.hpp>
+
+class GripperService : public rclcpp::Node
+{
+private:
+  /// @brief your serial port goes here
+  const char * kComPort = "/tmp/ttyUR";
+  const int kSlaveID = 0x09;
+
+  // rclcpp::Node::SharedPtr node_;
+  RobotiqGripperInterface gripper_;
+
+/// @brief Gripper commands as enums for ease of use
+  enum class Gripper_Command {OPEN, CLOSE, PARTIAL, ACTIVE, DEACTIVE};
+
+  std::map<std::string, Gripper_Command> 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<pdf_beamtime_interfaces::srv::GripperControlMsg::Request> request,
+    std::shared_ptr<pdf_beamtime_interfaces::srv::GripperControlMsg::Response> response);
+
+public:
+  GripperService();
+};

src/gripper_service/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>gripper_service</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="wfernando1@bnl.gov">wfernando1</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <buildtool_depend>pdf_beamtime_interfaces</buildtool_depend>
+  <buildtool_depend>robotiq_driver</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/gripper_service/src/gripper_service.cpp

@@ -0,0 +1,98 @@
+/*Copyright 2023 Brookhaven National Laboratory
+BSD 3 Clause License. See LICENSE.txt for details.*/
+#include <gripper_service/gripper_service.hpp>
+
+using namespace std::placeholders;
+
+GripperService::GripperService()
+: Node("gripper_server_node"),
+  gripper_(kComPort, kSlaveID)
+{
+  RCLCPP_INFO(this->get_logger(), "Activate the gripper ...");
+  // Clear the registers
+  gripper_.deactivateGripper();
+
+  // Activate the gripper
+  gripper_.activateGripper();
+  gripper_.setSpeed(0x0F);
+
+  RCLCPP_INFO(this->get_logger(), "Activation is successful");
+
+  rclcpp::Service<pdf_beamtime_interfaces::srv::GripperControlMsg>::SharedPtr service =
+    this->create_service<pdf_beamtime_interfaces::srv::GripperControlMsg>(
+    "gripper_service",
+    std::bind(
+      &GripperService::gripper_controller, this, _1, _2));
+  RCLCPP_INFO(this->get_logger(), "Ready to receive gripper commands.");
+}
+
+void GripperService::gripper_controller(
+  const std::shared_ptr<pdf_beamtime_interfaces::srv::GripperControlMsg::Request> request,
+  std::shared_ptr<pdf_beamtime_interfaces::srv::GripperControlMsg::Response> response)
+{
+  int status = 0;
+  try {
+    //  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();
+        gripper_.activateGripper();
+        RCLCPP_INFO(this->get_logger(), "Activation is successful");
+
+        break;
+
+      case Gripper_Command::DEACTIVE:
+        // Deactivate the gripper
+        gripper_.deactivateGripper();
+        RCLCPP_INFO(this->get_logger(), "Gripper is Deactivated");
+
+        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");
+        }
+        break;
+
+      case Gripper_Command::OPEN:
+        //  Open the gripper fully
+        gripper_.setGripperPosition(0x00);
+        RCLCPP_INFO(this->get_logger(), "Gripper is Open");
+        break;
+
+      case Gripper_Command::CLOSE:
+        //  Close the gripper fully
+        gripper_.setGripperPosition(0xFF);
+        RCLCPP_INFO(this->get_logger(), "Gripper is Close");
+        break;
+
+      default:
+        break;
+    }
+    status = 1;
+  } catch (const std::exception & e) {
+    RCLCPP_ERROR(this->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_node = std::make_shared<GripperService>();
+
+  rclcpp::spin(gripper_server_node);
+  rclcpp::shutdown();
+
+  return 0;
+}

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 = [] {

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/GripperControlMsg.srv"
 )
 
 

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

src/ros2.repos

@@ -4,7 +4,11 @@
 # Example:
 ############
 repositories:
-  examples:
+  gripper:
     type: git
-    url: https://github.com/ros2/examples.git
-    version: humble
+    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