diff --git a/costar_bringup/launch/config/dvrk.launch b/costar_bringup/launch/config/dvrk.launch index 82775ea7..56b094c1 100644 --- a/costar_bringup/launch/config/dvrk.launch +++ b/costar_bringup/launch/config/dvrk.launch @@ -3,12 +3,16 @@ [0.30, -1.33, -1.80, -0.27, 1.50, 1.60] + false - 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_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_bringup/launch/dvrk_real.launch b/costar_bringup/launch/dvrk_real.launch new file mode 100644 index 00000000..39d1d786 --- /dev/null +++ b/costar_bringup/launch/dvrk_real.launch @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/costar_bringup/launch/dvrk_teleop.launch b/costar_bringup/launch/dvrk_teleop.launch new file mode 100644 index 00000000..a99082f6 --- /dev/null +++ b/costar_bringup/launch/dvrk_teleop.launch @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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..8dc2c2ba --- /dev/null +++ b/costar_bringup/launch/dvrk_teleop_two_arms.launch @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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/CMakeLists.txt b/costar_gripper/gripper_dvrk/CMakeLists.txt new file mode 100644 index 00000000..bd986ef1 --- /dev/null +++ b/costar_gripper/gripper_dvrk/CMakeLists.txt @@ -0,0 +1,197 @@ +cmake_minimum_required(VERSION 2.8.3) +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 + gripper_manager +) + +## 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 +# std_msgs # Or other packages containing 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 gripper_dvrk +# CATKIN_DEPENDS gripper_manager +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# 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(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## 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(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_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 ${PROJECT_NAME} ${PROJECT_NAME}_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_gripper_dvrk.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_gripper/gripper_dvrk/package.xml b/costar_gripper/gripper_dvrk/package.xml new file mode 100644 index 00000000..de2726b0 --- /dev/null +++ b/costar_gripper/gripper_dvrk/package.xml @@ -0,0 +1,52 @@ + + + gripper_dvrk + 0.0.0 + The gripper_dvrk package + + + + + ziri + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + gripper_manager + gripper_manager + + + + + + + + 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 new file mode 100644 index 00000000..f6993b08 --- /dev/null +++ b/costar_gripper/gripper_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=['gripper_dvrk'], + package_dir={'': 'src'}, +) + +setup(**d) + 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..83406535 --- /dev/null +++ b/costar_gripper/gripper_dvrk/src/gripper_dvrk/__init__.py @@ -0,0 +1,7 @@ +### ROS imports +import rospy +from psm_gripper_server import * +# __all__ = ['SimpleSModelServer','SimpleCModelServer'] + +# from s_model_server import * +# from c_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/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_gripper/gripper_manager/src/gripper_manager/costar_gripper.py b/costar_gripper/gripper_manager/src/gripper_manager/costar_gripper.py index b6e43510..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,8 +77,8 @@ def statusInterpreter(self,status): def status_cb(self,msg): if self.verbose: - rospy.loginfo(self.statusInterpreter(msg)) - self.predicator.handle(msg) - self.predicator.tick() + rospy.loginfo(self.statusInterpreter(msg)) + if self.predicator is not None: + self.predicator.handle(msg) 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_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': 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 75f40698..26c8d0bc 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 @@ -167,9 +167,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 ff74edc0..f0cd148c 100644 --- a/costar_instructor/instructor_plugins/src/instructor_plugins/smartmove_multipurpose.py +++ b/costar_instructor/instructor_plugins/src/instructor_plugins/smartmove_multipurpose.py @@ -179,9 +179,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_predicator/predicator_dvrk/CMakeLists.txt b/costar_predicator/predicator_dvrk/CMakeLists.txt new file mode 100644 index 00000000..400d181c --- /dev/null +++ b/costar_predicator/predicator_dvrk/CMakeLists.txt @@ -0,0 +1,189 @@ +cmake_minimum_required(VERSION 2.8.3) +project(predicator_dvrk) + +## 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..e0f80c0c --- /dev/null +++ b/costar_predicator/predicator_dvrk/package.xml @@ -0,0 +1,56 @@ + + + predicator_dvrk + 0.0.0 + The predicator_dvrk package + + + + + chris + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + predicator_core + predicator_msgs + + predicator_core + predicator_msgs + + + + + + + + diff --git a/costar_predicator/predicator_dvrk/scripts/simple_predicator_dvrk.py b/costar_predicator/predicator_dvrk/scripts/simple_predicator_dvrk.py new file mode 100755 index 00000000..7b2c230a --- /dev/null +++ b/costar_predicator/predicator_dvrk/scripts/simple_predicator_dvrk.py @@ -0,0 +1,9 @@ +#!/usr/bin/env python + +import rospy +from predicator_dvrk import dvrkPredicator + +rospy.init_node('predicator_dvrk') + +dvrkpre = dvrkPredicator() +dvrkpre.spin() diff --git a/costar_predicator/predicator_dvrk/setup.py b/costar_predicator/predicator_dvrk/setup.py new file mode 100644 index 00000000..a75a096d --- /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_dvrk'], + 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..6dbd446a --- /dev/null +++ b/costar_predicator/predicator_dvrk/src/predicator_dvrk/__init__.py @@ -0,0 +1 @@ +from dvrkp import * diff --git a/costar_predicator/predicator_dvrk/src/predicator_dvrk/dvrkp.py b/costar_predicator/predicator_dvrk/src/predicator_dvrk/dvrkp.py new file mode 100644 index 00000000..68c54b90 --- /dev/null +++ b/costar_predicator/predicator_dvrk/src/predicator_dvrk/dvrkp.py @@ -0,0 +1,81 @@ +import rospy +from predicator_msgs.msg import * +from sensor_msgs.msg import Joy + +class dvrkPredicator: + + def __init__(self,publish_predicates=True,start_subscriber=True,gripper_name='psm_gripper'): + + 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 + self.camera_pressed = False + + if publish_predicates: + # create predicator things + self.pub = rospy.Publisher("predicator/input",PredicateList,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.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) + #self.sub5 = rospy.Subscriber("/dvrk/footpedals/camera",Joy,self.callback) + + self.name = rospy.get_name() + + def callbackClutch(self, msg): + # if this is for the clutch -- set clutch pressed + 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 + + 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 + ''' + 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.predicate_msg = PredicateList() + 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) + + ''' + update and spin + ''' + def spin(self,rate=10): + spin_rate = rospy.Rate(rate) + while not rospy.is_shutdown(): + self.tick() + spin_rate.sleep() + 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/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/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_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/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/src/costar_robot/costar_arm.py b/costar_robot/costar_robot_manager/src/costar_robot/costar_arm.py index 243e904b..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))) @@ -196,6 +197,7 @@ def __init__(self, # self.robot_state.joint_state.name = self.joint_names 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")) if has_gripper: @@ -661,6 +663,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": 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..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 @@ -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", @@ -142,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' diff --git a/costar_robot/dvrk_model/Readme.md b/costar_robot/dvrk_model/Readme.md new file mode 100644 index 00000000..a02216ee --- /dev/null +++ b/costar_robot/dvrk_model/Readme.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 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] + + + + + + + + + + + + + diff --git a/install_kinetic.sh b/install_kinetic.sh new file mode 100644 index 00000000..73d1e0e8 --- /dev/null +++ b/install_kinetic.sh @@ -0,0 +1,27 @@ +#!/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/gt-ros-pkg/hrl-kdl.git +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 +echo "Ignore COSTAR_PERCEPTION until you have installed its dependencies." +touch costar_stack/costar_perception/CATKIN_IGNORE +catkin build --continue +source ../devel/setup.bash