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