def tf_from_pose(self, pose_msg):
# Update message timestamp
self.tf_msg.header.stamp = rospy.Time.now()
# Update translation
self.tf_msg.transform.translation.x = pose_msg.position.x
self.tf_msg.transform.translation.y = pose_msg.position.y
self.tf_msg.transform.translation.z = pose_msg.position.z
# Update rotation
self.tf_msg.transform.rotation.x = pose_msg.orientation.x
self.tf_msg.transform.rotation.y = pose_msg.orientation.y
self.tf_msg.transform.rotation.z = pose_msg.orientation.z
self.tf_msg.transform.rotation.w = pose_msg.orientation.w
if sum ([self.tf_msg.transform.rotation.x, self.tf_msg.transform.rotation.y,
self.tf_msg.transform.rotation.z, self.tf_msg.transform.rotation.w]) != 0.0:
if self.invert == "True":
# Create transformerROS and add our transform
transformer = TransformerROS()
transformer.setTransform(self.tf_msg)
# Lookup the transform
(trans, rot) = transformer.lookupTransform(self.tf_msg.header.frame_id, self.tf_msg.child_frame_id, rospy.Time(0))
# Create transform object
transform = tft.concatenate_matrices(tft.translation_matrix(trans), tft.quaternion_matrix(rot))
# Invert
inverse_tf = tft.inverse_matrix(transform)
# Get translation, rotation vectors back out
translation = tft.translation_from_matrix(inverse_tf)
quaternion = tft.quaternion_from_matrix(inverse_tf)
# Get RPY
rpy = tft.euler_from_quaternion(quaternion)
rpy_new = [rpy[0], rpy[1], -rpy[2]]
# Back to quaternion
quaternion = tft.quaternion_from_euler(rpy_new[0], rpy_new[1], rpy_new[2])
# Update translation
self.tf_msg.transform.translation.x = translation[0]
self.tf_msg.transform.translation.y = -translation[1]
self.tf_msg.transform.translation.z = translation[2]
# Update rotation
self.tf_msg.transform.rotation.x = quaternion[0]
self.tf_msg.transform.rotation.y = quaternion[1]
self.tf_msg.transform.rotation.z = quaternion[2]
self.tf_msg.transform.rotation.w = quaternion[3]
# Publish transform
self.broadcaster.sendTransform(self.tf_msg)
def get_transformation(frame_from='/base_link',
frame_to='/local_map',
time_from=None,
time_to=None,
static_frame=None,
tf_listener=None,
tf_ros=None):
if tf_listener is None:
tf_listener = TransformListener()
if tf_ros is None:
tf_ros = TransformerROS()
try:
if time_from is None or time_to is None:
# tf_listener.waitForTransform(frame_from, frame_to, rospy.Time(), rospy.Duration(1.0))
(trans, rot) = tf_listener.lookupTransform(frame_to, frame_from,
rospy.Time(0))
else:
# tf_listener.waitForTransformFull(frame_from, time_from, frame_to, time_to, static_frame, rospy.Duration(1.0))
(trans,
rot) = tf_listener.lookupTransformFull(frame_to, time_to,
frame_from, time_from,
static_frame)
except (LookupException, ConnectivityException, ExtrapolationException):
rospy.logerr("exception, from %s to %s frame may not have setup!",
frame_from, frame_to)
return None, None, None, None
# pose.pose.orientation.w = 1.0 # Neutral orientation
# tf_pose = self.tf_listener_.transformPose("/world", pose)
# R_local = quaternion_matrix(tf_pose.pose.orientation)
T = tf_ros.fromTranslationRotation(trans, rot)
euler = euler_from_quaternion(rot)
# print "Position of the pose in the local map:"
# print trans, euler
return T, trans, rot, euler
def get_transform_from_pose(pose, tf_ros=None):
""" from pose to origin (assumed 0,0,0) """
if tf_ros is None:
tf_ros = TransformerROS()
translation = (pose.position.x, pose.position.y, pose.position.z)
rotation = (pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w)
T_pose_to_origin = tf_ros.fromTranslationRotation(translation, rotation)
return T_pose_to_origin
def __init__(self,
num_particles=100,
vis_noise=10,
ultra_noise=100,
mag_noise=37,
linear_noise=.002,
angular_noise=2.3,
ar_noise=10,
ar_resample_rate=10,
ar_resample=True):
print("Starting a particle filer with %d particles" % num_particles)
self.filename = os.path.expanduser(
"~/Dropbox/thesis_data/" +
time.strftime('%Y_%m_%d_%H_%M_%S')) #in the format YYYYMMDDHHMMSS
self.fp = open(self.filename + ".txt", "w")
self.fp_part = open(self.filename + "_part.txt", "w")
self.fp_ar = open(self.filename + "_ar.txt", "w")
self.fp_alt = open(self.filename + "_alt.txt", "w")
self.fp_sensor = open(self.filename + "_sensor.txt", "w")
self.num_particles = num_particles
self.vis_noise = vis_noise
self.ultra_noise = ultra_noise
self.mag_noise = mag_noise
self.linear_noise = linear_noise
self.angular_noise = angular_noise
self.ar_noise = ar_noise
self.ar_resample_rate = ar_resample_rate
self.ar_resample = ar_resample
self.rotY = 0
self.rotX = 0
self.start_mag_heading = 0
self.start_gyr_heading = 0
self.gyr_theta = 0
self.particle_list = []
self.weight_dict = dict()
self.first_propagate = True
self.first_correct = True
self.step = 0
self.est = particle(self)
self.acc_est = particle(self) #Estimation using acc and gyr
self.vis_est = particle(
self) #Estimation using vis odometry and ultrasound
self.tag_est = particle(self) #Estimation using visual tags
for i in range(num_particles):
self.particle_list.append(particle(self))
self.est_pose = Pose()
self.est_pub = rospy.Publisher('pf_localization', Pose)
self.listener = TransformListener()
self.transformer = TransformerROS()
self.publish_pose()
def invert_pose(pose):
transformer = TransformerROS()
transform = transform_from_pose('frame1', 'frame2', pose)
transformer.setTransform(transform)
frame1_origin_frame1 = pose_stamped('frame1', [0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 1.0])
frame1_origin_frame2 = transformer.transformPose('frame2',
frame1_origin_frame1)
return frame1_origin_frame2.pose
def get_ik(pose):
matrix = TransformerROS()
# The orientation of /tool0 will be constant
q = quaternion_from_euler(0, 3.14, 1.57)
matrix2 = matrix.fromTranslationRotation((pose[0]*(-1), pose[1]*(-1), pose[2]), (q[0], q[1], q[2], q[3]))
th = invKine(matrix2)
sol1 = th[:, 2].transpose()
joint_values_from_ik = np.array(sol1)
joint_values = joint_values_from_ik[0, :]
return joint_values.tolist()
def __init__(self):
self.node_name = "Mocap Localization"
self.global_map = ObstaclePoseList()
self.first = True # process first time
self.radius = 1 # to check whether the measurement obstacle is correspond to global map or not
self.confi_threshold = 5 # confidence threshold, to determine whether to add point to map or not
self.tf = TransformListener()
self.transformer = TransformerROS()
# Subscribers
self.obstacle_list = rospy.Subscriber("/obstacle_list", ObstaclePoseList, self.call_back, queue_size=1)
# Publishers
self.rviz_pub = rospy.Publisher("/map_viz", Marker, queue_size=1)
self.map_list = rospy.Publisher("/map_list", ObstaclePoseList, queue_size=1)
def __init__(self, map_frame, odom_frame, projection_namespace,
kitchen_namespace):
# Frame names of the map and odom frame
self.map_frame = map_frame
self.odom_frame = odom_frame
# Namespaces
self.projection_namespace = projection_namespace
self.kitchen_namespace = kitchen_namespace
# TF tf_stampeds and static_tf_stampeds of the Robot in the BulletWorld:
# These are initialized with the function init_transforms_from_urdf and are
# used to update the local transformer with the function update_local_transformer_from_btr
self.tf_stampeds = []
self.static_tf_stampeds = []
self.local_transformer = TransformerROS(interpolate=True,
cache_time=Duration(10.0))
self.init_local_tf_with_tfs_from_urdf()
def __init__(self):
# Origin of ther intersection coordinate system is the center of stopline 0.
tile_side_length = 0.61
lane_width = 0.205
stopline_thickness = 0.048
center_markings_thickness = 0.025
q = lane_width
p = stopline_thickness
s = center_markings_thickness
self.stopline_poses = []
self.tf1 = TransformerROS()
self.tf2 = TransformerROS()
def add_stopline_transform(idx, position, angle):
orientation = unit_vector(
quaternion_from_euler(0, 0, angle, axes='sxyz'))
pose = utils.pose(position, orientation)
# Add transform for reference stopline poses
child_frame = self.stopline_frame(idx)
parent_frame = 'intersection'
transform = utils.transform_from_pose('intersection', child_frame,
pose)
self.tf1.setTransform(transform)
# Add transform for measured stopline poses
child_frame = self.intersection_frame(idx)
parent_frame = self.stopline_frame(idx)
inverted_pose = utils.invert_pose(utils.pose(
position, orientation))
transform = utils.transform_from_pose(parent_frame, child_frame,
inverted_pose)
self.tf2.setTransform(transform)
add_stopline_transform(0, [0.0, 0.0, 0.0], 0)
add_stopline_transform(
1, [-(q / 2.0 + s + q + p / 2.0), p / 2.0 + q / 2.0, 0.0],
-np.pi / 2.0)
add_stopline_transform(
2, [-(q / 2.0 + s + q / 2.0), p / 2.0 + 2 * q + s + p / 2.0, 0.0],
-np.pi)
add_stopline_transform(
3, [q / 2.0 + p / 2.0, p / 2.0 + q + s + q / 2.0, 0.0],
-3.0 * np.pi / 2.0)
def transform_ar_tag(message):
"""
Input: message of type AlvarMarkers
Transforms the encoded PoseStamped to the base frame
and stores it in transformed_message
"""
global transformed_message
if message.markers == []:
return
global counter
if counter > 1:
return
# Create a TransformerROS to transform the AR tag poses we get
t = TransformerROS(True, rospy.Duration(10.0))
# Wait for our transform and get it
tf_listener.waitForTransform('/head_camera', '/base', rospy.Time(),
rospy.Duration(5.0))
(trans, rot) = tf_listener.lookupTransform('/head_camera', '/base',
rospy.Time(0))
# Create our TransformStamped object
transform = TransformStamped()
transform.child_frame_id = 'base'
transform.header.frame_id = 'head_camera'
transform.header.stamp = rospy.Time(0)
transform.transform.translation.x = trans[0]
transform.transform.translation.y = trans[1]
transform.transform.translation.z = trans[2]
transform.transform.rotation.x = rot[0]
transform.transform.rotation.y = rot[1]
transform.transform.rotation.z = rot[2]
transform.transform.rotation.w = rot[3]
# Set the transform for t
t.setTransform(transform)
pose = message.markers[0].pose # Assume one marker for now
pose.header.frame_id = '/head_camera'
transformed_message = t.transformPose('/base', pose)
global board_x, board_y, board_z
board_x = transformed_message.pose.position.x
board_y = transformed_message.pose.position.y - 0.177 / 2
board_z = transformed_message.pose.position.z - 0.177 / 2
counter += 1
def __init__(self): pub = 0 self.tf = TransformListener() self.tf1 = TransformerROS() self.fdata = geometry_msgs.msg.TransformStamped() self.fdata_base = geometry_msgs.msg.TransformStamped() self.transform = tf.TransformBroadcaster() self.wrench = WrenchStamped() self.wrench_bl = WrenchStamped()
class Transformation():
def __init__(self):
pub = 0
self.tf = TransformListener()
self.tf1 = TransformerROS()
self.fdata = geometry_msgs.msg.TransformStamped()
self.fdata_base = geometry_msgs.msg.TransformStamped()
self.transform = tf.TransformBroadcaster()
self.wrench = WrenchStamped()
self.wrench_bl = WrenchStamped()
def wrench_cb(self,msg):
self.wrench = msg.wrench
self.transform.sendTransform((0,0,-0.025),quaternion_from_euler(3.14, 0, 3.665195102, 'rxyz'),rospy.Time.now(),'/ft_debug_link','/arm_7_link')
self.fdata.transform.translation.x = self.wrench.force.x
self.fdata.transform.translation.y = self.wrench.force.y
self.fdata.transform.translation.z = self.wrench.force.z
try:
if self.tf.frameExists("/dummy_link") and self.tf.frameExists("ft_debug_link"):
t = self.tf.getLatestCommonTime("/dummy_link", "/ft_debug_link")
(transform_ee_base_position,transform_ee_base_quaternion) = self.tf.lookupTransform("/dummy_link", '/ft_debug_link', t)
#print transform_ee_base_position
#print transform_ee_base_quaternion
#print self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)
except(tf.LookupException,tf.ConnectivityException):
print("TRANSFORMATION ERROR")
sss.say(["error"])
#return 'failed'
self.fdata_base.transform.translation.x =((self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[0,0] * self.fdata.transform.translation.x)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[0,1] * self.fdata.transform.translation.y)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[0,2] * self.fdata.transform.translation.z)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[0,3]))
self.fdata_base.transform.translation.y =((self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[1,0] * self.fdata.transform.translation.x)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[1,1] * self.fdata.transform.translation.y)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[1,2] * self.fdata.transform.translation.z)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[1,3]))
self.fdata_base.transform.translation.z =((self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[2,0] * self.fdata.transform.translation.x)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[2,1] * self.fdata.transform.translation.y)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[2,2] * self.fdata.transform.translation.z)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[2,3]))
self.wrench_bl.wrench.force.y = self.fdata_base.transform.translation.y
self.wrench_bl.wrench.force.x = self.fdata_base.transform.translation.x
self.wrench_bl.wrench.force.z = self.fdata_base.transform.translation.z
self.wrench_bl.header.stamp = rospy.Time.now();
self.pub.publish(self.wrench_bl)
def __init__(self):
self.node_name = rospy.get_name()
self.tf = TransformListener()
self.transformer = TransformerROS()
rospy.loginfo("[%s] Initializing " % (self.node_name))
#rospy.Subscriber("/pcl_points", PoseArray, self.call_back, queue_size=1, buff_size = 2**24)
sub_pcl = message_filters.Subscriber("/pcl_points", PoseArray)
sub_odom = message_filters.Subscriber("/odometry/ground_truth",
Odometry)
ats = ApproximateTimeSynchronizer((sub_pcl, sub_odom),
queue_size=1,
slop=0.1)
ats.registerCallback(self.call_back)
rospy.Subscriber("/move_base_simple/goal",
PoseStamped,
self.cb_new_goal,
queue_size=1)
self.pub_map = rospy.Publisher('/local_map',
OccupancyGrid,
queue_size=1)
self.pub_rviz = rospy.Publisher("/wp_line", Marker, queue_size=1)
self.pub_poses = rospy.Publisher("/path_points",
PoseArray,
queue_size=1)
self.resolution = 0.25
self.robot = Pose()
self.width = 200
self.height = 200
self.origin = Pose()
self.local_map = OccupancyGrid()
self.dilating_size = 6
self.wall_width = 3
self.start_planning = False
self.transpose_matrix = None
self.goal = []
self.astar = AStar()
self.msg_count = 0
self.planning_range = 20
self.frame_id = "map"
def __init__(self):
self.tfr = TransformerROS()
self.projector = LaserProjection()
self.outputTopic = rospy.get_param('output_topic', 'cloud')
self.inputTopic = rospy.get_param('input_topic', 'scan')
self.outputFrame = rospy.get_param('output_frame', 'base_link')
self.publisher = rospy.Publisher(self.outputTopic,
PointCloud2,
queue_size=10)
self.subscriber = rospy.Subscriber(self.inputTopic, LaserScan,
self.laser_callback)
def __init__(self, num_particles=100, vis_noise=10, ultra_noise=100, mag_noise=37, linear_noise=.002, angular_noise=2.3, ar_noise=10, ar_resample_rate=10, ar_resample=True):
print("Starting a particle filer with %d particles" % num_particles)
self.filename = os.path.expanduser("~/Dropbox/thesis_data/" + time.strftime('%Y_%m_%d_%H_%M_%S')) #in the format YYYYMMDDHHMMSS
self.fp = open(self.filename + ".txt", "w")
self.fp_part = open(self.filename+"_part.txt", "w")
self.fp_ar = open(self.filename+"_ar.txt", "w")
self.fp_alt = open(self.filename+"_alt.txt", "w")
self.fp_sensor = open(self.filename+"_sensor.txt", "w")
self.num_particles = num_particles
self.vis_noise = vis_noise
self.ultra_noise = ultra_noise
self.mag_noise = mag_noise
self.linear_noise = linear_noise
self.angular_noise = angular_noise
self.ar_noise = ar_noise
self.ar_resample_rate = ar_resample_rate
self.ar_resample = ar_resample
self.rotY = 0
self.rotX = 0
self.start_mag_heading = 0
self.start_gyr_heading = 0
self.gyr_theta = 0
self.particle_list = []
self.weight_dict = dict()
self.first_propagate = True
self.first_correct = True
self.step = 0
self.est = particle(self)
self.acc_est = particle(self) #Estimation using acc and gyr
self.vis_est = particle(self) #Estimation using vis odometry and ultrasound
self.tag_est = particle(self) #Estimation using visual tags
for i in range(num_particles):
self.particle_list.append(particle(self))
self.est_pose = Pose()
self.est_pub = rospy.Publisher('pf_localization', Pose)
self.listener = TransformListener()
self.transformer = TransformerROS()
self.publish_pose()
def __init__(self):
# specify topic and data type
self.sub_bbox_1 = rospy.Subscriber("camera1/detection/vision_objects",
DetectedObjectArray,
self.bbox_array_callback)
self.sub_bbox_6 = rospy.Subscriber("camera6/detection/vision_objects",
DetectedObjectArray,
self.bbox_array_callback)
self.sub_pcd = rospy.Subscriber("/points_raw", PointCloud2,
self.pcd_callback)
# self.sub_pose = rospy.Subscriber("/current_pose", PoseStamped, self.pose_callback)
# Publisher
self.pub_intersect_markers = rospy.Publisher(
"/vision_objects_position_rviz", MarkerArray, queue_size=10)
self.pub_plane_markers = rospy.Publisher("/estimated_plane_rviz",
MarkerArray,
queue_size=10)
self.pub_plane = rospy.Publisher("/estimated_plane",
Plane,
queue_size=10)
self.pub_pcd_inlier = rospy.Publisher("/points_inlier",
PointCloud2,
queue_size=10)
self.pub_pcd_outlier = rospy.Publisher("/points_outlier",
PointCloud2,
queue_size=10)
self.cam1 = camera_setup_1()
self.cam6 = camera_setup_6()
self.plane = Plane3D(-0.157, 0, 0.988, 1.9)
self.plane_world = None
self.plane_tracker = None
self.sac = RANSAC(Plane3D,
min_sample_num=3,
threshold=0.22,
iteration=200,
method="MSAC")
self.tf_listener = TransformListener()
self.tfmr = Transformer()
self.tf_ros = TransformerROS()
try:
(trans,
rot) = listener.lookupTransform(self.map_frame,
self.robot_frame, t)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
transpose_matrix = transformer.fromTranslationRotation(trans, rot)
# print(rot)
p = np.array([0, 0, 0, 1])
new_p = np.dot(transpose_matrix, p)
posestamped = PoseStamped()
posestamped.header.frame_id = self.map_frame
posestamped.header.stamp = t
posestamped.pose.position.x = new_p[0]
posestamped.pose.position.y = new_p[1]
posestamped.pose.position.z = new_p[2]
posestamped.pose.orientation.x = rot[0]
posestamped.pose.orientation.y = rot[1]
posestamped.pose.orientation.z = rot[2]
posestamped.pose.orientation.w = rot[3]
self.pub_pose.publish(posestamped)
rospy.sleep(0.1)
if __name__ == '__main__':
rospy.init_node('tf2topic')
listener = TransformListener()
transformer = TransformerROS()
foo = tf2topic()
rospy.spin()
else:
overriding_robot_base_frame = rospy.get_param('robot_base_frame')
if overriding_robot_base_frame != "":
calib.transformation.header.frame_id = overriding_robot_base_frame
overriding_tracking_base_frame = rospy.get_param('tracking_base_frame')
if overriding_tracking_base_frame != "":
calib.transformation.child_frame_id = overriding_tracking_base_frame
rospy.loginfo('loading calibration parameters into namespace {}'.format(
rospy.get_namespace()))
calib.to_parameters()
orig = calib.transformation.header.frame_id # tool or base link
dest = calib.transformation.child_frame_id # tracking_base_frame
transformer = TransformerROS()
result_tf = calib.transformation.transform
transl = result_tf.translation.x, result_tf.translation.y, result_tf.translation.z
rot = result_tf.rotation.x, result_tf.rotation.y, result_tf.rotation.z, result_tf.rotation.w
cal_mat = transformer.fromTranslationRotation(transl, rot)
if inverse:
cal_mat = tfs.inverse_matrix(cal_mat)
orig, dest = dest, orig
translation = tfs.translation_from_matrix(cal_mat)
rotation = tfs.quaternion_from_matrix(cal_mat)
rospy.loginfo('publishing transformation ' + orig + ' -> ' + dest + ':\n' +
str((translation, rotation)))
broad = TransformBroadcaster()
class Intersection4wayModel():
def __init__(self):
# Origin of ther intersection coordinate system is the center of stopline 0.
tile_side_length = 0.61
lane_width = 0.205
stopline_thickness = 0.048
center_markings_thickness = 0.025
q = lane_width
p = stopline_thickness
s = center_markings_thickness
self.stopline_poses = []
self.tf1 = TransformerROS()
self.tf2 = TransformerROS()
def add_stopline_transform(idx, position, angle):
orientation = unit_vector(
quaternion_from_euler(0, 0, angle, axes='sxyz'))
pose = utils.pose(position, orientation)
# Add transform for reference stopline poses
child_frame = self.stopline_frame(idx)
parent_frame = 'intersection'
transform = utils.transform_from_pose('intersection', child_frame,
pose)
self.tf1.setTransform(transform)
# Add transform for measured stopline poses
child_frame = self.intersection_frame(idx)
parent_frame = self.stopline_frame(idx)
inverted_pose = utils.invert_pose(utils.pose(
position, orientation))
transform = utils.transform_from_pose(parent_frame, child_frame,
inverted_pose)
self.tf2.setTransform(transform)
add_stopline_transform(0, [0.0, 0.0, 0.0], 0)
add_stopline_transform(
1, [-(q / 2.0 + s + q + p / 2.0), p / 2.0 + q / 2.0, 0.0],
-np.pi / 2.0)
add_stopline_transform(
2, [-(q / 2.0 + s + q / 2.0), p / 2.0 + 2 * q + s + p / 2.0, 0.0],
-np.pi)
add_stopline_transform(
3, [q / 2.0 + p / 2.0, p / 2.0 + q + s + q / 2.0, 0.0],
-3.0 * np.pi / 2.0)
def stopline_frame(self, idx):
return 'stopline_' + str(idx)
def intersection_frame(self, idx):
return 'intersection_' + str(idx)
# axle_pose needs to be in intersection coordinates
def get_stopline_poses_reference(self, axle_pose):
transform = utils.transform_from_pose('intersection', 'axle',
axle_pose)
self.tf1.setTransform(transform)
stopline_poses = [
self.tf1.transformPose('axle',
utils.origin_pose(
self.stopline_frame(i))).pose
for i in range(4)
]
return stopline_poses
def get_axle_pose_from_stopline_pose(self, stopline_pose, idx):
transform = utils.transform_from_pose('axle', self.stopline_frame(idx),
stopline_pose)
self.tf2.setTransform(transform)
axle_origin = utils.origin_pose('axle')
axle_pose = self.tf2.transformPose(self.intersection_frame(idx),
axle_origin)
return axle_pose
class mapping():
def __init__(self):
self.node_name = rospy.get_name()
self.tf = TransformListener()
self.transformer = TransformerROS()
rospy.loginfo("[%s] Initializing " % (self.node_name))
#rospy.Subscriber("/pcl_points", PoseArray, self.call_back, queue_size=1, buff_size = 2**24)
sub_pcl = message_filters.Subscriber("/pcl_points", PoseArray)
sub_odom = message_filters.Subscriber("/odometry/ground_truth",
Odometry)
ats = ApproximateTimeSynchronizer((sub_pcl, sub_odom),
queue_size=1,
slop=0.1)
ats.registerCallback(self.call_back)
rospy.Subscriber("/move_base_simple/goal",
PoseStamped,
self.cb_new_goal,
queue_size=1)
self.pub_map = rospy.Publisher('/local_map',
OccupancyGrid,
queue_size=1)
self.pub_rviz = rospy.Publisher("/wp_line", Marker, queue_size=1)
self.pub_poses = rospy.Publisher("/path_points",
PoseArray,
queue_size=1)
self.resolution = 0.25
self.robot = Pose()
self.width = 200
self.height = 200
self.origin = Pose()
self.local_map = OccupancyGrid()
self.dilating_size = 6
self.wall_width = 3
self.start_planning = False
self.transpose_matrix = None
self.goal = []
self.astar = AStar()
self.msg_count = 0
self.planning_range = 20
self.frame_id = "map"
def init_param(self):
self.occupancygrid = np.zeros((self.height, self.width))
self.local_map = OccupancyGrid()
self.local_map.info.resolution = self.resolution
self.local_map.info.width = self.width
self.local_map.info.height = self.height
self.origin.position.x = -self.width * self.resolution / 2. + self.robot.position.x
self.origin.position.y = -self.height * self.resolution / 2. + self.robot.position.y
self.local_map.info.origin = self.origin
def cb_rviz(self, msg):
self.click_pt = [msg.pose.position.x, msg.pose.position.y]
self.publish_topic()
def call_back(self, pcl_msg, odom_msg):
self.robot = odom_msg.pose.pose
self.msg_count = self.msg_count + 1
self.init_param()
self.local_map.header = pcl_msg.header
self.local_map.header.frame_id = self.frame_id
self.get_tf()
if self.transpose_matrix is None:
return
for i in range(len(pcl_msg.poses)):
p = (pcl_msg.poses[i].position.x, pcl_msg.poses[i].position.y,
pcl_msg.poses[i].position.z, 1)
local_p = np.array(p)
global_p = np.dot(self.transpose_matrix, local_p)
x, y = self.map2occupancygrid(global_p)
width_in_range = (x >= self.width - self.dilating_size
or x <= self.dilating_size)
height_in_range = (y >= self.height - self.dilating_size
or y <= self.dilating_size)
if width_in_range or height_in_range:
continue # To prevent point cloud range over occupancy grid range
self.occupancygrid[y][x] = 100
# map dilating
for i in range(self.height):
for j in range(self.width):
if self.occupancygrid[i][j] == 100:
for m in range(-self.dilating_size,
self.dilating_size + 1):
for n in range(-self.dilating_size,
self.dilating_size + 1):
if self.occupancygrid[i + m][j + n] != 100:
if m > self.wall_width or m < -self.wall_width or n > self.wall_width or n < -self.wall_width:
if self.occupancygrid[i + m][j + n] != 90:
self.occupancygrid[i + m][j + n] = 50
else:
self.occupancygrid[i + m][j + n] = 90
for i in range(self.height):
for j in range(self.width):
self.local_map.data.append(self.occupancygrid[i][j])
self.pub_map.publish(self.local_map)
if self.start_planning:
self.path_planning()
def get_tf(self):
try:
position, quaternion = self.tf.lookupTransform(
"/map", "/X1/front_laser", rospy.Time(0))
self.transpose_matrix = self.transformer.fromTranslationRotation(
position, quaternion)
except (LookupException, ConnectivityException,
ExtrapolationException):
print("Nothing Happen")
def path_planning(self):
if self.msg_count % 5 != 0:
return
self.msg_count = 0
cost_map = np.zeros((self.height, self.width))
border = self.planning_range / self.resolution
h_min = int((self.height - border) / 2.)
h_max = int(self.height - (self.height - border) / 2.)
w_min = int((self.height - border) / 2.)
w_max = int(self.width - (self.width - border) / 2.)
for i in range(self.width):
for j in range(self.width):
if i > h_min and i < h_max:
if j > w_min and j < w_max:
cost_map[i][j] = self.occupancygrid[i][j]
start_point = self.map2occupancygrid(
(self.robot.position.x, self.robot.position.y))
start = (start_point[1], start_point[0])
goal = self.map2occupancygrid(self.goal)
end = (goal[1], goal[0])
self.astar.initial(cost_map, start, end)
path = self.astar.planning()
self.pub_path(path)
self.rviz(path)
def cb_new_goal(self, p):
self.goal = [p.pose.position.x, p.pose.position.y]
self.start_planning = True
def occupancygrid2map(self, p):
x = p[
0] * self.resolution + self.origin.position.x + self.resolution / 2.
y = p[
1] * self.resolution + self.origin.position.y + self.resolution / 2.
return [x, y]
def map2occupancygrid(self, p):
x = int((p[0] - self.origin.position.x) / self.resolution)
y = int((p[1] - self.origin.position.y) / self.resolution)
return [x, y]
def pub_path(self, path):
poses = PoseArray()
for i in range(len(path)):
p = self.occupancygrid2map([path[i][1], path[i][0]])
pose = Pose()
pose.position.x = p[0]
pose.position.y = p[1]
pose.position.z = 0
poses.poses.append(pose)
self.pub_poses.publish(poses)
def rviz(self, path):
marker = Marker()
marker.header.frame_id = self.frame_id
marker.type = marker.LINE_STRIP
marker.action = marker.ADD
marker.scale.x = 0.3
marker.scale.y = 0.3
marker.scale.z = 0.3
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 0.
marker.color.b = 0.
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 0.0
marker.pose.orientation.w = 1.0
marker.pose.position.x = 0.0
marker.pose.position.y = 0.0
marker.pose.position.z = 0.0
marker.points = []
for i in range(len(path)):
p = self.occupancygrid2map([path[i][1], path[i][0]])
point = Point()
point.x = p[0]
point.y = p[1]
point.z = 0
marker.points.append(point)
self.pub_rviz.publish(marker)
def __init__(self, *args):
TransformerROS.__init__(self, *args)
class particlefilter:
def __init__(self, num_particles=100, vis_noise=10, ultra_noise=100, mag_noise=37, linear_noise=.002, angular_noise=2.3, ar_noise=10, ar_resample_rate=10, ar_resample=True):
print("Starting a particle filer with %d particles" % num_particles)
self.filename = os.path.expanduser("~/Dropbox/thesis_data/" + time.strftime('%Y_%m_%d_%H_%M_%S')) #in the format YYYYMMDDHHMMSS
self.fp = open(self.filename + ".txt", "w")
self.fp_part = open(self.filename+"_part.txt", "w")
self.fp_ar = open(self.filename+"_ar.txt", "w")
self.fp_alt = open(self.filename+"_alt.txt", "w")
self.fp_sensor = open(self.filename+"_sensor.txt", "w")
self.num_particles = num_particles
self.vis_noise = vis_noise
self.ultra_noise = ultra_noise
self.mag_noise = mag_noise
self.linear_noise = linear_noise
self.angular_noise = angular_noise
self.ar_noise = ar_noise
self.ar_resample_rate = ar_resample_rate
self.ar_resample = ar_resample
self.rotY = 0
self.rotX = 0
self.start_mag_heading = 0
self.start_gyr_heading = 0
self.gyr_theta = 0
self.particle_list = []
self.weight_dict = dict()
self.first_propagate = True
self.first_correct = True
self.step = 0
self.est = particle(self)
self.acc_est = particle(self) #Estimation using acc and gyr
self.vis_est = particle(self) #Estimation using vis odometry and ultrasound
self.tag_est = particle(self) #Estimation using visual tags
for i in range(num_particles):
self.particle_list.append(particle(self))
self.est_pose = Pose()
self.est_pub = rospy.Publisher('pf_localization', Pose)
self.listener = TransformListener()
self.transformer = TransformerROS()
self.publish_pose()
def propagate_alt(self, delta_t, x_vel, y_vel, altd, rotX, rotY, rotZ):
if (self.first_propagate):
self.start_gyr_heading = rotZ
self.first_propagate = False
self.rotX = rotX
self.rotY = rotY
# print "Zeroed rotZ: %f" % clamp_angle(rotZ - self.start_gyr_heading)
delta_theta = clamp_angle((rotZ - self.start_gyr_heading) - self.est.theta)
self.vis_est.propagate_alt(delta_t, x_vel, y_vel, altd, delta_theta, False)
for particle in self.particle_list:
particle.propagate_alt(delta_t, x_vel, y_vel, altd, delta_theta, True)
#Move this to correct_alt at some point
self.step += 1
self.estimate_equal()
self.publish_pose()
self.fp_alt.write("%d,%f,%f,%f,%f,%f,%f,%f,%f,%f\n" % (self.step, delta_t, self.est.x, self.est.y, self.est.z, self.est.theta, self.vis_est.x, self.vis_est.y, self.vis_est.z, self.vis_est.theta))
self.fp_sensor.write("%d,%f,%f,%f,%f,%f,%f,%f\n" % (self.step, delta_t, x_vel, y_vel, altd, rotX, rotY, rotZ))
def ar_correct(self, marker):
print "Correcting"
marker_id = marker.id
pose = marker.pose
pose_trans = ()
pose_rot = ()
#Is this necessary
pose_trans = (pose.position.x, pose.position.y, pose.position.z)
pose_rot = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w)
marker_name = "/marker_%d" % marker_id
#Get the offset of the marker from the origin
try:
(marker_trans,marker_rot) = self.listener.lookupTransform('world', marker_name, rospy.Time(0))
except (LookupException, ConnectivityException, ExtrapolationException):
print("Unable to find marker transform for marker %d" % marker_id)
return
try:
(base_trans,base_rot) = self.listener.lookupTransform('/ardrone/ardrone_base_link', '/ardrone/ardrone_base_bottomcam', rospy.Time(0))
except (LookupException, ConnectivityException, ExtrapolationException):
print("Unable to find ardrone transform")
return
#Everything is in m not mm
pose_mat = numpy.matrix(self.transformer.fromTranslationRotation(pose_trans, pose_rot))
pose_mat_inv = pose_mat.getI()
marker_mat = numpy.matrix(self.transformer.fromTranslationRotation(marker_trans, marker_rot))
marker_mat_inv = marker_mat.getI()
base_mat = numpy.matrix(self.transformer.fromTranslationRotation(base_trans, base_rot))
base_mat_inv = base_mat.getI()
origin = numpy.matrix([[0], [0], [0], [1]])
global_mat = marker_mat * pose_mat_inv * base_mat_inv
global_trans = translation_from_matrix(global_mat)
global_rot = rotation_from_matrix(global_mat)
global_trans = global_trans*1000 #Convert to mm
global_heading = clamp_angle(global_rot[0]*180/pi + 180) #Convert to degrees
self.tag_est.x = global_trans[0]
self.tag_est.y = global_trans[1]
self.tag_est.z = global_trans[2]
self.tag_est.theta = global_heading
if (self.ar_resample):
#Weight particles
self.weight_particles(self.tag_est.x, self.tag_est.y, self.tag_est.z, self.tag_est.theta, True)
#Create new set of particles
self.particle_list = []
#Initialize the random selector. Can select items in O(1) time
wrand = walkerrandom(self.weight_dict.values(), self.weight_dict.keys())
for i in range(self.num_particles):
#Create particle from new value
new_particle = particle(self)
rand_val = random.random()
threshold = self.ar_resample_rate/100.0
if (rand_val < threshold):
# print "Particle at (%f, %f, %f, %f)" % (self.tag_est.x, self.tag_est.y, self.tag_est.z, self.tag_est.theta)
new_particle = particle(self, self.tag_est.x, self.tag_est.y, self.tag_est.z, self.tag_est.theta)
#Randomly pick existing particle
else:
# print "Picking existing"
new_particle = copy(wrand.random())
self.particle_list.append(new_particle)
if (len(self.particle_list) != self.num_particles):
print("WRONG NUMBER OF PARTICLES")
self.fp_ar.write("%d,%d,%f,%f,%f,%f\n" % (self.step, marker_id, self.tag_est.x, self.tag_est.y, self.tag_est.z, self.tag_est.theta))
#Calculate the weight for particles
def weight_particles(self, x_est, y_est, z_est, theta_est, ar=False):
# print "Weighting......."
self.weight_dict = dict()
# print "Particle List:"
# for particle in self.particle_list:
# print particle.to_string()
weight_sum = 0
for particle in self.particle_list:
#Calculate distances
lat_dist = ((x_est - particle.x)**2 + (y_est - particle.y)**2)**.5
vert_dist = abs(z_est - particle.z)
theta_dist = abs(theta_est - particle.theta)
lat_noise = self.vis_noise
vert_noise = self.ultra_noise
theta_noise = self.mag_noise
#If this module is being called with ar_tag data, use ar_noise
if (ar):
lat_noise = self.ar_noise
vert_noise = self.ar_noise
theta_noise = self.ar_noise
lat_weight = normpdf(lat_dist, 0, lat_noise) #Potentially do the z distance separately?
vert_weight = normpdf(vert_dist, 0, vert_noise)
theta_weight = normpdf(theta_dist, 0, theta_noise)
weight = lat_weight + vert_weight + theta_weight
self.weight_dict[copy(particle)] = weight
weight_sum += weight
# print "Compare to (%f, %f, %f, %f)" % (x_est, y_est, z_est, theta_est)
#Normalize the weights
for particle, weight in self.weight_dict.iteritems():
if (weight_sum != 0):
self.weight_dict[particle] = weight/weight_sum
# print "(%s, %f)" % (particle.to_string(), self.weight_dict[particle])
#Return an estimate of the pose
#For now just use linear combination. Should we cluster instead?
def estimate(self):
self.est = particle(self)
total = 0
for part, weight in self.weight_dict.iteritems():
self.est.x += part.x*weight
self.est.y += part.y*weight
self.est.z += part.z*weight
self.est.theta += part.z*weight
total += weight
self.est.x = self.est.x/total
self.est.y = self.est.y/total
self.est.z = self.est.z/total
self.est.theta = self.est.theta/total
def publish_pose(self):
self.est_pose.position.x = self.est.x
self.est_pose.position.y = self.est.y
self.est_pose.position.z = self.est.z
rad_theta = self.est.theta*(pi/180)
quat = quaternion_about_axis(rad_theta, (0, 0, 1))
self.est_pose.orientation.x = quat[0]
self.est_pose.orientation.y = quat[1]
self.est_pose.orientation.z = quat[2]
self.est_pose.orientation.w = quat[3]
self.est_pub.publish(self.est_pose)
def estimate_equal(self):
self.est = particle(self)
x_val = []
y_val = []
z_val = []
theta_val = []
total = 0
for part in self.particle_list:
self.est.x += part.x
self.est.y += part.y
self.est.z += part.z
self.est.theta += part.theta
x_val.append(part.x)
y_val.append(part.y)
z_val.append(part.z)
theta_val.append(part.theta)
total += 1
self.est.x = self.est.x/total
self.est.y = self.est.y/total
self.est.z = self.est.z/total
self.est.theta = self.est.theta/total
print "%s : Est" % self.est.to_string()
# print self.est.theta
def print_particles(self):
for particle in self.particle_list:
print particle.to_string()
def __init__(self, *args):
TransformerROS.__init__(self, *args)
thr = TransformListenerThread(self)
thr.setDaemon(True)
thr.start()
self.setUsingDedicatedThread(True)
class map():
def __init__(self):
self.node_name = "Map"
self.global_map = ObstaclePoseList()
self.first = True # process first time
self.radius = 1 # to check whether the measurement obstacle is correspond to global map or not
self.confi_threshold = 5 # confidence threshold, to determine whether to add point to map or not
self.tf = TransformListener()
self.transformer = TransformerROS()
# Subscribers
self.obstacle_list = rospy.Subscriber("/obstacle_list",
ObstaclePoseList,
self.call_back,
queue_size=1)
# Publishers
self.rviz_pub = rospy.Publisher("/map_viz", Marker, queue_size=1)
self.map_list = rospy.Publisher("/map_list",
ObstaclePoseList,
queue_size=1)
def draw(self, ob_list):
marker = Marker()
marker.header.frame_id = "map"
marker.header.stamp = rospy.Time.now()
marker.type = marker.SPHERE_LIST
marker.action = marker.ADD
marker.scale.x = 0.4
marker.scale.y = 0.4
marker.scale.z = 0.4
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 1.0
for i in range(0, ob_list.size):
p = Point()
p.x = ob_list.list[i].x
p.y = ob_list.list[i].y
p.z = ob_list.list[i].z
marker.points.append(p)
self.rviz_pub.publish(marker)
def distance(self, a, b): # caculate distance between two 3d points
return math.sqrt((a.x - b.x) * (a.x - b.x) + (a.y - b.y) *
(a.y - b.y) + (a.z - b.z) * (a.z - b.z))
def update_obs_pos(
self, measurement, prior
): # update obstacle position by weighting measurement and original map
posterior = ObstaclePose()
posterior = prior
posterior.x = measurement.x * 0.3 + prior.x * 0.7
posterior.y = measurement.y * 0.3 + prior.y * 0.7
posterior.z = measurement.z * 0.3 + prior.z * 0.7
return posterior
def call_back(self, msg):
position, quaternion = self.tf.lookupTransform("/utm", "/velodyne",
rospy.Time(0))
transpose_matrix = self.transformer.fromTranslationRotation(
position, quaternion)
print("Process Obstacle List")
obs_list = ObstaclePoseList()
obs_list = msg
for i in range(0, obs_list.size):
origin_p = np.array([
obs_list.list[i].x, obs_list.list[i].y, obs_list.list[i].z, 1
])
new_p = np.dot(transpose_matrix, origin_p)
obs_list.list[i].x = new_p[0]
obs_list.list[i].y = new_p[1]
obs_list.list[i].z = new_p[2]
if self.first: # record the first detection to be global map
self.global_map = obs_list
for i in range(0, self.global_map.size):
self.global_map.list[i].confidence = 0
self.first = False
for i in range(0, self.global_map.size):
self.global_map.list[i].occupy = False
for i in range(0, obs_list.size):
min_dis = 10000
index = None
for j in range(0, self.global_map.size):
if not self.global_map.list[
j].occupy: # check whether the point has already been corresponded to another point
dis = self.distance(obs_list.list[i],
self.global_map.list[j])
if dis < min_dis:
index = j
min_dis = dis
if min_dis < self.radius: # believe that measurement[i] is corresponded to the map[index]
self.global_map.list[index].occupy = True
self.global_map.list[index] = self.update_obs_pos(
obs_list.list[i], self.global_map.list[index])
if self.global_map.list[
index].confidence < self.confi_threshold:
self.global_map.list[
index].confidence = self.global_map.list[
index].confidence + 1
else:
obs_list.list[i].confidence = 0
self.global_map.list.append(obs_list.list[i])
self.global_map.size = self.global_map.size + 1
map_confi = ObstaclePoseList()
map_confi.header = self.global_map.header
for i in range(0, self.global_map.size):
if self.global_map.list[i].confidence >= self.confi_threshold:
map_confi.list.append(self.global_map.list[i])
map_confi.size = map_confi.size + 1
self.draw(map_confi)
self.map_list.publish(map_confi)
def onShutdown(self):
rospy.loginfo("[Global_Map] Shutdown.")
class particlefilter:
def __init__(self,
num_particles=100,
vis_noise=10,
ultra_noise=100,
mag_noise=37,
linear_noise=.002,
angular_noise=2.3,
ar_noise=10,
ar_resample_rate=10,
ar_resample=True):
print("Starting a particle filer with %d particles" % num_particles)
self.filename = os.path.expanduser(
"~/Dropbox/thesis_data/" +
time.strftime('%Y_%m_%d_%H_%M_%S')) #in the format YYYYMMDDHHMMSS
self.fp = open(self.filename + ".txt", "w")
self.fp_part = open(self.filename + "_part.txt", "w")
self.fp_ar = open(self.filename + "_ar.txt", "w")
self.fp_alt = open(self.filename + "_alt.txt", "w")
self.fp_sensor = open(self.filename + "_sensor.txt", "w")
self.num_particles = num_particles
self.vis_noise = vis_noise
self.ultra_noise = ultra_noise
self.mag_noise = mag_noise
self.linear_noise = linear_noise
self.angular_noise = angular_noise
self.ar_noise = ar_noise
self.ar_resample_rate = ar_resample_rate
self.ar_resample = ar_resample
self.rotY = 0
self.rotX = 0
self.start_mag_heading = 0
self.start_gyr_heading = 0
self.gyr_theta = 0
self.particle_list = []
self.weight_dict = dict()
self.first_propagate = True
self.first_correct = True
self.step = 0
self.est = particle(self)
self.acc_est = particle(self) #Estimation using acc and gyr
self.vis_est = particle(
self) #Estimation using vis odometry and ultrasound
self.tag_est = particle(self) #Estimation using visual tags
for i in range(num_particles):
self.particle_list.append(particle(self))
self.est_pose = Pose()
self.est_pub = rospy.Publisher('pf_localization', Pose)
self.listener = TransformListener()
self.transformer = TransformerROS()
self.publish_pose()
def propagate_alt(self, delta_t, x_vel, y_vel, altd, rotX, rotY, rotZ):
if (self.first_propagate):
self.start_gyr_heading = rotZ
self.first_propagate = False
self.rotX = rotX
self.rotY = rotY
# print "Zeroed rotZ: %f" % clamp_angle(rotZ - self.start_gyr_heading)
delta_theta = clamp_angle((rotZ - self.start_gyr_heading) -
self.est.theta)
self.vis_est.propagate_alt(delta_t, x_vel, y_vel, altd, delta_theta,
False)
for particle in self.particle_list:
particle.propagate_alt(delta_t, x_vel, y_vel, altd, delta_theta,
True)
#Move this to correct_alt at some point
self.step += 1
self.estimate_equal()
self.publish_pose()
self.fp_alt.write("%d,%f,%f,%f,%f,%f,%f,%f,%f,%f\n" %
(self.step, delta_t, self.est.x, self.est.y,
self.est.z, self.est.theta, self.vis_est.x,
self.vis_est.y, self.vis_est.z, self.vis_est.theta))
self.fp_sensor.write(
"%d,%f,%f,%f,%f,%f,%f,%f\n" %
(self.step, delta_t, x_vel, y_vel, altd, rotX, rotY, rotZ))
def ar_correct(self, marker):
print "Correcting"
marker_id = marker.id
pose = marker.pose
pose_trans = ()
pose_rot = ()
#Is this necessary
pose_trans = (pose.position.x, pose.position.y, pose.position.z)
pose_rot = (pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w)
marker_name = "/marker_%d" % marker_id
#Get the offset of the marker from the origin
try:
(marker_trans, marker_rot) = self.listener.lookupTransform(
'world', marker_name, rospy.Time(0))
except (LookupException, ConnectivityException,
ExtrapolationException):
print("Unable to find marker transform for marker %d" % marker_id)
return
try:
(base_trans, base_rot) = self.listener.lookupTransform(
'/ardrone/ardrone_base_link',
'/ardrone/ardrone_base_bottomcam', rospy.Time(0))
except (LookupException, ConnectivityException,
ExtrapolationException):
print("Unable to find ardrone transform")
return
#Everything is in m not mm
pose_mat = numpy.matrix(
self.transformer.fromTranslationRotation(pose_trans, pose_rot))
pose_mat_inv = pose_mat.getI()
marker_mat = numpy.matrix(
self.transformer.fromTranslationRotation(marker_trans, marker_rot))
marker_mat_inv = marker_mat.getI()
base_mat = numpy.matrix(
self.transformer.fromTranslationRotation(base_trans, base_rot))
base_mat_inv = base_mat.getI()
origin = numpy.matrix([[0], [0], [0], [1]])
global_mat = marker_mat * pose_mat_inv * base_mat_inv
global_trans = translation_from_matrix(global_mat)
global_rot = rotation_from_matrix(global_mat)
global_trans = global_trans * 1000 #Convert to mm
global_heading = clamp_angle(global_rot[0] * 180 / pi +
180) #Convert to degrees
self.tag_est.x = global_trans[0]
self.tag_est.y = global_trans[1]
self.tag_est.z = global_trans[2]
self.tag_est.theta = global_heading
if (self.ar_resample):
#Weight particles
self.weight_particles(self.tag_est.x, self.tag_est.y,
self.tag_est.z, self.tag_est.theta, True)
#Create new set of particles
self.particle_list = []
#Initialize the random selector. Can select items in O(1) time
wrand = walkerrandom(self.weight_dict.values(),
self.weight_dict.keys())
for i in range(self.num_particles):
#Create particle from new value
new_particle = particle(self)
rand_val = random.random()
threshold = self.ar_resample_rate / 100.0
if (rand_val < threshold):
# print "Particle at (%f, %f, %f, %f)" % (self.tag_est.x, self.tag_est.y, self.tag_est.z, self.tag_est.theta)
new_particle = particle(self, self.tag_est.x,
self.tag_est.y, self.tag_est.z,
self.tag_est.theta)
#Randomly pick existing particle
else:
# print "Picking existing"
new_particle = copy(wrand.random())
self.particle_list.append(new_particle)
if (len(self.particle_list) != self.num_particles):
print("WRONG NUMBER OF PARTICLES")
self.fp_ar.write("%d,%d,%f,%f,%f,%f\n" %
(self.step, marker_id, self.tag_est.x, self.tag_est.y,
self.tag_est.z, self.tag_est.theta))
#Calculate the weight for particles
def weight_particles(self, x_est, y_est, z_est, theta_est, ar=False):
# print "Weighting......."
self.weight_dict = dict()
# print "Particle List:"
# for particle in self.particle_list:
# print particle.to_string()
weight_sum = 0
for particle in self.particle_list:
#Calculate distances
lat_dist = ((x_est - particle.x)**2 + (y_est - particle.y)**2)**.5
vert_dist = abs(z_est - particle.z)
theta_dist = abs(theta_est - particle.theta)
lat_noise = self.vis_noise
vert_noise = self.ultra_noise
theta_noise = self.mag_noise
#If this module is being called with ar_tag data, use ar_noise
if (ar):
lat_noise = self.ar_noise
vert_noise = self.ar_noise
theta_noise = self.ar_noise
lat_weight = normpdf(
lat_dist, 0,
lat_noise) #Potentially do the z distance separately?
vert_weight = normpdf(vert_dist, 0, vert_noise)
theta_weight = normpdf(theta_dist, 0, theta_noise)
weight = lat_weight + vert_weight + theta_weight
self.weight_dict[copy(particle)] = weight
weight_sum += weight
# print "Compare to (%f, %f, %f, %f)" % (x_est, y_est, z_est, theta_est)
#Normalize the weights
for particle, weight in self.weight_dict.iteritems():
if (weight_sum != 0):
self.weight_dict[particle] = weight / weight_sum
# print "(%s, %f)" % (particle.to_string(), self.weight_dict[particle])
#Return an estimate of the pose
#For now just use linear combination. Should we cluster instead?
def estimate(self):
self.est = particle(self)
total = 0
for part, weight in self.weight_dict.iteritems():
self.est.x += part.x * weight
self.est.y += part.y * weight
self.est.z += part.z * weight
self.est.theta += part.z * weight
total += weight
self.est.x = self.est.x / total
self.est.y = self.est.y / total
self.est.z = self.est.z / total
self.est.theta = self.est.theta / total
def publish_pose(self):
self.est_pose.position.x = self.est.x
self.est_pose.position.y = self.est.y
self.est_pose.position.z = self.est.z
rad_theta = self.est.theta * (pi / 180)
quat = quaternion_about_axis(rad_theta, (0, 0, 1))
self.est_pose.orientation.x = quat[0]
self.est_pose.orientation.y = quat[1]
self.est_pose.orientation.z = quat[2]
self.est_pose.orientation.w = quat[3]
self.est_pub.publish(self.est_pose)
def estimate_equal(self):
self.est = particle(self)
x_val = []
y_val = []
z_val = []
theta_val = []
total = 0
for part in self.particle_list:
self.est.x += part.x
self.est.y += part.y
self.est.z += part.z
self.est.theta += part.theta
x_val.append(part.x)
y_val.append(part.y)
z_val.append(part.z)
theta_val.append(part.theta)
total += 1
self.est.x = self.est.x / total
self.est.y = self.est.y / total
self.est.z = self.est.z / total
self.est.theta = self.est.theta / total
print "%s : Est" % self.est.to_string()
# print self.est.theta
def print_particles(self):
for particle in self.particle_list:
print particle.to_string()
# Compute camera base -> camera_optical transformation
opt_base = get_transform(rospy.get_param('camera_optical_frame'),
rospy.get_param('camera_base_frame'))
# Compute tracking_base_frame -> o2as_ground transformation
if not calib.eye_on_hand:
grnd_bot = get_transform('o2as_ground', bot)
broad = TransformBroadcaster()
rate = rospy.Rate(50)
baseEst = rospy.get_param('camera_body_frame')
try:
while not rospy.is_shutdown():
now = rospy.Time.now()
broad.sendTransform(trns, rot, now, optEst, bot) # ..., child, parent
broad.sendTransform(opt_base[0], opt_base[1], now, baseEst, optEst)
rate.sleep()
except rospy.ROSInterruptException:
transformer = TransformerROS()
mat = transformer.fromTranslationRotation(trns, rot)
print "\n=== Estimated camera -> robot(effector or base_link) transformation ==="
print_mat(mat)
if not calib.eye_on_hand:
mat = tfs.concatenate_matrices( \
transformer.fromTranslationRotation(*grnd_bot), mat,
transformer.fromTranslationRotation(*opt_base))
print "\n=== Estimated camera_base -> ground transformation ==="
print_mat(mat)
class LocalTransformer:
"""This class allows to use the TF class TransformerROS without using the ROS
network system or the topic /tf, where transforms are usually published to.
Instead a local transformer is saved and allows to publish local transforms,
as well the use of TFs convenient lookup functions (see functions below).
This class uses the robots (currently only one! supported) URDF file to
initialize the tfs for the robot. Moreover, the function update_local_transformer_from_btr
updates these tfs by copying the tfs state from the pybullet world."""
def __init__(self, map_frame, odom_frame, projection_namespace,
kitchen_namespace):
# Frame names of the map and odom frame
self.map_frame = map_frame
self.odom_frame = odom_frame
# Namespaces
self.projection_namespace = projection_namespace
self.kitchen_namespace = kitchen_namespace
# TF tf_stampeds and static_tf_stampeds of the Robot in the BulletWorld:
# These are initialized with the function init_transforms_from_urdf and are
# used to update the local transformer with the function update_local_transformer_from_btr
self.tf_stampeds = []
self.static_tf_stampeds = []
self.local_transformer = TransformerROS(interpolate=True,
cache_time=Duration(10.0))
self.init_local_tf_with_tfs_from_urdf()
def init_transforms_from_urdf(self):
"""
This static function gets with the robot name in robot_description.py, the
robots URDF file from the "resources" folder in the ROS "pycram" folder.
All joints of the URDF are extracted and saved in a list of static and
normal TFStamped.
:returns: list of static_tf_stampeds, list of tf_stampeds
"""
# Save joint translations and orientations in tf_stampeds
# static_tf_stampeds saves only the static joint translations and orientations
self.tf_stampeds = []
self.static_tf_stampeds = []
# Get URDF file path
robot_name = robot_description.i.name
rospack = rospkg.RosPack()
filename = rospack.get_path(
'pycram') + '/resources/' + robot_name + '.urdf'
# ... and open it
with open(filename) as f:
s = f.read()
robot = URDF.from_xml_string(s)
# Save for every joint in the robot the source_frame, target_frame,
# aswell the joints origin, where the translation xyz and rotation rpy
# are saved
for joint in robot.joints:
source_frame = self.projection_namespace + '/' + joint.parent
target_frame = self.projection_namespace + '/' + joint.child
if joint.origin:
if joint.origin.xyz:
translation = joint.origin.xyz
else:
translation = [0, 0, 0]
if joint.origin.rpy:
roll = joint.origin.rpy[0]
pitch = joint.origin.rpy[1]
yaw = joint.origin.rpy[2]
rotation = transformations.quaternion_from_euler(
roll, pitch, yaw)
else:
rotation = [0, 0, 0, 1]
if joint.name in robot_description.i.odom_joints:
# since pybullet wont update this joints, these are declared as static
translation = [0, 0, 0]
rotation = [0, 0, 0, 1]
# Wrap the joint attributes in a TFStamped and append it to static_tf_stamped if the joint was fixed
tf_stamped = helper.list2tfstamped(source_frame, target_frame,
[translation, rotation])
if (joint.type and joint.type == 'fixed'
) or joint.name in robot_description.i.odom_joints:
self.static_tf_stampeds.append(tf_stamped)
else:
self.tf_stampeds.append(tf_stamped)
def init_local_tf_with_tfs_from_urdf(self):
"This function initializes the local transformer with TFs from the robots URDF."
self.init_transforms_from_urdf()
for static_tfstamped in self.static_tf_stampeds:
self.local_transformer._buffer.set_transform_static(
static_tfstamped, "default_authority")
for tfstamped in self.tf_stampeds:
self.local_transformer.setTransform(tfstamped)
def update_robot_state(self):
robot = BulletWorld.robot
if robot:
# First publish (static) joint states to tf
# Update tf stampeds which might have changed
for tf_stamped in self.tf_stampeds:
source_frame = tf_stamped.header.frame_id
target_frame = tf_stamped.child_frame_id
# Push calculated transformation to the local transformer
p, q = robot.get_link_relative_to_other_link(
source_frame.replace(self.projection_namespace + "/", ""),
target_frame.replace(self.projection_namespace + "/", ""))
self.local_transformer.setTransform(
helper.list2tfstamped(source_frame, target_frame, [p, q]))
# Update the static transforms
for static_tf_stamped in self.static_tf_stampeds:
self.local_transformer._buffer.set_transform_static(
static_tf_stamped, "default_authority")
def update_robot_pose(self):
robot = BulletWorld.robot
if robot:
# Then publish pose of robot
try:
robot_pose = BulletWorld.robot.get_link_position_and_orientation_tf(
robot_description.i.base_frame)
except KeyError:
robot_pose = BulletWorld.robot.get_position_and_orientation()
self.publish_robot_pose(helper.ensure_pose(robot_pose))
def update_robot(self):
self.update_robot_state()
self.update_robot_pose()
def update_objects(self):
if BulletWorld.current_bullet_world:
for obj in list(BulletWorld.current_bullet_world.objects):
if obj.name == robot_description.i.name or obj.type == "environment":
continue
else:
published = local_transformer.publish_object_pose(
obj.name, obj.get_position_and_orientation())
if not published:
logerr("(publisher) Could not publish given pose of %s.",
obj.name)
def update_static_odom(self):
self.publish_pose(self.map_frame,
self.projection_namespace + '/' + self.odom_frame,
[[0, 0, 0], [0, 0, 0, 1]],
static=True)
def update_from_btr(self):
# Update static odom
self.update_static_odom()
# Update pose and state of robot
self.update_robot()
# Update pose of objects which are possibly attached on the robot
self.update_objects()
############################################################################################################
######################################### Query Local Transformer ##########################################
############################################################################################################
def tf_pose_transform(self, source_frame, target_frame, pose, time=None):
tf_pose = helper.ensure_pose(pose)
tf_time = time if time else self.local_transformer.getLatestCommonTime(
source_frame, target_frame)
tf_pose_stamped = PoseStamped(Header(0, tf_time, source_frame),
tf_pose)
return self.tf_pose_stamped_transform(target_frame, tf_pose_stamped)
def tf_pose_stamped_transform(self, target_frame, pose_stamped):
return helper.pose_stamped2tuple(
self.local_transformer.transformPose(target_frame, pose_stamped))
def tf_transform(self, source_frame, target_frame, time=None):
tf_time = time if time else self.local_transformer.getLatestCommonTime(
source_frame, target_frame)
return self.local_transformer.lookupTransform(source_frame,
target_frame, tf_time)
############################################################################################################
####################################### Publish to Local Transformer ###########'###########################
############################################################################################################
def publish_pose(self, frame_id, child_frame_id, pose, static=None):
if frame_id != child_frame_id:
pose = helper.ensure_pose(pose)
tf_stamped = helper.pose2tfstamped(frame_id, child_frame_id, pose)
if tf_stamped:
if static:
self.local_transformer._buffer.set_transform_static(
tf_stamped, "default_authority")
else:
self.local_transformer.setTransform(tf_stamped)
return True
def publish_object_pose(self, name, pose):
"""
Publishes the given pose of the object of given name in reference to the map frame to tf.
:type name: str
:type pose: list or Pose
"""
if name in robot_description.i.name:
return None
if name not in robot_description.i.name:
pose = helper.ensure_pose(pose)
if pose:
tf_name = self.projection_namespace + '/' + name if self.projection_namespace else name
return self.publish_pose(self.map_frame, tf_name, pose)
else:
logerr(
"(publisher) Could not publish given pose of %s since"
" it could not be converted to a Pose object.", name)
return None
def publish_robot_pose(self, pose):
"Publishes the base_frame of the robot in reference to the map frame to tf."
robot_pose = helper.ensure_pose(pose)
if robot_pose:
tf_base_frame = self.projection_namespace + '/' + robot_description.i.base_frame \
if self.projection_namespace \
else robot_description.i.base_frame
return self.publish_pose(self.map_frame, tf_base_frame, robot_pose)
else:
logerr("(publisher) Could not publish given pose of robot since"
" it could not be converted to a Pose object.")
return None
def publish_robots_link_pose(self, link, pose):
"Publishes the frame link of the robot in reference to the base_frame of the robot to tf."
robot_pose = helper.ensure_pose(pose)
if robot_pose:
tf_base_frame = self.projection_namespace + '/' + robot_description.i.base_frame \
if self.projection_namespace \
else robot_description.i.base_frame
tf_link_frame = self.projection_namespace + '/' + link \
if self.projection_namespace \
else link
if not tf_base_frame in tf_link_frame:
return self.publish_pose(tf_base_frame, tf_link_frame,
robot_pose)
else:
logerr("(publisher) Could not publish given pose of robot since"
" it could not be converted to a Pose object.")
return None
# except rospy.ServiceException, e:
# print "Service call failed: %s"%e
# def usage():
# return "%s [plate_id]"%sys.argv[0]
if __name__ == "__main__":
rospy.init_node("all")
pub_tilt = rospy.Publisher("/teleop/tilt",Float64, queue_size=1)
listener = tf.TransformListener()
transformer = TransformerROS()
rospy.wait_for_service('get_object_pose')
get_object_pose = rospy.ServiceProxy('get_object_pose', get_object_pose)
object_pose = Pose()
try:
response = get_object_pose()
object_pose = response.pose
print(object_pose)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
exit()
try:
(trans,rot) = listener.lookupTransform('/base_link', "/camera_color_optical_frame", rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
