def get_pub_string(self):
s1 = "%s,%s,%s,%s" % (self.node_name, self.drink_topic,
self.drink_topic_type, rospy.get_node_uri())
#s1= "%s,%s,%s" % (self.node_name,self.drink_topic,rospy.get_node_uri())
s2 = "%s,%s,%s,%s" % (self.node_name, self.orders_topic,
self.orders_topic_type, rospy.get_node_uri())
#return s1
return s1 + "\n" + s2
def test_unpublish(self):
node_proxy = xmlrpclib.ServerProxy(rospy.get_node_uri())
_, _, pubs = node_proxy.getPublications('/foo')
pubs = [p for p in pubs if p[0] != '/rosout']
self.assert_(not pubs, pubs)
print "Publishing ", PUBTOPIC
pub = rospy.Publisher(PUBTOPIC, String)
topic = rospy.resolve_name(PUBTOPIC)
_, _, pubs = node_proxy.getPublications('/foo')
pubs = [p for p in pubs if p[0] != '/rosout']
self.assertEquals([[topic, String._type]], pubs, "Pubs were %s" % pubs)
# publish about 10 messages for fun
for i in xrange(0, 10):
pub.publish(String("hi [%s]" % i))
time.sleep(0.1)
# begin actual test by unsubscribing
pub.unregister()
# make sure no new messages are received in the next 2 seconds
timeout_t = time.time() + 2.0
while timeout_t < time.time():
time.sleep(1.0)
self.assert_(_last_callback is None)
# verify that close cleaned up master and node state
_, _, pubs = node_proxy.getPublications('/foo')
pubs = [p for p in pubs if p[0] != '/rosout']
self.assert_(not pubs, "Node still has pubs: %s" % pubs)
n = rospy.get_caller_id()
self.assert_(not rostest.is_publisher(topic, n),
"publication is still active on master")
def __init__(self):
# data members
self.__bridge = CvBridge()
self.__img_color = []
self.__img_depth = []
self.__points_list = []
self.__points_ros_msg = PointCloud2()
# load calibration data
loadfilename = ('calibration_files/calib_zivid.npz')
with np.load(loadfilename) as X:
_, self.__mtx, self.__dist, _, _ = [
X[n] for n in ('ret', 'mtx', 'dist', 'rvecs', 'tvecs')
]
# ROS subscriber
rospy.Subscriber('/zivid_camera/color/image_color/compressed',
CompressedImage, self.__img_color_cb)
rospy.Subscriber('/zivid_camera/depth/image_raw', Image,
self.__img_depth_cb)
# rospy.Subscriber('/zivid_camera/points', PointCloud2, self.__pcl_cb) # not used in this time
# create ROS node
if not rospy.get_node_uri():
rospy.init_node('Image_pipeline_node',
anonymous=True,
log_level=rospy.WARN)
print("ROS node initialized")
else:
rospy.logdebug(rospy.get_caller_id() +
' -> ROS already initialized')
self.interval_ms = 300
self.rate = rospy.Rate(1000.0 / self.interval_ms)
self.main()
def __init__(self):
# data members
self.__bridge = CvBridge()
self.__img_raw_left = []
self.__img_raw_right = []
# threading
threading.Thread.__init__(self)
self.__interval_ms = 10
self.__stop_flag = False
# subscriber
self.__sub_list = [
rospy.Subscriber('/kinect2/hd/image_color/compressed',
CompressedImage, self.__img_raw_left_cb),
rospy.Subscriber('/kinect2/hd/image_color/compressed',
CompressedImage, self.__img_raw_right_cb)
]
# create node
if not rospy.get_node_uri():
rospy.init_node('ImageConverter_node',
anonymous=True,
log_level=rospy.WARN)
self.rate = rospy.Rate(1000.0 / self.__interval_ms)
else:
rospy.logdebug(rospy.get_caller_id() +
' -> ROS already initialized')
self.start()
rospy.spin()
def test_unpublish(self):
node_proxy = ServerProxy(rospy.get_node_uri())
_, _, pubs = node_proxy.getPublications('/foo')
pubs = [p for p in pubs if p[0] != '/rosout']
self.assert_(not pubs, pubs)
print("Publishing ", PUBTOPIC)
pub = rospy.Publisher(PUBTOPIC, String)
topic = rospy.resolve_name(PUBTOPIC)
_, _, pubs = node_proxy.getPublications('/foo')
pubs = [p for p in pubs if p[0] != '/rosout']
self.assertEquals([[topic, String._type]], pubs, "Pubs were %s"%pubs)
# publish about 10 messages for fun
for i in range(0, 10):
pub.publish(String("hi [%s]"%i))
time.sleep(0.1)
# begin actual test by unsubscribing
pub.unregister()
# make sure no new messages are received in the next 2 seconds
timeout_t = time.time() + 2.0
while timeout_t < time.time():
time.sleep(1.0)
self.assert_(_last_callback is None)
# verify that close cleaned up master and node state
_, _, pubs = node_proxy.getPublications('/foo')
pubs = [p for p in pubs if p[0] != '/rosout']
self.assert_(not pubs, "Node still has pubs: %s"%pubs)
n = rospy.get_caller_id()
self.assert_(not rostest.is_publisher(topic, n), "publication is still active on master")
def test_image_encoding(self):
if not rospy.get_node_uri():
rospy.init_node("topic_store_tests")
client = MongoStorage(collection="python_tests")
# Test ROS_Message Serialisation with common Image Types
from topic_store.utils import _numpy_types as numpy_types
for encoding, np_type in numpy_types.items():
size = (32, 32, np_type[1]) if np_type[1] != 1 else (32, 32)
random_array = np.random.random(size)
typed_array = (random_array * 255.0).astype(np_type[0])
message = ros_numpy.msgify(Image, typed_array, encoding=encoding)
# Insert image message
im_document = TopicStore({"image": message})
im_result = client.insert_one(im_document)
# Get image message and check data is the same
returned_im_document = client.find_by_id(im_result.inserted_id)
assert returned_im_document.id == im_result.inserted_id
retrieved_array = ros_numpy.numpify(
returned_im_document.msgs["image"])
np.testing.assert_equal(typed_array, retrieved_array)
# Delete image message
client.delete_by_id(im_result.inserted_id)
def test_scenarios(self):
if not rospy.get_node_uri():
rospy.init_node("topic_store_tests")
scenario_file = get_package_root(
) / "scenarios" / "default_config.yaml"
scenario = ScenarioFileParser(scenario_file)
print("All scenario parser tests passed!")
def __init__(self, interval_ms):
"""
:param interval_ms: sampling interval in ms
"""
self.__topic_cnt = 0
self.__sub_list = []
self.__topic_list = []
self.__msg_type_list = []
self.__data_recorded = []
self.__data_received = []
self.__file_name = []
# thread
self.__interval_ms = interval_ms
self.__stop_flag = False
self.__data_receive_event = threading.Event()
# create node
if not rospy.get_node_uri():
rospy.init_node('Recording_ROS_Data_node', anonymous=True, log_level=rospy.WARN)
self.rate = rospy.Rate(1000.0 / self.__interval_ms)
self.start()
else:
rospy.logdebug(rospy.get_caller_id() + ' -> ROS already initialized')
def new_init_node(*args, **nargs):
global check_interval
global our_uri
original_rospy_init_node(*args, **nargs)
our_uri = rospy.get_node_uri()
check_interval = rospy.Duration(master_check_interval)
check_master()
def __init__(self, arm_name, ros_namespace = '/dvrk/SUJ/'):
"""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
`/dvrk/SUJ/PSM1`"""
# data members, event based
self.__arm_name = arm_name
self.__ros_namespace = ros_namespace
# continuous publish from dvrk_bridge
self.__position_joint_current = numpy.array(0, dtype = numpy.float)
self.__position_cartesian_current = PyKDL.Frame()
self.__position_cartesian_local_current = PyKDL.Frame()
# publishers
self.__full_ros_namespace = self.__ros_namespace + self.__arm_name
self.__set_position_joint_pub = rospy.Publisher(self.__full_ros_namespace
+ '/set_position_joint',
JointState, latch = False, queue_size = 1)
# subscribers
rospy.Subscriber(self.__full_ros_namespace + '/state_joint_current',
JointState, self.__state_joint_current_cb),
rospy.Subscriber(self.__full_ros_namespace + '/position_cartesian_current',
PoseStamped, self.__position_cartesian_current_cb)
rospy.Subscriber(self.__full_ros_namespace + '/position_cartesian_local_current',
PoseStamped, self.__position_cartesian_local_current_cb)
# create node
if not rospy.get_node_uri():
rospy.init_node('suj_api', anonymous = True, log_level = rospy.WARN)
else:
rospy.logdebug(rospy.get_caller_id() + ' -> ROS already initialized')
def new_init_node(*args, **nargs):
global check_interval
global our_uri
original_rospy_init_node(*args, **nargs)
our_uri = rospy.get_node_uri()
check_interval = rospy.Duration(master_check_interval)
check_master()
def __init__(self, config):
super().__init__()
node_name = config.get('node_name', 'GazeboEnvNode')
rospy.init_node(node_name, disable_signals=True)
self.gz_state = GazeboState.INIT
self.config = config
self.state_pub = Publisher('/gazebo/set_model_state',
ModelState,
queue_size=10)
self.pause = ServiceProxy("/gazebo/pause_physics", Empty)
self.unpause = ServiceProxy("/gazebo/unpause_physics", Empty)
self.reset_proxy = ServiceProxy("/gazebo/reset_simulation", Empty)
self.add_agents()
time.sleep(0.2)
self.action_space = Dict(
{k: v.action_space
for k, v in self.agents.items()})
self.observation_space = Dict(
{k: v.observation_space
for k, v, in self.agents.items()})
self.unpause_gazebo()
# check_for_ROS()
self.pause_gazebo()
print('=' * 100)
print(self)
print(rospy.get_name())
print(rospy.get_namespace())
print(rospy.get_node_uri())
print('=' * 100)
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 __init_teleop_psm(self, teleop_name, ros_namespace = '/dvrk/'):
"""Constructor. This initializes a few data members. It
requires a teleop name, this will be used to find the ROS topics
for the console being controlled."""
# data members
self.__teleop_name = teleop_name
self.__ros_namespace = ros_namespace
self.__full_ros_namespace = self.__ros_namespace + self.__teleop_name
self.__scale = 0.0
# publishers
self.__set_scale_pub = rospy.Publisher(self.__full_ros_namespace
+ '/set_scale',
Float32, latch = True, queue_size = 1)
self.__set_registration_rotation_pub = rospy.Publisher(self.__full_ros_namespace
+ '/set_registration_rotation',
Quaternion, latch = True, queue_size = 1)
self.__set_desired_state_pub = rospy.Publisher(self.__full_ros_namespace
+ '/set_desired_state',
String, latch = True, queue_size = 1)
# subscribers
rospy.Subscriber(self.__full_ros_namespace
+ '/scale',
Float32, self.__scale_cb)
# create node
if not rospy.get_node_uri():
rospy.init_node('teleop_api', anonymous = True, log_level = rospy.WARN)
else:
rospy.logdebug(rospy.get_caller_id() + ' -> ROS already initialized')
def __init__(self, ros_namespace='/dvrk'):
# data members, event based
self.__arm_name = ['/PSM1', '/PSM2']
self.__ros_namespace = ros_namespace
self.__get_position_event = [threading.Event(), threading.Event()]
self.__get_jaw_event = [threading.Event(), threading.Event()]
# continuous publish from dvrk_bridge
self.__position_cartesian_current_msg = [Pose(), Pose()]
self.__position_cartesian_current_frame = [PyKDL.Frame(), PyKDL.Frame()]
self.__position_joint_current = [0.0, 0.0]
self.__position_jaw_current = [0.0, 0.0]
self.__pose_cartesian_current = [0.0, 0.0]
self.__sub_list = []
self.__pub_list = []
# publisher
frame = PyKDL.Frame()
self.__full_ros_namespace = [self.__ros_namespace + name for name in self.__arm_name]
self.__set_position_joint_pub = [rospy.Publisher(name + '/set_position_joint', JointState,
latch=True, queue_size=1), name in self.__full_ros_namespace]
self.__set_position_goal_joint_pub = [rospy.Publisher(name + '/set_position_goal_joint',
JointState, latch=True, queue_size=1) for name in self.__full_ros_namespace]
self.__set_position_cartesian_pub = [rospy.Publisher(name + '/set_position_cartesian', Pose, latch=True, queue_size=1) for name in self.__full_ros_namespace]
self.__set_position_goal_cartesian_pub = [rospy.Publisher(name + '/set_position_goal_cartesian', Pose, latch=True, queue_size=1) for name in self.__full_ros_namespace]
self.__set_position_jaw_pub = [rospy.Publisher(name + '/set_position_jaw', JointState, latch=True, queue_size=1) for name in self.__full_ros_namespace]
self.__set_position_goal_jaw_pub = [rospy.Publisher(name + '/set_position_goal_jaw', JointState, latch=True, queue_size=1) for name in self.__full_ros_namespace]
self.__pub_list = [self.__set_position_joint_pub,
self.__set_position_goal_joint_pub,
self.__set_position_cartesian_pub,
self.__set_position_goal_cartesian_pub,
self.__set_position_jaw_pub,
self.__set_position_goal_jaw_pub]
self.__position_cartesian_current_sub = [rospy.Subscriber(name + '/position_cartesian_current',
PoseStamped, self.__position_cartesian_current_cb) for name in self.__full_ros_namespace]
self.__position_joint_current_sub = [rospy.Subscriber(self.__full_ros_namespace[0] + '/state_joint_current',
JointState, self.__position_joint_current1_cb),
rospy.Subscriber(self.__full_ros_namespace[1] + '/state_joint_current',
JointState, self.__position_joint_current2_cb)]
self.__position_jaw_current_sub = [rospy.Subscriber(self.__full_ros_namespace[0] + '/state_jaw_current',
JointState, self.__position_jaw_current1_cb),
rospy.Subscriber(self.__full_ros_namespace[1] + '/state_jaw_current',
JointState, self.__position_jaw_current2_cb)]
self.__sub_list = [self.__position_cartesian_current_sub,
self.__position_joint_current_sub,
self.__position_jaw_current_sub]
# create node
if not rospy.get_node_uri():
rospy.init_node('dvrkDualArm_node', anonymous=True, log_level=rospy.WARN)
self.interval_ms = 20
self.rate = rospy.Rate(1000.0 / self.interval_ms)
else:
rospy.logdebug(rospy.get_caller_id() + ' -> ROS already initialized')
def __init__(self, node_name):
rospy.logdebug("YoubotProxy __init__")
super(YoubotProxy,self).__init__()
self.init_done = False # indicates that the object was initialized
# init ros node
rospy.init_node(node_name, anonymous=True, log_level=rospy.INFO)
rospy.loginfo("ROS node initialized: " + rospy.get_name())
rospy.loginfo("node namespace is : " + rospy.get_namespace())
rospy.loginfo("node uri is : " + rospy.get_node_uri())
# init object attributes
self.arm_num = rospy.get_param("~arm_num",1)
rospy.loginfo("arm number: " + str(self.arm_num))
self.gripper_move_duration = rospy.Duration(rospy.get_param("~gripper_move_duration",600.0))
rospy.loginfo("gripper move duration: " + str(self.gripper_move_duration))
self.arm_move_duration = rospy.Duration(rospy.get_param("~arm_move_duration",600.0))
rospy.loginfo("arm move duration: " + str(self.arm_move_duration))
# init joint_states subscriber
self._joint_positions_arm = [0]*5
self._joint_velocities_arm = [0]*5
self._joint_positions_gripper = [0]*2
# todo: create thread event object for joint_states variable
self._joint_states_topic = rospy.get_param("~joint_states_topic", "/arm_" + str(self.arm_num) + "/joint_states")
self._joint_states_sub = rospy.Subscriber(self._joint_states_topic, JointState, self.joint_states_cb)
# Gripper distance tolerance: 1 mm
self.gripper_distance_tol = rospy.get_param("~gripper_distance_tol", 0.05)
# Joint distance tolerance: 1/10 radian tolerance (1.2 degrees)
self.arm_joint_distance_tol = rospy.get_param("~joint_distance_tol",0.025)
rospy.loginfo("arm joint position tolerance: " + str(self.arm_joint_distance_tol))
self.arm_joint_velocity_tol = rospy.get_param("~joint_velocity_tol",0.05)
rospy.loginfo("arm joint velocity tolerance: " + str(self.arm_joint_velocity_tol))
# connect to modbus service
rospy.wait_for_service('youbot_modbus_server/get_station_status')
rospy.wait_for_service('youbot_modbus_server/get_button_status')
self.get_station_status = rospy.ServiceProxy('youbot_modbus_server/get_station_status', YoubotModbusSensorMsg)
self.get_button_status = rospy.ServiceProxy('youbot_modbus_server/get_button_status', YoubotModbusButtonMsg)
rospy.loginfo("modbus clients started")
print "I have made it here!"
# init arm publisher
self._arm_pub = rospy.Publisher("arm_" + str(self.arm_num) + "/arm_controller/position_command", JointPositions)
rospy.loginfo("Created arm joint position publisher ")
# init gripper action client
self._gripper_pub = rospy.Publisher("arm_" + str(self.arm_num) + "/gripper_controller/position_command", JointPositions)
rospy.loginfo("Created gripper joint position publisher ")
rospack = rospkg.RosPack()
path_to_posedict_yaml = rospy.get_param('~joint_pose_dict', rospack.get_path('twoarm_cage') + "/config/joint_pos_defs.yaml")
path_to_cmds_yaml = rospy.get_param('~cmd_seq', rospack.get_path('twoarm_cage') + "/config/arm" + str(self.arm_num) + "_commands.yaml")
self.load_control_plan(path_to_posedict_yaml, path_to_cmds_yaml)
# set init done flag
self.init_done = True
def test_type_coercion(self):
if not rospy.get_node_uri():
rospy.init_node("topic_store_tests")
actual_expected = [
({
"0": 0,
1: [1, 1],
2.0: {1, 1, 1}
}, {
"0": 0,
"1": [1, 1],
"2.0": [1]
}), # set {1, 1, 1} should now be a list
([1, 2, 3, 4, 5], [1, 2, 3, 4, 5]),
((1, 2, 3, 4, 5), [1, 2, 3, 4, 5]),
({1, 2, 3, 4, 5}, [1, 2, 3, 4, 5]),
]
# Default Parser should do very little
parser = DefaultTypeParser()
for actual, expected in actual_expected:
parsed = parser(actual)
print(parsed, expected)
assert parsed == expected
parser.add_converters({int: float, float: int})
actual_expected = [
({
"0": 0,
1: [1, 1],
2.0: {1, 1, 1}
}, {
"0": 0.0,
"1": [1.0, 1.0],
"2.0": [1.0]
}),
([1, 2, 3, 4, 5], [1.0, 2.0, 3.0, 4.0, 5.0]),
((1, 2, 3, 4, 5), [1.0, 2.0, 3.0, 4.0, 5.0]),
({1.0, 2.0, 3.0, 4.0, 5.0}, [1, 2, 3, 4, 5]),
]
for actual, expected in actual_expected:
parsed = parser(actual)
print(parsed, expected)
assert parsed == expected
mongo_parser = MongoDBParser()
actual_expected = [
(rospy.Time.now(), dict),
(datetime.now(), type(datetime.now())),
("test".encode("utf-8"), str),
("test".encode("utf-16"), bson.binary.Binary),
]
for actual, expected in actual_expected:
parsed = mongo_parser(actual)
print(parsed, expected)
assert type(parsed) == expected
print("All Type Coercion tests passed!")
def get_publishers_info(self):
s1 = {
"node": self.node_name,
"topic": self.annotation_topic,
"topic_type": self.annotation_topic_type,
"publisher_url": rospy.get_node_uri()
}
return [s1]
def trigger(self, msg):
if self.experiment_timestamp_given == False:
self.experiment_timestamp_offset = rospy.get_rostime()
else:
rospy.logerr(
'[' + rospy.get_node_uri() +
'] Experimental time already triggered. Not overridding the previous value.'
)
def test_topic_store(self):
if not rospy.get_node_uri():
rospy.init_node("topic_store_tests")
sample_tree = {"ros_msg": "/rosout", "int": 1, "float": 1.0, "str": "1", "dict": {0: 0}, "list": [0]}
tree = SubscriberTree(sample_tree)
rospy.sleep(1)
messages = tree.get_message_tree()
test_file = pathlib.Path(__file__).parent / "topic_store_tests.topic_store"
try:
test_file.unlink()
except OSError:
pass
storage = TopicStorage(test_file)
# Try getting an element that doesn't exist
try:
s = list(storage)[0]
raise AssertionError()
except IndexError:
pass
# Try looping over elements that don't exist
for _ in storage:
raise AssertionError()
# Add a message and check
write_n = 5
for _ in range(write_n):
storage.insert_one(messages)
# Check iterator indexing
stored_items = write_n
read_items = 0
for _ in storage:
read_items += 1
assert read_items == stored_items
# Check loading
storage = TopicStorage(test_file)
storage.insert_one(TopicStore({0: 0}))
stored_items += 1
for s in storage:
print(s)
# Test API
from topic_store import load
loaded_messages = load(test_file)
read_items = 0
for _ in storage:
read_items += 1
assert read_items == stored_items
print("All TopicStorage tests passed!")
def __init__(self, node_name):
rospy.logdebug("YoubotProxy __init__")
super(YoubotProxy,self).__init__()
self.init_done = False # indicates that the object was initialized
# init ros node
rospy.init_node(node_name, anonymous=True, log_level=rospy.INFO)
rospy.loginfo("ROS node initialized: " + rospy.get_name())
rospy.loginfo("node namespace is : " + rospy.get_namespace())
rospy.loginfo("node uri is : " + rospy.get_node_uri())
# init object attributes
self.arm_num = rospy.get_param("~arm_num",2)
rospy.loginfo("arm number: " + str(self.arm_num))
self.gripper_move_duration = rospy.Duration(rospy.get_param("~gripper_move_duration",600.0))
rospy.loginfo("gripper move duration: " + str(self.gripper_move_duration))
self.arm_move_duration = rospy.Duration(rospy.get_param("~arm_move_duration",600.0))
rospy.loginfo("arm move duration: " + str(self.arm_move_duration))
# init joint_states subscriber
self._joint_positions_arm = [0]*5
self._joint_positions_gripper = [0]*2
# todo: create thread event object for joint_states variable
self._joint_states_topic = rospy.get_param("~joint_states_topic", "/arm_" + str(self.arm_num) + "/joint_states")
self._joint_states_sub = rospy.Subscriber(self._joint_states_topic, JointState, self.joint_states_cb)
# Gripper distance tolerance: 1 mm
self.gripper_distance_tol = rospy.get_param("~gripper_distance_tol", 0.05)
# Joint distance tolerance: 1/10 radian tolerance (1.2 degrees)
self.arm_joint_distance_tol = rospy.get_param("~joint_distance_tol",0.025)
# init moveit
# try:
# moveit_commander.roscpp_initialize(sys.argv)
# self.arm_group = moveit_commander.MoveGroupCommander("manipulator")
# self.arm_group.set_planning_time(8)
# self.arm_group.set_pose_reference_frame("base_link")
# rospy.loginfo("planning group created for manipulator")
# except:
# pass
# init arm publisher
self._arm_pub = rospy.Publisher("arm_" + str(self.arm_num) + "/arm_controller/position_command", JointPositions)
rospy.loginfo("Created arm joint position publisher ")
# init gripper action client
self._gripper_pub = rospy.Publisher("arm_" + str(self.arm_num) + "/gripper_controller/position_command", JointPositions)
rospy.loginfo("Created gripper joint position publisher ")
rospack = rospkg.RosPack()
path_to_posedict_yaml = rospy.get_param('~joint_pose_dict', rospack.get_path('twoarm_cage') + "/config/joint_pos_defs.yaml")
path_to_cmds_yaml = rospy.get_param('~cmd_seq', rospack.get_path('twoarm_cage') + "/config/arm" + str(self.arm_num) + "_commands.yaml")
self.load_control_plan(path_to_posedict_yaml, path_to_cmds_yaml)
# set init done flag
self.init_done = True
def test_sanitation(self):
if not rospy.get_node_uri():
rospy.init_node("topic_store_tests")
test_dict = {
# Data heavy types (binary)
"1_Image": ("sensor_msgs/Image", sensor_msgs.msg.Image()),
"1_RegionOfInterest": ("sensor_msgs/RegionOfInterest", sensor_msgs.msg.RegionOfInterest()),
"1_CameraInfo": ("sensor_msgs/CameraInfo", sensor_msgs.msg.CameraInfo()),
# Time types
"1_Time_genpy": ("genpy.Time", rospy.rostime.Time(0)),
"1_Duration_genpy": ("genpy.Duration", rospy.rostime.Duration(0)),
"1_Time_rospy": ("genpy.Time", rospy.rostime.Time(0)),
"1_Duration_rospy": ("genpy.Duration", rospy.rostime.Duration(0)),
# Standard types
"1_Header": ("std_msgs/Header", std_msgs.msg.Header()),
# Duplicate the above to avoid memory copy not value copy errors
# Data heavy types (binary)
"2_Image": ("sensor_msgs/Image", sensor_msgs.msg.Image()),
"2_RegionOfInterest": ("sensor_msgs/RegionOfInterest", sensor_msgs.msg.RegionOfInterest()),
"2_CameraInfo": ("sensor_msgs/CameraInfo", sensor_msgs.msg.CameraInfo()),
# Time types
"2_Time_genpy": ("genpy.Time", rospy.rostime.Time(0)),
"2_Duration_genpy": ("genpy.Duration", rospy.rostime.Duration(0)),
"2_Time_rospy": ("genpy.Time", rospy.rostime.Time(0)),
"2_Duration_rospy": ("genpy.Duration", rospy.rostime.Duration(0)),
# Standard types
"2_Header": ("std_msgs/Header", std_msgs.msg.Header()),
}
test_dict.update({
# Test nested objects
"1_ImageHeader": ("std_msgs/Header", test_dict["1_Image"][1].header),
"1_CameraInfo": ("std_msgs/Header", test_dict["1_CameraInfo"][1].header),
"1_CameraInfoRegionOfInterest": ("sensor_msgs/RegionOfInterest", test_dict["1_CameraInfo"][1].roi),
"2_ImageHeader": ("std_msgs/Header", test_dict["2_Image"][1].header),
"2_CameraInfo": ("std_msgs/Header", test_dict["2_CameraInfo"][1].header),
"2_CameraInfoRegionOfInterest": ("sensor_msgs/RegionOfInterest", test_dict["2_CameraInfo"][1].roi),
})
# Check topic store functions
store = TopicStore(test_dict)
for key, (string_type, type_class) in test_dict.items():
assert store[key][1]["_ros_meta"]["type"] == string_type # check to python sanitation keeps type
assert type(store(key)[1]) == type(type_class) # check to rosify sanitation returns correct type
# Check main sanitation functions
sanitised = sanitise_dict(test_dict)
rosified = rosify_dict(test_dict)
for key, (string_type, type_class) in test_dict.items():
assert sanitised[key][1]["_ros_meta"]["type"] == string_type # check to python sanitation keeps type
assert type(rosified[key][1]) == type(type_class) # check to rosify sanitation returns correct type
print("All sanitation tests passed!")
def __init__(self):
self.img_sub = rospy.Subscriber("/img_left", Image, self.img_sub_cb)
self.bridge = CvBridge()
# create node
if not rospy.get_node_uri():
rospy.init_node('img_sub_node', anonymous=True, log_level=rospy.WARN)
else:
rospy.logdebug(rospy.get_caller_id() + ' -> ROS already initialized')
self.main()
def __init__(self, checkerboard_row, checkerboard_col, drawing_box_height,
cam_type, loadfilename, savefilename):
# data members
self.__row = checkerboard_row
self.__col = checkerboard_col
self.__height = drawing_box_height
self.__cam_type = cam_type
self.__loadfilename = loadfilename
self.__savefilename = savefilename
self.__bridge = CvBridge()
self.__img_raw_cam = [[], []]
# threading
self.img_thr = threading.Thread(
target=self.__img_raw_cam_thr) # img receiving thread
self.img_thr.daemon = True
self.__stop_flag = False
# initialize camera
for type in self.__cam_type:
self.__img_raw_cam[self.__cam_type.index(type)] = []
if type == 'USB':
# USB camera initialize
try:
print("camera ON")
self.cap = cv2.VideoCapture(0)
except Exception as e:
print("camera failed: ", e)
elif type == 'REALSENSE':
# Realsense configuring depth and color streams
self.pipeline = rs.pipeline()
self.config = rs.config()
self.config.enable_stream(rs.stream.color, 640, 480,
rs.format.bgr8, 30)
# Realsense streaming start
self.pipeline.start(self.config)
elif type == 'ROS_TOPIC':
# create ROS node
if not rospy.get_node_uri():
rospy.init_node('Image_pipeline_node',
anonymous=True,
log_level=rospy.WARN)
print("ROS node initialized\n")
else:
rospy.logdebug(rospy.get_caller_id() +
' -> ROS already initialized')
# ROS subscriber
rospy.Subscriber('/kinect2/qhd/image_color', Image,
self.__img_raw_cam_cb)
# start threading
self.img_thr.start()
self.main()
def updateHostName(self):
self.RosHostName = rospy.get_node_uri().split("/")[2].split(":")[0]
rospy.logdebug(self.RosHostName)
rospy.logdebug(self.ownHostName)
#assert(self.HostName is self.ownHostName) # maybe they will differ if ROS_HOSTNAME is used; I still don't know which one will be correct.
##Should not be an afps parameter because I don't want it to be changed
rospy.logdebug("Hostname running this node: {}".format(
self.RosHostName))
rospy.logdebug("Own Hostname running this node: {}".format(
self.ownHostName))
rospy.set_param("~HostName", self.ownHostName)
def _interface(self):
if not rospy.get_node_uri():
rospy.init_node('calibrate_test', anonymous=True, log_level=rospy.WARN)
else:
rospy.logdebug(rospy.get_caller_id() + ' -> ROS already initialized')
self.arm = [None]*self.num
for i in range(self.num):
if self.names[i][0:3] == 'ECM':
self.arm[i] = Arms(self.names[i])
else:
self.arm[i] = Arms(self.names[i])
def __init__(self,
planner=default_planner,
links={"ee": (LinkType.INTERMEDIATE, "manipulator")}):
# initialize interface node (if needed)
if rospy.get_node_uri() is None:
self.node_name = 'ur5_interface_node'
rospy.init_node(self.node_name)
ROS_INFO(
"The Arm interface was created outside a ROS node, a new node will be initialized!"
)
else:
ROS_INFO(
"The Arm interface was created within a ROS node, no need to create a new one!"
)
# store parameters
self._planner = planner
self._links = links
# create a RobotCommander
self._robot = RobotCommander()
# create semaphore
self._semaphore = BoundedSemaphore(value=1)
# default link (for utility functions only)
self._default_link = None
# create links objects
num_valid_links = 0
for link in self._links.items():
link_name, link_description = link
link_type, link_group_name = link_description
link_class = Arm.link_type_map[link_type]
link_obj = link_class(self, link_group_name, self._planner)
if not self._robot.has_group(link_group_name):
ROS_ERR("Move group `%s` not found!", link_group_name)
else:
self.__dict__[link_name] = link_obj
self._default_link = link_obj
num_valid_links += 1
if num_valid_links <= 0:
ROS_ERR("No valid links were found")
return None
# keep track of the status of the node
self._is_shutdown = False
self.verbose = False
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 __init__(self, cam_type=('REALSENSE','KINECT')):
# data members
self.__cam_type = cam_type
self.__bridge = CvBridge()
self.__img_raw_cam1 = []
self.__img_raw_cam2 = []
# threading
t1 = threading.Thread(target=self.run, args=(lambda: self.__stop_flag,)) # mainthread
t2 = threading.Thread(target=self.__img_raw_cam1_th) # img receiving thread
t1.daemon = True
t2.daemon = True
self.__interval_ms = 10
self.__stop_flag = False
for type in self.__cam_type:
if type == 'REALSENSE':
# Realsense configuring depth and color streams
self.pipeline = rs.pipeline()
self.config = rs.config()
self.config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
# Realsense streaming start
self.pipeline.start(self.config)
elif type == 'USB':
# Laptop camera initialize
try:
print ("camera ON")
self.cap = cv2.VideoCapture(0)
except Exception as e:
print ("camera failed: ", e)
elif type == 'KINECT':
# create ROS node
if not rospy.get_node_uri():
rospy.init_node('Image_pipeline_node', anonymous=True, log_level=rospy.WARN)
self.rate = rospy.Rate(1000.0 / self.__interval_ms)
print ("ROS node initialized")
else:
rospy.logdebug(rospy.get_caller_id() + ' -> ROS already initialized')
# ROS subscriber
self.__sub = rospy.Subscriber('/kinect2/hd/image_color/compressed', CompressedImage,
self.__img_raw_cam2_cb)
# start threading
t1.start()
t2.start()
rospy.spin()
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 shutdown(self):
'''
Shutdown the RPC Server.
'''
if hasattr(self, 'rpcServer'):
if not self._master is None:
rospy.loginfo("Unsubscribe from parameter `/roslaunch/uris`")
self._master.unsubscribeParam(self.ros_node_name, rospy.get_node_uri(), '/roslaunch/uris')
rospy.loginfo("shutdown own RPC server")
self.rpcServer.shutdown()
if not self._timer_update_launch_uris is None:
self._timer_update_launch_uris.cancel()
del self.rpcServer.socket
del self.rpcServer
rospy.loginfo("exit")
def __init__(self, threshold_PSM1=0.3, threshold_PSM2=0.2):
# initializing variables
self.threshold_PSM1 = threshold_PSM1 # (A)
self.threshold_PSM2 = threshold_PSM2 # (A)
self.__jaw1_curr_PSM1_prev = 0
self.__jaw2_curr_PSM1_prev = 0
self.__jaw1_curr_PSM2_prev = 0
self.__jaw2_curr_PSM2_prev = 0
# data members
self.__bridge = CvBridge()
self.__actuator_current_measured_PSM1 = []
self.__actuator_current_measured_PSM2 = []
self.__state_jaw_current_PSM1 = []
self.__state_jaw_current_PSM2 = []
self.grasp_detected_PSM1 = False
self.grasp_detected_PSM2 = False
# subscriber
self.__sub_list = [
rospy.Subscriber('/dvrk/PSM1/io/actuator_current_measured',
JointState,
self.__actuator_current_measured_PSM1_cb),
rospy.Subscriber('/dvrk/PSM2/io/actuator_current_measured',
JointState,
self.__actuator_current_measured_PSM2_cb),
rospy.Subscriber('/dvrk/PSM1/state_jaw_current', JointState,
self.__state_jaw_current_PSM1_cb),
rospy.Subscriber('/dvrk/PSM2/state_jaw_current', JointState,
self.__state_jaw_current_PSM2_cb)
]
# create node
self.__interval_ms = 10
if not rospy.get_node_uri():
rospy.init_node('grasping_detection_node',
anonymous=True,
log_level=rospy.WARN)
self.rate = rospy.Rate(1000.0 / self.__interval_ms)
else:
rospy.logdebug(rospy.get_caller_id() +
' -> ROS already initialized')
# threading
threading.Thread.__init__(self)
self.thread = threading.Thread(target=self.loop)
self.thread.daemon = True
self.thread.start()
def __init_console(self, console_namespace=''):
"""Constructor. This initializes a few data members. It
requires a arm name, this will be used to find the ROS topics
for the console being controlled. The default is
'console' and it would be necessary to change it only if
you have multiple dVRK consoles"""
# data members, event based
self.__console_namespace = console_namespace + 'console'
self.__teleop_scale = 0.0
# publishers
self.__power_off_pub = rospy.Publisher(self.__console_namespace +
'/power_off',
Empty,
latch=True,
queue_size=1)
self.__power_on_pub = rospy.Publisher(self.__console_namespace +
'/power_on',
Empty,
latch=True,
queue_size=1)
self.__home_pub = rospy.Publisher(self.__console_namespace + '/home',
Empty,
latch=True,
queue_size=1)
self.__teleop_enable_pub = rospy.Publisher(self.__console_namespace +
'/teleop/enable',
Bool,
latch=True,
queue_size=1)
self.__teleop_set_scale_pub = rospy.Publisher(
self.__console_namespace + '/teleop/set_scale',
Float64,
latch=True,
queue_size=1)
# subscribers
rospy.Subscriber(self.__console_namespace + '/teleop/scale', Float64,
self.__teleop_scale_cb)
# create node
if not rospy.get_node_uri():
rospy.init_node('console_api',
anonymous=True,
log_level=rospy.WARN)
else:
rospy.logdebug(rospy.get_caller_id() +
' -> ROS already initialized')
def register_node(self, name, help, ntype, health):
self._node_stats = {
'name': name,
'help': help,
'type': ntype.value,
'health': health.value,
'health_reason': '',
'health_stamp': rospy.get_time(),
'enabled': True,
'uri': rospy.get_node_uri().rstrip('/'),
'machine': socket.gethostname(),
'module_type': get_module_type(),
'module_instance': get_module_instance()
}
# publish new node information
self._publish_node_diagnostics()
def shutdown(self):
'''
Shutdown the RPC Server.
'''
if hasattr(self, 'rpcServer'):
if self._master is not None:
rospy.loginfo("Unsubscribe from parameter `/roslaunch/uris`")
try:
self._master.unsubscribeParam(self.ros_node_name, rospy.get_node_uri(), '/roslaunch/uris')
except Exception as e:
rospy.logwarn("Error while unsubscribe from `/roslaunch/uris`: %s" % e)
rospy.loginfo("shutdown own RPC server")
self.rpcServer.shutdown()
if self._timer_update_launch_uris is not None:
self._timer_update_launch_uris.cancel()
del self.rpcServer.socket
del self.rpcServer
def test_documents(self):
if not rospy.get_node_uri():
rospy.init_node("topic_store_tests")
client = MongoStorage(collection="python_tests")
# Insert a test document
insert_result = client.insert_one(
TopicStore({
"name": "test_name",
"number": 1
}))
# Retrieve the inserted document
inserted_document = client.find_by_id(insert_result.inserted_id)
assert inserted_document.id == insert_result.inserted_id
# Update the document name and number fields
new_name = ''.join(random.sample('raymond', 7))
new_number = random.randint(0, 100)
update_result = client.update_one_by_id(inserted_document.id,
name=new_name,
number=new_number)
inserted_document_after_update = client.find_by_id(
insert_result.inserted_id)
assert inserted_document_after_update.id == insert_result.inserted_id
assert inserted_document_after_update.dict["number"] == new_number
assert inserted_document_after_update.dict["name"] == new_name
# Print all documents in the collection
cursor = client.find()
for x in cursor:
print(
"Doc:\n\t-As Structure: {}\n\t-As Dict: {}\n\t-As ROS Msgs: {}"
.format(str(x), x.dict, x.msgs))
# Or print using the same API as TopicStorage
for x in client:
print(
"Doc:\n\t-As Structure: {}\n\t-As Dict: {}\n\t-As ROS Msgs: {}"
.format(str(x), x.dict, x.msgs))
# Cleanup test by deleting document
delete_result = client.delete_by_id(insert_result.inserted_id)
def configure(self, master_namespace, puppet_namespace, gripper_namespace, jaw_namespace):
# ROS initialization
if not rospy.get_node_uri():
rospy.init_node('crtk_teleop_example', anonymous = True, log_level = rospy.WARN)
self.master = self.Master(master_namespace)
self.puppet = self.Puppet(puppet_namespace)
self.has_gripper = False;
if ((gripper_namespace != '') and (jaw_namespace != '')):
self.has_gripper = True
self.gripper = self.Gripper(gripper_namespace)
self.jaw = self.Jaw(jaw_namespace)
# for all examples
self.duration = 10 # 10 seconds
self.rate = 500 # aiming for 200 Hz
self.samples = self.duration * self.rate
def shutdown(self):
'''
Shutdown the RPC Server.
'''
if hasattr(self, 'rpcServer'):
if self._timer_update_launch_uris is not None:
try:
self._timer_update_launch_uris.cancel()
except:
pass
if self._master is not None:
rospy.loginfo("Unsubscribe from parameter `/roslaunch/uris`")
try:
self._master.unsubscribeParam(self.ros_node_name, rospy.get_node_uri(), '/roslaunch/uris')
except Exception as e:
rospy.logwarn("Error while unsubscribe from `/roslaunch/uris`: %s" % e)
rospy.loginfo("shutdown own RPC server")
self.rpcServer.shutdown()
del self.rpcServer.socket
del self.rpcServer
def __init__(self, node_name):
rospy.logdebug("YoubotGazeboProxy __init__")
super(YoubotGazeboProxy,self).__init__()
self.init_done = False # indicates that the object was initialized
# init ros node
rospy.init_node(node_name, anonymous=True)
rospy.loginfo("ROS node initialized: " + rospy.get_name())
rospy.loginfo("node namespace is : " + rospy.get_namespace())
rospy.loginfo("node uri is : " + rospy.get_node_uri())
# init object attributes
self.arm_num = rospy.get_param("~arm_num")
self._arm_as_name = '/arm_' + str(self.arm_num) + '/arm_controller/follow_joint_trajectory'
self._gripper_as_name = '/arm_' +str(self.arm_num) + '/gripper_controller/follow_joint_trajectory'
# init moveit
try:
moveit_commander.roscpp_initialize(sys.argv)
self.arm_group = moveit_commander.MoveGroupCommander("manipulator")
self.arm_group.set_planning_time(8)
self.arm_group.set_pose_reference_frame("base_link")
rospy.loginfo("planning group created for manipulator")
except:
pass
# init arm action client
self._ac_arm = actionlib.SimpleActionClient(self._arm_as_name, FollowJointTrajectoryAction)
self._ac_arm.wait_for_server()
rospy.loginfo("Created arm joint trajectory action client " + self._arm_as_name)
# init gripper action client
self._ac_gripper = actionlib.SimpleActionClient(self._gripper_as_name, FollowJointTrajectoryAction)
self._ac_gripper.wait_for_server()
rospy.loginfo("Created gripper joint trajectory action client " + self._gripper_as_name)
# set init done flag
self.init_done = True
def test_unsubscribe(self):
global _last_callback
uri = rospy.get_node_uri()
node_proxy = ServerProxy(uri)
_, _, subscriptions = node_proxy.getSubscriptions('/foo')
self.assert_(not subscriptions, 'subscriptions present: %s'%str(subscriptions))
print("Subscribing to ", SUBTOPIC)
sub = rospy.Subscriber(SUBTOPIC, String, callback)
topic = rospy.resolve_name(SUBTOPIC)
_, _, subscriptions = node_proxy.getSubscriptions('/foo')
self.assertEquals([[topic, String._type]], subscriptions, "Subscriptions were %s"%subscriptions)
# wait for the first message to be received
timeout_t = time.time() + TIMEOUT
while _last_callback is None and time.time() < timeout_t:
time.sleep(0.1)
self.assert_(_last_callback is not None, "No messages received from talker")
# begin actual test by unsubscribing
sub.unregister()
# clear last callback data, i.e. last message received
_last_callback = None
timeout_t = time.time() + 2.0
# make sure no new messages are received in the next 2 seconds
while timeout_t < time.time():
time.sleep(1.0)
self.assert_(_last_callback is None)
# verify that close cleaned up master and node state
_, _, subscriptions = node_proxy.getSubscriptions('/foo')
self.assert_(not subscriptions, "Node still has subscriptions: %s"%subscriptions)
n = rospy.get_caller_id()
self.assert_(not rostest.is_subscriber(topic, n), "subscription is still active on master")
def __init_console(self, console_namespace = '/dvrk/console'):
"""Constructor. This initializes a few data members. It
requires a arm name, this will be used to find the ROS topics
for the console being controlled. The default is
'/dvrk/console' and it would be necessary to change it only if
you have multiple dVRK consoles"""
# data members, event based
self.__console_namespace = console_namespace
self.__teleop_scale = 0.0
# publishers
self.__power_off_pub = rospy.Publisher(self.__console_namespace
+ '/power_off',
Empty, latch = True, queue_size = 1)
self.__power_on_pub = rospy.Publisher(self.__console_namespace
+ '/power_on',
Empty, latch = True, queue_size = 1)
self.__home_pub = rospy.Publisher(self.__console_namespace
+ '/home',
Empty, latch = True, queue_size = 1)
self.__teleop_enable_pub = rospy.Publisher(self.__console_namespace
+ '/teleop/enable',
Bool, latch = True, queue_size = 1)
self.__teleop_set_scale_pub = rospy.Publisher(self.__console_namespace
+ '/teleop/set_scale',
Float32, latch = True, queue_size = 1)
# subscribers
rospy.Subscriber(self.__console_namespace
+ '/teleop/scale',
Float32, self.__teleop_scale_cb)
# create node
if not rospy.get_node_uri():
rospy.init_node('console_api', anonymous = True, log_level = rospy.WARN)
else:
rospy.logdebug(rospy.get_caller_id() + ' -> ROS already initialized')
def __init_arm(self, arm_name, ros_namespace = '/dvrk/'):
"""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
`/dvrk/PSM1`"""
# data members, event based
self.__arm_name = arm_name
self.__ros_namespace = ros_namespace
self.__arm_current_state = ''
self.__arm_current_state_event = threading.Event()
self.__arm_desired_state = ''
self.__goal_reached = False
self.__goal_reached_event = threading.Event()
# continuous publish from dvrk_bridge
self.__position_joint_desired = numpy.array(0, dtype = numpy.float)
self.__effort_joint_desired = numpy.array(0, dtype = numpy.float)
self.__position_cartesian_desired = PyKDL.Frame()
self.__position_cartesian_local_desired = PyKDL.Frame()
self.__position_joint_current = numpy.array(0, dtype = numpy.float)
self.__velocity_joint_current = numpy.array(0, dtype = numpy.float)
self.__effort_joint_current = numpy.array(0, dtype = numpy.float)
self.__position_cartesian_current = PyKDL.Frame()
self.__position_cartesian_local_current = PyKDL.Frame()
self.__twist_body_current = numpy.zeros(6, dtype = numpy.float)
self.__wrench_body_current = numpy.zeros(6, dtype = numpy.float)
self.__jacobian_spatial = numpy.ndarray(0, dtype = numpy.float)
self.__jacobian_body = numpy.ndarray(0, dtype = numpy.float)
self.__sub_list = []
self.__pub_list = []
# publishers
frame = PyKDL.Frame()
self.__full_ros_namespace = self.__ros_namespace + self.__arm_name
self.__set_arm_desired_state_pub = rospy.Publisher(self.__full_ros_namespace
+ '/set_desired_state',
String, latch = True, queue_size = 1)
self.__set_position_joint_pub = rospy.Publisher(self.__full_ros_namespace
+ '/set_position_joint',
JointState, latch = True, queue_size = 1)
self.__set_position_goal_joint_pub = rospy.Publisher(self.__full_ros_namespace
+ '/set_position_goal_joint',
JointState, latch = True, queue_size = 1)
self.__set_position_cartesian_pub = rospy.Publisher(self.__full_ros_namespace
+ '/set_position_cartesian',
Pose, latch = True, queue_size = 1)
self.__set_position_goal_cartesian_pub = rospy.Publisher(self.__full_ros_namespace
+ '/set_position_goal_cartesian',
Pose, latch = True, queue_size = 1)
self.__set_effort_joint_pub = rospy.Publisher(self.__full_ros_namespace
+ '/set_effort_joint',
JointState, latch = True, queue_size = 1)
self.__set_wrench_body_pub = rospy.Publisher(self.__full_ros_namespace
+ '/set_wrench_body',
Wrench, latch = True, queue_size = 1)
self.__set_wrench_body_orientation_absolute_pub = rospy.Publisher(self.__full_ros_namespace
+ '/set_wrench_body_orientation_absolute',
Bool, latch = True, queue_size = 1)
self.__set_wrench_spatial_pub = rospy.Publisher(self.__full_ros_namespace
+ '/set_wrench_spatial',
Wrench, latch = True, queue_size = 1)
self.__set_gravity_compensation_pub = rospy.Publisher(self.__full_ros_namespace
+ '/set_gravity_compensation',
Bool, latch = True, queue_size = 1)
self.__pub_list = [self.__set_arm_desired_state_pub,
self.__set_position_joint_pub,
self.__set_position_goal_joint_pub,
self.__set_position_cartesian_pub,
self.__set_position_goal_cartesian_pub,
self.__set_effort_joint_pub,
self.__set_wrench_body_pub,
self.__set_wrench_body_orientation_absolute_pub,
self.__set_wrench_spatial_pub,
self.__set_gravity_compensation_pub]
# subscribers
self.__sub_list = [rospy.Subscriber(self.__full_ros_namespace + '/current_state',
String, self.__arm_current_state_cb),
rospy.Subscriber(self.__full_ros_namespace + '/desired_state',
String, self.__arm_desired_state_cb),
rospy.Subscriber(self.__full_ros_namespace + '/goal_reached',
Bool, self.__goal_reached_cb),
rospy.Subscriber(self.__full_ros_namespace + '/state_joint_desired',
JointState, self.__state_joint_desired_cb),
rospy.Subscriber(self.__full_ros_namespace + '/position_cartesian_desired',
PoseStamped, self.__position_cartesian_desired_cb),
rospy.Subscriber(self.__full_ros_namespace + '/position_cartesian_local_desired',
PoseStamped, self.__position_cartesian_local_desired_cb),
rospy.Subscriber(self.__full_ros_namespace + '/state_joint_current',
JointState, self.__state_joint_current_cb),
rospy.Subscriber(self.__full_ros_namespace + '/position_cartesian_current',
PoseStamped, self.__position_cartesian_current_cb),
rospy.Subscriber(self.__full_ros_namespace + '/position_cartesian_local_current',
PoseStamped, self.__position_cartesian_local_current_cb),
rospy.Subscriber(self.__full_ros_namespace + '/twist_body_current',
TwistStamped, self.__twist_body_current_cb),
rospy.Subscriber(self.__full_ros_namespace + '/wrench_body_current',
WrenchStamped, self.__wrench_body_current_cb),
rospy.Subscriber(self.__full_ros_namespace + '/jacobian_spatial',
Float64MultiArray, self.__jacobian_spatial_cb),
rospy.Subscriber(self.__full_ros_namespace + '/jacobian_body',
Float64MultiArray, self.__jacobian_body_cb)]
# 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 get_publishers_info(self):
s1= {"node":self.node_name,"topic":self.annotation_topic,"topic_type":self.annotation_topic_type,"publisher_url":rospy.get_node_uri()}
return [s1]
def trigger(self, msg):
if self.experiment_timestamp_given == False:
self.experiment_timestamp_offset = rospy.get_rostime()
else:
rospy.logerr('['+rospy.get_node_uri()+'] Experimental time already triggered. Not overridding the previous value.')
def __init__(self, rpcport=11611, do_retry=True):
'''
Initialize method. Creates an XML-RPC server on given port and starts this
in its own thread.
:param rpcport: the port number for the XML-RPC server
:type rpcport: int
:param do_retry: retry to create XML-RPC server
:type do_retry: bool
'''
self._state_access_lock = threading.RLock()
self._create_access_lock = threading.RLock()
self._lock = threading.RLock()
self.__masteruri = masteruri_from_ros()
self.__new_master_state = None
self.__masteruri_rpc = None
self.__mastername = None
self.__cached_nodes = dict()
self.__cached_services = dict()
self.ros_node_name = str(rospy.get_name())
if rospy.has_param('~name'):
self.__mastername = rospy.get_param('~name')
self.__mastername = self.getMastername()
rospy.set_param('/mastername', self.__mastername)
self.__master_state = None
'''the current state of the ROS master'''
self.rpcport = rpcport
'''the port number of the RPC server'''
self._printed_errors = dict()
self._last_clearup_ts = time.time()
# Create an XML-RPC server
self.ready = False
while not self.ready and (not rospy.is_shutdown()):
try:
self.rpcServer = RPCThreading(('', rpcport), logRequests=False, allow_none=True)
rospy.loginfo("Start RPC-XML Server at %s", self.rpcServer.server_address)
self.rpcServer.register_introspection_functions()
self.rpcServer.register_function(self.getListedMasterInfo, 'masterInfo')
self.rpcServer.register_function(self.getListedMasterInfoFiltered, 'masterInfoFiltered')
self.rpcServer.register_function(self.getMasterContacts, 'masterContacts')
self._rpcThread = threading.Thread(target = self.rpcServer.serve_forever)
self._rpcThread.setDaemon(True)
self._rpcThread.start()
self.ready = True
except socket.error:
if not do_retry:
raise Exception(''.join(["Error while start RPC-XML server on port ", str(rpcport), ". Is a Node Manager already running?"]))
rospy.logwarn(''.join(["Error while start RPC-XML server on port ", str(rpcport), ". Try again..."]))
time.sleep(1)
except:
import traceback
print traceback.format_exc()
if not do_retry:
raise
self._master = xmlrpclib.ServerProxy(self.getMasteruri())
# === UPDATE THE LAUNCH URIS Section ===
# subscribe to get parameter updates
rospy.loginfo("Subscribe to parameter `/roslaunch/uris`")
self.__mycache_param_server = rospy.impl.paramserver.get_param_server_cache()
# HACK: use own method to get the updates also for parameters in the subgroup
self.__mycache_param_server.update = self.__update_param
# first access, make call to parameter server
self._update_launch_uris_lock = threading.RLock()
self.__launch_uris = {}
code, msg, value = self._master.subscribeParam(self.ros_node_name, rospy.get_node_uri(), '/roslaunch/uris')
# the new timer will be created in self._update_launch_uris()
self._timer_update_launch_uris = None
if code == 1:
for k,v in value.items():
self.__launch_uris[roslib.names.ns_join('/roslaunch/uris', k)] = v
self._update_launch_uris()
def get_pub_string(self):
s1= "%s,%s,%s,%s" % (self.node_name,self.drink_topic,self.drink_topic_type,rospy.get_node_uri())
#s1= "%s,%s,%s" % (self.node_name,self.drink_topic,rospy.get_node_uri())
s2= "%s,%s,%s,%s" % (self.node_name,self.orders_topic,self.orders_topic_type,rospy.get_node_uri())
#return s1
return s1+"\n"+s2
def drivearound():
rospy.init_node("drivearound_robot")
print "ROS Node URI:", rospy.get_node_uri()
print "ROS Time:", rospy.get_rostime()
print "ROS_ROOT:", rospy.get_ros_root()
def close_cb():
rospy.logerr('['+rospy.get_node_uri()+'] Experimental time not triggered.')
file_handle = open(self.filename, 'w')
file_handle.write(str(self.experiment_timestamp_offset))
file_handle.close()
