workspace without submodules

src/arbotix_ros/.gitignore

+*.pyc
+.project
+.cproject
+.pydevproject
+.vscode

src/arbotix_ros/LICENSE

+Copyright (c) 2008-2011 Vanadium Labs LLC. 
+All right reserved.
+
+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 Vanadium Labs LLC 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 VANADIUM LABS 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.

src/arbotix_ros/README.md

+## Arbotix Drivers
+
+This repository contains the Arbotix ROS drivers, catkinized, and ready for ROS Noetic.

src/arbotix_ros/arbotix/CHANGELOG.rst

+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package arbotix
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.11.0 (2020-12-29)
+-------------------
+
+0.10.0 (2014-07-14)
+-------------------
+
+0.9.2 (2014-02-12)
+------------------
+
+0.9.1 (2014-01-28)
+------------------
+
+0.9.0 (2013-08-22)
+------------------
+
+0.8.2 (2013-03-28)
+------------------
+* update metapackage
+
+0.8.1 (2013-03-09)
+------------------
+
+0.8.0 (2013-02-21)
+------------------
+* fix package.xml
+* add metapackage for arbotix

src/arbotix_ros/arbotix/CMakeLists.txt

+cmake_minimum_required(VERSION 3.0.2)
+project(arbotix)
+find_package(catkin REQUIRED)
+catkin_metapackage()

src/arbotix_ros/arbotix/package.xml

+<package>
+  <name>arbotix</name>
+  <version>0.11.0</version>
+  <description>ArbotiX Drivers</description>
+  <author>Michael Ferguson</author>
+  <maintainer email="mike@vanadiumlabs.com">Michael Ferguson</maintainer>
+  <license>BSD</license>
+  <url>http://ros.org/wiki/arbotix</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <run_depend>arbotix_msgs</run_depend>
+  <run_depend>arbotix_python</run_depend>
+  <run_depend>arbotix_sensors</run_depend>
+  <run_depend>arbotix_controllers</run_depend>
+  <run_depend>arbotix_firmware</run_depend>
+
+  <export>
+    <metapackage/>
+  </export>
+</package>
+
+

src/arbotix_ros/arbotix_controllers/CHANGELOG.rst

+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package arbotix_controllers
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.11.0 (2020-12-29)
+-------------------
+* Update all python shebangs to python3 + rosdep dependency (`#48 <https://github.com/vanadiumlabs/arbotix_ros/issues/48>`_)
+  Co-authored-by: Murat Calis <mc@pirate-robotics.net>
+* Merge pull request `#22 <https://github.com/vanadiumlabs/arbotix_ros/issues/22>`_ from corot/indigo-devel
+  roslib.load_manifest should not be used on catkin packages
+* roslib.load_manifest should not be used on catkin packages according to http://wiki.ros.org/PyStyleGuide
+* Contributors: Jorge Santos, Michael Ferguson, calismurat
+
+0.10.0 (2014-07-14)
+-------------------
+* Set queue_size=5 on all publishers
+* Check if command exceeds opening limits
+* Contributors: Jorge Santos
+
+0.9.2 (2014-02-12)
+------------------
+* cleanup gripper controllers, mark deprecations
+* Contributors: Michael Ferguson
+
+0.9.1 (2014-01-28)
+------------------
+
+0.9.0 (2013-08-22)
+------------------
+* fix joint_states subscriber
+* fix name of singlesided model
+* add new gripper action controller
+
+0.8.2 (2013-03-28)
+------------------
+
+0.8.1 (2013-03-09)
+------------------
+
+0.8.0 (2013-02-21)
+------------------
+* import drivers and catkinize

src/arbotix_ros/arbotix_controllers/CMakeLists.txt

+cmake_minimum_required(VERSION 3.0.2)
+project(arbotix_controllers)
+
+find_package(catkin REQUIRED)
+catkin_package()
+
+install(
+  PROGRAMS
+    bin/gripper_controller
+    bin/one_side_gripper_controller.py
+    bin/parallel_gripper_controller.py
+    bin/parallel_single_servo_controller.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)

src/arbotix_ros/arbotix_controllers/bin/gripper_controller

+#!/usr/bin/env python3
+
+"""
+  gripper_controller - action based controller for grippers.
+  Copyright (c) 2011-2014 Vanadium Labs LLC.  All right reserved.
+
+  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 Vanadium Labs LLC 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 VANADIUM LABS 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.
+"""
+
+import rospy, actionlib
+import threading
+
+from control_msgs.msg import GripperCommandAction
+from sensor_msgs.msg import JointState
+from std_msgs.msg import Float64
+from math import asin
+
+class TrapezoidGripperModel:
+    """ A simple gripper with two opposing servos to open/close non-parallel jaws. """
+
+    def __init__(self):
+        # trapezoid model: base width connecting each gripper's rotation point
+            #              + length of gripper fingers to computation point
+            #              = compute angles based on a desired width at comp. point
+        self.pad_width = rospy.get_param('~pad_width', 0.01)
+        self.finger_length = rospy.get_param('~finger_length', 0.02)
+        self.min_opening = rospy.get_param('~min_opening', 0.0)
+        self.max_opening = rospy.get_param('~max_opening', 0.09)
+        self.center_l = rospy.get_param('~center_left', 0.0)
+        self.center_r = rospy.get_param('~center_right', 0.0)
+        self.invert_l = rospy.get_param('~invert_left', False)
+        self.invert_r = rospy.get_param('~invert_right', False)
+
+        self.left_joint = rospy.get_param('~joint_left', 'l_gripper_joint')
+        self.right_joint = rospy.get_param('~joint_right', 'r_gripper_joint')
+
+        # publishers
+        self.l_pub = rospy.Publisher(self.left_joint+'/command', Float64, queue_size=5)
+        self.r_pub = rospy.Publisher(self.right_joint+'/command', Float64, queue_size=5)
+
+    def setCommand(self, command):
+        # check limits
+        if command.position > self.max_opening or command.position < self.min_opening:
+            rospy.logerr("Command (%f) exceeds opening limits (%f, %f)",
+                          command.position, self.max_opening, self.min_opening)
+            return False
+        # compute angles
+        angle = asin((command.position - self.pad_width)/(2*self.finger_length))
+        if self.invert_l:
+            l = -angle + self.center_l
+        else:
+            l = angle + self.center_l
+        if self.invert_r:
+            r = angle + self.center_r
+        else:
+            r = -angle + self.center_r
+        # publish msgs
+        lmsg = Float64(l)
+        rmsg = Float64(r)
+        self.l_pub.publish(lmsg)
+        self.r_pub.publish(rmsg)
+        return True
+
+    def getPosition(self, js):
+        left = right = 0
+        for i in range(len(js.name)):
+            if js.name[i] == self.left_joint:
+                left = js.position[i]
+            elif js.name[i] == self.right_joint:
+                right = js.position[i]
+        # TODO
+
+        return 0.0
+
+    def getEffort(self, joint_states):
+        return 1.0
+
+class ParallelGripperModel:
+    """ One servo to open/close parallel jaws, typically via linkage. """
+
+    def __init__(self):
+        self.center = rospy.get_param('~center', 0.0)
+        self.scale = rospy.get_param('~scale', 1.0)
+        self.joint = rospy.get_param('~joint', 'gripper_joint')
+
+        # publishers
+        self.pub = rospy.Publisher(self.joint+'/command', Float64, queue_size=5)
+
+    def setCommand(self, command):
+        self.pub.publish((command.position * self.scale) + self.center)
+
+    def getPosition(self, joint_states):
+        return 0.0
+
+    def getEffort(self, joint_states):
+        return 1.0
+
+
+class OneSideGripperModel:
+    """ Simplest of grippers, one servo opens or closes to achieve a particular size opening. """
+
+    def __init__(self):
+        self.pad_width = rospy.get_param('~pad_width', 0.01)
+        self.finger_length = rospy.get_param('~finger_length', 0.02)
+        self.min_opening = rospy.get_param('~min_opening', 0.0)
+        self.max_opening = rospy.get_param('~max_opening', 0.09)
+        self.center = rospy.get_param('~center', 0.0)
+        self.invert = rospy.get_param('~invert', False)
+        self.joint = rospy.get_param('~joint', 'gripper_joint')
+
+        # publishers
+        self.pub = rospy.Publisher(self.joint+'/command', Float64, queue_size=5)
+
+    def setCommand(self, command):
+        """ Take an input command of width to open gripper. """
+        # check limits
+        if command.position > self.max_opening or command.position < self.min_opening:
+            rospy.logerr("Command (%f) exceeds opening limits (%f, %f)",
+                          command.position, self.max_opening, self.min_opening)
+            return False
+        # compute angle
+        angle = asin((command.position - self.pad_width)/(2*self.finger_length))
+        # publish message
+        if self.invert:
+            self.pub.publish(-angle + self.center)
+        else:
+            self.pub.publish(angle + self.center)
+
+    def getPosition(self, joint_states):
+        # TODO
+        return 0.0
+
+    def getEffort(self, joint_states):
+        # TODO
+        return 1.0
+
+
+class GripperActionController:
+    """ The actual action callbacks. """
+    def __init__(self):
+        rospy.init_node('gripper_controller')
+
+        # setup model
+        try:
+            model = rospy.get_param('~model')
+        except:
+            rospy.logerr('no model specified, exiting')
+            exit()
+        if model == 'dualservo':
+            self.model = TrapezoidGripperModel()
+        elif model == 'parallel':
+            self.model = ParallelGripperModel()
+        elif model == 'singlesided':
+            self.model = OneSideGripperModel()
+        else:
+            rospy.logerr('unknown model specified, exiting')
+            exit()
+
+        # subscribe to joint_states
+        rospy.Subscriber('joint_states', JointState, self.stateCb)
+
+        # subscribe to command and then spin
+        self.server = actionlib.SimpleActionServer('~gripper_action', GripperCommandAction, execute_cb=self.actionCb, auto_start=False)
+        self.server.start()
+        rospy.spin()
+
+    def actionCb(self, goal):
+        """ Take an input command of width to open gripper. """
+        rospy.loginfo('Gripper controller action goal recieved:%f' % goal.command.position)
+        # send command to gripper
+        self.model.setCommand(goal.command)
+        # publish feedback
+        while True:
+            if self.server.is_preempt_requested():
+                self.server.set_preemtped()
+                rospy.loginfo('Gripper Controller: Preempted.')
+                return
+            # TODO: get joint position, break when we have reached goal
+            break
+        self.server.set_succeeded()
+        rospy.loginfo('Gripper Controller: Succeeded.')
+
+    def stateCb(self, msg):
+        self.state = msg
+
+if __name__=='__main__':
+    try:
+        GripperActionController()
+    except rospy.ROSInterruptException:
+        rospy.loginfo('Hasta la Vista...')
+

src/arbotix_ros/arbotix_controllers/bin/gripper_controller.py

+#!/usr/bin/env python3
+
+"""
+  gripper_controller - action based controller for grippers.
+  Copyright (c) 2011-2014 Vanadium Labs LLC.  All right reserved.
+
+  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 Vanadium Labs LLC 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 VANADIUM LABS 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.
+"""
+
+import rospy, actionlib
+import threading
+
+from control_msgs.msg import GripperCommandAction
+from sensor_msgs.msg import JointState
+from std_msgs.msg import Float64
+from math import asin
+
+class TrapezoidGripperModel:
+    """ A simple gripper with two opposing servos to open/close non-parallel jaws. """
+
+    def __init__(self):
+        # trapezoid model: base width connecting each gripper's rotation point
+            #              + length of gripper fingers to computation point
+            #              = compute angles based on a desired width at comp. point
+        self.pad_width = rospy.get_param('~pad_width', 0.01)
+        self.finger_length = rospy.get_param('~finger_length', 0.02)
+        self.min_opening = rospy.get_param('~min_opening', 0.0)
+        self.max_opening = rospy.get_param('~max_opening', 0.09)
+        self.center_l = rospy.get_param('~center_left', 0.0)
+        self.center_r = rospy.get_param('~center_right', 0.0)
+        self.invert_l = rospy.get_param('~invert_left', False)
+        self.invert_r = rospy.get_param('~invert_right', False)
+
+        self.left_joint = rospy.get_param('~joint_left', 'l_gripper_joint')
+        self.right_joint = rospy.get_param('~joint_right', 'r_gripper_joint')
+
+        # publishers
+        self.l_pub = rospy.Publisher(self.left_joint+'/command', Float64, queue_size=5)
+        self.r_pub = rospy.Publisher(self.right_joint+'/command', Float64, queue_size=5)
+
+    def setCommand(self, command):
+        # check limits
+        if command.position > self.max_opening or command.position < self.min_opening:
+            rospy.logerr("Command (%f) exceeds opening limits (%f, %f)",
+                          command.position, self.max_opening, self.min_opening)
+            return False
+        # compute angles
+        angle = asin((command.position - self.pad_width)/(2*self.finger_length))
+        if self.invert_l:
+            l = -angle + self.center_l
+        else:
+            l = angle + self.center_l
+        if self.invert_r:
+            r = angle + self.center_r
+        else:
+            r = -angle + self.center_r
+        # publish msgs
+        lmsg = Float64(l)
+        rmsg = Float64(r)
+        self.l_pub.publish(lmsg)
+        self.r_pub.publish(rmsg)
+        return True
+
+    def getPosition(self, js):
+        left = right = 0
+        for i in range(len(js.name)):
+            if js.name[i] == self.left_joint:
+                left = js.position[i]
+            elif js.name[i] == self.right_joint:
+                right = js.position[i]
+        # TODO
+
+        return 0.0
+
+    def getEffort(self, joint_states):
+        return 1.0
+
+class ParallelGripperModel:
+    """ One servo to open/close parallel jaws, typically via linkage. """
+
+    def __init__(self):
+        self.center = rospy.get_param('~center', 0.0)
+        self.scale = rospy.get_param('~scale', 1.0)
+        self.joint = rospy.get_param('~joint', 'gripper_joint')
+
+        # publishers
+        self.pub = rospy.Publisher(self.joint+'/command', Float64, queue_size=5)
+
+    def setCommand(self, command):
+        self.pub.publish((command.position * self.scale) + self.center)
+
+    def getPosition(self, joint_states):
+        return 0.0
+
+    def getEffort(self, joint_states):
+        return 1.0
+
+
+class OneSideGripperModel:
+    """ Simplest of grippers, one servo opens or closes to achieve a particular size opening. """
+
+    def __init__(self):
+        self.pad_width = rospy.get_param('~pad_width', 0.01)
+        self.finger_length = rospy.get_param('~finger_length', 0.02)
+        self.min_opening = rospy.get_param('~min_opening', 0.0)
+        self.max_opening = rospy.get_param('~max_opening', 0.09)
+        self.center = rospy.get_param('~center', 0.0)
+        self.invert = rospy.get_param('~invert', False)
+        self.joint = rospy.get_param('~joint', 'gripper_joint')
+
+        # publishers
+        self.pub = rospy.Publisher(self.joint+'/command', Float64, queue_size=5)
+
+    def setCommand(self, command):
+        """ Take an input command of width to open gripper. """
+        # check limits
+        if command.position > self.max_opening or command.position < self.min_opening:
+            rospy.logerr("Command (%f) exceeds opening limits (%f, %f)",
+                          command.position, self.max_opening, self.min_opening)
+            return False
+        # compute angle
+        angle = asin((command.position - self.pad_width)/(2*self.finger_length))
+        # publish message
+        if self.invert:
+            self.pub.publish(-angle + self.center)
+        else:
+            self.pub.publish(angle + self.center)
+
+    def getPosition(self, joint_states):
+        # TODO
+        return 0.0
+
+    def getEffort(self, joint_states):
+        # TODO
+        return 1.0
+
+
+class GripperActionController:
+    """ The actual action callbacks. """
+    def __init__(self):
+        rospy.init_node('gripper_controller')
+
+        # setup model
+        try:
+            model = rospy.get_param('~model')
+        except:
+            rospy.logerr('no model specified, exiting')
+            exit()
+        if model == 'dualservo':
+            self.model = TrapezoidGripperModel()
+        elif model == 'parallel':
+            self.model = ParallelGripperModel()
+        elif model == 'singlesided':
+            self.model = OneSideGripperModel()
+        else:
+            rospy.logerr('unknown model specified, exiting')
+            exit()
+
+        # subscribe to joint_states
+        rospy.Subscriber('joint_states', JointState, self.stateCb)
+
+        # subscribe to command and then spin
+        self.server = actionlib.SimpleActionServer('~gripper_action', GripperCommandAction, execute_cb=self.actionCb, auto_start=False)
+        self.server.start()
+        rospy.spin()
+
+    def actionCb(self, goal):
+        """ Take an input command of width to open gripper. """
+        rospy.loginfo('Gripper controller action goal recieved:%f' % goal.command.position)
+        # send command to gripper
+        self.model.setCommand(goal.command)
+        # publish feedback
+        while True:
+            if self.server.is_preempt_requested():
+                self.server.set_preemtped()
+                rospy.loginfo('Gripper Controller: Preempted.')
+                return
+            # TODO: get joint position, break when we have reached goal
+            break
+        self.server.set_succeeded()
+        rospy.loginfo('Gripper Controller: Succeeded.')
+
+    def stateCb(self, msg):
+        self.state = msg
+
+if __name__=='__main__':
+    try:
+        rospy.logwarn("Please use gripper_controller (no .py)")
+        GripperActionController()
+    except rospy.ROSInterruptException:
+        rospy.loginfo('Hasta la Vista...')
+

src/arbotix_ros/arbotix_controllers/bin/one_side_gripper_controller.py

+#!/usr/bin/env python3
+
+"""
+  one_side_gripper_controller.py - controls a gripper built with one servo
+  Copyright (c) 2011 Vanadium Labs LLC.  All right reserved.
+
+  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 Vanadium Labs LLC 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 VANADIUM LABS 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.
+"""
+
+import rospy
+import thread
+
+from std_msgs.msg import Float64
+from math import asin
+
+class OneSideGripperController:
+    """ A simple controller that operates a servos to
+        open/close to a particular size opening. """
+    def __init__(self):
+        rospy.init_node("one_side_gripper_controller")
+        rospy.logwarn("one_side_gripper_controller.py is deprecated and will be removed in ROS Indigo, please use gripper_controller")
+
+        self.pad_width = rospy.get_param("~pad_width", 0.01)
+        self.finger_length = rospy.get_param("~finger_length", 0.02)
+        self.center = rospy.get_param("~center", 0.0)
+        self.invert = rospy.get_param("~invert", False)
+
+        # publishers
+        self.pub = rospy.Publisher("gripper_joint/command", Float64, queue_size=5)
+
+        # subscribe to command and then spin
+        rospy.Subscriber("~command", Float64, self.commandCb)
+        rospy.spin()
+
+    def commandCb(self, msg):
+        """ Take an input command of width to open gripper. """
+        # check limits
+        #if msg.data > self.max_opening or msg.data < self.min_opening:
+        #    rospy.logerr("Command exceeds limits.")
+        #    return
+        # compute angle
+        angle = asin((msg.data - self.pad_width)/(2*self.finger_length))
+        # publish message
+        if self.invert:
+            self.pub.publish(-angle + self.center)
+        else:
+            self.pub.publish(angle + self.center)
+
+if __name__=="__main__":
+    try:
+        OneSideGripperController()
+    except rospy.ROSInterruptException:
+        rospy.loginfo("Hasta la Vista...")
+        

src/arbotix_ros/arbotix_controllers/bin/parallel_gripper_action_controller.py

+#!/usr/bin/env python3
+
+"""
+  parallel_gripper_controller.py - controls a gripper built of two servos
+  Copyright (c) 2011 Vanadium Labs LLC.  All right reserved.
+
+  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 Vanadium Labs LLC 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 VANADIUM LABS 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.
+"""
+
+import rospy, actionlib
+import thread
+
+from control_msgs.msg import GripperCommandAction
+from std_msgs.msg import Float64
+from math import asin
+
+class ParallelGripperActionController:
+    """ A simple controller that operates two opposing servos to
+        open/close to a particular size opening. """
+    def __init__(self):
+        rospy.init_node('gripper_controller')
+        rospy.logwarn("parallel_gripper_action_controller.py is deprecated and will be removed in ROS Indigo, please use gripper_controller")
+
+        # trapezoid model: base width connecting each gripper's rotation point
+            #              + length of gripper fingers to computation point
+            #              = compute angles based on a desired width at comp. point
+        self.pad_width = rospy.get_param('~pad_width', 0.01)
+        self.finger_length = rospy.get_param('~finger_length', 0.02)
+        self.min_opening = rospy.get_param('~min', 0.0)
+        self.max_opening = rospy.get_param('~max', 2*self.finger_length)
+
+        self.center_l = rospy.get_param('~center_left', 0.0)
+        self.center_r = rospy.get_param('~center_right', 0.0)
+        self.invert_l = rospy.get_param('~invert_left', False)
+        self.invert_r = rospy.get_param('~invert_right', False)
+
+        # publishers
+        self.l_pub = rospy.Publisher('l_gripper_joint/command', Float64, queue_size=5)
+        self.r_pub = rospy.Publisher('r_gripper_joint/command', Float64, queue_size=5)
+
+        # subscribe to command and then spin
+        self.server = actionlib.SimpleActionServer('~gripper_action', GripperCommandAction, execute_cb=self.actionCb, auto_start=False)
+        self.server.start()
+        rospy.spin()
+
+    def actionCb(self, goal):
+        """ Take an input command of width to open gripper. """
+        rospy.loginfo('Gripper controller action goal recieved:%f' % goal.command.position)
+        command = goal.command.position
+        # check limits
+        if command > self.max_opening:
+            command = self.max_opening
+        if command < self.min_opening:
+            command = self.min_opening
+        # compute angles
+        angle = asin((command - self.pad_width)/(2*self.finger_length))
+        if self.invert_l:
+            l = -angle + self.center_l
+        else:
+            l = angle + self.center_l
+        if self.invert_r:
+            r = angle + self.center_r
+        else:
+            r = -angle + self.center_r
+        # publish msgs
+        lmsg = Float64(l)
+        rmsg = Float64(r)
+        self.l_pub.publish(lmsg)
+        self.r_pub.publish(rmsg)
+        rospy.sleep(5.0)
+        self.server.set_succeeded()
+        rospy.loginfo('Gripper Controller: Done.')
+
+if __name__=='__main__': 
+    try:
+        ParallelGripperActionController()
+    except rospy.ROSInterruptException:
+        rospy.loginfo('Hasta la Vista...')
+        

src/arbotix_ros/arbotix_controllers/bin/parallel_gripper_controller.py

+#!/usr/bin/env python3
+
+"""
+  parallel_gripper_controller.py - controls a gripper built of two servos
+  Copyright (c) 2011 Vanadium Labs LLC.  All right reserved.
+
+  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 Vanadium Labs LLC 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 VANADIUM LABS 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.
+"""
+
+import rospy
+import thread
+
+from std_msgs.msg import Float64
+from math import asin
+
+class ParallelGripperController:
+    """ A simple controller that operates two opposing servos to
+        open/close to a particular size opening. """
+    def __init__(self):
+        rospy.init_node("parallel_gripper_controller")
+        rospy.logwarn("parallel_gripper_controller.py is deprecated and will be removed in ROS Indigo, please use gripper_controller")
+
+        # trapezoid model: base width connecting each gripper's rotation point
+            #              + length of gripper fingers to computation point
+            #              = compute angles based on a desired width at comp. point
+        self.pad_width = rospy.get_param("~pad_width", 0.01)
+        self.finger_length = rospy.get_param("~finger_length", 0.02)
+        self.min_opening = rospy.get_param("~min", 0.0)
+        self.max_opening = rospy.get_param("~max", 2*self.finger_length)
+
+        self.center_l = rospy.get_param("~center_left", 0.0)
+        self.center_r = rospy.get_param("~center_right", 0.0)
+        self.invert_l = rospy.get_param("~invert_left", False)
+        self.invert_r = rospy.get_param("~invert_right", False)
+
+        # publishers
+        self.l_pub = rospy.Publisher("l_gripper_joint/command", Float64, queue_size=5)
+        self.r_pub = rospy.Publisher("r_gripper_joint/command", Float64, queue_size=5)
+
+        # subscribe to command and then spin
+        rospy.Subscriber("~command", Float64, self.commandCb)
+        rospy.spin()
+
+    def commandCb(self, msg):
+        """ Take an input command of width to open gripper. """
+        # check limits
+        if msg.data > self.max_opening or msg.data < self.min_opening:
+            rospy.logerr("Command exceeds limits.")
+            return
+        # compute angles
+        angle = asin((msg.data - self.pad_width)/(2*self.finger_length))
+        if self.invert_l:
+            l = -angle + self.center_l
+        else:
+            l = angle + self.center_l
+        if self.invert_r:
+            r = angle + self.center_r
+        else:
+            r = -angle + self.center_r
+        # publish msgs
+        lmsg = Float64(l)
+        rmsg = Float64(r)
+        self.l_pub.publish(lmsg)
+        self.r_pub.publish(rmsg)
+
+if __name__=="__main__": 
+    try:
+        ParallelGripperController()
+    except rospy.ROSInterruptException:
+        rospy.loginfo("Hasta la Vista...")
+        

src/arbotix_ros/arbotix_controllers/bin/parallel_single_servo_controller.py

+#!/usr/bin/env python3
+
+"""
+  parallel_single_servo_controller.py - controls a single-servo parallel-jaw gripper
+  Copyright (c) 2011 Vanadium Labs LLC.  All right reserved.
+
+  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 Vanadium Labs LLC 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 VANADIUM LABS 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.
+"""
+
+import rospy, tf
+import thread
+
+from std_msgs.msg import Float64
+from sensor_msgs.msg import JointState
+from math import asin
+
+class ParallelGripperController:
+    """ A simple controller that operates a single servo parallel jaw gripper. """
+    def __init__(self):
+        rospy.init_node("gripper_controller")
+
+        # TODO: load calibration data. Form: opening->servo angle
+        self.calib = { 0.0000 : 1.8097, 0.0159: 1.2167, 0.0254 : 0.8997, 0.0381 : 0.4499, 0.042 : 0.1943 }
+        #self.calib = { 0.0000 : 866, 0.0159: 750, 0.0254 : 688, 0.0381 : 600, 0.042 : 550 }
+
+        # parameters
+        self.min = rospy.get_param("~min", 0.0)
+        self.max = rospy.get_param("~max", 0.042)
+        self.center = rospy.get_param("~center", 512)
+        self.invert = rospy.get_param("~invert", False)
+        
+        # publishers
+        self.commandPub = rospy.Publisher("gripper_joint/command", Float64, queue_size=5)
+        self.br = tf.TransformBroadcaster()
+    
+        # current width of gripper
+        self.width = 0.0
+
+        # subscribe to command and then spin
+        rospy.Subscriber("~command", Float64, self.commandCb)
+        rospy.Subscriber("joint_states", JointState, self.stateCb)
+        
+        r = rospy.Rate(15)
+        while not rospy.is_shutdown():
+            # output tf
+            self.br.sendTransform((0, -self.width/2.0, 0),
+                                   tf.transformations.quaternion_from_euler(0, 0, 0),
+                                   rospy.Time.now(),
+                                   "gripper_left_link",
+                                   "gripper_link")
+            self.br.sendTransform((0, self.width/2.0, 0),
+                                   tf.transformations.quaternion_from_euler(0, 0, 0),
+                                   rospy.Time.now(),
+                                   "gripper_right_link",
+                                   "gripper_link")
+            r.sleep()
+
+    def getCommand(self, width):
+        """ Get servo command for an opening width. """
+        keys = self.calib.keys(); keys.sort()   
+        # find end points of segment
+        low = keys[0]; 
+        high = keys[-1]
+        for w in keys[1:-1]:
+            if w > low and w < width:
+                low = w
+            if w < high and w > width:
+                high = w
+        # linear interpolation
+        scale = (width-low)/(high-low)
+        return ((self.calib[high]-self.calib[low])*scale) + self.calib[low]
+
+    def getWidth(self, command):
+        """ Get opening width for a particular servo command. """
+        reverse_calib = dict()
+        for k, v in self.calib.items():
+            reverse_calib[v] = k
+        keys = reverse_calib.keys(); keys.sort()
+        # find end points of segment
+        low = keys[0]
+        high = keys[-1]
+        for c in keys[1:-1]:
+            if c > low and c < command:
+                low = c
+            if c < high and c > command:
+                high = c
+        # linear interpolation
+        scale = (command-low)/(high-low)
+        return ((reverse_calib[high]-reverse_calib[low])*scale) + reverse_calib[low]
+
+    def commandCb(self, msg):
+        """ Take an input command of width to open gripper. """
+        # check limits
+        if msg.data > self.max or msg.data < self.min:
+            rospy.logerr("Command exceeds limits.")
+            return
+        # compute angle
+        self.commandPub.publish( Float64( self.getCommand(msg.data) ) )
+
+    def stateCb(self, msg):
+        """ The callback that listens for joint_states. """
+        try:
+            index = msg.name.index("gripper_joint")
+        except ValueError:
+            return
+        self.width = self.getWidth(msg.position[index])
+
+if __name__=="__main__": 
+    try:
+        ParallelGripperController()
+    except rospy.ROSInterruptException:
+        rospy.loginfo("Hasta la Vista...")
+        

src/arbotix_ros/arbotix_controllers/package.xml

+<package>
+  <name>arbotix_controllers</name>
+  <version>0.11.0</version>
+  <description>
+    Extends the arbotix_python package with a number of more sophisticated ROS wrappers for common devices.
+  </description>
+  <author>Michael Ferguson</author>
+  <maintainer email="mike@vanadiumlabs.com">Michael Ferguson</maintainer>
+  <license>BSD</license>
+  <url>http://ros.org/wiki/arbotix_controllers</url>
+  
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <run_depend>arbotix_python</run_depend>
+  <run_depend>trajectory_msgs</run_depend>
+  <run_depend>tf</run_depend>
+</package>

src/arbotix_ros/arbotix_firmware/CHANGELOG.rst

+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package arbotix_firmware
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.11.0 (2020-12-29)
+-------------------
+
+0.10.0 (2014-07-14)
+-------------------
+
+0.9.2 (2014-02-12)
+------------------
+
+0.9.1 (2014-01-28)
+------------------
+
+0.9.0 (2013-08-22)
+------------------
+
+0.8.2 (2013-03-28)
+------------------
+
+0.8.1 (2013-03-09)
+------------------
+
+0.8.0 (2013-02-21)
+------------------
+* import drivers and catkinize

src/arbotix_ros/arbotix_firmware/CMakeLists.txt

+cmake_minimum_required(VERSION 3.0.2)
+project(arbotix_firmware)
+
+find_package(catkin REQUIRED)
+catkin_package()

src/arbotix_ros/arbotix_firmware/package.xml

+<package>
+  <name>arbotix_firmware</name>
+  <version>0.11.0</version>
+  <description>
+    Firmware source code for ArbotiX ROS bindings.
+  </description>
+  <author>Michael Ferguson</author>
+  <maintainer email="mike@vanadiumlabs.com">Michael Ferguson</maintainer>
+  <license>BSD</license>
+  <url>http://ros.org/wiki/arbotix_firmware</url>
+  
+  <buildtool_depend>catkin</buildtool_depend>
+</package>

src/arbotix_ros/arbotix_firmware/src/diff_controller.h

+/* 
+  ArbotiX Firmware for ROS driver
+  Copyright (c) 2009-2011 Vanadium Labs LLC.  All right reserved.
+ 
+  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 Vanadium Labs LLC 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 VANADIUM LABS 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 <Arduino.h>
+#include <math.h>
+
+/* Register Storage */
+int left_pwm;
+int right_pwm;
+int left_speed;
+int right_speed;
+
+/* PID Parameters */
+int Kp;
+int Kd;
+int Ki;
+int Ko;                      // output scale
+int maxAccel;                // maximum acceleration per frame (ticks)
+
+/* PID modes */
+unsigned int PIDmode;
+#define PID_NONE        0
+#define PID_SPEED       1
+
+#define FRAME_RATE      33   // frame rate in millis (30Hz)
+#define FRAMES          30
+unsigned long f_time;        // last frame
+
+unsigned char moving = 0;    // base in motion
+unsigned char paused = 0;    // base was in motion, can resume
+#define MAXOUTPUT       255  // motor PWM
+
+/* Setpoint Info For a Motor */
+typedef struct{
+  int Velocity;              // desired actual speed (count/frame)
+  long Encoder;              // actual reading
+  long PrevEnc;              // last reading
+  int PrevErr;
+  int Ierror;   
+  int output;                // last motor setting
+} SetPointInfo;
+
+SetPointInfo left, right;
+
+/* Initialize PID parameters to something known */
+void setupPID(){
+  // Default values for the PR-MINI
+  Kp = 25;
+  Kd = 30;
+  Ki = 0;
+  Ko = 100;
+  maxAccel = 50;
+  f_time = 0;
+}
+
+/* PID control of motor speed */
+void DoPid(SetPointInfo * p){
+  long Perror;
+  long output;
+  
+  Perror = p->Velocity - (p->Encoder-p->PrevEnc);
+          
+  // Derivative error is the delta Perror
+  output = (Kp*Perror + Kd*(Perror - p->PrevErr) + Ki*p->Ierror)/Ko;
+  p->PrevErr = Perror;
+  p->PrevEnc = p->Encoder;
+  
+  output += p->output;   
+  // Accumulate Integral error *or* Limit output.
+  // Stop accumulating when output saturates
+  if (output >= MAXOUTPUT)
+    output = MAXOUTPUT;
+  else if (output <= -MAXOUTPUT)
+    output = -MAXOUTPUT;
+  else
+    p->Ierror += Perror;
+  
+  p->output = output;
+}
+
+/* Clear accumulators */
+void ClearPID(){
+  PIDmode = 0; moving = 0;
+  left.PrevErr = 0;
+  left.Ierror = 0;
+  left.output = 0;
+  right.PrevErr = 0;
+  right.Ierror = 0;
+  right.output = 0;
+}
+
+/* This is called by the main loop, does a X HZ PID loop. */
+void updatePID(){
+  if((moving > 0) && (PIDmode > 0)){  // otherwise, just return
+    unsigned long j = millis();
+    if(j > f_time){
+      // update encoders
+      left.Encoder = Encoders.left;
+      right.Encoder = Encoders.right;
+      // do PID update on PWM
+      DoPid(&left);
+      DoPid(&right);
+      // set updated motor outputs      
+      if(PIDmode > 0){
+        drive.set(left.output, right.output);
+      }
+      // update timing
+      f_time = j + FRAME_RATE;
+    }
+  }
+}
+
+void clearAll(){
+  PIDmode = 0;
+  left.Encoder = 0;
+  right.Encoder = 0;
+  left.PrevEnc = 0;
+  right.PrevEnc = 0;
+  left.output = 0;
+  right.output = 0;
+  Encoders.Reset();  
+}
+

src/arbotix_ros/arbotix_firmware/src/ros.h

+/* 
+  Common Definitions for ROS driver ArbotiX Firmware
+  Copyright (c) 2008-2012 Vanadium Labs LLC.  All right reserved.
+ 
+  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 Vanadium Labs LLC 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 VANADIUM LABS 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.
+*/ 
+
+/* ArbotiX (id:253) Instruction Definitions */
+#define ARB_SIZE_POSE       7    // pose size: a single param for size of pose
+#define ARB_LOAD_POSE       8    // load pose: index, then pose positions (# of params = 2*pose_size)
+#define ARB_LOAD_SEQ        9    // seq size: a single param for the size of the seq
+#define ARB_PLAY_SEQ        10   // load seq: index/times (# of params = 3*seq_size)
+#define ARB_LOOP_SEQ        11   // play seq: no params
+//#define ARB_TEST          25   // hardware test: no params
+#define ARB_CONTROL_SETUP   26   // write ids: id of controller, params (usually ids of servos, # of params = pose_size + 1)
+#define ARB_CONTROL_WRITE   27   // write positions: positions in order of servos (# of params = 2*pose_size)
+#define ARB_CONTROL_STAT    28   // retrieve status: id of controller
+#define ARB_SYNC_READ       0x84
+
+/* ArbotiX (id:253) Register Table Definitions */
+#define REG_MODEL_NUMBER_L  0
+#define REG_MODEL_NUMBER_H  1
+#define REG_VERSION         2
+#define REG_ID              3
+#define REG_BAUD_RATE       4
+
+#define REG_DIGITAL_IN0     5  // First block of digital pins to read
+#define REG_DIGITAL_IN1     6
+#define REG_DIGITAL_IN2     7
+#define REG_DIGITAL_IN3     8
+
+#define REG_RESCAN          15
+#define REG_RETURN_LEVEL    16
+#define REG_ALARM_LED       17
+#define REG_ANA_BASE        18  // First Analog Port
+#define REG_SERVO_BASE      26  // Up to 10 servos, each uses 2 bytes (L, then H), pulse width (0, 1000-2000ms)
+#define REG_MOVING          46
+
+#define REG_DIGITAL_OUT0    47  // First digital pin to write
+                                // base + index, bit 1 = value (0,1), bit 0 = direction (0,1)
+
+#define REG_RESERVED        79  // 79 -- 99 are reserved for future use
+#define REG_USER            100 // 
+
+/* Packet Decoding */
+int mode = 0;                   // where we are in the frame
+
+unsigned char id = 0;           // id of this frame
+unsigned char length = 0;       // length of this frame
+unsigned char ins = 0;          // instruction of this frame
+
+unsigned char params[143];      // parameters (match RX-64 buffer size)
+unsigned char index = 0;        // index in param buffer
+
+int checksum;                   // checksum