def __init__(self): self.db_name = rospy.get_param('robot/database', 'jsk_robot_lifelog') try: self.col_name = rospy.get_param('robot/name') except KeyError as e: rospy.logerr( "please specify param \"robot/name\" (e.g. pr1012, olive)") exit(1) self.update_cycle = rospy.get_param('~update_cycle', 1.0) self.map_frame = rospy.get_param('~map_frame', 'map') self.robot_frame = rospy.get_param('~robot_frame', 'base_link') self.tf_listener = tf2_ros.BufferClient("tf2_buffer_server") self.initialpose_pub = rospy.Publisher( '/initialpose', geometry_msgs.msg.PoseWithCovarianceStamped) self.msg_store = MessageStoreProxy(database=self.db_name, collection=self.col_name) rospy.loginfo("connected to %s.%s" % (self.db_name, self.col_name)) rospy.loginfo("map->robot: %s -> %s" % (self.map_frame, self.robot_frame)) self.current_pose = None self.latest_pose = None self.latest_stamp = rospy.Time(0) self._load_latest_pose() self.pub_latest_pose() self.latest_exception = None
def __init__(self): super(PointIt, self).__init__() # variables self.subscribers = [] self.objects = [] # parameters if rospy.has_param("~dist_threshold"): rospy.logwarn("Parameter ~dist_threshold is deprecated. " "Use ~max_dist_threshold instead.") self.max_dist_threshold = rospy.get_param("~dist_threshold") self.min_dist_threshold = rospy.get_param("~min_dist_threshold", 0.0) self.max_dist_threshold = rospy.get_param("~max_dist_threshold", 0.1) self.min_norm_threshold = rospy.get_param("~min_norm_threshold", 0.2) self.use_arm = rospy.get_param("~use_arm", ["rarm", "larm"]) if "rarm" not in self.use_arm and "larm" not in self.use_arm: rospy.logfatal("~use_arm must contain at least 'rarm' or 'larm'.") # tf listener use_tf2_buffer_client = rospy.get_param("~use_tf2_buffer_client", True) if use_tf2_buffer_client: self.tfl = tf2_ros.BufferClient("/tf2_buffer_server") if not self.tfl.wait_for_server(rospy.Duration(10)): rospy.logerr("Waiting /tf2_buffer_server timed out.") use_tf2_buffer_client = False if not use_tf2_buffer_client: self.tfl = tf2_ros.Buffer() # advertise self.pub_bbox = self.advertise("~output", BoundingBoxArray, queue_size=1) self.pub_marker = self.advertise("~output/marker_array", MarkerArray, queue_size=1)
def add_kitchen_request(): pr2 = WorldBody() pr2.name = "kitchen" pr2.type = WorldBody.URDF_BODY pr2.urdf = rospy.get_param('/kitchen_description') pr2.joint_state_topic = '/kitchen_joint_states' tf = tf2_ros.BufferClient('tf2_buffer_server') tf.wait_for_server(rospy.Duration(0.5)) transform = tf.lookup_transform('map', 'world', rospy.Time.now(), rospy.Duration(0.5)) return UpdateWorldRequest(UpdateWorldRequest.ADD, pr2, False, to_pose_stamped_msg(transform))
def __init__(self, use_tf2=True): if use_tf2: try: self.tf_listener = tf2_ros.BufferClient("/tf2_buffer_server") ok = self.tf_listener.wait_for_server(rospy.Duration(10)) if not ok: raise Exception( "timed out: wait_for_server for 10.0 seconds") except Exception as e: rospy.logerr("Failed to initialize tf2 client: %s" % str(e)) rospy.logwarn("Fallback to tf client") use_tf2 = False if not use_tf2: self.tf_listener = tf.TransformListener() self.use_tf2 = use_tf2
def test_transform_type(self): client = tf2_ros.BufferClient("tf_action") client.wait_for_server() p1 = PointStamped() p1.header.frame_id = "a" p1.header.stamp = rospy.Time(0.0) p1.point.x = 0.0 p1.point.y = 0.0 p1.point.z = 0.0 try: p2 = client.transform(p1, "b", new_type=PyKDL.Vector) rospy.loginfo("p1: %s, p2: %s" % (str(p1), str(p2))) except tf2.TransformException as e: rospy.logerr("%s" % e)
def __init__(self): self.tf_broadcaster = tf2_ros.StaticTransformBroadcaster() self.tf_listener = tf2_ros.BufferClient('tf2_buffer_server') self.tf_listener.wait_for_server(rospy.Duration(1.0)) self.marker_pub = rospy.Publisher("/visualization_marker_array", MarkerArray, queue_size=5) self.js_pub = rospy.Publisher("/new_joint_states", JointState, queue_size=5) self.marker_ns = "plotting_world" self.joint_state = read_js_from_param_server('~joints') self.objects = self.read_objects() self.frames = self.read_frames() if rospy.has_param('~goals'): self.goals = rospy.get_param('~goals') else: self.goals = []
def test_buffer_client(self): buffer_client = tf2_ros.BufferClient(self.node, '/tf_action', check_frequency=10.0, timeout_padding=0.0) p1 = PointStamped() p1.header.frame_id = "a" p1.header.stamp = rclpy.time.Time(seconds=1.0).to_msg() p1.point.x = 0.0 p1.point.y = 0.0 p1.point.z = 0.0 buffer_client.action_client.wait_for_server() try: p2 = buffer_client.transform( p1, "b", timeout=rclpy.time.Duration(seconds=1)) self.node.get_logger().info("p1: %s, p2: %s" % (p1, p2)) except tf2.TransformException as e: self.node.get_logger().error("%s" % e) self.assertEqual(0, 4)
def __init__(self): super(PointIt, self).__init__() # variables self.subscribers = [] self.objects = [] # parameters self.dist_threshold = rospy.get_param("~dist_threshold", 0.1) self.min_norm_threshold = rospy.get_param("~min_norm_threshold", 0.2) # tf listener use_tf2_buffer_client = rospy.get_param("~use_tf2_buffer_client", True) if use_tf2_buffer_client: self.tfl = tf2_ros.BufferClient("/tf2_buffer_server") if not self.tfl.wait_for_server(rospy.Duration(10)): rospy.logerr("Waiting /tf2_buffer_server timed out.") use_tf2_buffer_client = False if not use_tf2_buffer_client: self.tfl = tf2_ros.Buffer() # advertise self.pub_bbox = self.advertise("~output", BoundingBoxArray, queue_size=1) self.pub_marker = self.advertise("~output/marker_array", MarkerArray, queue_size=1)
def __init__(self, connection=None, db_lock=None): db_name = rospy.get_param('~db_name', 'pr2db') host = rospy.get_param('~host_name', 'c1') port = rospy.get_param('~port', 5432) username = rospy.get_param('~user_name', 'pr2admin') passwd = rospy.get_param('~passwd', '') self.update_cycle = rospy.get_param('update_cycle', 1) # args dbname, host, port, opt, tty, user, passwd self.con = pgdb.connect(database=db_name, host=host, user=username, password=passwd) self.map_frame = rospy.get_param('~map_frame', 'map') self.robot_frame = rospy.get_param('~robot_frame', 'base_link') self.tf_listener = tf2_ros.BufferClient("tf2_buffer_server") self.initialpose_pub = rospy.Publisher( '/initialpose', geometry_msgs.msg.PoseWithCovarianceStamped) self.current_pose = None self.latest_pose = None self.latest_stamp = rospy.Time(0) self.load_latest_pose() self.pub_latest_pose() return
def __init__(self): super(ColorHistogramDetector, self).__init__() self.queue_size = rospy.get_param("~queue_size", 100) self.publish_tf = rospy.get_param("~publish_tf", True) if self.publish_tf: self.tfb = tf2_ros.TransformBroadcaster() self.align_box_pose = rospy.get_param("~align_box_pose", True) self.fixed_frame_id = rospy.get_param("~fixed_frame_id", None) if self.fixed_frame_id is not None: self.tfl = tf2_ros.BufferClient("/tf2_buffer_server") if not self.tfl.wait_for_server(rospy.Duration(10)): rospy.logerr("Failed to wait /tf2_buffer_server") self.fixed_frame_id = None self.pub_detect = self.advertise("~output", ObjectDetection, queue_size=1) self.pub_detect_nearest = self.advertise("~output/nearest", ObjectDetection, queue_size=1)
import sys import rospy import tf2_ros import tf_service if __name__ == "__main__": parser = argparse.ArgumentParser() parser.add_argument("--lookup_frequency", type=float, default=10) parser.add_argument("--use_old_version", action="store_true") args, _ = parser.parse_known_args(rospy.myargv(sys.argv)[1:]) rospy.init_node("client_test_py") if args.use_old_version: buffer = tf2_ros.BufferClient("/tf2_buffer_server") else: buffer = tf_service.BufferClient("/tf_service") buffer.wait_for_server() rate = rospy.Rate(args.lookup_frequency) while not rospy.is_shutdown(): try: buffer.lookup_transform("map", "odom", rospy.Time(0), rospy.Duration(1)) except tf2_ros.TransformException as e: rospy.logerr("%s: %s" % (str(type(e)), str(e))) break rate.sleep()