Indigo

README.md

@@ -8,6 +8,7 @@ The youbot_manipulation meta-package contains the following packages:
 * youbot_arm_kinematics: A framework-independent library which implements an analytical inverse position kinematics solver for the youBot manipulator.
 * youbot_arm_kinematics_moveit: A MoveIt! plugin which exposes the youbot_arm_kinematics library (see above) to MoveIt!.
 * youbot_moveit: Configuration files to use the youBot with MoveIt! and the analytical inverse kinematics solver from this repository. These files have been auto-generated by the MoveIt! setup assistant.
+* youbot_gripper: A controller manager to use the gripper with MoveIt!
 
 
 ### Installation
@@ -36,6 +37,10 @@ Using the markers attached to the manipulator, the end-effector can be dragged a
 First, bring up the real or simulated robot. Then, start MoveIt! via:
 
     roslaunch youbot_moveit move_group.launch
+
+Launch the controller for the gripper
+
+    rosrun youbot_gripper youbot_gripper_server.py
 
 With the MoveIt! Commander (http://moveit.ros.org/wiki/MoveIt_Commander) the manipulator can be moved by typing in commands. To start the MoveIt! Commander run:
 
@@ -45,8 +50,11 @@ Then select the hardware group to move and command it to some known position (e.
 
     > use arm_1
     arm_1> go candle
+    > use arm_1_gripper
+    arm_1_gripper> go open
 
+
 
 ### Documentation
 
-The MoveIt! kinematics plugin in the youbot_arm_kinematics_moveit package implements the standard MoveIt! interfaces. Therefore, consult the MoveIt! website (http://moveit.ros.org/) for further documentation related to using MoveIt!.
+The MoveIt! kinematics plugin in the youbot_arm_kinematics_moveit package implements the standard MoveIt! interfaces. Therefore, consult the MoveIt! website (http://moveit.ros.org/) for further documentation related to using MoveIt!.

youbot_gripper/CMakeLists.txt

@@ -0,0 +1,14 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(youbot_gripper)
+
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  std_msgs
+)
+
+
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)

youbot_gripper/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<package>
+  <name>youbot_gripper</name>
+  <version>0.0.0</version>
+  <description>Custom controller manager to use the youbot gripper</description>
+
+  <maintainer email="ferherranz@gmail.com">Fernando Herranz</maintainer> 
+  <author email="evan@srtlabs.com">Evan Sonderegger</author>
+
+  <license>BSD</license>
+
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>rospy</run_depend>
+  <run_depend>std_msgs</run_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

youbot_gripper/scripts/youbot_gripper_server.py

@@ -0,0 +1,94 @@
+#!/usr/bin/env python
+
+# Code based on Evan Sonderegger's code on https://github.com/esonderegger/youbot_srt
+
+
+import roslib
+import rospy
+from brics_actuator.msg import JointPositions
+from brics_actuator.msg import JointValue
+from brics_actuator.msg import Poison
+# from control_msgs.msg import FollowJointTrajectoryAction
+from control_msgs.msg import GripperCommandAction
+# from sensor_msgs.msg import JointState
+from actionlib import SimpleActionServer
+# import yaml
+# import datetime
+
+def nonZeroVelocities(velocities):
+    # sometimes moveit gives us velocities of 0.0, when we want something
+    # very slow, but the lib_robotis takes 0 as "unlimited"
+    outVels = []
+    for velocity in velocities:
+        if velocity == 0.0:
+            outVels.append(0.1)
+        else:
+            outVels.append(velocity)
+    return outVels
+
+
+class ybGripperServer:
+    def __init__(self, name):
+        self.fullname = name
+        self.currentValue = 0.0
+        gripperTopic = '/arm_1/gripper_controller/position_command'
+        self.jppub = rospy.Publisher(gripperTopic, JointPositions)
+        self.server = SimpleActionServer(self.fullname,
+                                         GripperCommandAction,
+                                         execute_cb=self.execute_cb,
+                                         auto_start=False)
+        self.server.start()
+
+    def execute_cb(self, goal):
+        rospy.loginfo(goal)
+        self.currentValue = goal.command.position
+        self.moveGripper(self.jppub, self.currentValue)
+        attempts = 0
+        # here we should be checking if the gripper has gotten to its goal
+        for i in range(5):
+            rospy.sleep(0.1)
+            attempts += 1
+        if attempts < 20:
+            self.server.set_succeeded()
+        else:
+            self.server.set_aborted()
+
+    def moveGripper(self, gPublisher, floatVal):
+        jp = JointPositions()
+        myPoison = Poison()
+        myPoison.originator = 'arm1_gripper'
+        myPoison.description = 'gripper command'
+        myPoison.qos = 0.0
+        jp.poisonStamp = myPoison
+        nowTime = rospy.Time.now()
+        jvl = JointValue()
+        jvl.timeStamp = nowTime
+        jvl.joint_uri = 'gripper_finger_joint_l'
+        jvl.unit = 'm'
+        jvl.value = floatVal
+        jp.positions.append(jvl)
+        jvr = JointValue()
+        jvr.timeStamp = nowTime
+        jvr.joint_uri = 'gripper_finger_joint_r'
+        jvr.unit = 'm'
+        jvr.value = floatVal
+        jp.positions.append(jvr)
+        gPublisher.publish(jp)
+
+    def ybspin(self):
+        rospy.sleep(0.1)
+
+
+def ybserver():
+    rospy.init_node('youbot_gripper_server')
+    gripperCommandTopic = 'youbot_gripper/gripper_command'
+    gripperServer = ybGripperServer(gripperCommandTopic)
+    while not rospy.is_shutdown():
+        gripperServer.ybspin()
+
+
+if __name__ == '__main__':
+    try:
+        ybserver()
+    except rospy.ROSInterruptException:
+        pass

youbot_moveit/config/controllers.yaml

@@ -8,4 +8,11 @@ controller_list:
       - arm_joint_2
       - arm_joint_3
       - arm_joint_4
-      - arm_joint_5
+      - arm_joint_5
+
+  - name: youbot_gripper
+    action_ns: gripper_command
+    type: GripperCommand
+    joints:
+      - gripper_finger_joint_l
+      - gripper_finger_joint_r