def __init__(self, arm_name, ros_namespace='SUJ/', expected_interval=1.0):
"""Constructor. This initializes a few data members.It
requires a arm name, this will be used to find the ROS
topics for the arm being controlled. For example if the
user wants `PSM1`, the ROS topics will be from the namespace
`SUJ/PSM1`"""
# data members, event based
self.__arm_name = arm_name
self.__ros_namespace = ros_namespace
self.__full_ros_namespace = self.__ros_namespace + self.__arm_name
# crtk features
self.__crtk_utils = crtk.utils(self, self.__full_ros_namespace,
expected_interval)
# add crtk features that we need and are supported by the dVRK
self.__crtk_utils.add_operating_state()
self.__crtk_utils.add_measured_js()
self.__crtk_utils.add_measured_cp()
self.__crtk_utils.add_move_jp()
self.local = self.__Local(self.__full_ros_namespace + '/local',
expected_interval)
# create node
if not rospy.get_node_uri():
rospy.init_node('arm_suj_api_' + self.__arm_name,
anonymous=True,
log_level=rospy.WARN)
else:
rospy.logdebug(rospy.get_caller_id() +
' -> ROS already initialized')
def configure(self, device_namespace):
# ROS initialization
if not rospy.get_node_uri():
rospy.init_node('crtk_move_cp_example',
anonymous=True,
log_level=rospy.WARN)
print(rospy.get_caller_id() +
' -> configuring crtk_device_test for: ' + device_namespace)
# populate this class with all the ROS topics we need
self.crtk_utils = crtk.utils(self, device_namespace)
self.crtk_utils.add_operating_state()
self.crtk_utils.add_setpoint_cp()
self.crtk_utils.add_move_cp()
def configure(self, device_namespace):
# ROS initialization
if not rospy.get_node_uri():
rospy.init_node('crtk_servo_cp_example', anonymous = True, log_level = rospy.WARN)
print(rospy.get_caller_id() + ' -> configuring crtk_device_test for: ' + device_namespace)
# populate this class with all the ROS topics we need
self.crtk_utils = crtk.utils(self, device_namespace)
self.crtk_utils.add_operating_state()
self.crtk_utils.add_setpoint_cp()
self.crtk_utils.add_servo_cp()
# for all examples
self.duration = 10 # 10 seconds
self.rate = 200 # aiming for 200 Hz
self.samples = self.duration * self.rate
def __init__(self, namespace):
self.crtk = crtk.utils(self, namespace)
self.crtk.add_setpoint_js()
self.crtk.add_servo_jp()
def __init__(self, namespace):
self.crtk = crtk.utils(self, namespace)
self.crtk.add_operating_state()
self.crtk.add_setpoint_cp()
self.crtk.add_servo_cp()
def __init__(self, namespace):
self.crtk = crtk.utils(self, namespace)
self.crtk.add_measured_js()
def __init__(self, namespace):
self.crtk = crtk.utils(self, namespace)
self.crtk.add_operating_state()
self.crtk.add_measured_cp()
def __init__(self, ros_namespace, expected_interval):
self.__crtk_utils = crtk.utils(self, ros_namespace,
expected_interval)
self.__crtk_utils.add_measured_cp()
def __init_arm(self, arm_name, ros_namespace, expected_interval):
"""Constructor. This initializes a few data members.It
requires a arm name, this will be used to find the ROS
topics for the arm being controlled. For example if the
user wants `PSM1`, the ROS topics will be from the namespace
`PSM1`"""
# data members, event based
self.__arm_name = arm_name
self.__ros_namespace = ros_namespace
self.__full_ros_namespace = ros_namespace + arm_name
# crtk features
self.__crtk_utils = crtk.utils(self, self.__full_ros_namespace,
expected_interval)
# add crtk features that we need and are supported by the dVRK
self.__crtk_utils.add_operating_state()
self.__crtk_utils.add_setpoint_js()
self.__crtk_utils.add_setpoint_cp()
self.__crtk_utils.add_measured_js()
self.__crtk_utils.add_measured_cp()
self.__crtk_utils.add_measured_cv()
self.__crtk_utils.add_servo_jp()
self.__crtk_utils.add_servo_jr()
self.__crtk_utils.add_servo_cp()
self.__crtk_utils.add_servo_jf()
self.__crtk_utils.add_move_jp()
self.__crtk_utils.add_move_jr()
self.__crtk_utils.add_move_cp()
self.spatial = self.__MeasuredServoCf(
self.__full_ros_namespace + '/spatial', expected_interval)
self.body = self.__MeasuredServoCf(self.__full_ros_namespace + '/body',
expected_interval)
self.local = self.__Local(self.__full_ros_namespace + '/local',
expected_interval)
self.__sub_list = []
self.__pub_list = []
# publishers
frame = PyKDL.Frame()
self.__body_set_cf_orientation_absolute_pub = rospy.Publisher(
self.__full_ros_namespace + '/body/set_cf_orientation_absolute',
std_msgs.msg.Bool,
latch=True,
queue_size=1)
self.__use_gravity_compensation_pub = rospy.Publisher(
self.__full_ros_namespace + '/use_gravity_compensation',
std_msgs.msg.Bool,
latch=True,
queue_size=1)
self.__trajectory_j_set_ratio_pub = rospy.Publisher(
self.__full_ros_namespace + '/trajectory_j/set_ratio',
std_msgs.msg.Float64,
latch=True,
queue_size=1)
self.__pub_list = [
self.__body_set_cf_orientation_absolute_pub,
self.__use_gravity_compensation_pub,
self.__trajectory_j_set_ratio_pub
]
# subscribers
self.__trajectory_j_ratio_sub = rospy.Subscriber(
self.__full_ros_namespace + '/trajectory_j/ratio',
std_msgs.msg.Float64, self.__trajectory_j_ratio_cb)
self.__sub_list = [self.__trajectory_j_ratio_sub]
# create node
if not rospy.get_node_uri():
rospy.init_node('arm_api', anonymous=True, log_level=rospy.WARN)
else:
rospy.logdebug(rospy.get_caller_id() +
' -> ROS already initialized')
def __init__(self, ros_namespace, expected_interval):
self.__crtk_utils = crtk.utils(self, ros_namespace,
expected_interval)
self.__crtk_utils.add_measured_cf()
self.__crtk_utils.add_servo_cf()
self.__crtk_utils.add_jacobian()
