class Localization():
def __init__(self):
self.icp = ICP()
self.ekf = EKF()
self.extraction = Extraction()
# odom robot init states
self.robot_x = rospy.get_param('/icp/robot_x',0)
self.robot_y = rospy.get_param('/icp/robot_y',0)
self.robot_theta = rospy.get_param('/icp/robot_theta',0)
self.sensor_sta = [self.robot_x,self.robot_y,self.robot_theta]
self.isFirstScan = True
self.src_pc = []
self.tar_pc = []
# State Vector [x y yaw]
self.xOdom = np.zeros((3,1))
self.xEst = np.zeros((3,1))
self.PEst = np.eye(3)
# map observation
self.obstacle = []
# radius
self.obstacle_r = 10
# init map
self.updateMap()
# ros topic
self.laser_sub = rospy.Subscriber('/course_agv/laser/scan',LaserScan,self.laserCallback)
self.location_pub = rospy.Publisher('ekf_location',Odometry,queue_size=3)
self.odom_pub = rospy.Publisher('icp_odom',Odometry,queue_size=3)
self.odom_broadcaster = tf.TransformBroadcaster()
self.landMark_pub = rospy.Publisher('/landMarks',MarkerArray,queue_size=1)
def updateMap(self):
print("debug: try update map obstacle")
rospy.wait_for_service('/static_map')
try:
getMap = rospy.ServiceProxy('/static_map',GetMap)
self.map = getMap().map
# print(self.map)
except:
e = sys.exc_info()[0]
print('Service call failed: %s'%e)
# Update for planning algorithm
map_data = np.array(self.map.data).reshape((-1,self.map.info.height)).transpose()
tx,ty = np.nonzero((map_data > 20)|(map_data < -0.5))
ox = (tx*self.map.info.resolution+self.map.info.origin.position.x)*1.0
oy = (ty*self.map.info.resolution+self.map.info.origin.position.y)*1.0
self.obstacle = np.vstack((ox,oy)).transpose()
self.obstacle_r = self.map.info.resolution
print("debug: update map obstacle success! ")
def laserCallback(self,msg):
u = self.calc_odometry(msg)
z = self.ekf.odom_model(self.xEst,self.calc_map_observation(self.laserToNumpy(msg)))
self.xEst,self.PEst = self.ekf.estimate(self.xEst,self.PEst,z,u)
self.publishResult()
pass
def laserEstimation(self):
# TODO
return pc
def calc_map_observation(self,msg):
landmarks_src = self.extraction.process(msg,True)
landmarks_tar = self.extraction.process(self.laserEstimation(),True)
print("lm : ",len(landmarks_src.id),len(landmarks_tar.id))
self.publishLandMark(landmarks_src,landmarks_tar)
src_pc = self.lm2pc(landmarks_src)
tar_pc = self.lm2pc(landmarks_tar)
transform_acc = self.icp.process(tar_pc,src_pc)
return self.T2u(transform_acc)
def calc_odometry(self,msg):
if self.isFirstScan:
self.tar_pc = self.laserToNumpy(msg)
self.isFirstScan = False
return np.array([[0.0,0.0,0.0]]).T
self.src_pc = self.laserToNumpy(msg)
transform_acc = self.icp.process(self.tar_pc,self.src_pc)
self.tar_pc = self.laserToNumpy(msg)
return self.T2u(transform_acc)
def lm2pc(self,lm):
total_num = len(lm.id)
dy = lm.position_y
dx = lm.position_x
range_l = np.hypot(dy,dx)
angle_l = np.arctan2(dy,dx)
pc = np.ones((3,total_num))
pc[0:2,:] = np.vstack((np.multiply(np.cos(angle_l),range_l),np.multiply(np.sin(angle_l),range_l)))
print("mdbg ",total_num)
return pc
def laserToNumpy(self,msg):
total_num = len(msg.ranges)
pc = np.ones([3,total_num])
range_l = np.array(msg.ranges)
angle_l = np.linspace(msg.angle_min,msg.angle_max,total_num)
pc[0:2,:] = np.vstack((np.multiply(np.cos(angle_l),range_l),np.multiply(np.sin(angle_l),range_l)))
return pc
def T2u(self,t):
dw = math.atan2(t[1,0],t[0,0])
u = np.array([[t[0,2],t[1,2],dw]])
return u.T
def publishResult(self):
# tf
s = self.xEst.reshape(-1)
q = tf.transformations.quaternion_from_euler(0,0,s[2])
self.odom_broadcaster.sendTransform((s[0],s[1],0.001),(q[0],q[1],q[2],q[3]),
rospy.Time.now(),"ekf_location","world_base")
# odom
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = "world_base"
odom.pose.pose.position.x = s[0]
odom.pose.pose.position.y = s[1]
odom.pose.pose.position.z = 0.001
odom.pose.pose.orientation.x = q[0]
odom.pose.pose.orientation.y = q[1]
odom.pose.pose.orientation.z = q[2]
odom.pose.pose.orientation.w = q[3]
self.location_pub.publish(odom)
s = self.xOdom
q = tf.transformations.quaternion_from_euler(0,0,s[2])
# odom
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = "world_base"
odom.pose.pose.position.x = s[0]
odom.pose.pose.position.y = s[1]
odom.pose.pose.position.z = 0.001
odom.pose.pose.orientation.x = q[0]
odom.pose.pose.orientation.y = q[1]
odom.pose.pose.orientation.z = q[2]
odom.pose.pose.orientation.w = q[3]
self.odom_pub.publish(odom)
# self.laser_pub.publish(self.target_laser)
pass
def publishLandMark(self,msg,msg2):
# msg = LandMarkSet()
if len(msg.id) <= 0:
return
landMark_array_msg = MarkerArray()
for i in range(len(msg.id)):
marker = Marker()
marker.header.frame_id = "course_agv__hokuyo__link"
marker.header.stamp = rospy.Time(0)
marker.ns = "lm"
marker.id = i
marker.type = Marker.SPHERE
marker.action = Marker.ADD
marker.pose.position.x = msg.position_x[i]
marker.pose.position.y = msg.position_y[i]
marker.pose.position.z = 0 # 2D
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.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.color.a = 0.5 # Don't forget to set the alpha
marker.color.r = 0.0
marker.color.g = 0.0
marker.color.b = 1.0
landMark_array_msg.markers.append(marker)
for i in range(len(msg2.id)):
marker = Marker()
marker.header.frame_id = "course_agv__hokuyo__link"
marker.header.stamp = rospy.Time(0)
marker.ns = "lm"
marker.id = i+len(msg.id)
marker.type = Marker.SPHERE
marker.action = Marker.ADD
marker.pose.position.x = msg2.position_x[i]
marker.pose.position.y = msg2.position_y[i]
marker.pose.position.z = 0 # 2D
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.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.color.a = 0.5 # Don't forget to set the alpha
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
landMark_array_msg.markers.append(marker)
self.landMark_pub.publish(landMark_array_msg)
