Merge pull request b-it-bots#294 from vamsikalagaturu/noetic

[mir_gripper_controller] openrb controller support for ros

mir_manipulation/mir_gripper_controller/README.md

@@ -1,25 +1,24 @@
-# gripper_control
-Repository containing code for Teensy microcontroller and ROS interface for transferring gripper commands and feedback data. The software is designed in a way to prevent occurence of the overcurrent in the Dynamixels and enable slippage detection.
+# Gripper controller
+Package containing code to control two finger gripper with Dynamixel 12A motors and OpenRB-150 controller.
+
+More details on the controller and corresponding code can be found [here](https://github.com/b-it-bots/youbot_dynamixel_gripper_controller).
+
+> Note: Please make sure to follow all the steps and setup the Dynamixel correctly as described in the above link.
 
 ## Requirements
 * python3
 * ROS Noetic
 * pySerial
 
 ## Getting started
-1. Download [Arduino IDE](https://www.arduino.cc/en/software) (do not use Ubuntu software manager!).
-2. Download Teensy plugin for Arduino IDE [here](https://www.pjrc.com/teensy/teensyduino.html).
-3. Download libraries for Arduino IDE: Dynamixel2Arduino, CircularBuffer, Arduino_JSON, this is done in the Arduino IDE (Tools->Manage Libraries).
-4. Open file `control_with_comm.ino` in the `teensy/src` directory, compile it and upload on the microcontroller.
+1. Make sure to uplaod the latest [code](https://github.com/b-it-bots/youbot_dynamixel_gripper_controller/tree/master/openrb_150_controller) on to the controller.
 5. Run `roscore` command.
-5. Run `gripper_controller.py` in the `ros/src` directory.
-6. To control the gripper publish to the `/arm_1/gripper_command` topic, e.g. `rostopic pub /arm_1/gripper_command std_msgs/String "data: '0'"`
+5. Run `dynamixel_gripper_controller_ros` in the `ros/scripts` directory.
+6. To control the gripper publish to the `/arm_1/gripper_command` topic, e.g. `rostopic pub /arm_1/gripper_command std_msgs/String "command: 0.0"`
 7. To see the state of the gripper subscribe to the `/arm_1/gripper_feedback`, e.g. `rostopic echo /arm_1/gripper_feedback`.
 
-## Setting up the Dynamixel
-- Please follow the guide [here](https://emanual.robotis.com/docs/en/parts/interface/usb2dynamixel/).
-- For setting the baudrate install [here](https://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_wizard2/).
-
 ## Usage
-* One can publish 0 (open) or 1 (close) command to the gripper.
-* One can subscribe the state of the gripper, the format consists of the following fields: `state` (`GRIPPER_CLOSE`, `OBJECT_GRASPED`, `OBJECT_SLIPPED`, `GRIPPER_OPEN`), `parsing_error` (0 - false, 1 - true), `last_command` (0, 1).
+* One can publish `0.0 (open)` or `1.0 (close)` command to the gripper.
+* For more specific position control, one can publish a value between `0.0 and 1.0`.
+* To further open the gripper, one can publish a value between `-1.5 and 0.0`.
+* One can subscribe the state of the gripper, the format consists of the following fields: `state` (`GRIPPER_CLOSE`, `OBJECT_GRASPED`, `OBJECT_SLIPPED`, `GRIPPER_OPEN`, `GRIPPER_INTER`), `parsing_error` (0 - false, 1 - true), `last_command` (0, 1).

mir_manipulation/mir_gripper_controller/ros/launch/teensy_dynamixel_gripper.launch → mir_manipulation/mir_gripper_controller/ros/launch/dynamixel_gripper.launch

@@ -4,7 +4,7 @@
 
    <arg name="robot" default="$(optenv ROBOT !!NO_ROBOT_SET!!)" />
 
-   <node pkg="mir_gripper_controller" type="teensy_gripper_controller_ros" name="gripper_controller" output="screen" respawn="true" >
+   <node pkg="mir_gripper_controller" type="dynamixel_gripper_controller_ros" name="gripper_controller" output="screen" respawn="true" >
       <remap from="joint_state" to="/joint_states" />
    </node>
 

mir_manipulation/mir_gripper_controller/ros/scripts/teensy_gripper_controller_ros → mir_manipulation/mir_gripper_controller/ros/scripts/dynamixel_gripper_controller_ros

@@ -10,7 +10,7 @@ import json
 
 
 class GripperController:
-    """ROS wrapper for receiving commands for the gripper and forwarding them to the Teensy board.
+    """ROS wrapper for receiving commands for the gripper and forwarding them to the OpenRB-150 board.
     """
 
     component_name = 'gripper_controller'
@@ -32,9 +32,9 @@ class GripperController:
 
             # Perform gripper close open at initialization
             rospy.loginfo("Closing and opening the gripper at initialization.")
-            self.serial_msg.send({'command': 1})
+            self.serial_msg.send({'command': 1.0})
             rospy.sleep(1)
-            self.serial_msg.send({'command': 0})
+            self.serial_msg.send({'command': 0.0})
 
         except rospy.ROSInterruptException as ex:
             rospy.logerr(ex)
@@ -46,17 +46,29 @@ class GripperController:
             Keyword arguments:
             @param data -- command from the gripper
         """
-        command = int(data.command)
+        command = float(data.command)
         json_command = {
-                "command": 0,
+                "command": 0.0,
         }
 
-        if command == 1:
+        if command > 1.0:
+            command = 1.0
+        elif command < -1.5:
+            command = 0.0
+
+        if command == 1.0:
             rospy.logdebug('Closing the gripper.')
-            json_command['command'] = 1
+            json_command['command'] = 1.0
 
-        else:
+        elif command == 0.0:
             rospy.logdebug('Opening the gripper.')
+            json_command['command'] = 0.0
+        elif command > 0.0 and command < 1.0:
+            rospy.logdebug('Moving the gripper to a specific position.')
+            json_command['command'] = command
+        elif command < 0.0 and command > -1.5:
+            rospy.logdebug('Opening the gripper above and beyond.')
+            json_command['command'] = command
 
         self.serial_msg.send(json_command)
 
@@ -65,11 +77,11 @@ class GripperController:
         
         Assuming feedback is in the below json format:
         {
-            "state": "open" or "closed",
+            "state": [GRIPPER_OPENING, GRIPPER_INTER, GRIPPER_CLOSING, OBJECT_GRASPED, OBJECT_SLIPPED, GRIPPER_CLOSED, GRIPPER_OPEN],
             "right_gripper_pos": 0 to 360 degrees,
             "left_gripper_pos": 0 to 360 degrees,
             "parsing_error": 0,
-            "last_command": 1 or 0,
+            "last_command": [-1.5 to 1.0],
             # other fields can be added here
         }
         """

mir_manipulation/mir_manipulation_msgs/CMakeLists.txt

@@ -9,6 +9,11 @@ find_package(catkin REQUIRED
     std_msgs
 )
 
+add_message_files(
+  FILES
+    GripperCommand.msg
+)
+
 add_service_files(
   FILES
     IsGripperClosed.srv

mir_manipulation/mir_manipulation_msgs/msg/GripperCommand.msg

@@ -0,0 +1,4 @@
+float32 command
+
+float32 OPEN = 0.0
+float32 CLOSE = 1.0

mir_robots/mir_bringup/robots/youbot-brsu-2.launch

@@ -30,7 +30,7 @@
   <include file="$(find youbot_dashboard)/launch/dashboard_aggregator.launch" />
   <include file="$(find youbot_diagnostic_aggregator)/launch/diagnostic_aggregator.launch" />
 
-  <include file="$(find mir_gripper_controller)/ros/launch/teensy_dynamixel_gripper.launch" />
+  <include file="$(find mir_gripper_controller)/ros/launch/dynamixel_gripper.launch" />
 
   <!-- top camera -->
   <include file="$(find mir_bringup)/components/realsense_camera.launch">

mir_scenarios/mir_states/ros/src/mir_states/common/manipulation_states.py

@@ -16,7 +16,7 @@
 from mir_pregrasp_planning_ros.orientation_independent_ik import OrientationIndependentIK
 import tf
 from tf.transformations import euler_from_quaternion
-from mcr_manipulation_msgs.msg import GripperCommand
+from mir_manipulation_msgs.msg import GripperCommand
 from std_msgs.msg import String
 
 class Bunch:
@@ -215,8 +215,10 @@ def __init__(self, target=None, blocking=True, tolerance=None, timeout=10.0):
         self.grasped_counter = 0
         if 'open' in target:
             self.command.command = GripperCommand.OPEN
-        else:
+        elif 'close' in target:
             self.command.command = GripperCommand.CLOSE
+        elif type(target) == float:
+            self.command.command = target
         self.pub = rospy.Publisher('/arm_1/gripper_command', GripperCommand, queue_size=1)
         self.sub = rospy.Subscriber('/arm_1/gripper_feedback', String, self.feedback_cb)
 
@@ -229,8 +231,8 @@ def feedback_cb(self, msg):
     def execute(self, userdata):
         self.pub.publish(self.command)
         while True:
-            if self.current_state == "GRIPPER_OPEN" and\
-                    self.command.command == GripperCommand.OPEN:
+            if (self.current_state == "GRIPPER_OPEN" or self.current_state == "GRIPPER_INTER") and\
+                    self.command.command != GripperCommand.CLOSE:
                 self.grasped_counter = 0
                 return "succeeded"
             elif self.current_state == "GRIPPER_CLOSED" and\
@@ -264,6 +266,7 @@ def execute(self, userdata):
         while (rospy.Time.now() - start_time < self.timeout):
             rospy.sleep(0.1)
             if self.current_state == "GRIPPER_CLOSED" or\
+               self.current_state == "GRIPPER_INTER" or\
                self.current_state == "GRIPPER_OPEN":
                 return "failed"
             elif self.current_state == "OBJECT_GRASPED" and\
@@ -301,8 +304,10 @@ def __init__(self, conf, target=None, blocking=True, tolerance=None, timeout=10.
         self.gripper_command = GripperCommand()
         if 'open' in self.conf:
             self.gripper_command.command = GripperCommand.OPEN
-        else:
+        elif 'close' in self.conf:
             self.gripper_command.command = GripperCommand.CLOSE
+        elif type(self.conf) == float:
+            self.gripper_command.command = self.conf
 
     def get_targets(self, group_name):
         text = rospy.get_param("/robot_description_semantic")