Developing Teleoperation Node for 1-DOF On-Off Gripper

July 23, 2021 by Abdur Rosyid

In 2019, I developed a node in Python to tele-operate a 1-DOF on-off gripper by using Logitech gamepad. What I mean by 1-DOF on-off gripper is a gripper having just two binary states: attaching and detaching, or closed and open. This can be, for example, a magnetic gripper, a vacuum gripper, or a 1-DOF binary (closed and open) mechanical gripper.

To control the gripper, I used an Arduino microcontroller which receives the data from an onboard computer via USB cable (which is considered serial communication) and gives the control command to the gripper. Since the gripper has binary states, it is convenient to use ROS service to control it. The ROS service for this is created and run in the onboard computer.

Arduino Code

The Arduino code to control the gripper is the following:

int incomingByte = 0;
int ledPin = 9;    
int chk = 0;
int chk2 = 0;
void setup() {
  // nothing happens in setup
  Serial.begin(9600);
}

void loop() {

  if (Serial.available() > 0) 
  {
    // read the incoming byte:
    incomingByte = Serial.read();

    if (incomingByte == '1')
    {
      if (chk == 0)
      {
        Serial.print("attaching: ");
        Serial.println(incomingByte, DEC);
        analogWrite(ledPin, 250);
        delay(2000);
        analogWrite(ledPin, 0);
      chk = 1;
      chk2 = 0;
      }

    }
    if (incomingByte == '2') 
    {
      if (chk2 == 0) 
      {
        Serial.print("detaching: ");
        Serial.println(incomingByte, DEC);
        analogWrite(ledPin, 50);
        delay(1100);
        analogWrite(ledPin, 138);
        delay(1500);
        analogWrite(ledPin, 0);
      chk2 = 1;
      chk = 0;
      }
    }

int incomingByte = 0;

int ledPin = 9;

int chk = 0;

int chk2 = 0;

void setup() {

// nothing happens in setup

Serial.begin(9600);

}

void loop() {

if (Serial.available() > 0)

{

// read the incoming byte:

incomingByte = Serial.read();

if (incomingByte == '1')

{

if (chk == 0)

{

Serial.print("attaching: ");

Serial.println(incomingByte, DEC);

analogWrite(ledPin, 250);

delay(2000);

analogWrite(ledPin, 0);

chk = 1;

chk2 = 0;

}

if (incomingByte == '2')

{

if (chk2 == 0)

{

Serial.print("detaching: ");

Serial.println(incomingByte, DEC);

analogWrite(ledPin, 50);

delay(1100);

analogWrite(ledPin, 138);

delay(1500);

analogWrite(ledPin, 0);

chk2 = 1;

chk = 0;

}

ROS Service Codes

Here are the steps to create the service nodes.

Step 1: Create your package. Let’s call your package “manual_drive”. Create “srv”, “script”, and “launch” folders inside the package folder.

Step 2: Modify your package.xml:

<?xml version="1.0"?>

<package>

  <name>manual_drive</name>
  <version>1.0.0</version>
  <description>A package for tele-operation of on-off gripper</description>

  <license>TODO</license>

  <url type="website">https://abdurrosyid.com</url>
  <author email="abdoorasheed@gmail.com">Abdur Rosyid</author> 
  <maintainer email="abdoorasheed@gmail.com">Abdur Rosyid</maintainer>

  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>rospy</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>message_generation</build_depend>

  <run_depend>rospy</run_depend>
  <run_depend>std_msgs</run_depend>
  <run_depend>message_runtime</run_depend>

  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <!-- Other tools can request additional information be placed here -->
  </export>

</package>

<?xml version="1.0"?>

<name>manual_drive</name>

<description>A package for tele-operation of on-off gripper</description>

<url type="website">https://abdurrosyid.com</url>

<author email="abdoorasheed@gmail.com">Abdur Rosyid</author>

<maintainer email="abdoorasheed@gmail.com">Abdur Rosyid</maintainer>

<buildtool_depend>catkin</buildtool_depend>

<build_depend>rospy</build_depend>

<build_depend>std_msgs</build_depend>

<build_depend>message_generation</build_depend>

<run_depend>rospy</run_depend>

<run_depend>std_msgs</run_depend>

<run_depend>message_runtime</run_depend>

</export>

</package>

Step 3: Modify your CMakeLists.txt:

cmake_minimum_required(VERSION 2.8.3)
project(manual_drive)

find_package(catkin REQUIRED COMPONENTS
  rospy
  std_msgs
  sensor_msgs
  message_generation
)

## Generate services in the 'srv' folder
add_service_files(
   FILES
   Attach.srv
)

## Generate added messages and services with any dependencies listed here
generate_messages(
   DEPENDENCIES
   std_msgs  # Or other packages containing msgs
)

catkin_package(
LIBRARIES manual_drive
CATKIN_DEPENDS std_msgs rospy
)

catkin_install_python(PROGRAMS script/gripper_server.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} )
catkin_install_python(PROGRAMS script/gripper_client.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} )
catkin_install_python(PROGRAMS script/gripper_joystick_client.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} )

cmake_minimum_required(VERSION 2.8.3)

project(manual_drive)

find_package(catkin REQUIRED COMPONENTS

rospy

std_msgs

sensor_msgs

message_generation

)

## Generate services in the 'srv' folder

add_service_files(

FILES

Attach.srv

)

## Generate added messages and services with any dependencies listed here

generate_messages(

DEPENDENCIES

std_msgs # Or other packages containing msgs

)

catkin_package(

LIBRARIES manual_drive

CATKIN_DEPENDS std_msgs rospy

)

catkin_install_python(PROGRAMS script/gripper_server.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} )

catkin_install_python(PROGRAMS script/gripper_client.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} )

catkin_install_python(PROGRAMS script/gripper_joystick_client.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} )

Step 4: Create your service in the “srv” folder inside your package folder. Let’s call the service file “Attach.srv”:

bool OnOff
---
bool ok

bool OnOff

---

bool ok

Step 5: Write your service server node in the “script” folder inside your package folder. Let’s call it “gripper_server.py”:

from manual_drive.srv import Attach, AttachResponse
import rospy
import serial
import serial.tools.list_ports

class testV:
    rr=0

ports = list(serial.tools.list_ports.comports())
for p in ports:
    print p
ser = serial.Serial(p[0], 9600)    

def ActuateGripperCallback(req):
    
    if req.OnOff== True:
        commandtosend="1"
        gripperstatus="on"
        ser.write(str(commandtosend))
        testV.rr = 0
    else:
        commandtosend="2"
        gripperstatus="off"
        testV.rr = testV.rr + 1
        if testV.rr <= 2:
            ser.write(str(commandtosend))

    
	print "Sending to serial port [%s] to turn the gripper [%s]"%(commandtosend, gripperstatus)

    return AttachResponse(req.OnOff)

def gripper_server():
    rospy.init_node('GripperActuationServer')
    s = rospy.Service('GripperOnOFF', Attach, ActuateGripperCallback)
    print "Ready to Actuate Gripper"
    rospy.spin()

if __name__ == "__main__":
    gripper_server()

from manual_drive.srv import Attach, AttachResponse

import rospy

import serial

import serial.tools.list_ports

class testV:

rr=0

ports = list(serial.tools.list_ports.comports())

for p in ports:

print p

ser = serial.Serial(p[0], 9600)

def ActuateGripperCallback(req):

if req.OnOff== True:

commandtosend="1"

gripperstatus="on"

ser.write(str(commandtosend))

testV.rr = 0

else:

commandtosend="2"

gripperstatus="off"

testV.rr = testV.rr + 1

if testV.rr <= 2:

ser.write(str(commandtosend))

print "Sending to serial port [%s] to turn the gripper [%s]"%(commandtosend, gripperstatus)

return AttachResponse(req.OnOff)

def gripper_server():

rospy.init_node('GripperActuationServer')

s = rospy.Service('GripperOnOFF', Attach, ActuateGripperCallback)

print "Ready to Actuate Gripper"

rospy.spin()

if __name__ == "__main__":

gripper_server()

Step 6: Write your service client node in the “script” folder inside your package folder. Let’s call it “gripper_client.py”:

import sys 
import rospy
from manual_drive.srv import Attach, AttachRequest
 
# init a node as usual
rospy.init_node('GripperActuationClient')
 
# wait for this sevice to be running
rospy.wait_for_service('GripperOnOFF')
 
# Create the connection to the service. Remember it's an Attach service
gripper_on_off = rospy.ServiceProxy('GripperOnOFF', Attach)
 
# Create an object of the type AttachRequest. 
on_off = AttachRequest(True)
 
# Now send the request through the connection
result = gripper_on_off(on_off)
 
# Done
print result

import sys

import rospy

from manual_drive.srv import Attach, AttachRequest

# init a node as usual

rospy.init_node('GripperActuationClient')

# wait for this sevice to be running

rospy.wait_for_service('GripperOnOFF')

# Create the connection to the service. Remember it's an Attach service

gripper_on_off = rospy.ServiceProxy('GripperOnOFF', Attach)

# Create an object of the type AttachRequest.

on_off = AttachRequest(True)

# Now send the request through the connection

result = gripper_on_off(on_off)

# Done

print result

Step 7: Write your joystick tele-operation client node in the “script” folder inside your package folder. Let’s call it “gripper_joystick_client.py”:

import sys 
import rospy
from std_msgs.msg import Bool
from sensor_msgs.msg import Joy
from manual_drive.srv import Attach, AttachRequest
 
class gripper_joy_client():
    def __init__(self):
        global gripper_on_off
        # Name this node, it must be unique
	rospy.init_node('gripper_joy_client', anonymous=False)
	
	# Subscribe to /joy and Publish /joy_gripper/state
	rospy.Subscriber("/joy", Joy, self.callback)
	self.gripper_on_off = rospy.Publisher('/joy_gripper/state', Bool, queue_size=5)

        # wait for this sevice to be running
        rospy.wait_for_service('GripperOnOFF')
 
        # Create the connection to the service. Remember it's an Attach service
        gripper_on_off = rospy.ServiceProxy('GripperOnOFF', Attach)

    def callback(self, joy):
        global on_off
        global result
        global gripper_on_off
	gripper_state = Bool()
	gripper_state.data = joy.buttons[3]	# button Y
	if gripper_state.data == 1:
	    on_off = AttachRequest(True)
            result = gripper_on_off(on_off)
	    print result
	else:
	    on_off = AttachRequest(False)
            result = gripper_on_off(on_off)
	    print result
	    
if __name__ == '__main__':
    try:
	gripper_joy_client()
	rospy.spin()
    except KeyboardInterrupt:
	print("Shuting down gripper_joy_client node")

import sys

import rospy

from std_msgs.msg import Bool

from sensor_msgs.msg import Joy

from manual_drive.srv import Attach, AttachRequest

class gripper_joy_client():

def __init__(self):

global gripper_on_off

# Name this node, it must be unique

rospy.init_node('gripper_joy_client', anonymous=False)

# Subscribe to /joy and Publish /joy_gripper/state

rospy.Subscriber("/joy", Joy, self.callback)

self.gripper_on_off = rospy.Publisher('/joy_gripper/state', Bool, queue_size=5)

# wait for this sevice to be running

rospy.wait_for_service('GripperOnOFF')

# Create the connection to the service. Remember it's an Attach service

gripper_on_off = rospy.ServiceProxy('GripperOnOFF', Attach)

def callback(self, joy):

global on_off

global result

global gripper_on_off

gripper_state = Bool()

gripper_state.data = joy.buttons[3] # button Y

if gripper_state.data == 1:

on_off = AttachRequest(True)

result = gripper_on_off(on_off)

print result

else:

on_off = AttachRequest(False)

result = gripper_on_off(on_off)

print result

if __name__ == '__main__':

try:

gripper_joy_client()

rospy.spin()

except KeyboardInterrupt:

print("Shuting down gripper_joy_client node")

Step 8: Create a launch file in the launch file inside your package folder. Let’s call it “joy_teleop.launch”:

<launch>
  <arg name="joy_dev" default="/dev/input/js0" />
  <arg name="joystick" default="true" />

  <!-- <node pkg="manual_drive" name="gripper_server" type="gripper_server_test.py" output="screen"> -->
  <node pkg="manual_drive" name="gripper_server" type="gripper_server.py" output="screen">
  </node>

  <node pkg="manual_drive" name="gripper_joy_client" type="gripper_joystick_client.py" output="screen">
  </node>

  <node pkg="joy" type="joy_node" name="joy_node">
      <param name="dev" value="$(arg joy_dev)" />
  </node>
  
</launch>

</node>

</node>

</node>

</launch>

Step 9: Build your package by using the catkin_make command.

Step 10: To run the tele-operation, simply launch the “joy_teleop.launch” file:

roslaunch manual_drive joy_teleop.launch

1	roslaunch manual_drive joy_teleop.launch

Leave a Reply Cancel reply