From 3de8839f140a312cc6921504103ea5b2b11a58eb Mon Sep 17 00:00:00 2001 From: Baichuan Jiang Date: Mon, 19 Jun 2017 15:16:53 -0400 Subject: [PATCH 01/25] update install scripts --- install_indigo.sh | 4 ++-- install_kinetic.sh | 25 +++++++++++++++++++++++++ 2 files changed, 27 insertions(+), 2 deletions(-) create mode 100644 install_kinetic.sh diff --git a/install_indigo.sh b/install_indigo.sh index d44f9daa..fdbebda3 100755 --- a/install_indigo.sh +++ b/install_indigo.sh @@ -13,8 +13,8 @@ git clone https://github.com/cpaxton/costar_stack.git git clone https://github.com/SalvoVirga/iiwa_stack.git git clone https://github.com/ros-industrial/robotiq.git git clone https://github.com/jbohren/rqt_dot.git -git clone https://github.com/sniekum/ar_track_alvar.git -git clone https://github.com/sniekum/ar_track_alvar_msgs.git +git clone https://github.com/sniekum/ar_track_alvar.git --branch indigo-devel +git clone https://github.com/sniekum/ar_track_alvar_msgs.git git clone https://github.com/gt-ros-pkg/hrl-kdl.git git clone https://github.com/xqms/ur_modern_driver.git --branch thread_safety rosdep install -y --from-paths ./ --ignore-src --rosdistro $ROS_DISTRO diff --git a/install_kinetic.sh b/install_kinetic.sh new file mode 100644 index 00000000..ce1bc9f3 --- /dev/null +++ b/install_kinetic.sh @@ -0,0 +1,25 @@ +#!/usr/bin/env sh + + +cd ~ +mkdir -p costar_ws/src +cd ~/costar_ws +export ROS_DISTRO=kinetic +source /opt/ros/$ROS_DISTRO/setup.bash +sudo apt-get update -qq +sudo apt-get install -y python-catkin-pkg python-rosdep python-wstool python-catkin-tools ros-$ROS_DISTRO-catkin +catkin init +cd ~/costar_ws/src +git clone https://github.com/cpaxton/costar_stack.git +git clone https://github.com/SalvoVirga/iiwa_stack.git +git clone https://github.com/ros-industrial/robotiq.git +git clone https://github.com/jbohren/rqt_dot.git +git clone https://github.com/sniekum/ar_track_alvar.git --branch $ROS_DISTRO-devel +git clone https://github.com/sniekum/ar_track_alvar_msgs.git +git clone https://github.com/gt-ros-pkg/hrl-kdl.git +git clone https://github.com/xqms/ur_modern_driver.git --branch thread_safety +rosdep install -y --from-paths ./ --ignore-src --rosdistro $ROS_DISTRO +echo "Ignore COSTAR_PERCEPTION until you have installed its dependencies." +touch costar_stack/costar_perception/CATKIN_IGNORE +catkin build --continue +source ../devel/setup.bash From 9f5a36b57e46ba2ff41567a2831bd95ac5c425c7 Mon Sep 17 00:00:00 2001 From: Baichuan Jiang Date: Mon, 19 Jun 2017 15:20:33 -0400 Subject: [PATCH 02/25] update install for kinetic again --- install_kinetic.sh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/install_kinetic.sh b/install_kinetic.sh index ce1bc9f3..2bf59ea0 100644 --- a/install_kinetic.sh +++ b/install_kinetic.sh @@ -14,10 +14,10 @@ git clone https://github.com/cpaxton/costar_stack.git git clone https://github.com/SalvoVirga/iiwa_stack.git git clone https://github.com/ros-industrial/robotiq.git git clone https://github.com/jbohren/rqt_dot.git -git clone https://github.com/sniekum/ar_track_alvar.git --branch $ROS_DISTRO-devel -git clone https://github.com/sniekum/ar_track_alvar_msgs.git +git clone https://github.com/sniekum/ar_track_alvar.git --branch $ROS_DISTRO-devel git clone https://github.com/gt-ros-pkg/hrl-kdl.git git clone https://github.com/xqms/ur_modern_driver.git --branch thread_safety +git clone https://github.com/ros-planning/moveit --branch $ROS_DISTRO-devel rosdep install -y --from-paths ./ --ignore-src --rosdistro $ROS_DISTRO echo "Ignore COSTAR_PERCEPTION until you have installed its dependencies." touch costar_stack/costar_perception/CATKIN_IGNORE From 7b4577396c87630594f5c4b9cd19be888380b21b Mon Sep 17 00:00:00 2001 From: Baichuan Jiang Date: Mon, 19 Jun 2017 15:30:32 -0400 Subject: [PATCH 03/25] commenting out robotiq issues in gripper manager and in robotiq predicator module --- costar_gripper/gripper_manager/CMakeLists.txt | 2 +- costar_gripper/gripper_manager/package.xml | 2 +- costar_predicator/predicator_robotiq/CMakeLists.txt | 4 ++-- costar_predicator/predicator_robotiq/package.xml | 6 +++--- install_kinetic.sh | 3 ++- 5 files changed, 9 insertions(+), 8 deletions(-) diff --git a/costar_gripper/gripper_manager/CMakeLists.txt b/costar_gripper/gripper_manager/CMakeLists.txt index 0212becf..d9b8eae4 100644 --- a/costar_gripper/gripper_manager/CMakeLists.txt +++ b/costar_gripper/gripper_manager/CMakeLists.txt @@ -5,7 +5,7 @@ project(gripper_manager) ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS - robotiq_s_model_control + #robotiq_s_model_control ) ## System dependencies are found with CMake's conventions diff --git a/costar_gripper/gripper_manager/package.xml b/costar_gripper/gripper_manager/package.xml index 7365d883..0d11be46 100644 --- a/costar_gripper/gripper_manager/package.xml +++ b/costar_gripper/gripper_manager/package.xml @@ -41,7 +41,7 @@ catkin - robotiq_s_model_control + robotiq_s_model_control predicator_robotiq predicator_robotiq diff --git a/costar_predicator/predicator_robotiq/CMakeLists.txt b/costar_predicator/predicator_robotiq/CMakeLists.txt index 843f7860..fe42d6c5 100644 --- a/costar_predicator/predicator_robotiq/CMakeLists.txt +++ b/costar_predicator/predicator_robotiq/CMakeLists.txt @@ -7,8 +7,8 @@ project(predicator_robotiq) find_package(catkin REQUIRED COMPONENTS predicator_core predicator_msgs - robotiq_c_model_control - robotiq_s_model_control + #robotiq_c_model_control + #robotiq_s_model_control ) ## System dependencies are found with CMake's conventions diff --git a/costar_predicator/predicator_robotiq/package.xml b/costar_predicator/predicator_robotiq/package.xml index 8db0d103..87d6288a 100644 --- a/costar_predicator/predicator_robotiq/package.xml +++ b/costar_predicator/predicator_robotiq/package.xml @@ -42,8 +42,8 @@ catkin predicator_core predicator_msgs - robotiq_c_model_control - robotiq_s_model_control + predicator_core predicator_msgs robotiq_c_model_control @@ -55,4 +55,4 @@ - \ No newline at end of file + diff --git a/install_kinetic.sh b/install_kinetic.sh index 2bf59ea0..92e5e55e 100644 --- a/install_kinetic.sh +++ b/install_kinetic.sh @@ -17,7 +17,8 @@ git clone https://github.com/jbohren/rqt_dot.git git clone https://github.com/sniekum/ar_track_alvar.git --branch $ROS_DISTRO-devel git clone https://github.com/gt-ros-pkg/hrl-kdl.git git clone https://github.com/xqms/ur_modern_driver.git --branch thread_safety -git clone https://github.com/ros-planning/moveit --branch $ROS_DISTRO-devel +#git clone https://github.com/ros-planning/moveit --branch $ROS_DISTRO-devel +#git clone https://github.com/flexible-collision-library/fcl.git rosdep install -y --from-paths ./ --ignore-src --rosdistro $ROS_DISTRO echo "Ignore COSTAR_PERCEPTION until you have installed its dependencies." touch costar_stack/costar_perception/CATKIN_IGNORE From fc04a95213c53522d14bb3cb4c37274faf834a4f Mon Sep 17 00:00:00 2001 From: Baichuan Jiang Date: Tue, 20 Jun 2017 15:02:03 -0400 Subject: [PATCH 04/25] bring in fixes to costar dvrk --- costar_bringup/launch/config/dvrk.launch | 5 + .../smart_waypoint_manager/CMakeLists.txt | 190 +++++++++++++++ .../smart_waypoint_manager/package.xml | 60 +++++ .../scripts/test_smart_waypoint_manager.py | 16 ++ costar_tools/smart_waypoint_manager/setup.py | 13 ++ .../src/smart_waypoint_manager/__init__.py | 1 + .../smart_waypoint_manager.py | 217 ++++++++++++++++++ 7 files changed, 502 insertions(+) create mode 100644 costar_tools/smart_waypoint_manager/CMakeLists.txt create mode 100644 costar_tools/smart_waypoint_manager/package.xml create mode 100755 costar_tools/smart_waypoint_manager/scripts/test_smart_waypoint_manager.py create mode 100644 costar_tools/smart_waypoint_manager/setup.py create mode 100644 costar_tools/smart_waypoint_manager/src/smart_waypoint_manager/__init__.py create mode 100644 costar_tools/smart_waypoint_manager/src/smart_waypoint_manager/smart_waypoint_manager.py diff --git a/costar_bringup/launch/config/dvrk.launch b/costar_bringup/launch/config/dvrk.launch index 12e88c46..d401e8c5 100644 --- a/costar_bringup/launch/config/dvrk.launch +++ b/costar_bringup/launch/config/dvrk.launch @@ -8,6 +8,11 @@ 6 PSM1_psm_base_link PSM1_tool_tip_link_virtual + [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] + 1.0 + 1.0 + true + true diff --git a/costar_tools/smart_waypoint_manager/CMakeLists.txt b/costar_tools/smart_waypoint_manager/CMakeLists.txt new file mode 100644 index 00000000..1d09d7ea --- /dev/null +++ b/costar_tools/smart_waypoint_manager/CMakeLists.txt @@ -0,0 +1,190 @@ +cmake_minimum_required(VERSION 2.8.3) +project(smart_waypoint_manager) + +## 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 + librarian_core + librarian_msgs + predicator_core + predicator_landmark + predicator_msgs +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## 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() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a run_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# librarian_msgs# predicator_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a run_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES smart_waypoint_manager +# CATKIN_DEPENDS librarian_core librarian_msgs predicator_core predicator_landmark predicator_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +# include_directories(include) +include_directories( + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(smart_waypoint_manager +# src/${PROJECT_NAME}/smart_waypoint_manager.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(smart_waypoint_manager ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +# add_executable(smart_waypoint_manager_node src/smart_waypoint_manager_node.cpp) + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(smart_waypoint_manager_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(smart_waypoint_manager_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS smart_waypoint_manager smart_waypoint_manager_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_smart_waypoint_manager.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/costar_tools/smart_waypoint_manager/package.xml b/costar_tools/smart_waypoint_manager/package.xml new file mode 100644 index 00000000..888757ce --- /dev/null +++ b/costar_tools/smart_waypoint_manager/package.xml @@ -0,0 +1,60 @@ + + + smart_waypoint_manager + 0.0.0 + The smart_waypoint_manager package + + + + + cpaxton + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + librarian_core + librarian_msgs + predicator_core + predicator_landmark + predicator_msgs + librarian_core + librarian_msgs + predicator_core + predicator_landmark + predicator_msgs + + + + + + + + \ No newline at end of file diff --git a/costar_tools/smart_waypoint_manager/scripts/test_smart_waypoint_manager.py b/costar_tools/smart_waypoint_manager/scripts/test_smart_waypoint_manager.py new file mode 100755 index 00000000..4d8c6dd4 --- /dev/null +++ b/costar_tools/smart_waypoint_manager/scripts/test_smart_waypoint_manager.py @@ -0,0 +1,16 @@ +#!/usr/bin/env python + +import rospy +from smart_waypoint_manager import * + +rospy.init_node('test_smartmove_waypoints') +swm = SmartWaypointManager() + +swm.load_all() + +rate = rospy.Rate(10) +while not rospy.is_shutdown(): + swm.update_tf() + #print swm.get_new_waypoint('gbeam_link_1/gbeam_link') + print swm.get_new_waypoint('Obj::link_uniform::1') + rate.sleep() diff --git a/costar_tools/smart_waypoint_manager/setup.py b/costar_tools/smart_waypoint_manager/setup.py new file mode 100644 index 00000000..34b116ae --- /dev/null +++ b/costar_tools/smart_waypoint_manager/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=['smart_waypoint_manager'], + package_dir={'': 'src'}, +) + +setup(**d) + diff --git a/costar_tools/smart_waypoint_manager/src/smart_waypoint_manager/__init__.py b/costar_tools/smart_waypoint_manager/src/smart_waypoint_manager/__init__.py new file mode 100644 index 00000000..3184bc72 --- /dev/null +++ b/costar_tools/smart_waypoint_manager/src/smart_waypoint_manager/__init__.py @@ -0,0 +1 @@ +from smart_waypoint_manager import * diff --git a/costar_tools/smart_waypoint_manager/src/smart_waypoint_manager/smart_waypoint_manager.py b/costar_tools/smart_waypoint_manager/src/smart_waypoint_manager/smart_waypoint_manager.py new file mode 100644 index 00000000..19a9fc60 --- /dev/null +++ b/costar_tools/smart_waypoint_manager/src/smart_waypoint_manager/smart_waypoint_manager.py @@ -0,0 +1,217 @@ +import rospy +import yaml +import tf +import tf_conversions.posemath as pm + +from predicator_msgs.srv import * +from predicator_msgs.msg import * + +from librarian_msgs.srv import * +from librarian_msgs.msg import * + +from costar_objrec_msgs.msg import * +from costar_robot_msgs.srv import * + +from predicator_landmark import GetWaypointsService + +''' +SmartWaypointManager +This class will create and load smart waypoints from the $LIBRARIAN_HOME/smart_waypoints directory +It also provides ways to use and access these methods +- stores smartmove waypoints with associated class info +- stores joint space waypoints +- stores a set of 6dof cartesian waypoints (for now in the world frame) +''' +class SmartWaypointManager: + + + def __init__(self,world="world",ns="",endpoint="/endpoint", listener=None, broadcaster=None): + self.get_waypoints_srv = GetWaypointsService(world=world,service=False) + + rospy.loginfo("[SmartMove] Waiting for LIBRARIAN to handle file I/O...") + rospy.wait_for_service('/librarian/add_type',5) + self.add_type_service = rospy.ServiceProxy('/librarian/add_type', librarian_msgs.srv.AddType) + self.save_service = rospy.ServiceProxy('/librarian/save', librarian_msgs.srv.Save) + self.load_service = rospy.ServiceProxy('/librarian/load', librarian_msgs.srv.Load) + self.list_service = rospy.ServiceProxy('/librarian/list', librarian_msgs.srv.List) + self.delete_service = rospy.ServiceProxy('/librarian/delete', librarian_msgs.srv.Delete) + + self.get_assignment_service = rospy.ServiceProxy('/predicator/get_assignment', predicator_msgs.srv.GetAssignment) + + self.detected_objects = rospy.Subscriber(ns + '/detected_object_list', DetectedObjectList, self.detected_objects_cb) + + if not broadcaster is None: + self.broadcaster = broadcaster + else: + self.broadcaster = tf.TransformBroadcaster() + + if not listener is None: + self.listener = listener + else: + self.listener = tf.TransformListener() + + self.world = world + self.endpoint = endpoint + + self.folder = 'smartmove_waypoint' + self.info_folder = 'smartmove_info' + self.add_type_service(self.folder) + + self.waypoints = {} + self.waypoint_names = {} + + self.all_moves = [] + + self.objs = [] + self.obj_classes = [] + self.obj_class = {} + + self.available_object_classes = None + self.available_regions = None + self.available_references = None + + self.add_type_service(self.info_folder) + self.available_obj_classes = yaml.load(self.load_service(type="smartmove_info",id="obj_classes").text) + self.available_regions = yaml.load(self.load_service(type="smartmove_info",id="regions").text) + self.available_references = yaml.load(self.load_service(type="smartmove_info",id="references").text) + rospy.logwarn("[SMARTMOVE] Available classes = " + str(self.available_obj_classes)) + + ''' + reads in costar object detection messages + this includes symmetry information produced by the vision pipeline + ''' + def detected_objects_cb(self,msg): + + for obj in msg.objects: + if not obj.id in self.objs: + self.objs.append(obj.id) + if not obj.object_class in self.obj_classes: + self.objs.append(obj.object_class) + self.obj_class[obj.id] = obj.object_class + + + print self.objs + print self.obj_classes + + ''' + get all waypoints from the disk + now including some joint space waypoints + ''' + def load_all(self): + + self.waypoints = {} + self.waypoint_names = {} + self.all_moves = [] + + ''' + this section loads class and pose information for computing smartmoves + ''' + + waypoint_filenames = self.list_service(self.folder).entries + + for name in waypoint_filenames: + data = yaml.load(self.load_service(id=name,type=self.folder).text) + if not data[1] in self.waypoints.keys(): + self.waypoints[data[1]] = [] + self.waypoint_names[data[1]] = [] + + self.waypoints[data[1]].append(data[0]) + self.waypoint_names[data[1]].append(name) + self.all_moves.append(data[1] + "/" + name) + + def lookup_waypoint(self,obj_class,name): + rospy.logwarn("looking for %s"%name) + rospy.logwarn(self.waypoints) + rospy.logwarn(self.waypoint_names) + return self.waypoints[obj_class][self.waypoint_names[obj_class].index(name)] + + def get_reference_frames(self): + return self.available_references + + def get_available_predicates(self): + return self.available_regions + + def get_all_moves(self): + return self.all_moves + + def get_detected_objects(self): + self.objs = [] + self.obj_classes = [] + rospy.logwarn("Available object classes: " + str(self.available_obj_classes)) + if not self.available_obj_classes is None: + rospy.logwarn("- not none") + for oc in self.available_obj_classes: + rospy.logwarn("-- %s"%oc) + resp = self.get_assignment_service(PredicateStatement(predicate=oc,params=["*","",""])) + oc_objs = [p.params[0] for p in resp.values] + if len(oc_objs) > 0: + self.obj_classes.append(oc) + for obj in oc_objs: + self.obj_class[obj] = oc + rospy.logwarn("%s = %s"%(obj,oc)) + self.objs += oc_objs + + return self.objs + + def delete(self,move): + self.delete_service(id=move.strip('/'),type=self.folder) + + def get_detected_object_classes(self): + self.get_detected_objects() + return self.obj_classes + + def get_available_object_classes(self): + return self.available_object_classes + + def save_new_waypoint(self,obj,name): + pose = self.get_new_waypoint(obj) + if not pose is None: + self.save_service(id=name.strip('/'),type=self.folder,text=yaml.dump([pose,self.obj_class[obj]])) + + def get_new_waypoint(self,obj): + try: + (trans,rot) = self.listener.lookupTransform(obj,self.endpoint,rospy.Time(0)) + return pm.toMsg(pm.fromTf((trans,rot))) + except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): + rospy.logerr('Failed to lookup transform from %s to %s'%(obj,self.endpoint)) + return None + + def get_moves_for_class(self,obj_class): + wpts = [] + try: + wpts = self.waypoint_names[obj_class] + except KeyError: + wpts = [] + return wpts + + def get_moves_for_object(self,obj): + try: + print "obj class = %s"%self.obj_class[obj] + except KeyError: + rospy.logerr("Could not find object %s! Are you sure this is a valid object?"%obj) + + wpts = [] + try: + wpts = self.waypoint_names[self.obj_class[obj]] + except KeyError: + wpts = [] + return wpts + + def publish_tf(self): + + for key in self.waypoints.keys(): + (poses,names) = self.get_waypoints_srv.get_waypoints(key,[],self.waypoints[key],self.waypoint_names[key]) + for (pose,name) in zip(poses,names): + (trans,rot) = pm.toTf(pm.fromMsg(pose)) + self.broadcaster.sendTransform(trans,rot,rospy.Time.now(),name,self.world) + #print (trans, rot, name) + + ''' + Send any non-smart waypoints we might be managing. + ''' + def publish_cartesian_waypoints(self): + + for (name,pose) in self.cart_waypoints: + (trans,rot) = pm.toTf(pm.fromMsg(pose)) + self.broadcaster.sendTransform(trans,rot,rospy.Time.now(),name,self.world) + From 56d89569bd5eb32b72ff630d7a3656abd5431377 Mon Sep 17 00:00:00 2001 From: Baichuan Jiang Date: Thu, 22 Jun 2017 09:31:51 -0400 Subject: [PATCH 05/25] Add CATKIN_IGNORE to packages not compilable in kinetic. --- .../gripper_dvrk}/CMakeLists.txt | 43 ++-- .../gripper_dvrk}/package.xml | 30 +-- .../gripper_dvrk}/setup.py | 1 - .../gripper_dvrk/src/gripper_dvrk/__init__.py | 7 + costar_gripper/gripper_robotiq/CATKIN_IGNORE | 0 .../instructor_action_smartmove.py | 3 +- .../smartmove_multipurpose.py | 3 +- costar_perception/sp_segmenter/CATKIN_IGNORE | 0 costar_robot/costar_dmp/FETCH_HEAD | 0 costar_robot/costar_dmp/docs/development.md | 0 costar_robot/costar_dmp/docs/install.md | 0 costar_robot/costar_dmp/docs/startup.md | 0 .../simple_handeye_calibration/CATKIN_IGNORE | 0 .../alvar_data_collection/CATKIN_IGNORE | 0 .../scripts/test_smart_waypoint_manager.py | 16 -- .../src/smart_waypoint_manager/__init__.py | 1 - .../smart_waypoint_manager.py | 217 ------------------ 17 files changed, 47 insertions(+), 274 deletions(-) rename {costar_tools/smart_waypoint_manager => costar_gripper/gripper_dvrk}/CMakeLists.txt (81%) rename {costar_tools/smart_waypoint_manager => costar_gripper/gripper_dvrk}/package.xml (61%) rename {costar_tools/smart_waypoint_manager => costar_gripper/gripper_dvrk}/setup.py (86%) create mode 100644 costar_gripper/gripper_dvrk/src/gripper_dvrk/__init__.py create mode 100644 costar_gripper/gripper_robotiq/CATKIN_IGNORE create mode 100644 costar_perception/sp_segmenter/CATKIN_IGNORE create mode 100644 costar_robot/costar_dmp/FETCH_HEAD create mode 100644 costar_robot/costar_dmp/docs/development.md create mode 100644 costar_robot/costar_dmp/docs/install.md create mode 100644 costar_robot/costar_dmp/docs/startup.md create mode 100644 costar_robot/simple_handeye_calibration/CATKIN_IGNORE create mode 100644 costar_tools/alvar_data_collection/CATKIN_IGNORE delete mode 100755 costar_tools/smart_waypoint_manager/scripts/test_smart_waypoint_manager.py delete mode 100644 costar_tools/smart_waypoint_manager/src/smart_waypoint_manager/__init__.py delete mode 100644 costar_tools/smart_waypoint_manager/src/smart_waypoint_manager/smart_waypoint_manager.py diff --git a/costar_tools/smart_waypoint_manager/CMakeLists.txt b/costar_gripper/gripper_dvrk/CMakeLists.txt similarity index 81% rename from costar_tools/smart_waypoint_manager/CMakeLists.txt rename to costar_gripper/gripper_dvrk/CMakeLists.txt index 1d09d7ea..bd986ef1 100644 --- a/costar_tools/smart_waypoint_manager/CMakeLists.txt +++ b/costar_gripper/gripper_dvrk/CMakeLists.txt @@ -1,15 +1,14 @@ cmake_minimum_required(VERSION 2.8.3) -project(smart_waypoint_manager) +project(gripper_dvrk) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) ## 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 - librarian_core - librarian_msgs - predicator_core - predicator_landmark - predicator_msgs + gripper_manager ) ## System dependencies are found with CMake's conventions @@ -69,7 +68,7 @@ catkin_python_setup() ## Generate added messages and services with any dependencies listed here # generate_messages( # DEPENDENCIES -# librarian_msgs# predicator_msgs +# std_msgs # Or other packages containing msgs # ) ################################################ @@ -103,8 +102,8 @@ catkin_python_setup() ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( # INCLUDE_DIRS include -# LIBRARIES smart_waypoint_manager -# CATKIN_DEPENDS librarian_core librarian_msgs predicator_core predicator_landmark predicator_msgs +# LIBRARIES gripper_dvrk +# CATKIN_DEPENDS gripper_manager # DEPENDS system_lib ) @@ -114,30 +113,38 @@ catkin_package( ## Specify additional locations of header files ## Your package locations should be listed before other locations -# include_directories(include) include_directories( +# include ${catkin_INCLUDE_DIRS} ) ## Declare a C++ library -# add_library(smart_waypoint_manager -# src/${PROJECT_NAME}/smart_waypoint_manager.cpp +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/gripper_dvrk.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(smart_waypoint_manager ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ## Declare a C++ executable -# add_executable(smart_waypoint_manager_node src/smart_waypoint_manager_node.cpp) +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/gripper_dvrk_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") ## Add cmake target dependencies of the executable ## same as for the library above -# add_dependencies(smart_waypoint_manager_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ## Specify libraries to link a library or executable target against -# target_link_libraries(smart_waypoint_manager_node +# target_link_libraries(${PROJECT_NAME}_node # ${catkin_LIBRARIES} # ) @@ -156,7 +163,7 @@ include_directories( # ) ## Mark executables and/or libraries for installation -# install(TARGETS smart_waypoint_manager smart_waypoint_manager_node +# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} @@ -181,7 +188,7 @@ include_directories( ############# ## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_smart_waypoint_manager.cpp) +# catkin_add_gtest(${PROJECT_NAME}-test test/test_gripper_dvrk.cpp) # if(TARGET ${PROJECT_NAME}-test) # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) # endif() diff --git a/costar_tools/smart_waypoint_manager/package.xml b/costar_gripper/gripper_dvrk/package.xml similarity index 61% rename from costar_tools/smart_waypoint_manager/package.xml rename to costar_gripper/gripper_dvrk/package.xml index 888757ce..de2726b0 100644 --- a/costar_tools/smart_waypoint_manager/package.xml +++ b/costar_gripper/gripper_dvrk/package.xml @@ -1,13 +1,13 @@ - smart_waypoint_manager + gripper_dvrk 0.0.0 - The smart_waypoint_manager package + The gripper_dvrk package - + - cpaxton + ziri @@ -16,14 +16,14 @@ TODO - + - + - - + + @@ -40,16 +40,8 @@ catkin - librarian_core - librarian_msgs - predicator_core - predicator_landmark - predicator_msgs - librarian_core - librarian_msgs - predicator_core - predicator_landmark - predicator_msgs + gripper_manager + gripper_manager @@ -57,4 +49,4 @@ - \ No newline at end of file + diff --git a/costar_tools/smart_waypoint_manager/setup.py b/costar_gripper/gripper_dvrk/setup.py similarity index 86% rename from costar_tools/smart_waypoint_manager/setup.py rename to costar_gripper/gripper_dvrk/setup.py index 34b116ae..31657820 100644 --- a/costar_tools/smart_waypoint_manager/setup.py +++ b/costar_gripper/gripper_dvrk/setup.py @@ -5,7 +5,6 @@ d = generate_distutils_setup( ## don't do this unless you want a globally visible script - packages=['smart_waypoint_manager'], package_dir={'': 'src'}, ) diff --git a/costar_gripper/gripper_dvrk/src/gripper_dvrk/__init__.py b/costar_gripper/gripper_dvrk/src/gripper_dvrk/__init__.py new file mode 100644 index 00000000..333ec931 --- /dev/null +++ b/costar_gripper/gripper_dvrk/src/gripper_dvrk/__init__.py @@ -0,0 +1,7 @@ +### ROS imports +import rospy + +# __all__ = ['SimpleSModelServer','SimpleCModelServer'] + +# from s_model_server import * +# from c_model_server import * diff --git a/costar_gripper/gripper_robotiq/CATKIN_IGNORE b/costar_gripper/gripper_robotiq/CATKIN_IGNORE new file mode 100644 index 00000000..e69de29b diff --git a/costar_instructor/instructor_plugins/src/instructor_plugins/instructor_action_smartmove.py b/costar_instructor/instructor_plugins/src/instructor_plugins/instructor_action_smartmove.py index 63cc7c67..018eabea 100644 --- a/costar_instructor/instructor_plugins/src/instructor_plugins/instructor_action_smartmove.py +++ b/costar_instructor/instructor_plugins/src/instructor_plugins/instructor_action_smartmove.py @@ -126,9 +126,10 @@ def update_references(self): self.selected_reference = str(self.waypoint_ui.region_list.currentItem().text()) def update_objects(self): - objects = [] objects = self.manager.get_available_object_classes() self.waypoint_ui.object_list.clear() + if objects is None: + return for m in objects: self.waypoint_ui.object_list.addItem(QListWidgetItem(m.strip('/'))) self.waypoint_ui.object_list.sortItems() diff --git a/costar_instructor/instructor_plugins/src/instructor_plugins/smartmove_multipurpose.py b/costar_instructor/instructor_plugins/src/instructor_plugins/smartmove_multipurpose.py index 49b9b243..d36ecf18 100644 --- a/costar_instructor/instructor_plugins/src/instructor_plugins/smartmove_multipurpose.py +++ b/costar_instructor/instructor_plugins/src/instructor_plugins/smartmove_multipurpose.py @@ -135,9 +135,10 @@ def update_references(self): self.selected_reference = str(self.waypoint_ui.reference_list.currentItem().text()) def update_objects(self): - objects = [] objects = self.manager.get_available_object_classes() self.waypoint_ui.object_list.clear() + if objects is None: + return for m in objects: self.waypoint_ui.object_list.addItem(QListWidgetItem(m.strip('/'))) self.waypoint_ui.object_list.sortItems() diff --git a/costar_perception/sp_segmenter/CATKIN_IGNORE b/costar_perception/sp_segmenter/CATKIN_IGNORE new file mode 100644 index 00000000..e69de29b diff --git a/costar_robot/costar_dmp/FETCH_HEAD b/costar_robot/costar_dmp/FETCH_HEAD new file mode 100644 index 00000000..e69de29b diff --git a/costar_robot/costar_dmp/docs/development.md b/costar_robot/costar_dmp/docs/development.md new file mode 100644 index 00000000..e69de29b diff --git a/costar_robot/costar_dmp/docs/install.md b/costar_robot/costar_dmp/docs/install.md new file mode 100644 index 00000000..e69de29b diff --git a/costar_robot/costar_dmp/docs/startup.md b/costar_robot/costar_dmp/docs/startup.md new file mode 100644 index 00000000..e69de29b diff --git a/costar_robot/simple_handeye_calibration/CATKIN_IGNORE b/costar_robot/simple_handeye_calibration/CATKIN_IGNORE new file mode 100644 index 00000000..e69de29b diff --git a/costar_tools/alvar_data_collection/CATKIN_IGNORE b/costar_tools/alvar_data_collection/CATKIN_IGNORE new file mode 100644 index 00000000..e69de29b diff --git a/costar_tools/smart_waypoint_manager/scripts/test_smart_waypoint_manager.py b/costar_tools/smart_waypoint_manager/scripts/test_smart_waypoint_manager.py deleted file mode 100755 index 4d8c6dd4..00000000 --- a/costar_tools/smart_waypoint_manager/scripts/test_smart_waypoint_manager.py +++ /dev/null @@ -1,16 +0,0 @@ -#!/usr/bin/env python - -import rospy -from smart_waypoint_manager import * - -rospy.init_node('test_smartmove_waypoints') -swm = SmartWaypointManager() - -swm.load_all() - -rate = rospy.Rate(10) -while not rospy.is_shutdown(): - swm.update_tf() - #print swm.get_new_waypoint('gbeam_link_1/gbeam_link') - print swm.get_new_waypoint('Obj::link_uniform::1') - rate.sleep() diff --git a/costar_tools/smart_waypoint_manager/src/smart_waypoint_manager/__init__.py b/costar_tools/smart_waypoint_manager/src/smart_waypoint_manager/__init__.py deleted file mode 100644 index 3184bc72..00000000 --- a/costar_tools/smart_waypoint_manager/src/smart_waypoint_manager/__init__.py +++ /dev/null @@ -1 +0,0 @@ -from smart_waypoint_manager import * diff --git a/costar_tools/smart_waypoint_manager/src/smart_waypoint_manager/smart_waypoint_manager.py b/costar_tools/smart_waypoint_manager/src/smart_waypoint_manager/smart_waypoint_manager.py deleted file mode 100644 index 19a9fc60..00000000 --- a/costar_tools/smart_waypoint_manager/src/smart_waypoint_manager/smart_waypoint_manager.py +++ /dev/null @@ -1,217 +0,0 @@ -import rospy -import yaml -import tf -import tf_conversions.posemath as pm - -from predicator_msgs.srv import * -from predicator_msgs.msg import * - -from librarian_msgs.srv import * -from librarian_msgs.msg import * - -from costar_objrec_msgs.msg import * -from costar_robot_msgs.srv import * - -from predicator_landmark import GetWaypointsService - -''' -SmartWaypointManager -This class will create and load smart waypoints from the $LIBRARIAN_HOME/smart_waypoints directory -It also provides ways to use and access these methods -- stores smartmove waypoints with associated class info -- stores joint space waypoints -- stores a set of 6dof cartesian waypoints (for now in the world frame) -''' -class SmartWaypointManager: - - - def __init__(self,world="world",ns="",endpoint="/endpoint", listener=None, broadcaster=None): - self.get_waypoints_srv = GetWaypointsService(world=world,service=False) - - rospy.loginfo("[SmartMove] Waiting for LIBRARIAN to handle file I/O...") - rospy.wait_for_service('/librarian/add_type',5) - self.add_type_service = rospy.ServiceProxy('/librarian/add_type', librarian_msgs.srv.AddType) - self.save_service = rospy.ServiceProxy('/librarian/save', librarian_msgs.srv.Save) - self.load_service = rospy.ServiceProxy('/librarian/load', librarian_msgs.srv.Load) - self.list_service = rospy.ServiceProxy('/librarian/list', librarian_msgs.srv.List) - self.delete_service = rospy.ServiceProxy('/librarian/delete', librarian_msgs.srv.Delete) - - self.get_assignment_service = rospy.ServiceProxy('/predicator/get_assignment', predicator_msgs.srv.GetAssignment) - - self.detected_objects = rospy.Subscriber(ns + '/detected_object_list', DetectedObjectList, self.detected_objects_cb) - - if not broadcaster is None: - self.broadcaster = broadcaster - else: - self.broadcaster = tf.TransformBroadcaster() - - if not listener is None: - self.listener = listener - else: - self.listener = tf.TransformListener() - - self.world = world - self.endpoint = endpoint - - self.folder = 'smartmove_waypoint' - self.info_folder = 'smartmove_info' - self.add_type_service(self.folder) - - self.waypoints = {} - self.waypoint_names = {} - - self.all_moves = [] - - self.objs = [] - self.obj_classes = [] - self.obj_class = {} - - self.available_object_classes = None - self.available_regions = None - self.available_references = None - - self.add_type_service(self.info_folder) - self.available_obj_classes = yaml.load(self.load_service(type="smartmove_info",id="obj_classes").text) - self.available_regions = yaml.load(self.load_service(type="smartmove_info",id="regions").text) - self.available_references = yaml.load(self.load_service(type="smartmove_info",id="references").text) - rospy.logwarn("[SMARTMOVE] Available classes = " + str(self.available_obj_classes)) - - ''' - reads in costar object detection messages - this includes symmetry information produced by the vision pipeline - ''' - def detected_objects_cb(self,msg): - - for obj in msg.objects: - if not obj.id in self.objs: - self.objs.append(obj.id) - if not obj.object_class in self.obj_classes: - self.objs.append(obj.object_class) - self.obj_class[obj.id] = obj.object_class - - - print self.objs - print self.obj_classes - - ''' - get all waypoints from the disk - now including some joint space waypoints - ''' - def load_all(self): - - self.waypoints = {} - self.waypoint_names = {} - self.all_moves = [] - - ''' - this section loads class and pose information for computing smartmoves - ''' - - waypoint_filenames = self.list_service(self.folder).entries - - for name in waypoint_filenames: - data = yaml.load(self.load_service(id=name,type=self.folder).text) - if not data[1] in self.waypoints.keys(): - self.waypoints[data[1]] = [] - self.waypoint_names[data[1]] = [] - - self.waypoints[data[1]].append(data[0]) - self.waypoint_names[data[1]].append(name) - self.all_moves.append(data[1] + "/" + name) - - def lookup_waypoint(self,obj_class,name): - rospy.logwarn("looking for %s"%name) - rospy.logwarn(self.waypoints) - rospy.logwarn(self.waypoint_names) - return self.waypoints[obj_class][self.waypoint_names[obj_class].index(name)] - - def get_reference_frames(self): - return self.available_references - - def get_available_predicates(self): - return self.available_regions - - def get_all_moves(self): - return self.all_moves - - def get_detected_objects(self): - self.objs = [] - self.obj_classes = [] - rospy.logwarn("Available object classes: " + str(self.available_obj_classes)) - if not self.available_obj_classes is None: - rospy.logwarn("- not none") - for oc in self.available_obj_classes: - rospy.logwarn("-- %s"%oc) - resp = self.get_assignment_service(PredicateStatement(predicate=oc,params=["*","",""])) - oc_objs = [p.params[0] for p in resp.values] - if len(oc_objs) > 0: - self.obj_classes.append(oc) - for obj in oc_objs: - self.obj_class[obj] = oc - rospy.logwarn("%s = %s"%(obj,oc)) - self.objs += oc_objs - - return self.objs - - def delete(self,move): - self.delete_service(id=move.strip('/'),type=self.folder) - - def get_detected_object_classes(self): - self.get_detected_objects() - return self.obj_classes - - def get_available_object_classes(self): - return self.available_object_classes - - def save_new_waypoint(self,obj,name): - pose = self.get_new_waypoint(obj) - if not pose is None: - self.save_service(id=name.strip('/'),type=self.folder,text=yaml.dump([pose,self.obj_class[obj]])) - - def get_new_waypoint(self,obj): - try: - (trans,rot) = self.listener.lookupTransform(obj,self.endpoint,rospy.Time(0)) - return pm.toMsg(pm.fromTf((trans,rot))) - except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): - rospy.logerr('Failed to lookup transform from %s to %s'%(obj,self.endpoint)) - return None - - def get_moves_for_class(self,obj_class): - wpts = [] - try: - wpts = self.waypoint_names[obj_class] - except KeyError: - wpts = [] - return wpts - - def get_moves_for_object(self,obj): - try: - print "obj class = %s"%self.obj_class[obj] - except KeyError: - rospy.logerr("Could not find object %s! Are you sure this is a valid object?"%obj) - - wpts = [] - try: - wpts = self.waypoint_names[self.obj_class[obj]] - except KeyError: - wpts = [] - return wpts - - def publish_tf(self): - - for key in self.waypoints.keys(): - (poses,names) = self.get_waypoints_srv.get_waypoints(key,[],self.waypoints[key],self.waypoint_names[key]) - for (pose,name) in zip(poses,names): - (trans,rot) = pm.toTf(pm.fromMsg(pose)) - self.broadcaster.sendTransform(trans,rot,rospy.Time.now(),name,self.world) - #print (trans, rot, name) - - ''' - Send any non-smart waypoints we might be managing. - ''' - def publish_cartesian_waypoints(self): - - for (name,pose) in self.cart_waypoints: - (trans,rot) = pm.toTf(pm.fromMsg(pose)) - self.broadcaster.sendTransform(trans,rot,rospy.Time.now(),name,self.world) - From bdfc97eedce475660245979f5405c16cd59a1f0c Mon Sep 17 00:00:00 2001 From: Baichuan Jiang Date: Thu, 22 Jun 2017 10:42:50 -0400 Subject: [PATCH 06/25] modify install files --- install_indigo.sh | 1 + install_kinetic.sh | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/install_indigo.sh b/install_indigo.sh index fdbebda3..176029f4 100755 --- a/install_indigo.sh +++ b/install_indigo.sh @@ -13,6 +13,7 @@ git clone https://github.com/cpaxton/costar_stack.git git clone https://github.com/SalvoVirga/iiwa_stack.git git clone https://github.com/ros-industrial/robotiq.git git clone https://github.com/jbohren/rqt_dot.git +git clone https://github.com/cpaxton/dmp.git --branch indigo git clone https://github.com/sniekum/ar_track_alvar.git --branch indigo-devel git clone https://github.com/sniekum/ar_track_alvar_msgs.git git clone https://github.com/gt-ros-pkg/hrl-kdl.git diff --git a/install_kinetic.sh b/install_kinetic.sh index 92e5e55e..73d1e0e8 100644 --- a/install_kinetic.sh +++ b/install_kinetic.sh @@ -16,7 +16,8 @@ git clone https://github.com/ros-industrial/robotiq.git git clone https://github.com/jbohren/rqt_dot.git git clone https://github.com/sniekum/ar_track_alvar.git --branch $ROS_DISTRO-devel git clone https://github.com/gt-ros-pkg/hrl-kdl.git -git clone https://github.com/xqms/ur_modern_driver.git --branch thread_safety +g +it clone https://github.com/xqms/ur_modern_driver.git --branch thread_safety #git clone https://github.com/ros-planning/moveit --branch $ROS_DISTRO-devel #git clone https://github.com/flexible-collision-library/fcl.git rosdep install -y --from-paths ./ --ignore-src --rosdistro $ROS_DISTRO From 71a4e487bac38c18b0b66facce078d9f87b3f0bf Mon Sep 17 00:00:00 2001 From: Baichuan Jiang Date: Tue, 27 Jun 2017 15:30:32 -0400 Subject: [PATCH 07/25] Solved the psm gripper issue and robot not-connected issue. --- costar_bringup/launch/config/dvrk.launch | 3 +- costar_bringup/launch/dvrk_psm.launch | 3 + .../src/costar_component/component.py | 4 +- .../gripper_dvrk/scripts/psm_gripper.py | 12 +++ costar_gripper/gripper_dvrk/setup.py | 1 + .../gripper_dvrk/src/gripper_dvrk/__init__.py | 2 +- .../src/gripper_dvrk/psm_gripper_server.py | 84 +++++++++++++++++++ .../src/gripper_manager/costar_gripper.py | 25 ++++-- .../src/costar_robot/costar_arm.py | 4 +- 9 files changed, 123 insertions(+), 15 deletions(-) create mode 100755 costar_gripper/gripper_dvrk/scripts/psm_gripper.py create mode 100644 costar_gripper/gripper_dvrk/src/gripper_dvrk/psm_gripper_server.py diff --git a/costar_bringup/launch/config/dvrk.launch b/costar_bringup/launch/config/dvrk.launch index d401e8c5..7633b066 100644 --- a/costar_bringup/launch/config/dvrk.launch +++ b/costar_bringup/launch/config/dvrk.launch @@ -4,14 +4,13 @@ [0.30, -1.33, -1.80, -0.27, 1.50, 1.60] false - false 6 PSM1_psm_base_link PSM1_tool_tip_link_virtual [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] 1.0 1.0 - true + false true diff --git a/costar_bringup/launch/dvrk_psm.launch b/costar_bringup/launch/dvrk_psm.launch index b9e2292d..3457a186 100755 --- a/costar_bringup/launch/dvrk_psm.launch +++ b/costar_bringup/launch/dvrk_psm.launch @@ -19,4 +19,7 @@ + + + diff --git a/costar_component/src/costar_component/component.py b/costar_component/src/costar_component/component.py index d306913a..3130e0bc 100644 --- a/costar_component/src/costar_component/component.py +++ b/costar_component/src/costar_component/component.py @@ -23,9 +23,9 @@ def make_service_proxy(self, name, srv_t, use_namespace=True): service_name = os.path.join(self.namespace, name) else: service_name = name - rospy.logerr("Connecting to service with name: %s"%service_name) + rospy.loginfo("Connecting to service with name: %s"%service_name) rospy.wait_for_service(service_name) - rospy.logerr("Connected to service successfully.") + rospy.loginfo("Connected to service successfully.") return rospy.ServiceProxy(service_name,srv_t) def __init__(self, name, namespace): diff --git a/costar_gripper/gripper_dvrk/scripts/psm_gripper.py b/costar_gripper/gripper_dvrk/scripts/psm_gripper.py new file mode 100755 index 00000000..e2ddc1f2 --- /dev/null +++ b/costar_gripper/gripper_dvrk/scripts/psm_gripper.py @@ -0,0 +1,12 @@ +#!/usr/bin/env python + +import rospy +from gripper_dvrk import SimplePSMGripperServer + +# rospy.init_node("simple_psm_gripper_server") +verbose = rospy.get_param('~verbose',False) +server = SimplePSMGripperServer("/costar/gripper",verbose=verbose) +server.init_gripper() +rospy.sleep(.5) +server.open_gripper() +rospy.spin() \ No newline at end of file diff --git a/costar_gripper/gripper_dvrk/setup.py b/costar_gripper/gripper_dvrk/setup.py index 31657820..f6993b08 100644 --- a/costar_gripper/gripper_dvrk/setup.py +++ b/costar_gripper/gripper_dvrk/setup.py @@ -5,6 +5,7 @@ d = generate_distutils_setup( ## don't do this unless you want a globally visible script + packages=['gripper_dvrk'], package_dir={'': 'src'}, ) diff --git a/costar_gripper/gripper_dvrk/src/gripper_dvrk/__init__.py b/costar_gripper/gripper_dvrk/src/gripper_dvrk/__init__.py index 333ec931..83406535 100644 --- a/costar_gripper/gripper_dvrk/src/gripper_dvrk/__init__.py +++ b/costar_gripper/gripper_dvrk/src/gripper_dvrk/__init__.py @@ -1,6 +1,6 @@ ### ROS imports import rospy - +from psm_gripper_server import * # __all__ = ['SimpleSModelServer','SimpleCModelServer'] # from s_model_server import * diff --git a/costar_gripper/gripper_dvrk/src/gripper_dvrk/psm_gripper_server.py b/costar_gripper/gripper_dvrk/src/gripper_dvrk/psm_gripper_server.py new file mode 100644 index 00000000..e1e9e3e7 --- /dev/null +++ b/costar_gripper/gripper_dvrk/src/gripper_dvrk/psm_gripper_server.py @@ -0,0 +1,84 @@ +# Chris Paxton -- 2016 +# Based on the Robotiq code, but modified to provide a simple service interface for use with our user interface +# Portions of this code (c) Robotiq, Inc.: + +# Software License Agreement (BSD License) +# +# Copyright (c) 2012, Robotiq, Inc. +# All rights 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 Robotiq, Inc. 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 THE +# COPYRIGHT OWNER OR CONTRIBUTORS 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. + +from gripper_manager import CostarGripper +import rospy +import dvrk + +class SimplePSMGripperServer(CostarGripper): + + def __init__(self,ns="/costar/gripper",verbose=False): + + self.dvrk_arm = dvrk.psm('PSM1') + self.psm_initialized = False + + super(SimplePSMGripperServer, self).__init__( + "psm_gripper", + input_topic="PSMGripperInput", + output_topic="PSMGripperOutput", + InputMsgType=None, + OutputMsgType=None, + GripperPredicatorType=None, + ns=ns, + verbose=verbose) + + def init_gripper(self): + if self.psm_initialized != True: + self.dvrk_arm.home() + self.dvrk_arm.insert_tool(0.1) + self.psm_initialized = True + print "PSM Initialized" + return [] + + def open_gripper(self,msg=None): + self.dvrk_arm.open_jaw() + return [] + + def close_gripper(self,msg=None): + self.dvrk_arm.close_jaw() + return [] + + def getDefaultMsg(self): + return [] + + def activate(self,msg=None): + return [] + + def reset(self, msg=None): + return [] + + def statusInterpreter(self,status): + return [] diff --git a/costar_gripper/gripper_manager/src/gripper_manager/costar_gripper.py b/costar_gripper/gripper_manager/src/gripper_manager/costar_gripper.py index 5673d6c3..9b99d15f 100644 --- a/costar_gripper/gripper_manager/src/gripper_manager/costar_gripper.py +++ b/costar_gripper/gripper_manager/src/gripper_manager/costar_gripper.py @@ -11,20 +11,26 @@ def __init__(self, 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 + OutputMsgType, # type 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) + if GripperPredicatorType is not None: + self.predicator = GripperPredicatorType( + start_subscriber=False, + publish_predicates=True, + gripper_name=name) + else: + self.predicator = None + + if InputMsgType is not None: + self.sub = rospy.Subscriber(input_topic, InputMsgType, self.status_cb) + if OutputMsgType is not None: + 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) @@ -71,7 +77,8 @@ def statusInterpreter(self,status): def status_cb(self,msg): if self.verbose: - rospy.loginfo(self.statusInterpreter(msg)) - self.predicator.handle(msg) + rospy.loginfo(self.statusInterpreter(msg)) + if self.predicator is not None: + self.predicator.handle(msg) diff --git a/costar_robot/costar_robot_manager/src/costar_robot/costar_arm.py b/costar_robot/costar_robot_manager/src/costar_robot/costar_arm.py index d450f493..520a61f8 100644 --- a/costar_robot/costar_robot_manager/src/costar_robot/costar_arm.py +++ b/costar_robot/costar_robot_manager/src/costar_robot/costar_arm.py @@ -193,7 +193,8 @@ def __init__(self, has_gripper = rospy.get_param(os.path.join(self.namespace, "robot", "has_gripper")) has_planning_scene = rospy.get_param(os.path.join(self.namespace, "robot", "has_planning_scene")) - + #print has_planning_scene, os.path.join(self.namespace, "robot", "has_planning_scene") + if has_gripper: self.gripper_close = self.make_service_proxy('gripper/close',EmptyService) self.gripper_open = self.make_service_proxy('gripper/open',EmptyService) @@ -602,6 +603,7 @@ def shutdown_arm_cb(self,req): ''' # TODO: Modify this part def handle_tick(self): + #print "tick" br = tf.TransformBroadcaster() br.sendTransform((0,0,0),tf.transformations.quaternion_from_euler(0,0,0),rospy.Time.now(),"/endpoint",self.end_link) if not self.base_link == "base_link": From 054e5fab7f86c40b2036517633ddb8524fe7f4dc Mon Sep 17 00:00:00 2001 From: Baichuan Jiang Date: Thu, 6 Jul 2017 17:09:12 -0400 Subject: [PATCH 08/25] Switch to PSM2 for real system implementation. --- costar_bringup/launch/config/dvrk.launch | 4 +-- costar_bringup/launch/dvrk_psm.launch | 4 +-- costar_bringup/launch/dvrk_real.launch | 25 +++++++++++++++++++ .../src/gripper_dvrk/psm_gripper_server.py | 2 +- .../costar_dmp/docs/dmp_psm_instruction | 2 +- costar_robot/costar_robot_manager/Readme.md | 4 +-- .../launch/simple_psm_driver.launch | 4 +-- .../src/costar_robot/psm_driver.py | 8 +++--- 8 files changed, 39 insertions(+), 14 deletions(-) create mode 100644 costar_bringup/launch/dvrk_real.launch diff --git a/costar_bringup/launch/config/dvrk.launch b/costar_bringup/launch/config/dvrk.launch index 7633b066..90b46c6c 100644 --- a/costar_bringup/launch/config/dvrk.launch +++ b/costar_bringup/launch/config/dvrk.launch @@ -5,8 +5,8 @@ false 6 - PSM1_psm_base_link - PSM1_tool_tip_link_virtual + PSM2_psm_base_link + PSM2_tool_tip_link_virtual [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] 1.0 1.0 diff --git a/costar_bringup/launch/dvrk_psm.launch b/costar_bringup/launch/dvrk_psm.launch index 3457a186..5563d42a 100755 --- a/costar_bringup/launch/dvrk_psm.launch +++ b/costar_bringup/launch/dvrk_psm.launch @@ -1,9 +1,9 @@ - + - + diff --git a/costar_bringup/launch/dvrk_real.launch b/costar_bringup/launch/dvrk_real.launch new file mode 100644 index 00000000..5251fe3a --- /dev/null +++ b/costar_bringup/launch/dvrk_real.launch @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/costar_gripper/gripper_dvrk/src/gripper_dvrk/psm_gripper_server.py b/costar_gripper/gripper_dvrk/src/gripper_dvrk/psm_gripper_server.py index e1e9e3e7..5565e126 100644 --- a/costar_gripper/gripper_dvrk/src/gripper_dvrk/psm_gripper_server.py +++ b/costar_gripper/gripper_dvrk/src/gripper_dvrk/psm_gripper_server.py @@ -42,7 +42,7 @@ class SimplePSMGripperServer(CostarGripper): def __init__(self,ns="/costar/gripper",verbose=False): - self.dvrk_arm = dvrk.psm('PSM1') + self.dvrk_arm = dvrk.psm('PSM2') self.psm_initialized = False super(SimplePSMGripperServer, self).__init__( diff --git a/costar_robot/costar_dmp/docs/dmp_psm_instruction b/costar_robot/costar_dmp/docs/dmp_psm_instruction index 996ae884..59dd6568 100644 --- a/costar_robot/costar_dmp/docs/dmp_psm_instruction +++ b/costar_robot/costar_dmp/docs/dmp_psm_instruction @@ -4,7 +4,7 @@ roslaunch costar_dmp dmp_server.launch > Note: now switch instructor to SERVO mode -rosservice call costar/dmp/start_rec '/PSM1_psm_base_link' +rosservice call costar/dmp/start_rec '/PSM2_psm_base_link' > Note: now servo psm around to collect a trajectory diff --git a/costar_robot/costar_robot_manager/Readme.md b/costar_robot/costar_robot_manager/Readme.md index fa678ed9..2796b8d0 100644 --- a/costar_robot/costar_robot_manager/Readme.md +++ b/costar_robot/costar_robot_manager/Readme.md @@ -15,10 +15,10 @@ This code is the CoSTAR robot manager. It provides simple services for instantia roslaunch instructor_core instructor.launch - To launch dvrk rviz: - roslaunch dvrk_robot dvrk_arm_rviz.launch arm:=PSM1 config:=/PATH/TO/CATKIN_WS/src/cisst-saw/sawIntuitiveResearchKit/share/console-PSM1_KIN_SIMULATED.json + roslaunch dvrk_robot dvrk_arm_rviz.launch arm:=PSM2 config:=/PATH/TO/CATKIN_WS/src/cisst-saw/sawIntuitiveResearchKit/share/console-PSM2_KIN_SIMULATED.json - To launch dvrk console application only: - rosrun dvrk_robot dvrk_console_json -j /PATH/TO/CATKIN_WS/src/cisst-saw/sawIntuitiveResearchKit/share/console-PSM1_KIN_SIMULATED.json + rosrun dvrk_robot dvrk_console_json -j /PATH/TO/CATKIN_WS/src/cisst-saw/sawIntuitiveResearchKit/share/console-PSM2_KIN_SIMULATED.json - To set up the environment for running driver launch script: roslaunch costar_bringup utilities.launch diff --git a/costar_robot/costar_robot_manager/launch/simple_psm_driver.launch b/costar_robot/costar_robot_manager/launch/simple_psm_driver.launch index 75dacff2..2dc40149 100644 --- a/costar_robot/costar_robot_manager/launch/simple_psm_driver.launch +++ b/costar_robot/costar_robot_manager/launch/simple_psm_driver.launch @@ -4,8 +4,8 @@ - - + + diff --git a/costar_robot/costar_robot_manager/src/costar_robot/psm_driver.py b/costar_robot/costar_robot_manager/src/costar_robot/psm_driver.py index 34ac726b..30287e77 100644 --- a/costar_robot/costar_robot_manager/src/costar_robot/psm_driver.py +++ b/costar_robot/costar_robot_manager/src/costar_robot/psm_driver.py @@ -39,11 +39,11 @@ def __init__(self, max_q_diff = 1e-6): # TODO: correct these - base_link = 'PSM1_psm_base_link' - end_link = 'PSM1_tool_wrist_sca_ee_link_0' + base_link = 'PSM2_psm_base_link' + end_link = 'PSM2_tool_wrist_sca_ee_link_0' planning_group = 'manipulator' - self.dvrk_arm = dvrk.psm('PSM1') + self.dvrk_arm = dvrk.psm('PSM2') self.psm_initialized = False rospy.Subscriber("/instructor_marker/feedback", InteractiveMarkerFeedback, self.marker_cbback) @@ -77,7 +77,7 @@ def handle_tick(self): br.sendTransform((0, 0, 0), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), "/base_link", self.base_link) br.sendTransform((0, 0, 0), tf.transformations.quaternion_from_euler(-1.5708, 1.5708, 0), rospy.Time.now(), - "/PSM1_tool_tip_link_virtual", 'PSM1_tool_wrist_sca_ee_link_0') + "/PSM2_tool_tip_link_virtual", 'PSM2_tool_wrist_sca_ee_link_0') # The above: add tip link with the orientation offset to represent the frame of our concern if self.driver_status == 'SHUTDOWN': pass From 02b4975378bbf029de8748611b34a3324a001a1b Mon Sep 17 00:00:00 2001 From: Baichuan Jiang Date: Mon, 10 Jul 2017 20:04:47 -0400 Subject: [PATCH 09/25] Correct PSM2 config file directory in dvrk_real launch file. --- costar_bringup/launch/dvrk_real.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/costar_bringup/launch/dvrk_real.launch b/costar_bringup/launch/dvrk_real.launch index 5251fe3a..eb51cd84 100644 --- a/costar_bringup/launch/dvrk_real.launch +++ b/costar_bringup/launch/dvrk_real.launch @@ -3,7 +3,7 @@ - + From c02d5444ae40074a06b5dfad881c8c65a367bde3 Mon Sep 17 00:00:00 2001 From: Baichuan Jiang Date: Tue, 11 Jul 2017 21:36:59 -0400 Subject: [PATCH 10/25] Switch back to PSM1 due to hardware issue. --- costar_bringup/launch/config/dvrk.launch | 4 ++-- costar_bringup/launch/dvrk_psm.launch | 4 ++-- costar_bringup/launch/dvrk_real.launch | 4 ++-- .../gripper_dvrk/src/gripper_dvrk/psm_gripper_server.py | 2 +- .../costar_robot_manager/launch/simple_psm_driver.launch | 4 ++-- .../costar_robot_manager/src/costar_robot/psm_driver.py | 8 ++++---- 6 files changed, 13 insertions(+), 13 deletions(-) diff --git a/costar_bringup/launch/config/dvrk.launch b/costar_bringup/launch/config/dvrk.launch index 90b46c6c..7633b066 100644 --- a/costar_bringup/launch/config/dvrk.launch +++ b/costar_bringup/launch/config/dvrk.launch @@ -5,8 +5,8 @@ false 6 - PSM2_psm_base_link - PSM2_tool_tip_link_virtual + PSM1_psm_base_link + PSM1_tool_tip_link_virtual [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] 1.0 1.0 diff --git a/costar_bringup/launch/dvrk_psm.launch b/costar_bringup/launch/dvrk_psm.launch index 5563d42a..3457a186 100755 --- a/costar_bringup/launch/dvrk_psm.launch +++ b/costar_bringup/launch/dvrk_psm.launch @@ -1,9 +1,9 @@ - + - + diff --git a/costar_bringup/launch/dvrk_real.launch b/costar_bringup/launch/dvrk_real.launch index eb51cd84..39d1d786 100644 --- a/costar_bringup/launch/dvrk_real.launch +++ b/costar_bringup/launch/dvrk_real.launch @@ -1,9 +1,9 @@ - + - + diff --git a/costar_gripper/gripper_dvrk/src/gripper_dvrk/psm_gripper_server.py b/costar_gripper/gripper_dvrk/src/gripper_dvrk/psm_gripper_server.py index 5565e126..e1e9e3e7 100644 --- a/costar_gripper/gripper_dvrk/src/gripper_dvrk/psm_gripper_server.py +++ b/costar_gripper/gripper_dvrk/src/gripper_dvrk/psm_gripper_server.py @@ -42,7 +42,7 @@ class SimplePSMGripperServer(CostarGripper): def __init__(self,ns="/costar/gripper",verbose=False): - self.dvrk_arm = dvrk.psm('PSM2') + self.dvrk_arm = dvrk.psm('PSM1') self.psm_initialized = False super(SimplePSMGripperServer, self).__init__( diff --git a/costar_robot/costar_robot_manager/launch/simple_psm_driver.launch b/costar_robot/costar_robot_manager/launch/simple_psm_driver.launch index 2dc40149..75dacff2 100644 --- a/costar_robot/costar_robot_manager/launch/simple_psm_driver.launch +++ b/costar_robot/costar_robot_manager/launch/simple_psm_driver.launch @@ -4,8 +4,8 @@ - - + + diff --git a/costar_robot/costar_robot_manager/src/costar_robot/psm_driver.py b/costar_robot/costar_robot_manager/src/costar_robot/psm_driver.py index 30287e77..34ac726b 100644 --- a/costar_robot/costar_robot_manager/src/costar_robot/psm_driver.py +++ b/costar_robot/costar_robot_manager/src/costar_robot/psm_driver.py @@ -39,11 +39,11 @@ def __init__(self, max_q_diff = 1e-6): # TODO: correct these - base_link = 'PSM2_psm_base_link' - end_link = 'PSM2_tool_wrist_sca_ee_link_0' + base_link = 'PSM1_psm_base_link' + end_link = 'PSM1_tool_wrist_sca_ee_link_0' planning_group = 'manipulator' - self.dvrk_arm = dvrk.psm('PSM2') + self.dvrk_arm = dvrk.psm('PSM1') self.psm_initialized = False rospy.Subscriber("/instructor_marker/feedback", InteractiveMarkerFeedback, self.marker_cbback) @@ -77,7 +77,7 @@ def handle_tick(self): br.sendTransform((0, 0, 0), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), "/base_link", self.base_link) br.sendTransform((0, 0, 0), tf.transformations.quaternion_from_euler(-1.5708, 1.5708, 0), rospy.Time.now(), - "/PSM2_tool_tip_link_virtual", 'PSM2_tool_wrist_sca_ee_link_0') + "/PSM1_tool_tip_link_virtual", 'PSM1_tool_wrist_sca_ee_link_0') # The above: add tip link with the orientation offset to represent the frame of our concern if self.driver_status == 'SHUTDOWN': pass From 346348bfe33af4399842b6956a7bf4264aed51f5 Mon Sep 17 00:00:00 2001 From: Baichuan Jiang Date: Thu, 13 Jul 2017 16:34:06 -0400 Subject: [PATCH 11/25] Enabled recording waypoint through foot pedal clutch. --- costar_bringup/launch/dvrk_teleop.launch | 27 +++++++++++++++++++ .../src/costar_robot/psm_driver.py | 24 +++++++++++++++++ 2 files changed, 51 insertions(+) create mode 100644 costar_bringup/launch/dvrk_teleop.launch diff --git a/costar_bringup/launch/dvrk_teleop.launch b/costar_bringup/launch/dvrk_teleop.launch new file mode 100644 index 00000000..3949cbb0 --- /dev/null +++ b/costar_bringup/launch/dvrk_teleop.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/costar_robot/costar_robot_manager/src/costar_robot/psm_driver.py b/costar_robot/costar_robot_manager/src/costar_robot/psm_driver.py index 34ac726b..f0ea7dd5 100644 --- a/costar_robot/costar_robot_manager/src/costar_robot/psm_driver.py +++ b/costar_robot/costar_robot_manager/src/costar_robot/psm_driver.py @@ -6,9 +6,12 @@ from trajectory_msgs.msg import JointTrajectoryPoint from std_srvs.srv import Empty as EmptyService from sensor_msgs.msg import JointState +from sensor_msgs.msg import Joy +from geometry_msgs.msg import Pose from visualization_msgs.msg import * import tf_conversions.posemath as pm import numpy as np +from datetime import datetime import dvrk import PyKDL @@ -24,8 +27,11 @@ from moveit_msgs.msg import * from moveit_msgs.srv import * +from instructor_core.srv import AddWaypoint + from predicator_landmark import GetWaypointsService + class CostarPSMDriver(CostarArm): def __init__(self, @@ -45,12 +51,20 @@ def __init__(self, self.dvrk_arm = dvrk.psm('PSM1') self.psm_initialized = False + self.record_waypoint = 1 rospy.Subscriber("/instructor_marker/feedback", InteractiveMarkerFeedback, self.marker_cbback) self.last_marker_frame = PyKDL.Frame(PyKDL.Rotation.RPY(0,0,0),PyKDL.Vector(0,0,0)) self.last_marker_trans = (0,0,0) self.last_marker_rot = (0,0,0,1) + rospy.Subscriber("/dvrk/footpedals/clutch",Joy,self.clutch_cb) + rospy.wait_for_service("/instructor_core/AddWaypoint",5) + self.add_waypoint_service = rospy.ServiceProxy('/instructor_core/AddWaypoint', AddWaypoint) + + self.tf_listener = tf.TransformListener() + self.cur_pose = Pose() + super(CostarPSMDriver, self).__init__(base_link,end_link,planning_group, dof=6) def home(self): @@ -72,6 +86,16 @@ def insert_tool(self): def marker_cbback(self,data): (self.last_marker_trans,self.last_marker_rot) = pm.toTf(pm.fromMsg(data.pose)) + def clutch_cb(self,data): + if self.record_waypoint == data.buttons[0] and self.driver_status == 'TEACH': + # ((self.cur_pose.position.x,self.cur_pose.position.y,self.cur_pose.position.z), + # (self.cur_pose.orientation.x,self.cur_pose.orientation.y,self.cur_pose.orientation.z,self.cur_pose.orientation.w)) \ + # = self.tf_listener.lookupTransform('/PSM1_tool_wrist_sca_ee_link_0','/world',rospy.Time(0)) + (pos,rot) = self.tf_listener.lookupTransform('/world','/PSM1_tool_wrist_sca_ee_link_0',rospy.Time(0)) + self.cur_pose = pm.toMsg(pm.fromTf((pos,rot))) + self.add_waypoint_service(name = str(datetime.now()), world_pose = self.cur_pose, relative_pose = self.cur_pose, relative_frame_name = '') + print "Clutch pressed, add 1 waypoint" + def handle_tick(self): br = tf.TransformBroadcaster() br.sendTransform((0, 0, 0), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), "/base_link", From 4f6838c84f8002be94af2edb31b3e347df9cb893 Mon Sep 17 00:00:00 2001 From: Baichuan Jiang Date: Mon, 17 Jul 2017 16:25:40 -0400 Subject: [PATCH 12/25] hide the label in the middle --- .../instructor_core/src/instructor_core/robot_interface.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/costar_instructor/instructor_core/src/instructor_core/robot_interface.py b/costar_instructor/instructor_core/src/instructor_core/robot_interface.py index 8f8acc07..52d38eac 100644 --- a/costar_instructor/instructor_core/src/instructor_core/robot_interface.py +++ b/costar_instructor/instructor_core/src/instructor_core/robot_interface.py @@ -38,6 +38,7 @@ def __init__(self,status_label,teach_btn,servo_btn,sound_pub,toast): self.sound_pub = sound_pub self.driver_status = 'NOT CONNECTED' self.status_label.setText('ROBOT MODE: [NOT CONNECTED]') + self.status_label.hide() def driver_status_cb(self,msg): mode = str(msg.data) @@ -47,9 +48,11 @@ def driver_status_cb(self,msg): def update_status(self): if self.driver_status == 'TEACH': - self.status_label.setStyleSheet('background-color:'+colors['blue'].normal+'; color:#ffffff') + self.teach_btn.set_color(colors['blue']) + #self.status_label.setStyleSheet('background-color:'+colors['blue'].normal+'; color:#ffffff') elif self.driver_status == 'SERVO': - self.status_label.setStyleSheet('background-color:'+colors['green'].normal+'; color:#ffffff') + self.servo_btn.set_color(colors['green']) + #self.status_label.setStyleSheet('background-color:'+colors['green'].normal+'; color:#ffffff') elif self.driver_status == 'IDLE': self.status_label.setStyleSheet('background-color:'+colors['gray_light'].normal+'; color:#ffffff') elif self.driver_status == 'IDLE - WARN': From 370e948678623e17daf153b9dc36108d1bb40b2b Mon Sep 17 00:00:00 2001 From: Baichuan Jiang Date: Fri, 21 Jul 2017 12:57:01 -0400 Subject: [PATCH 13/25] Update instructor UI codes. --- costar_bringup/launch/dvrk_teleop.launch | 8 ++++- .../instructor_core/nodes/instructor_view.py | 8 ++--- .../instructor_plugins/package.xml | 30 +++++++++---------- 3 files changed, 26 insertions(+), 20 deletions(-) diff --git a/costar_bringup/launch/dvrk_teleop.launch b/costar_bringup/launch/dvrk_teleop.launch index 3949cbb0..a6794be2 100644 --- a/costar_bringup/launch/dvrk_teleop.launch +++ b/costar_bringup/launch/dvrk_teleop.launch @@ -6,6 +6,10 @@ + + + + @@ -13,7 +17,9 @@ - + + + diff --git a/costar_instructor/instructor_core/nodes/instructor_view.py b/costar_instructor/instructor_core/nodes/instructor_view.py index 44f35552..77c599f1 100755 --- a/costar_instructor/instructor_core/nodes/instructor_view.py +++ b/costar_instructor/instructor_core/nodes/instructor_view.py @@ -132,22 +132,22 @@ def __init__(self,app): self.app_ = app self.types__ = ['LOGIC', 'ACTION', - 'CONDITION', 'TASK', + 'CONDITION', 'PROCESS', 'SERVICE', 'VARIABLE'] self.colors__ = ['blue', 'green', - 'purple', 'orange', + 'purple', 'pink', 'gray', 'gray'] self.labels__ = ['BUILDING BLOCKS', - 'ROBOT ACTIONS', + 'BASIC ACTIONS', + 'ADVANCED ACTIONS', 'SYSTEM KNOWLEDGE', - 'TASK MODEL', 'PROCESSES', 'SERVICE', 'VARIABLES'] diff --git a/costar_instructor/instructor_plugins/package.xml b/costar_instructor/instructor_plugins/package.xml index 6f010351..c38297c4 100644 --- a/costar_instructor/instructor_plugins/package.xml +++ b/costar_instructor/instructor_plugins/package.xml @@ -21,7 +21,7 @@ type="ACTION" group="TIMING" description="Sleep Action" - cases="baseline" + cases="baseline drvk" name="Sleep"/> @@ -76,45 +76,45 @@ From 4f231a29ac3b6c0330dc3f6c3a818c779eb44f97 Mon Sep 17 00:00:00 2001 From: Baichuan Jiang Date: Fri, 21 Jul 2017 21:40:50 -0400 Subject: [PATCH 14/25] Initial commit for predicator_dvrk --- .../predicator_dvrk/CMakeLists.txt | 189 ++++++++++++++++++ costar_predicator/predicator_dvrk/Readme.md | 5 + costar_predicator/predicator_dvrk/package.xml | 56 ++++++ .../scripts/predicator_s_model.py | 10 + costar_predicator/predicator_dvrk/setup.py | 13 ++ .../src/predicator_dvrk/__init__.py | 2 + .../src/predicator_dvrk/dvrk.py | 52 +++++ 7 files changed, 327 insertions(+) create mode 100644 costar_predicator/predicator_dvrk/CMakeLists.txt create mode 100644 costar_predicator/predicator_dvrk/Readme.md create mode 100644 costar_predicator/predicator_dvrk/package.xml create mode 100755 costar_predicator/predicator_dvrk/scripts/predicator_s_model.py create mode 100644 costar_predicator/predicator_dvrk/setup.py create mode 100644 costar_predicator/predicator_dvrk/src/predicator_dvrk/__init__.py create mode 100644 costar_predicator/predicator_dvrk/src/predicator_dvrk/dvrk.py diff --git a/costar_predicator/predicator_dvrk/CMakeLists.txt b/costar_predicator/predicator_dvrk/CMakeLists.txt new file mode 100644 index 00000000..fe42d6c5 --- /dev/null +++ b/costar_predicator/predicator_dvrk/CMakeLists.txt @@ -0,0 +1,189 @@ +cmake_minimum_required(VERSION 2.8.3) +project(predicator_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 + predicator_core + predicator_msgs + #robotiq_c_model_control + #robotiq_s_model_control +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## 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() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a run_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# predicator_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a run_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES predicator_robotiq +# CATKIN_DEPENDS predicator_core predicator_msgs robotiq_c_model_control robotiq_s_model_control +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +# include_directories(include) +include_directories( + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(predicator_robotiq +# src/${PROJECT_NAME}/predicator_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(predicator_robotiq ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +# add_executable(predicator_robotiq_node src/predicator_robotiq_node.cpp) + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(predicator_robotiq_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(predicator_robotiq_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS predicator_robotiq predicator_robotiq_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_predicator_robotiq.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/costar_predicator/predicator_dvrk/Readme.md b/costar_predicator/predicator_dvrk/Readme.md new file mode 100644 index 00000000..1da65a08 --- /dev/null +++ b/costar_predicator/predicator_dvrk/Readme.md @@ -0,0 +1,5 @@ +# Predicator Robotiq + +This contains or will contain code that listens to control messages from the Robotiq grippers and outputs statemetns like "closed", "fully closed", "open", etc. + + diff --git a/costar_predicator/predicator_dvrk/package.xml b/costar_predicator/predicator_dvrk/package.xml new file mode 100644 index 00000000..30680f6d --- /dev/null +++ b/costar_predicator/predicator_dvrk/package.xml @@ -0,0 +1,56 @@ + + + predicator_robotiq + 0.0.0 + The predicator_robotiq package + + + + + chris + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + predicator_core + predicator_msgs + + predicator_core + predicator_msgs + + + + + + + + diff --git a/costar_predicator/predicator_dvrk/scripts/predicator_s_model.py b/costar_predicator/predicator_dvrk/scripts/predicator_s_model.py new file mode 100755 index 00000000..4f71cce0 --- /dev/null +++ b/costar_predicator/predicator_dvrk/scripts/predicator_s_model.py @@ -0,0 +1,10 @@ +#!/usr/bin/env python + +import rospy +from predicator_robotiq import SModelPredicator + +rospy.init_node('predicator_s_model') + +smp = SModelPredicator() + +rospy.spin() diff --git a/costar_predicator/predicator_dvrk/setup.py b/costar_predicator/predicator_dvrk/setup.py new file mode 100644 index 00000000..854aa0bf --- /dev/null +++ b/costar_predicator/predicator_dvrk/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=['predicator_robotiq'], + package_dir={'': 'src'}, +) + +setup(**d) + diff --git a/costar_predicator/predicator_dvrk/src/predicator_dvrk/__init__.py b/costar_predicator/predicator_dvrk/src/predicator_dvrk/__init__.py new file mode 100644 index 00000000..e374399c --- /dev/null +++ b/costar_predicator/predicator_dvrk/src/predicator_dvrk/__init__.py @@ -0,0 +1,2 @@ +from s_model import * +from c_model import * diff --git a/costar_predicator/predicator_dvrk/src/predicator_dvrk/dvrk.py b/costar_predicator/predicator_dvrk/src/predicator_dvrk/dvrk.py new file mode 100644 index 00000000..8660017d --- /dev/null +++ b/costar_predicator/predicator_dvrk/src/predicator_dvrk/dvrk.py @@ -0,0 +1,52 @@ +import rospy +from predicator_msgs.msg import * + +class SModelPredicator: + + def __init__(self,publish_predicates=True,start_subscriber=True,gripper_name='s_model'): + + self.valid_predicates = ValidPredicates( + assignments=["clutch"], # list all pedals here + predicates=['pressed']) + self.predicate_msg = PredicateList() + + if publish_predicates: + # create predicator things + self.pub = rospy.Publisher("predicator/input",PredicateList,queue_size=1000) + self.vpub = rospy.Publisher("predicator/valid_predicates",PredicateList,queue_size=1000) + + if start_subscriber: + self.sub = rospy.Subscriber("LISTEN ON PEDAL TOPIC",inputMsg,self.callback) + + self.name = rospy.get_name() + + def callback(self, msg): + self.handle(msg) + + def handle(self,status): + self.predicate_msg = PredicateList() + self.predicate_msg.pheader.source = self.name + # add all pressed pedals to the list by calling add predicate ("pressed", pedal name) + ''' + add a single message + ''' + def addPredicate(self,predicate, pedal_name): + p = PredicateStatement(predicate=predicate,params=[pedal_name,'','']) + self.predicate_msg.statements.append(p) + + ''' + publish current predicate messages + ''' + def tick(self): + self.pub.publish(self.predicate_msg) + self.vpub.publish(self.valid_predicates) + + ''' + update and spin + ''' + def spin(self,rate=10): + spin_rate = rospy.Rate(rate) + while not rospy.is_shutdown(): + self.tick() + spin_rate.sleep() + From 3ffa2902d8517cc7fb9d866a94ee2caf29ac1d8a Mon Sep 17 00:00:00 2001 From: Baichuan Jiang Date: Fri, 21 Jul 2017 22:25:34 -0400 Subject: [PATCH 15/25] Modified predicator codes for dvrk (without testing). --- .../predicator_dvrk/scripts/predicator_s_model.py | 6 +++--- .../predicator_dvrk/src/predicator_dvrk/dvrk.py | 15 ++++++++++----- 2 files changed, 13 insertions(+), 8 deletions(-) diff --git a/costar_predicator/predicator_dvrk/scripts/predicator_s_model.py b/costar_predicator/predicator_dvrk/scripts/predicator_s_model.py index 4f71cce0..d50ff76b 100755 --- a/costar_predicator/predicator_dvrk/scripts/predicator_s_model.py +++ b/costar_predicator/predicator_dvrk/scripts/predicator_s_model.py @@ -1,10 +1,10 @@ #!/usr/bin/env python import rospy -from predicator_robotiq import SModelPredicator +from predicator_dvrk import dvrkPredicator -rospy.init_node('predicator_s_model') +rospy.init_node('predicator_dvrk') -smp = SModelPredicator() +dvrkp = dvrkPredicator() rospy.spin() diff --git a/costar_predicator/predicator_dvrk/src/predicator_dvrk/dvrk.py b/costar_predicator/predicator_dvrk/src/predicator_dvrk/dvrk.py index 8660017d..27bb4a6d 100644 --- a/costar_predicator/predicator_dvrk/src/predicator_dvrk/dvrk.py +++ b/costar_predicator/predicator_dvrk/src/predicator_dvrk/dvrk.py @@ -1,12 +1,12 @@ import rospy from predicator_msgs.msg import * -class SModelPredicator: +class dvrkPredicator: - def __init__(self,publish_predicates=True,start_subscriber=True,gripper_name='s_model'): + def __init__(self,publish_predicates=True,start_subscriber=True,gripper_name='psm_gripper'): self.valid_predicates = ValidPredicates( - assignments=["clutch"], # list all pedals here + assignments=["clutch","coag","camera_plus","camera_minus","camera"], predicates=['pressed']) self.predicate_msg = PredicateList() @@ -16,7 +16,7 @@ def __init__(self,publish_predicates=True,start_subscriber=True,gripper_name='s_ self.vpub = rospy.Publisher("predicator/valid_predicates",PredicateList,queue_size=1000) if start_subscriber: - self.sub = rospy.Subscriber("LISTEN ON PEDAL TOPIC",inputMsg,self.callback) + self.sub = rospy.Subscriber("/dvrk/footpedals/clutch",inputMsg,self.callback) self.name = rospy.get_name() @@ -26,7 +26,12 @@ def callback(self, msg): def handle(self,status): self.predicate_msg = PredicateList() self.predicate_msg.pheader.source = self.name - # add all pressed pedals to the list by calling add predicate ("pressed", pedal name) + self.addPredicate("pressed","clutch") + self.addPredicate("pressed","coag") + self.addPredicate("pressed","camera") + self.addPredicate("pressed","camera_minus") + self.addPredicate("pressed","camera_plus") + ''' add a single message ''' From 692d320ed811cc42b9e39fd318363c9070a86114 Mon Sep 17 00:00:00 2001 From: Baichuan Jiang Date: Sun, 23 Jul 2017 16:01:37 -0400 Subject: [PATCH 16/25] Modified predicator_dvrk package, still not showing up in knowledge test. --- costar_bringup/launch/dvrk_teleop.launch | 1 + costar_predicator/predicator_dvrk/CMakeLists.txt | 2 +- costar_predicator/predicator_dvrk/package.xml | 4 ++-- .../{predicator_s_model.py => simple_predicator_dvrk.py} | 2 +- costar_predicator/predicator_dvrk/setup.py | 2 +- .../predicator_dvrk/src/predicator_dvrk/__init__.py | 3 +-- .../src/predicator_dvrk/{dvrk.py => dvrkp.py} | 7 ++++++- 7 files changed, 13 insertions(+), 8 deletions(-) rename costar_predicator/predicator_dvrk/scripts/{predicator_s_model.py => simple_predicator_dvrk.py} (82%) rename costar_predicator/predicator_dvrk/src/predicator_dvrk/{dvrk.py => dvrkp.py} (81%) diff --git a/costar_bringup/launch/dvrk_teleop.launch b/costar_bringup/launch/dvrk_teleop.launch index a6794be2..a99082f6 100644 --- a/costar_bringup/launch/dvrk_teleop.launch +++ b/costar_bringup/launch/dvrk_teleop.launch @@ -22,6 +22,7 @@ + diff --git a/costar_predicator/predicator_dvrk/CMakeLists.txt b/costar_predicator/predicator_dvrk/CMakeLists.txt index fe42d6c5..400d181c 100644 --- a/costar_predicator/predicator_dvrk/CMakeLists.txt +++ b/costar_predicator/predicator_dvrk/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 2.8.3) -project(predicator_robotiq) +project(predicator_dvrk) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) diff --git a/costar_predicator/predicator_dvrk/package.xml b/costar_predicator/predicator_dvrk/package.xml index 30680f6d..e0f80c0c 100644 --- a/costar_predicator/predicator_dvrk/package.xml +++ b/costar_predicator/predicator_dvrk/package.xml @@ -1,8 +1,8 @@ - predicator_robotiq + predicator_dvrk 0.0.0 - The predicator_robotiq package + The predicator_dvrk package diff --git a/costar_predicator/predicator_dvrk/scripts/predicator_s_model.py b/costar_predicator/predicator_dvrk/scripts/simple_predicator_dvrk.py similarity index 82% rename from costar_predicator/predicator_dvrk/scripts/predicator_s_model.py rename to costar_predicator/predicator_dvrk/scripts/simple_predicator_dvrk.py index d50ff76b..aa7cb0cd 100755 --- a/costar_predicator/predicator_dvrk/scripts/predicator_s_model.py +++ b/costar_predicator/predicator_dvrk/scripts/simple_predicator_dvrk.py @@ -5,6 +5,6 @@ rospy.init_node('predicator_dvrk') -dvrkp = dvrkPredicator() +dvrkpre = dvrkPredicator() rospy.spin() diff --git a/costar_predicator/predicator_dvrk/setup.py b/costar_predicator/predicator_dvrk/setup.py index 854aa0bf..a75a096d 100644 --- a/costar_predicator/predicator_dvrk/setup.py +++ b/costar_predicator/predicator_dvrk/setup.py @@ -5,7 +5,7 @@ d = generate_distutils_setup( ## don't do this unless you want a globally visible script - packages=['predicator_robotiq'], + packages=['predicator_dvrk'], package_dir={'': 'src'}, ) diff --git a/costar_predicator/predicator_dvrk/src/predicator_dvrk/__init__.py b/costar_predicator/predicator_dvrk/src/predicator_dvrk/__init__.py index e374399c..6dbd446a 100644 --- a/costar_predicator/predicator_dvrk/src/predicator_dvrk/__init__.py +++ b/costar_predicator/predicator_dvrk/src/predicator_dvrk/__init__.py @@ -1,2 +1 @@ -from s_model import * -from c_model import * +from dvrkp import * diff --git a/costar_predicator/predicator_dvrk/src/predicator_dvrk/dvrk.py b/costar_predicator/predicator_dvrk/src/predicator_dvrk/dvrkp.py similarity index 81% rename from costar_predicator/predicator_dvrk/src/predicator_dvrk/dvrk.py rename to costar_predicator/predicator_dvrk/src/predicator_dvrk/dvrkp.py index 27bb4a6d..67821737 100644 --- a/costar_predicator/predicator_dvrk/src/predicator_dvrk/dvrk.py +++ b/costar_predicator/predicator_dvrk/src/predicator_dvrk/dvrkp.py @@ -1,5 +1,6 @@ import rospy from predicator_msgs.msg import * +from sensor_msgs.msg import Joy class dvrkPredicator: @@ -16,7 +17,11 @@ def __init__(self,publish_predicates=True,start_subscriber=True,gripper_name='ps self.vpub = rospy.Publisher("predicator/valid_predicates",PredicateList,queue_size=1000) if start_subscriber: - self.sub = rospy.Subscriber("/dvrk/footpedals/clutch",inputMsg,self.callback) + self.sub = rospy.Subscriber("/dvrk/footpedals/clutch",Joy,self.callback) + self.sub2 = rospy.Subscriber("/dvrk/footpedals/coag",Joy,self.callback) + self.sub3 = rospy.Subscriber("/dvrk/footpedals/camera_plus",Joy,self.callback) + self.sub4 = rospy.Subscriber("/dvrk/footpedals/camera_minus",Joy,self.callback) + self.sub5 = rospy.Subscriber("/dvrk/footpedals/camera",Joy,self.callback) self.name = rospy.get_name() From 86b47a16026c757f342a1eabbef155a88d3d866a Mon Sep 17 00:00:00 2001 From: Baichuan Jiang Date: Sun, 23 Jul 2017 16:49:28 -0400 Subject: [PATCH 17/25] Add launch file for both arms teleoperation. --- .../launch/dvrk_teleop_two_arms.launch | 32 +++++++++++++++++++ 1 file changed, 32 insertions(+) create mode 100644 costar_bringup/launch/dvrk_teleop_two_arms.launch diff --git a/costar_bringup/launch/dvrk_teleop_two_arms.launch b/costar_bringup/launch/dvrk_teleop_two_arms.launch new file mode 100644 index 00000000..e4a820e7 --- /dev/null +++ b/costar_bringup/launch/dvrk_teleop_two_arms.launch @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From aca82d7ca5d8c0b3fa2f659631436ccecc3dab82 Mon Sep 17 00:00:00 2001 From: Baichuan Jiang Date: Mon, 24 Jul 2017 15:06:04 -0400 Subject: [PATCH 18/25] fix predicator dvrk messages --- .../scripts/simple_predicator_dvrk.py | 3 +-- .../src/predicator_dvrk/dvrkp.py | 21 +++++++++---------- 2 files changed, 11 insertions(+), 13 deletions(-) diff --git a/costar_predicator/predicator_dvrk/scripts/simple_predicator_dvrk.py b/costar_predicator/predicator_dvrk/scripts/simple_predicator_dvrk.py index aa7cb0cd..7b2c230a 100755 --- a/costar_predicator/predicator_dvrk/scripts/simple_predicator_dvrk.py +++ b/costar_predicator/predicator_dvrk/scripts/simple_predicator_dvrk.py @@ -6,5 +6,4 @@ rospy.init_node('predicator_dvrk') dvrkpre = dvrkPredicator() - -rospy.spin() +dvrkpre.spin() diff --git a/costar_predicator/predicator_dvrk/src/predicator_dvrk/dvrkp.py b/costar_predicator/predicator_dvrk/src/predicator_dvrk/dvrkp.py index 67821737..3080ef61 100644 --- a/costar_predicator/predicator_dvrk/src/predicator_dvrk/dvrkp.py +++ b/costar_predicator/predicator_dvrk/src/predicator_dvrk/dvrkp.py @@ -9,12 +9,15 @@ def __init__(self,publish_predicates=True,start_subscriber=True,gripper_name='ps self.valid_predicates = ValidPredicates( assignments=["clutch","coag","camera_plus","camera_minus","camera"], predicates=['pressed']) + self.valid_predicates.pheader.source = rospy.get_name() self.predicate_msg = PredicateList() + self.clutch_pressed = False + if publish_predicates: # create predicator things self.pub = rospy.Publisher("predicator/input",PredicateList,queue_size=1000) - self.vpub = rospy.Publisher("predicator/valid_predicates",PredicateList,queue_size=1000) + self.vpub = rospy.Publisher("predicator/valid_predicates",ValidPredicates,queue_size=1000) if start_subscriber: self.sub = rospy.Subscriber("/dvrk/footpedals/clutch",Joy,self.callback) @@ -26,16 +29,8 @@ def __init__(self,publish_predicates=True,start_subscriber=True,gripper_name='ps self.name = rospy.get_name() def callback(self, msg): - self.handle(msg) - - def handle(self,status): - self.predicate_msg = PredicateList() - self.predicate_msg.pheader.source = self.name - self.addPredicate("pressed","clutch") - self.addPredicate("pressed","coag") - self.addPredicate("pressed","camera") - self.addPredicate("pressed","camera_minus") - self.addPredicate("pressed","camera_plus") + # if this is for the clutch -- set clutch pressed + pass ''' add a single message @@ -48,6 +43,10 @@ def addPredicate(self,predicate, pedal_name): publish current predicate messages ''' def tick(self): + self.predicate_msg = PredicateList() + self.predicate_msg.pheader.source = self.name + if self.clutch_pressed: + self.addPredicate("pressed","clutch") self.pub.publish(self.predicate_msg) self.vpub.publish(self.valid_predicates) From 138c1b1248500b516262e74474347c5b3b44422b Mon Sep 17 00:00:00 2001 From: Baichuan Jiang Date: Mon, 24 Jul 2017 15:13:16 -0400 Subject: [PATCH 19/25] fix predicator clutch stuff --- costar_predicator/predicator_dvrk/src/predicator_dvrk/dvrkp.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/costar_predicator/predicator_dvrk/src/predicator_dvrk/dvrkp.py b/costar_predicator/predicator_dvrk/src/predicator_dvrk/dvrkp.py index 3080ef61..588b4836 100644 --- a/costar_predicator/predicator_dvrk/src/predicator_dvrk/dvrkp.py +++ b/costar_predicator/predicator_dvrk/src/predicator_dvrk/dvrkp.py @@ -17,7 +17,7 @@ def __init__(self,publish_predicates=True,start_subscriber=True,gripper_name='ps if publish_predicates: # create predicator things self.pub = rospy.Publisher("predicator/input",PredicateList,queue_size=1000) - self.vpub = rospy.Publisher("predicator/valid_predicates",ValidPredicates,queue_size=1000) + self.vpub = rospy.Publisher("predicator/valid_input",ValidPredicates,queue_size=1000) if start_subscriber: self.sub = rospy.Subscriber("/dvrk/footpedals/clutch",Joy,self.callback) From c7bdb9733f206ed11acdee9eb1191c70024f62c5 Mon Sep 17 00:00:00 2001 From: Baichuan Jiang Date: Mon, 24 Jul 2017 15:27:31 -0400 Subject: [PATCH 20/25] fix up description and if statement --- .../instructor_knowledge_test.py | 2 +- .../src/predicator_dvrk/dvrkp.py | 20 ++++++++++++------- 2 files changed, 14 insertions(+), 8 deletions(-) diff --git a/costar_instructor/instructor_plugins/src/instructor_plugins/instructor_knowledge_test.py b/costar_instructor/instructor_plugins/src/instructor_plugins/instructor_knowledge_test.py index b2358fe1..028a4bdc 100644 --- a/costar_instructor/instructor_plugins/src/instructor_plugins/instructor_knowledge_test.py +++ b/costar_instructor/instructor_plugins/src/instructor_plugins/instructor_knowledge_test.py @@ -173,7 +173,7 @@ def load_data(self,data): # NODE class NodeKnowledgeTest(Node): def __init__(self, name, label, target, value, predicate): - L = 'IF '+target.upper()+'['+predicate+'] = '+str(value).upper() + L = 'IF '+predicate+'('+target+') = '+str(value).upper() color = colors['purple'].normal super(NodeKnowledgeTest, self).__init__(name, L, color) self.target = target diff --git a/costar_predicator/predicator_dvrk/src/predicator_dvrk/dvrkp.py b/costar_predicator/predicator_dvrk/src/predicator_dvrk/dvrkp.py index 588b4836..240f7b63 100644 --- a/costar_predicator/predicator_dvrk/src/predicator_dvrk/dvrkp.py +++ b/costar_predicator/predicator_dvrk/src/predicator_dvrk/dvrkp.py @@ -20,17 +20,23 @@ def __init__(self,publish_predicates=True,start_subscriber=True,gripper_name='ps self.vpub = rospy.Publisher("predicator/valid_input",ValidPredicates,queue_size=1000) if start_subscriber: - self.sub = rospy.Subscriber("/dvrk/footpedals/clutch",Joy,self.callback) - self.sub2 = rospy.Subscriber("/dvrk/footpedals/coag",Joy,self.callback) - self.sub3 = rospy.Subscriber("/dvrk/footpedals/camera_plus",Joy,self.callback) - self.sub4 = rospy.Subscriber("/dvrk/footpedals/camera_minus",Joy,self.callback) - self.sub5 = rospy.Subscriber("/dvrk/footpedals/camera",Joy,self.callback) + self.sub = rospy.Subscriber("/dvrk/footpedals/clutch",Joy,self.callbackClutch) + #self.sub2 = rospy.Subscriber("/dvrk/footpedals/coag",Joy,self.callback) + #self.sub3 = rospy.Subscriber("/dvrk/footpedals/camera_plus",Joy,self.callback) + #self.sub4 = rospy.Subscriber("/dvrk/footpedals/camera_minus",Joy,self.callback) + #self.sub5 = rospy.Subscriber("/dvrk/footpedals/camera",Joy,self.callback) self.name = rospy.get_name() - def callback(self, msg): + def callbackClutch(self, msg): # if this is for the clutch -- set clutch pressed - pass + if len(msg.buttons) > 0: + if msg.buttons[0] == 1: + # clutch pressed + self.clutch_pressed = True + elif msg.buttons[0] == 0: + self.clutch_pressed = False + print "clutch pedal status:", self.clutch_pressed ''' add a single message From b27437edb3df5244b2bb7ab6ba26896c4c4ab328 Mon Sep 17 00:00:00 2001 From: Baichuan Jiang Date: Sat, 26 Aug 2017 21:10:05 -0400 Subject: [PATCH 21/25] Fix psm gripper orientation error and pedal press predicator issue. --- costar_bringup/launch/dvrk_teleop_two_arms.launch | 4 ++-- .../predicator_dvrk/src/predicator_dvrk/dvrkp.py | 14 ++++++++++++++ .../src/costar_robot/psm_driver.py | 6 +++++- 3 files changed, 21 insertions(+), 3 deletions(-) diff --git a/costar_bringup/launch/dvrk_teleop_two_arms.launch b/costar_bringup/launch/dvrk_teleop_two_arms.launch index e4a820e7..8dc2c2ba 100644 --- a/costar_bringup/launch/dvrk_teleop_two_arms.launch +++ b/costar_bringup/launch/dvrk_teleop_two_arms.launch @@ -3,7 +3,7 @@ - + @@ -11,7 +11,7 @@ - + diff --git a/costar_predicator/predicator_dvrk/src/predicator_dvrk/dvrkp.py b/costar_predicator/predicator_dvrk/src/predicator_dvrk/dvrkp.py index 240f7b63..68c54b90 100644 --- a/costar_predicator/predicator_dvrk/src/predicator_dvrk/dvrkp.py +++ b/costar_predicator/predicator_dvrk/src/predicator_dvrk/dvrkp.py @@ -13,6 +13,7 @@ def __init__(self,publish_predicates=True,start_subscriber=True,gripper_name='ps self.predicate_msg = PredicateList() self.clutch_pressed = False + self.camera_pressed = False if publish_predicates: # create predicator things @@ -21,6 +22,7 @@ def __init__(self,publish_predicates=True,start_subscriber=True,gripper_name='ps if start_subscriber: self.sub = rospy.Subscriber("/dvrk/footpedals/clutch",Joy,self.callbackClutch) + self.sub2 = rospy.Subscriber("/dvrk/footpedals/camera",Joy,self.callbackCamera) #self.sub2 = rospy.Subscriber("/dvrk/footpedals/coag",Joy,self.callback) #self.sub3 = rospy.Subscriber("/dvrk/footpedals/camera_plus",Joy,self.callback) #self.sub4 = rospy.Subscriber("/dvrk/footpedals/camera_minus",Joy,self.callback) @@ -37,6 +39,16 @@ def callbackClutch(self, msg): elif msg.buttons[0] == 0: self.clutch_pressed = False print "clutch pedal status:", self.clutch_pressed + + def callbackCamera(self, msg): + # if this is for the clutch -- set clutch pressed + if len(msg.buttons) > 0: + if msg.buttons[0] == 1: + # clutch pressed + self.camera_pressed = True + elif msg.buttons[0] == 0: + self.camera_pressed = False + print "camera pedal status:", self.camera_pressed ''' add a single message @@ -53,6 +65,8 @@ def tick(self): self.predicate_msg.pheader.source = self.name if self.clutch_pressed: self.addPredicate("pressed","clutch") + if self.camera_pressed: + self.addPredicate("pressed","camera") self.pub.publish(self.predicate_msg) self.vpub.publish(self.valid_predicates) diff --git a/costar_robot/costar_robot_manager/src/costar_robot/psm_driver.py b/costar_robot/costar_robot_manager/src/costar_robot/psm_driver.py index f0ea7dd5..df42d87d 100644 --- a/costar_robot/costar_robot_manager/src/costar_robot/psm_driver.py +++ b/costar_robot/costar_robot_manager/src/costar_robot/psm_driver.py @@ -166,7 +166,11 @@ def save_frame_cb(self,req): def servo_to_pose_cb(self,req): if self.driver_status == 'SERVO': print(req.target) - T = pm.fromMsg(req.target) + T_temp = pm.fromMsg(req.target) + #frame_offset = PyKDL.Frame(PyKDL.Rotation.RPY(-1.569,1.571,-1.395), PyKDL.Vector(0,0,0)) + # frame_offset = PyKDL.Frame(PyKDL.Rotation.RPY(-1.569,1.571,-1.219), PyKDL.Vector(0,0,0)) + frame_offset = PyKDL.Frame(PyKDL.Rotation.RPY(-1.569,1.571,-1.435), PyKDL.Vector(0,0,0)) + T = T_temp * frame_offset self.dvrk_arm.move(T) return 'SUCCESS - moved to pose' From 93f217babe850a616dd0020d10795ae7a8099f60 Mon Sep 17 00:00:00 2001 From: Baichuan Jiang Date: Tue, 29 Aug 2017 20:43:01 -0400 Subject: [PATCH 22/25] Fix the gripper bug. --- .../gripper_manager/src/gripper_manager/costar_gripper.py | 4 ---- costar_perception/sp_segmenter/CATKIN_IGNORE | 0 costar_robot/simple_handeye_calibration/CATKIN_IGNORE | 0 costar_tools/alvar_data_collection/CATKIN_IGNORE | 0 4 files changed, 4 deletions(-) delete mode 100644 costar_perception/sp_segmenter/CATKIN_IGNORE delete mode 100644 costar_robot/simple_handeye_calibration/CATKIN_IGNORE delete mode 100644 costar_tools/alvar_data_collection/CATKIN_IGNORE diff --git a/costar_gripper/gripper_manager/src/gripper_manager/costar_gripper.py b/costar_gripper/gripper_manager/src/gripper_manager/costar_gripper.py index 01272169..9b99d15f 100644 --- a/costar_gripper/gripper_manager/src/gripper_manager/costar_gripper.py +++ b/costar_gripper/gripper_manager/src/gripper_manager/costar_gripper.py @@ -18,10 +18,6 @@ def __init__(self, *args, **kwargs): self.verbose = verbose - self.predicator = GripperPredicatorType( - start_subscriber=False, - publish_predicates=True, - gripper_name=name) if GripperPredicatorType is not None: self.predicator = GripperPredicatorType( diff --git a/costar_perception/sp_segmenter/CATKIN_IGNORE b/costar_perception/sp_segmenter/CATKIN_IGNORE deleted file mode 100644 index e69de29b..00000000 diff --git a/costar_robot/simple_handeye_calibration/CATKIN_IGNORE b/costar_robot/simple_handeye_calibration/CATKIN_IGNORE deleted file mode 100644 index e69de29b..00000000 diff --git a/costar_tools/alvar_data_collection/CATKIN_IGNORE b/costar_tools/alvar_data_collection/CATKIN_IGNORE deleted file mode 100644 index e69de29b..00000000 From 9daa8735561d6b469adb189a3d6fab8a18126d67 Mon Sep 17 00:00:00 2001 From: Baichuan Jiang Date: Fri, 8 Jun 2018 16:08:43 -0400 Subject: [PATCH 23/25] Get merged CoSTAR running on dvrk. --- costar_bringup/launch/config/dvrk.launch | 5 + .../src/costar_robot/costar_arm.py | 1 + .../dvrk_model/config_files/MTMR-PSM1.rviz | 302 ++++++++++++++++++ .../config_files/PSM1_with_marker.rviz | 237 ++++++++++++++ .../console-MTMR-PSM1-MTML-PSM2-Teleop.json | 62 ++++ .../console-MTMR-PSM1-Teleop.json | 34 ++ .../config_files/dvrk_arm_rviz.launch | 39 +++ .../dvrk_master_slave_rviz.launch | 57 ++++ 8 files changed, 737 insertions(+) create mode 100644 costar_robot/dvrk_model/config_files/MTMR-PSM1.rviz create mode 100644 costar_robot/dvrk_model/config_files/PSM1_with_marker.rviz create mode 100644 costar_robot/dvrk_model/config_files/console-MTMR-PSM1-MTML-PSM2-Teleop.json create mode 100644 costar_robot/dvrk_model/config_files/console-MTMR-PSM1-Teleop.json create mode 100644 costar_robot/dvrk_model/config_files/dvrk_arm_rviz.launch create mode 100644 costar_robot/dvrk_model/config_files/dvrk_master_slave_rviz.launch diff --git a/costar_bringup/launch/config/dvrk.launch b/costar_bringup/launch/config/dvrk.launch index d605a61d..56b094c1 100644 --- a/costar_bringup/launch/config/dvrk.launch +++ b/costar_bringup/launch/config/dvrk.launch @@ -3,11 +3,16 @@ [0.30, -1.33, -1.80, -0.27, 1.50, 1.60] + false 6 + [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] + 1.0 + 1.0 PSM1_psm_base_link PSM1_tool_tip_link_virtual table_center + false diff --git a/costar_robot/costar_robot_manager/src/costar_robot/costar_arm.py b/costar_robot/costar_robot_manager/src/costar_robot/costar_arm.py index e3d3aa09..93f732b0 100644 --- a/costar_robot/costar_robot_manager/src/costar_robot/costar_arm.py +++ b/costar_robot/costar_robot_manager/src/costar_robot/costar_arm.py @@ -175,6 +175,7 @@ def __init__(self, self.joint_names = [joint.name for joint in self.robot.joints[:self.dof]] rospy.loginfo('Setting joint names to: %s'%(str(self.joint_names))) + # joint priority in the solver, not mass self.joint_weights = rospy.get_param(os.path.join(self.namespace + '/robot/', "joint_weights")) if not isinstance(self.joint_weights, list) and not len(self.joint_weights) == self.dof: raise RuntimeError('loaded bad weights: %s'%(str(self.joint_weights))) diff --git a/costar_robot/dvrk_model/config_files/MTMR-PSM1.rviz b/costar_robot/dvrk_model/config_files/MTMR-PSM1.rviz new file mode 100644 index 00000000..04b95477 --- /dev/null +++ b/costar_robot/dvrk_model/config_files/MTMR-PSM1.rviz @@ -0,0 +1,302 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1 + - /RobotModel2 + Splitter Ratio: 0.684564 + Tree Height: 568 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Class: rviz/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 0.2 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: false + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + PSM1_outer_insertion_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + PSM1_outer_pitch_back_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + PSM1_outer_pitch_bottom_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + PSM1_outer_pitch_front_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + PSM1_outer_pitch_link: + Alpha: 1 + Show Axes: false + Show Trail: false + PSM1_outer_pitch_top_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + PSM1_outer_yaw_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + PSM1_psm_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + PSM1_tool_main_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + PSM1_tool_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + PSM1_tool_wrist_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + PSM1_tool_wrist_sca_ee_link_0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + PSM1_tool_wrist_sca_ee_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + PSM1_tool_wrist_sca_ee_link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + PSM1_tool_wrist_sca_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + PSM1_tool_wrist_sca_shaft_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + PSM1_tool_wrist_shaft_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotModel + Robot Description: /dvrk/PSM1/robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + MTMR_back_parallel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + MTMR_bottom_parallel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + MTMR_outer_yaw_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + MTMR_top_panel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + MTMR_top_parallel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + MTMR_wrist_pitch_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + MTMR_wrist_platform_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + MTMR_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + MTMR_wrist_yaw_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotModel + Robot Description: /dvrk/MTMR/robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz/InteractiveMarkers + Enable Transparency: true + Enabled: true + Name: InteractiveMarkers + Show Axes: false + Show Descriptions: true + Show Visual Aids: false + Update Topic: /instructor_marker/update + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 1.28928 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.0486479 + Y: -0.187207 + Z: 0.675249 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.265398 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.64856 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000013c000002c7fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c7000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000002c7000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000026100fffffffb0000000800540069006d0065010000000000000450000000000000000000000259000002c700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 2103 + Y: 54 diff --git a/costar_robot/dvrk_model/config_files/PSM1_with_marker.rviz b/costar_robot/dvrk_model/config_files/PSM1_with_marker.rviz new file mode 100644 index 00000000..ea8410fe --- /dev/null +++ b/costar_robot/dvrk_model/config_files/PSM1_with_marker.rviz @@ -0,0 +1,237 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1/Links1 + - /InteractiveMarkers1 + Splitter Ratio: 0.684564 + Tree Height: 565 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Class: rviz/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 0.2 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: false + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + PSM1_outer_insertion_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + PSM1_outer_pitch_back_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + PSM1_outer_pitch_bottom_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + PSM1_outer_pitch_front_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + PSM1_outer_pitch_link: + Alpha: 1 + Show Axes: false + Show Trail: false + PSM1_outer_pitch_top_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + PSM1_outer_yaw_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + PSM1_psm_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + PSM1_tool_main_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + PSM1_tool_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + PSM1_tool_wrist_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + PSM1_tool_wrist_sca_ee_link_0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + PSM1_tool_wrist_sca_ee_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + PSM1_tool_wrist_sca_ee_link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + PSM1_tool_wrist_sca_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + PSM1_tool_wrist_sca_shaft_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + PSM1_tool_wrist_shaft_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotModel + Robot Description: /dvrk/PSM1/robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz/InteractiveMarkers + Enable Transparency: true + Enabled: true + Name: InteractiveMarkers + Show Axes: false + Show Descriptions: true + Show Visual Aids: false + Update Topic: /instructor_marker/update + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 2.24046 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.123912 + Y: 0.361157 + Z: 0.562127 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.120398 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.18856 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a000002c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c4000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000022b000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 390 + Y: 16 diff --git a/costar_robot/dvrk_model/config_files/console-MTMR-PSM1-MTML-PSM2-Teleop.json b/costar_robot/dvrk_model/config_files/console-MTMR-PSM1-MTML-PSM2-Teleop.json new file mode 100644 index 00000000..9d15c2ff --- /dev/null +++ b/costar_robot/dvrk_model/config_files/console-MTMR-PSM1-MTML-PSM2-Teleop.json @@ -0,0 +1,62 @@ +/* -*- Mode: Javascript; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */ +{ + "chatty": true + , + "io": { + "footpedals": "sawRobotIO1394-MTMR-foot-pedals.xml" + } + , + "arms": + [ + { + "name": "PSM1", + "type": "PSM", + "io": "sawRobotIO1394-PSM1-28007.xml", + "pid": "sawControllersPID-PSM.xml", + "kinematic": "psm-large-needle-driver.json" + } + , + { + "name": "PSM2", + "type": "PSM", + "io": "sawRobotIO1394-PSM2-27374.xml", + "pid": "sawControllersPID-PSM.xml", + "kinematic": "psm-large-needle-driver.json" + } + , + { + "name": "MTMR", + "type": "MTM", + "io": "sawRobotIO1394-MTMR-28247.xml", + "pid": "sawControllersPID-MTMR.xml", + "kinematic": "mtm.json" + } + , + { + "name": "MTML", + "type": "MTM", + "io": "sawRobotIO1394-MTML-22723.xml", + "pid": "sawControllersPID-MTML.xml", + "kinematic": "mtm.json" + } + ] + , + "psm-teleops": + [ + { + "master": "MTMR", + "slave": "PSM1", + "configure-parameter": { + "scale": 0.4 + } + } + , + { + "master": "MTML", + "slave": "PSM2", + "configure-parameter": { + "scale":0.4 + } + } + ] +} diff --git a/costar_robot/dvrk_model/config_files/console-MTMR-PSM1-Teleop.json b/costar_robot/dvrk_model/config_files/console-MTMR-PSM1-Teleop.json new file mode 100644 index 00000000..802ce2c7 --- /dev/null +++ b/costar_robot/dvrk_model/config_files/console-MTMR-PSM1-Teleop.json @@ -0,0 +1,34 @@ +/* -*- Mode: Javascript; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */ +{ + "io": + { + "footpedals": "sawRobotIO1394-MTMR-foot-pedals.xml" + } + , + "arms": + [ + { + "name": "PSM1", + "type": "PSM", + "io": "sawRobotIO1394-PSM1-28007.xml", + "pid": "sawControllersPID-PSM.xml", + "kinematic": "psm-large-needle-driver.json" + } + , + { + "name": "MTMR", + "type": "MTM", + "io": "sawRobotIO1394-MTMR-28247.xml", + "pid": "sawControllersPID-MTMR.xml", + "kinematic": "mtm.json" + } + ] + , + "psm-teleops": + [ + { + "master": "MTMR", + "slave": "PSM1" + } + ] +} diff --git a/costar_robot/dvrk_model/config_files/dvrk_arm_rviz.launch b/costar_robot/dvrk_model/config_files/dvrk_arm_rviz.launch new file mode 100644 index 00000000..d162d3e1 --- /dev/null +++ b/costar_robot/dvrk_model/config_files/dvrk_arm_rviz.launch @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + [/dvrk/$(arg arm)/state_joint_current, + /dvrk/$(arg arm)/state_jaw_current, + /dvrk/$(arg arm)/state_gripper_current] + + + + + + + + + + + + + diff --git a/costar_robot/dvrk_model/config_files/dvrk_master_slave_rviz.launch b/costar_robot/dvrk_model/config_files/dvrk_master_slave_rviz.launch new file mode 100644 index 00000000..0547e6ad --- /dev/null +++ b/costar_robot/dvrk_model/config_files/dvrk_master_slave_rviz.launch @@ -0,0 +1,57 @@ + + + + + + + + + + + + + + + + + + + + + [/dvrk/$(arg master)/state_joint_current] + + + + + + + + + + + + + + + + + + [/dvrk/$(arg slave)/state_joint_current] + + + + + + + + + + + + + From 18636b5d1592834607b90e56408a6e7c77a257d2 Mon Sep 17 00:00:00 2001 From: Baichuan Jiang Date: Sun, 10 Jun 2018 15:50:47 -0400 Subject: [PATCH 24/25] Add README file for CoSTAR-dVRK --- costar_robot/dvrk_model/CoSTAR-dVRK.md | 98 ++++++++++++++++++++++++++ 1 file changed, 98 insertions(+) create mode 100644 costar_robot/dvrk_model/CoSTAR-dVRK.md diff --git a/costar_robot/dvrk_model/CoSTAR-dVRK.md b/costar_robot/dvrk_model/CoSTAR-dVRK.md new file mode 100644 index 00000000..a02216ee --- /dev/null +++ b/costar_robot/dvrk_model/CoSTAR-dVRK.md @@ -0,0 +1,98 @@ +# CoSTAR-dVRK + +***A tutorial on enabling CoSTAR to work with dVRK*** + +This file contains instructions on deploying custom dVRK files and guidelines for system workflow. The aim of this work is to establish a human-robot-collaboration research platform in robotic surgery, based on CoSTAR, a Behavior-Tree based framework for end-user instruction of industrial robots. A more detailed description can be found in our [IROS 2017 Workshop paper](https://smarts.lcsr.jhu.edu/wp-content/uploads/2017/04/costar_in_surgery.pdf). + +[![Video demonstration](https://img.youtube.com/vi/RqQNLZuuRUE/0.jpg)](https://www.youtube.com/watch?v=RqQNLZuuRUE) + +## Installation and dVRK deployment + +### Start dVRK only + +The [da Vinci Research Kit](https://github.com/jhu-dvrk/sawIntuitiveResearchKit/wiki) (dVRK) is an open-source research platform for medical robotics research. The following steps can be skipped if dVRK is already in a working status. + +- Disconnect firewire +- Turn on the controllers +- Connect firewire again + +Some general commands to work dVRK include the following: + +* Command line input: ls -al /dev/fw* + * Should return a list of FPGA-QLA information +* Command line input: qladisp 8 9 + * when nothing turns up, try source ~/catkin_ws/devel_release/setup.bash + +Now, to start dVRK, follow the instructions below: + +* **Command line input: qlacloserelays** +* **Get roscore running** +* **To run single arm without RViz and teleoperation, run**: + * rosrun dvrk_robot dvrk_console_json -j /path/to/catkin_ws/src/cisst-saw/sawIntuitiveResearchKit/share/jhu-dVRK/console-PSM2.json + * Use ipython script to drive the arm, such as: + * import dvrk + * p = dvrk.psm('PSM1') + * import PyKDL + * p.home() + * p.insert_tool(0.1) + * p.dmove(PyKDL.Vector(0.05, 0.0, 0.0)) + * p.move_joint_one(0.05, 0) + * p.open_jaw() + * **REMEMBER!** EVERYTIME after stopping the program (changing config files), if you want to start the arms again using other .json or .launch files, you should run qlacloserelays AGAIN. +* **To run PSM1-MTMR teleoperation, follow the steps below**: + * roslaunch dvrk_robot dvrk_master_slave_rviz.launch master:=MTMR slave:=PSM1 config:=/path/to/catkin_ws/src/cisst-saw/sawIntuitiveResearchKit/share/jhu-dVRK/console-MTMR-PSM1-Teleop.json + * Note: should place PSM1 in a "good" start position, i.e., pose not too strange. + * In the dVRK console, click "Home". Wait a second, you should see PSM starting to get powered and MTMR move to home position. Both in real world and in RViz. + * Then in dVRK console, click "Start", the teleoperation will be started. Hold MTMR (master teleoperator, right-hand side), pressed COAG foot pedal, you should be able to move the PSM1 teleoperatively. + * Click "Stop", then "Off", if you want to stop. + +### Custom file deployment + +To facilitate the process of setting up CoSTAR-dVRK, you can just replace or add the following files to dVRK repositories ([cisst-saw](https://github.com/jhu-cisst/cisst-saw) and [dvrk-ros](https://github.com/jhu-dvrk/dvrk-ros)). The files are available in the config_files folder. + +* **In repository: dvrk-ros/** + * REPLACE: dvrk_robot/launch/dvrk_master_slave_rviz.launch (The CoSTAR part does not work with tf_static, set it to false to use tf frame chain.) + * REPLACE: dvrk_robot/launch/dvrk_arm_rviz.launch (using an interactive marker in Rviz) + * REPLACE: dvrk_model/rviz_config/MTMR_PSM1.rviz (this includes an interactive marker in Rviz, for dvrk_teleop.launch) + * ADD: dvrk_model/rviz_config/PSM1_with_marker.rviz (this includes an interactive marker in Rviz) +* **In repository: cisst-saw/sawIntuitiveResearchKit/** + * REPLACE: share/console-MTMR-PSM1-Teleop.json (so that the coordinates are defined with respect to RCM point, you can also choose to use the original json config file and publish an identity transformation at /set_base_frame ROS topic, which could also set the reference frame to RCM point) + * REPLACE: share/console-MTMR-PSM1-MTML-PSM2-Teleop.json (the same reason as above) + +## User Instruction + +### Launch options + +There are different launch files for different system configurations. The names of the launch files are self-explanatory. + +* Do simulated single arm manipulation: roslaunch costar_bringup dvrk_psm.launch +* Do real world single arm manipulation: roslaunch costar_bringup dvrk_real.launch +* Do single arm teleoperation: roslaunch costar_bringup dvrk_teleop.launch +* Do two arm teleoperation (full mode): roslaunch costar_bringup dvrk_teleop_two_arms.launch + +### System workflow for two-arm operation + +* Run: roslaunch costar_bringup dvrk_teleop_two_arms.launch +* Wait until all windows are loaded successfully. +* In dVRK console, press "home" button. (For version 1.6.0, press "Power ON", then "Home") +* In dVRK console, press "start" teleoperation button. +* In CoSTAR Instructor interface, press "TEACH" button on the top. +* Move to the robot master console, get hold of the master operators, press COAG foot pedal and start tele-manipulation. +* When feeling good to record a waypoint, press CLUTCH foot pedal (in the mean time you can move the master operator to re-adjust the pose for long-range movement). +* In CoSTAR Instructor interface, click Menu -> Waypoints, you can probably find the time-stamped waypoint recorded. (You have to click some button on the waypoint window to refresh the list if not seeing the new waypoint. ) +* After collecting all waypoints, stop operating, and in CoSTAR Instructor interface, click "SERVO" button on the top. +* You can build a behavior tree as described in our workshop paper, with the waypoints recorded. +* In CoSTAR Interface, click "EXECUTE" button, then move to the robot master console. +* Press "COAG" foot pedal to start manipulation, when you feel you need the robot to do its job, press "CAMERA" foot pedal. +* The video demonstration can be found above. + +### Interactive marker and DMP + +We also created an interactive marker in RViz for better instructing the dVRK (i.e., not through master console). DMP stands for dynamic movement primitives, which is an experimental feature for automating trajectory generation. Please refer to the following video demonstrations for more details. + +* Interactive marker: [video](https://drive.google.com/open?id=0B_yGdvqsvxSIYUdtTHNPU0hiYjg) +* DMP: [video](https://drive.google.com/open?id=0B_yGdvqsvxSITDhCRWdZYVlicEk) + +## Contact + +For more questions regarding CoSTAR-dVRK, please contact Baichuan Jiang (baichuan@jhu.edu). \ No newline at end of file From 4075a18968cc822812a97403992b2c7d57ccdec9 Mon Sep 17 00:00:00 2001 From: Baichuan Jiang Date: Sun, 10 Jun 2018 15:54:24 -0400 Subject: [PATCH 25/25] rename README file --- costar_robot/dvrk_model/{CoSTAR-dVRK.md => Readme.md} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename costar_robot/dvrk_model/{CoSTAR-dVRK.md => Readme.md} (100%) diff --git a/costar_robot/dvrk_model/CoSTAR-dVRK.md b/costar_robot/dvrk_model/Readme.md similarity index 100% rename from costar_robot/dvrk_model/CoSTAR-dVRK.md rename to costar_robot/dvrk_model/Readme.md