Merge pull request #79 from cpaxton/cpaxton/refactor

Refactor gripper and documentation

Readme.md

@@ -27,7 +27,7 @@ Interested in contributing? Check out the [development guidelines](Development.m
 
 ## Installation
 
-Check out [installation instructions](INSTALL.md).
+Check out [installation instructions](docs/install.md).
 
 We are working on experimental install scripts:
   - [ROS Indigo/Ubuntu 14.04 LTS](install_indigo.sh)

costar_bringup/launch/iiwa14_s_model.launch

@@ -40,7 +40,7 @@
     </include>
 
     <!-- start gripper server anyway, just don't connect it -->
-    <include file="$(find costar_gripper_manager)/launch/s_model.launch"/>
+    <include file="$(find gripper_robotiq)/launch/s_model.launch"/>
   </group>
 
   <!-- When not launching as a simulation, we do not need to namespace everything.-->
@@ -72,7 +72,7 @@
       args="$(arg gripper_ip_address)"/>
 
     <!-- Start S model-->
-    <include file="$(find costar_gripper_manager)/launch/s_model.launch">
+    <include file="$(find gripper_robotiq)/launch/s_model.launch">
       <arg name="ee_frame" value="$(arg ee_frame)"/>
     </include>
 

costar_bringup/launch/s_model.launch

@@ -9,15 +9,15 @@
     <include file="$(find costar_bringup)/launch/robotiq_s_model_endpoint.launch">
       <param name="ee_frame" value="$(arg ee_frame)"/>
     </include>
-    <node name="simple_s_model_server" pkg="costar_gripper_manager" type="s_model.py"/>
-    <include file="$(find costar_gripper_manager)/launch/s_model.launch"/>
+    <node name="simple_s_model_server" pkg="gripper_robotiq" type="s_model.py"/>
+    <include file="$(find gripper_robotiq)/launch/s_model.launch"/>
 
   </group>
   <group if="$(arg sim)">
 
     <!-- start gripper server anyway, just don't connect it -->
-    <node name="simple_s_model_server" pkg="costar_gripper_manager" type="s_model.py"/>
-    <include file="$(find costar_gripper_manager)/launch/s_model.launch"/>
+    <node name="simple_s_model_server" pkg="gripper_robotiq" type="s_model.py"/>
+    <include file="$(find gripper_robotiq)/launch/s_model.launch"/>
 
   </group>
 </launch>

costar_bringup/launch/ur5_c_model.launch

@@ -44,7 +44,7 @@
 
     <!-- gripper bringup: connect to s model via TCP/IP -->
     <node name="robotiq_c_model_interface" pkg="robotiq_c_model_control" type="CModelTcpNode.py" args="$(arg gripper_ip_address)"/>
-    <include file="$(find costar_gripper_manager)/launch/c_model.launch">
+    <include file="$(find gripper_robotiq)/launch/c_model.launch">
       <arg name="ee_frame" value="$(arg ee_frame)"/>
     </include>
 

costar_bringup/package.xml

@@ -41,10 +41,10 @@
   <!--   <test_depend>gtest</test_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
 
-  <build_depend>costar_gripper_manager</build_depend>
+  <build_depend>gripper_manager</build_depend>
   <build_depend>costar_robot_manager</build_depend>
   <build_depend>costar_waypoint_manager</build_depend>
-  <run_depend>costar_gripper_manager</run_depend>
+  <run_depend>gripper_manager</run_depend>
   <run_depend>costar_robot_manager</run_depend>
   <run_depend>costar_waypoint_manager</run_depend>
 

costar_gripper/costar_gripper_manager/CMakeLists.txt → costar_gripper/gripper_manager/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 2.8.3)
-project(costar_gripper_manager)
+project(gripper_manager)
 
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)

costar_gripper/costar_gripper_manager/package.xml → costar_gripper/gripper_manager/package.xml

@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <package>
-  <name>costar_gripper_manager</name>
+  <name>gripper_manager</name>
   <version>0.0.0</version>
   <description>The costar_gripper_manager package</description>
 

costar_gripper/costar_gripper_manager/setup.py → costar_gripper/gripper_manager/setup.py

@@ -5,7 +5,7 @@
 
 d = generate_distutils_setup(
     ##  don't do this unless you want a globally visible script
-    packages=['costar_gripper'],
+    packages=['gripper_manager'],
     package_dir={'': 'src'},
 )
 

costar_gripper/gripper_manager/src/gripper_manager/__init__.py

@@ -0,0 +1,4 @@
+# This is the equivalent of the CostarArm component.
+from costar_gripper import CostarGripper
+
+__all__ = ["CostarGripper"]

costar_gripper/gripper_manager/src/gripper_manager/costar_gripper.py

@@ -0,0 +1,77 @@
+from costar_component import CostarComponent
+from os.path import join
+import rospy
+from std_srvs.srv import Empty
+
+# The CoSTAR Gripper component contains a lot less special logic than the
+# CoSTAR Arm does. It mostly just has a few tools.
+class CostarGripper(CostarComponent):
+  def __init__(self,
+      name, #name of the gripper
+      input_topic, # topic on which we receive messages from the gripper
+      output_topic, # topic on which we send messages to the gripper
+      InputMsgType, # type of input message
+      OutputMsgType, # tpye of output message
+      GripperPredicatorType, # construct a predicator node to send status info
+      ns, # operating namespace
+      verbose, # verbose or not
+      *args, **kwargs):
+
+    self.verbose = verbose
+    self.predicator = GripperPredicatorType(
+        start_subscriber=False,
+        publish_predicates=True,
+        gripper_name=name)
+
+    self.sub = rospy.Subscriber(input_topic, InputMsgType, self.status_cb)
+    self.pub = rospy.Publisher(output_topic, OutputMsgType, queue_size = 100)
+    self.open = rospy.Service(join(ns,"open"), Empty, self.open_gripper)
+    self.close = rospy.Service(join(ns,"close"), Empty, self.close_gripper)
+    self.wide_mode_srv = rospy.Service(join(ns,"wide_mode"), Empty, self.wide_mode)
+    self.pinch_mode_srv = rospy.Service(join(ns,"pinch_mode"), Empty, self.pinch_mode)
+    self.basic_mode_srv = rospy.Service(join(ns,"basic_mode"), Empty, self.basic_mode)
+    self.scissor_mode_srv = rospy.Service(join(ns,"scissor_mode"), Empty, self.scissor_mode)
+    self.reactivate_srv = rospy.Service(join(ns,"activate"), Empty, self.activate)
+    self.reset_srv = rospy.Service(join(ns,"reset"), Empty, self.reset)
+    self.command = self.getDefaultMsg()
+
+    self.activated = True;
+
+    self.activate()
+
+  def getDefaultMsg(self):
+    raise NotImplementedError('not implemented in CostarGripper base class!')
+
+  def activate(self,msg=None):
+    raise NotImplementedError('not implemented in CostarGripper base class!')
+
+  def reset(self, msg=None):
+    raise NotImplementedError('not implemented in CostarGripper base class!')
+
+  def open_gripper(self,msg=None):
+    raise NotImplementedError('not implemented in CostarGripper base class!')
+
+  def close_gripper(self,msg=None):
+    raise NotImplementedError('not implemented in CostarGripper base class!')
+
+  def wide_mode(self,msg=None):
+    raise NotImplementedError('not implemented in CostarGripper base class!')
+
+  def pinch_mode(self,msg=None):
+    raise NotImplementedError('not implemented in CostarGripper base class!')
+
+  def basic_mode(self,msg=None):
+    raise NotImplementedError('not implemented in CostarGripper base class!')
+
+  def scissor_mode(self,msg=None):
+    raise NotImplementedError('not implemented in CostarGripper base class!')
+
+  def statusInterpreter(self,status):
+    raise NotImplementedError('not implemented in CostarGripper base class!')
+
+  def status_cb(self,msg):
+      if self.verbose:
+          rospy.loginfo(self.statusInterpreter(msg))
+      self.predicator.handle(msg)
+
+

costar_gripper/costar_gripper_robotiq/CMakeLists.txt → costar_gripper/gripper_robotiq/CMakeLists.txt

@@ -1,11 +1,11 @@
 cmake_minimum_required(VERSION 2.8.3)
-project(costar_gripper_robotiq)
+project(gripper_robotiq)
 
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS
-  costar_gripper_manager
+  gripper_manager
   predicator_robotiq
   robotiq_c_model_control
   robotiq_s_model_control
@@ -18,7 +18,7 @@ find_package(catkin REQUIRED COMPONENTS
 ## Uncomment this if the package has a setup.py. This macro ensures
 ## modules and global scripts declared therein get installed
 ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
+catkin_python_setup()
 
 ################################################
 ## Declare ROS messages, services and actions ##
@@ -102,8 +102,8 @@ find_package(catkin REQUIRED COMPONENTS
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
 #  INCLUDE_DIRS include
-#  LIBRARIES costar_gripper_robotiq
-#  CATKIN_DEPENDS costar_gripper_manager predicator_robotiq robotiq_c_model_control robotiq_s_model_control
+#  LIBRARIES gripper_robotiq
+#  CATKIN_DEPENDS gripper_manager predicator_robotiq robotiq_c_model_control robotiq_s_model_control
 #  DEPENDS system_lib
 )
 
@@ -119,24 +119,24 @@ include_directories(
 )
 
 ## Declare a C++ library
-# add_library(costar_gripper_robotiq
-#   src/${PROJECT_NAME}/costar_gripper_robotiq.cpp
+# add_library(gripper_robotiq
+#   src/${PROJECT_NAME}/gripper_robotiq.cpp
 # )
 
 ## Add cmake target dependencies of the library
 ## as an example, code may need to be generated before libraries
 ## either from message generation or dynamic reconfigure
-# add_dependencies(costar_gripper_robotiq ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+# add_dependencies(gripper_robotiq ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 ## Declare a C++ executable
-# add_executable(costar_gripper_robotiq_node src/costar_gripper_robotiq_node.cpp)
+# add_executable(gripper_robotiq_node src/gripper_robotiq_node.cpp)
 
 ## Add cmake target dependencies of the executable
 ## same as for the library above
-# add_dependencies(costar_gripper_robotiq_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+# add_dependencies(gripper_robotiq_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 ## Specify libraries to link a library or executable target against
-# target_link_libraries(costar_gripper_robotiq_node
+# target_link_libraries(gripper_robotiq_node
 #   ${catkin_LIBRARIES}
 # )
 
@@ -155,7 +155,7 @@ include_directories(
 # )
 
 ## Mark executables and/or libraries for installation
-# install(TARGETS costar_gripper_robotiq costar_gripper_robotiq_node
+# install(TARGETS gripper_robotiq gripper_robotiq_node
 #   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
 #   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
 #   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
@@ -180,7 +180,7 @@ include_directories(
 #############
 
 ## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_costar_gripper_robotiq.cpp)
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_gripper_robotiq.cpp)
 # if(TARGET ${PROJECT_NAME}-test)
 #   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
 # endif()

costar_gripper/costar_gripper_manager/launch/c_model.launch → costar_gripper/gripper_robotiq/launch/c_model.launch

@@ -2,12 +2,12 @@
   <arg name="ee_frame"/>
 
   <!-- Launch gripper endpoint setup -->
-  <include file="$(find costar_gripper_manager)/launch/robotiq_c_model_endpoint.launch">
+  <include file="$(find gripper_robotiq)/launch/robotiq_c_model_endpoint.launch">
     <arg name="ee_frame" value="$(arg ee_frame)"/>
   </include>
 
   <node name="simple_c_model_server"
-    pkg="costar_gripper_manager"
+    pkg="gripper_robotiq"
     type="c_model.py"/>
 
 </launch>

costar_gripper/costar_gripper_manager/launch/s_model.launch → costar_gripper/gripper_robotiq/launch/s_model.launch

@@ -4,7 +4,7 @@
   <arg name="start_joint_state_publisher" default="false" doc="(bool) bring up joint state publisher with UI"/>
 
   <!-- Launch gripper endpoint setup -->
-  <include file="$(find costar_gripper_manager)/launch/robotiq_s_model_endpoint.launch">
+  <include file="$(find gripper_robotiq)/launch/robotiq_s_model_endpoint.launch">
     <param name="ee_frame" value="$(arg ee_frame)"/>
   </include>
 
@@ -23,6 +23,6 @@
     <param name="use_gui" value="true"/>
   </node>
 
-  <node name="simple_s_model_server" pkg="costar_gripper_manager" type="s_model.py"/>
+  <node name="simple_s_model_server" pkg="gripper_robotiq" type="s_model.py"/>
 
 </launch>

costar_gripper/costar_gripper_robotiq/package.xml → costar_gripper/gripper_robotiq/package.xml

@@ -1,8 +1,8 @@
 <?xml version="1.0"?>
 <package>
-  <name>costar_gripper_robotiq</name>
+  <name>gripper_robotiq</name>
   <version>0.0.0</version>
-  <description>The costar_gripper_robotiq package</description>
+  <description>The gripper_robotiq package</description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag --> 
   <!-- Example:  -->
@@ -19,7 +19,7 @@
   <!-- Url tags are optional, but mutiple are allowed, one per tag -->
   <!-- Optional attribute type can be: website, bugtracker, or repository -->
   <!-- Example: -->
-  <!-- <url type="website">http://wiki.ros.org/costar_gripper_robotiq</url> -->
+  <!-- <url type="website">http://wiki.ros.org/gripper_robotiq</url> -->
 
 
   <!-- Author tags are optional, mutiple are allowed, one per tag -->
@@ -40,11 +40,11 @@
   <!-- Use test_depend for packages you need only for testing: -->
   <!--   <test_depend>gtest</test_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
-  <build_depend>costar_gripper_manager</build_depend>
+  <build_depend>gripper_manager</build_depend>
   <build_depend>predicator_robotiq</build_depend>
   <build_depend>robotiq_c_model_control</build_depend>
   <build_depend>robotiq_s_model_control</build_depend>
-  <run_depend>costar_gripper_manager</run_depend>
+  <run_depend>gripper_manager</run_depend>
   <run_depend>predicator_robotiq</run_depend>
   <run_depend>robotiq_c_model_control</run_depend>
   <run_depend>robotiq_s_model_control</run_depend>
@@ -55,4 +55,4 @@
     <!-- Other tools can request additional information be placed here -->
 
   </export>
-</package>
+</package>

costar_gripper/costar_gripper_manager/scripts/c_model.py → costar_gripper/gripper_robotiq/scripts/c_model.py

@@ -1,7 +1,7 @@
 #!/usr/bin/env python
 
 import rospy
-from costar_gripper import SimpleCModelServer
+from gripper_robotiq import SimpleCModelServer
 
 rospy.init_node("simple_c_model_server")
 verbose = rospy.get_param('~verbose',False)

costar_gripper/costar_gripper_manager/scripts/s_model.py → costar_gripper/gripper_robotiq/scripts/s_model.py

@@ -1,7 +1,7 @@
 #!/usr/bin/env python
 
 import rospy
-from costar_gripper import SimpleSModelServer
+from gripper_robotiq import SimpleSModelServer
 
 rospy.init_node("simple_s_model_server")
 server = SimpleSModelServer("/costar/gripper")

costar_gripper/gripper_robotiq/setup.py

@@ -0,0 +1,13 @@
+#!/usr/bin/env python
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+    ##  don't do this unless you want a globally visible script
+    packages=['gripper_robotiq'],
+    package_dir={'': 'src'},
+)
+
+setup(**d)
+