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 __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()