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 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()
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)
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()
rate = rospy.Rate(50)
while not rospy.is_shutdown():
broad.sendTransform(translation, rotation, rospy.Time.now(), dest,
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()
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.")
# 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)
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):
print("tf error %s"%e)
transpose_matrix = transformer.fromTranslationRotation(trans, rot)
matrix = quaternion_matrix([object_pose.orientation.x, object_pose.orientation.y, object_pose.orientation.z, object_pose.orientation.w])
position = np.array([object_pose.position.x, object_pose.position.y, object_pose.position.z, 1])
matrix[:,3] = position
base_position = np.dot(transpose_matrix, matrix)
print(base_position)
#-----------------------------------grasp-------------------------------------------
grasp_request = dataRequest()
pub_tilt.publish(0)
rospy.wait_for_service('grasp', 3)
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()
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)
