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 __init__(self):
# ros param
self.robot_x = rospy.get_param('/slam/robot_x',0)
self.robot_y = rospy.get_param('/slam/robot_y',0)
self.robot_theta = rospy.get_param('/slam/robot_theta',0)
## ros param of mapping
self.map_x_width = rospy.get_param('/slam/map_width')
self.map_y_width = rospy.get_param('/slam/map_height')
self.map_reso = rospy.get_param('/slam/map_resolution')
self.map_cellx_width = int(round(self.map_x_width/self.map_reso))
self.map_celly_width = int(round(self.map_y_width/self.map_reso))
self.icp = ICP()
self.ekf = EKF()
self.extraction = Extraction()
self.mapping = Mapping(self.map_cellx_width,self.map_celly_width,self.map_reso)
# odom robot init states
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((STATE_SIZE, 1))
self.xEst = np.zeros((STATE_SIZE, 1))
self.PEst = np.eye(STATE_SIZE)
# map observation
self.obstacle = []
# radius
self.obstacle_r = 10
# 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)
self.map_pub = rospy.Publisher('/slam_map',OccupancyGrid,queue_size=1)
def step(self):
scan, odom = self.raw_data[self.idx]
rot, trans = getRtFromM(self.scan_base)
scan = transpose(rot, trans, scan)
try:
if not self.checkupdate(matrix_to_pose(self.last_odom),matrix_to_pose(odom)):
return False
last_odom = self.last_odom
last_scan = self.last_scan
self.last_odom = odom
self.last_scan = scan
except AttributeError:
self.last_odom = odom
self.last_scan = scan
return False
print check_loop_candidate(scan)
delta_odom_init = getDeltaM(last_odom, odom)
delta_rot_init, delta_trans_init = getRtFromM(delta_odom_init)
#start_time = time.time()
icp = ICP(last_scan, scan)
#delta_rot, delta_trans, cost = icp.calculate(20, delta_rot_init, delta_trans_init, 0.01)
delta_rot = delta_rot_init
delta_trans = delta_trans_init
delta_odom = getMFromRt(delta_rot, delta_trans)
#print 'init yaw:',matrix_to_pose(delta_odom_init)[2]
#print 'icp yaw:',matrix_to_pose(delta_odom)[2]
self.pose_matrix = np.dot(self.pose_matrix, delta_odom)
self.graph_base.append((scan, delta_odom, self.pose_matrix))
#print self.pose_matrix
rot, trans = getRtFromM(self.pose_matrix)
scan_t = transpose(rot, trans, scan)
self.gui.setpcd(scan_t)
self.gui.setrobot(matrix_to_pose(self.pose_matrix))
return True
def __init__(self):
self.gui_ = ScanGUI()
# map params
self.map_w = 20.0 #5x5 physical map
self.map_h = 20.0
# self.map_ = SparseMap(res = 0.02)
# self.map_ = DenseMap(w=self.map_w, h=self.map_h, res = 0.1)
self.map_ = ProbMap(w=self.map_w, h=self.map_h, res=0.1)
# raycast params
self.dist_res = .01
self.ang_res = np.deg2rad(1.0)
# query params
self.sensor_radius = 5.0
#self.seen_thresh = 2
self.seen_thresh = 0.68 # probability! TODO: tune
#Robot properities
self.linVector = Vector3(x=0.0, y=0.0, z=0.0)
self.angVector = Vector3(x=0.0, y=0.0, z=0.0)
self.lidar_range = 5.0
# 2d pose
self.x = 0
self.y = 0
self.theta = 0
self.path = np.empty(shape=(0,3), dtype=np.float32)
# sensor data cache
self.ranges = [] #From stable scan
self.old_ranges = []
self.points = [] #From projected stable scan
self.old_points = []
self.img = []
#Offset based on ICP, (x,y,theta)
self.map_to_odom = np.zeros(shape=3)
self.bridge = CvBridge()
#ICP
self.icp = ICP()
#Ben
#self.data_path = "/home/bziemann/data/"
#Jaime
self.data_path = "/tmp/"
#ROS2/.02
self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size=2)
self.vizPub = rospy.Publisher('/visualization_marker', Marker, queue_size=10)
self.tfl_ = tf.TransformListener()
self.tfb_ = tf.TransformBroadcaster()
rospy.Subscriber("/stable_scan", LaserScan, self.checkLaser)
rospy.Subscriber("/projected_stable_scan", PointCloud, self.checkPoints)
#rospy.Subscriber("/odom", Odometry, self.setLocation)
rospy.Subscriber('/camera/image_raw', Image, self.setImage)
class dataCollector:
def __init__(self):
self.gui_ = ScanGUI()
# map params
self.map_w = 20.0 #5x5 physical map
self.map_h = 20.0
# self.map_ = SparseMap(res = 0.02)
# self.map_ = DenseMap(w=self.map_w, h=self.map_h, res = 0.1)
self.map_ = ProbMap(w=self.map_w, h=self.map_h, res=0.1)
# raycast params
self.dist_res = .01
self.ang_res = np.deg2rad(1.0)
# query params
self.sensor_radius = 5.0
#self.seen_thresh = 2
self.seen_thresh = 0.68 # probability! TODO: tune
#Robot properities
self.linVector = Vector3(x=0.0, y=0.0, z=0.0)
self.angVector = Vector3(x=0.0, y=0.0, z=0.0)
self.lidar_range = 5.0
# 2d pose
self.x = 0
self.y = 0
self.theta = 0
self.path = np.empty(shape=(0,3), dtype=np.float32)
# sensor data cache
self.ranges = [] #From stable scan
self.old_ranges = []
self.points = [] #From projected stable scan
self.old_points = []
self.img = []
#Offset based on ICP, (x,y,theta)
self.map_to_odom = np.zeros(shape=3)
self.bridge = CvBridge()
#ICP
self.icp = ICP()
#Ben
#self.data_path = "/home/bziemann/data/"
#Jaime
self.data_path = "/tmp/"
#ROS2/.02
self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size=2)
self.vizPub = rospy.Publisher('/visualization_marker', Marker, queue_size=10)
self.tfl_ = tf.TransformListener()
self.tfb_ = tf.TransformBroadcaster()
rospy.Subscriber("/stable_scan", LaserScan, self.checkLaser)
rospy.Subscriber("/projected_stable_scan", PointCloud, self.checkPoints)
#rospy.Subscriber("/odom", Odometry, self.setLocation)
rospy.Subscriber('/camera/image_raw', Image, self.setImage)
def convert_points(self, points):
"""
Apply offset from previous rounds to current scan
points - the current set of points from the LIDAR scan
"""
T_o2m = np.eye(3)
T_o2m[:2,:2] = R2(self.map_to_odom[-1])
T_o2m[:2,2] = self.map_to_odom[:2]
points = applyTransformToPoints(T_o2m, points)
return points
def publishVelocity(self, linX, angZ):
"""
Publishes velocities to make the robot move
linX - 0 and 1 to control the robot's x linear velocity (m/s)
angZ - 0 and 1 to control the robot's z angular velocity (rad/s)
"""
self.linVector.x = linX
self.angVector.z = angZ
self.pub.publish(Twist(linear=self.linVector, angular=self.angVector))
def checkLaser(self, scan):
"""
Deprecated in favor of projected stable scan
Pulls laser scan data
Scan: rosmsg with LIDAR Scan data
"""
self.old_ranges = self.ranges
self.ranges = scan.ranges
def queryPoints(self):
"""
Points from the map that we care about matching with the incoming scan.
"""
center = (self.x, self.y)
points = self.map_.query(
origin = (self.x, self.y, self.theta),
radius = self.sensor_radius,
thresh = self.seen_thresh
)
return points
def checkPoints(self, msg):
"""
Time matched LIDAR. Produces much better scans, however not all LIDAR
is collected
"""
self.old_points = self.points
points = [ [p.x, p.y] for p in msg.points]
# convert incoming points to map frame
self.points = self.convert_points(points)
def setTFLocation(self):
"""
Update the robot's estimated position based on offset
"""
try:
txn, qxn = self.tfl_.lookupTransform('odom', 'base_link', rospy.Time(0))
except Exception as e:
rospy.loginfo_throttle(1.0, 'Failed TF Transform : {}'.format(e))
return
# odom frame
x, y = txn[0], txn[1]
h = tf.transformations.euler_from_quaternion(qxn)[-1]
x, y = self.convert_points([[x, y]])[0]
h = h + self.map_to_odom[-1]
self.x, self.y = (x,y)
self.theta = h
def setLocation(self, odom):
"""
Convert pose (geometry_msgs.Pose) to a (x, y, theta) tuple
Constantly being called as it is the callback function for this node's subscription
odom is Neato ROS' nav_msgs/Odom msg composed of pose and orientation submessages
"""
pose = odom.pose.pose
orientation_tuple = (pose.orientation.x,
pose.orientation.y,
pose.orientation.z,
pose.orientation.w)
angles = euler_from_quaternion(orientation_tuple)
# convert to map position
# TODO : conversion happens here, or delay?
self.x, self.y = self.convert_points([[pose.position.x, pose.position.y]])[0]
self.theta = angles[2] + self.map_to_odom[-1]
def setImage(self, img):
"""
Pull image data from camera
img: rosmsg containing image grabbed from camera
"""
img = self.bridge.imgmsg_to_cv2(img, desired_encoding="bgr8")
img = cv2.resize(img, (160, 120), interpolation=cv2.INTER_AREA)
self.img = img
def update_current_map_to_odom_offset_estimate(self, t):
"""
Update the robot's offset based on ICP result. Used in estimated position offset from odometry
t = homogenuous transformation matrix output of ICP
"""
# what it used to be
dx0, dy0, dh0 = self.map_to_odom
# current error
ddx, ddy = t[:2,2]
ddh = math.atan2(t[1,0], t[0,0])
# update with coupled transform
dx1, dy1 = R2(ddh).dot([dx0,dy0]) + [ddx,ddy]
dh1 = (dh0 + ddh)
self.map_to_odom = np.asarray([dx1,dy1,dh1])
def raycast(self, map_points):
"""
Perform raycasting on current map to decide what points to use in ICP
"""
# Step 1 - Get list of map points + convert to polar coordinates centered around the robot
rh = convert_to_polar(map_points, [self.x, self.y, self.theta])
rads, angs = rh[:,0], rh[:,1]
angs = np.round(angs / self.ang_res).astype(np.int32)
# Step 2 - Put the points into bins based on specified angular resolution
rbins = defaultdict(lambda:[])
pbins = defaultdict(lambda:[])
for (p,r,a) in zip(map_points, rads, angs):
rbins[a].append(r)
pbins[a].append(p)
# Step 3 - Go through the points and find the coordinates of the minimum radius
points = []
for a in angs:
idx = np.argmin(rbins[a])
points.append(pbins[a][idx])
points = np.asarray(points, dtype=np.float32) # convert to np array
return points
def run(self):
#Generate data for debugging
filename = os.path.join(self.data_path, 'data.csv')
f = open(filename, "w+")
f.write("x,y,theta,scan\n")
count = 0
#To prevent image overflow
image_enabled = False
rate = rospy.Rate(100)
while not rospy.is_shutdown():
points = np.asarray(self.points)
#Don't do anything if there is no data to work with
if (not len(points)==0) and (not len(self.old_points)==0):
self.points = []
count += 1
if image_enabled and np.size(self.img) == 0:
continue
#Update robot position
self.setTFLocation()
map_points = self.queryPoints()
#Online visualization
rospy.loginfo_throttle(1.0, 'System Online : ({},{},{})'.format(self.x,self.y,self.theta))
# logging
line = str(self.x)+","+str(self.y)+","+str(self.theta)+","+str(self.ranges)[1:-1]+"\n"
f.write(line)
valid_map_points = np.empty(shape=(0,2), dtype=np.float32)
trans = None
try:
valid_map_points = self.raycast(map_points)
#Ensure enough data to work with
c_map_cnt = len(valid_map_points) >= 20
c_scan_cnt = len(points) >= 20
#ICP
if c_map_cnt and c_scan_cnt:
trans, diff, idx, num_iter, inl = self.icp.icp(
points,
valid_map_points)
#Ensure no massive jumps caused by incorrect transform
offset_euclidean = np.linalg.norm(trans[:2,2])
offset_h = trans
c_jump = ((offset_euclidean > 0.25) and (inl < 0.75)) # unsupported jump
c_jump = c_jump or offset_euclidean > 1.0 # "impossible" jump
c_sparse = (inl < 0.5) # not enough inliers
if c_jump or c_sparse:
cause = ('jump' if c_jump else 'insufficient constraint/stability')
msg = 'rejecting transform due to {} : d={} p={}%' .format(
cause, offset_euclidean, 100*inl)
print(msg)
trans = None
else:
trans = None
except Exception as e:
print 'e', e
print map_points
print map_points.shape
raise e
#Update the offset - but only if more than 5 scans have been registered so far
# (prevent premature transform computation)
# update the map with the transformed points
if (trans is not None) and (count >= 5):
self.update_current_map_to_odom_offset_estimate(trans)
self.map_.update(applyTransformToPoints(trans, points),
origin=[self.x, self.y, self.theta]
)
#Send correction to ROS TF Stream
m2ox, m2oy, m2oh = self.map_to_odom
self.tfb_.sendTransform(
[m2ox, m2oy, 0],
tf.transformations.quaternion_from_euler(0, 0, m2oh),
rospy.Time.now(),
'odom',
'map')
else:
self.map_.update(points, origin=[self.x, self.y, self.theta])
#Vizualize
if len(map_points) > 0:
self.gui_(points, valid_map_points, self.path,
origin=[self.x,self.y,self.theta])
self.map_.show(ax=self.gui_.ax1_)
self.gui_.show()
#Save Images
self.path = np.concatenate([self.path, [[self.x, self.y, self.theta]] ], axis=0)
if image_enabled:
fileNum = self.data_path+"img" +str(count) +".png"
cv2.imwrite(fileNum,self.img)
self.ranges=[]
rate.sleep()
class SLAM_EKF():
def __init__(self):
# ros param
self.robot_x = rospy.get_param('/slam/robot_x',0)
self.robot_y = rospy.get_param('/slam/robot_y',0)
self.robot_theta = rospy.get_param('/slam/robot_theta',0)
## ros param of mapping
self.map_x_width = rospy.get_param('/slam/map_width')
self.map_y_width = rospy.get_param('/slam/map_height')
self.map_reso = rospy.get_param('/slam/map_resolution')
self.map_cellx_width = int(round(self.map_x_width/self.map_reso))
self.map_celly_width = int(round(self.map_y_width/self.map_reso))
self.icp = ICP()
self.ekf = EKF()
self.extraction = Extraction()
self.mapping = Mapping(self.map_cellx_width,self.map_celly_width,self.map_reso)
# odom robot init states
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((STATE_SIZE, 1))
self.xEst = np.zeros((STATE_SIZE, 1))
self.PEst = np.eye(STATE_SIZE)
# map observation
self.obstacle = []
# radius
self.obstacle_r = 10
# 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)
self.map_pub = rospy.Publisher('/slam_map',OccupancyGrid,queue_size=1)
def laserCallback(self,msg):
np_msg = self.laserToNumpy(msg)
lm = self.extraction.process(np_msg)
# u = self.calc_odometry(self.lm2pc(lm))
u = self.calc_odometry(np_msg)
z = self.observation(lm)
self.xEst,self.PEst = self.ekf.estimate(self.xEst,self.PEst,z,u)
# TODO
# obs = self.u2T(self.xEst[0:3]).dot(np_msg)
# pmap = self.mapping.update(obs[0], obs[1], self.xEst[0], self.xEst[1])
# self.publishMap(pmap)
self.publishLandMark(lm)
self.publishResult()
pass
def observation(self,lm):
landmarks = lm
z = np.zeros((0, 3))
for i in range(len(landmarks.id)):
dx = landmarks.position_x[i]
dy = landmarks.position_y[i]
d = math.hypot(dx, dy)
angle = self.ekf.pi_2_pi(math.atan2(dy, dx))
zi = np.array([d, angle, i])
z = np.vstack((z, zi))
return z
def calc_odometry(self,np_msg):
if self.isFirstScan:
self.tar_pc = np_msg
self.isFirstScan = False
return np.array([[0.0,0.0,0.0]]).T
self.src_pc = np_msg
transform_acc = self.icp.process(self.tar_pc,self.src_pc)
self.tar_pc = np_msg
return self.T2u(transform_acc)
def laserToNumpy(self,msg):
total_num = len(msg.ranges)
pc = np.ones([3,total_num])
range_l = np.array(msg.ranges)
range_l[range_l == np.inf] = MAX_LASER_RANGE
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 u2T(self,u):
w = u[2]
dx = u[0]
dy = u[1]
return np.array([
[ math.cos(w),-math.sin(w), dx],
[ math.sin(w), math.cos(w), dy],
[0,0,1]
])
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 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)
print("mdbg ",self.xEst.shape)
# self.laser_pub.publish(self.target_laser)
pass
def publishLandMark(self,msg):
# 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(((self.xEst.shape[0]-STATE_SIZE)/2)):
marker = Marker()
marker.header.frame_id = "world_base"
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 = self.xEst[STATE_SIZE+i*2,0]
marker.pose.position.y = self.xEst[STATE_SIZE+i*2+1,0]
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)
def publishMap(self,pmap):
map_msg = OccupancyGrid()
map_msg.header.seq = 1
map_msg.header.stamp = rospy.Time().now()
map_msg.header.frame_id = "map"
map_msg.info.map_load_time = rospy.Time().now()
map_msg.info.resolution = self.map_reso
map_msg.info.width = self.map_cellx_width
map_msg.info.height = self.map_celly_width
map_msg.info.origin.position.x = -self.map_cellx_width*self.map_reso/2.0
map_msg.info.origin.position.y = -self.map_celly_width*self.map_reso/2.0
map_msg.info.origin.position.z = 0
map_msg.info.origin.orientation.x = 0
map_msg.info.origin.orientation.y = 0
map_msg.info.origin.orientation.z = 0
map_msg.info.origin.orientation.w = 1.0
map_msg.data = list(pmap.T.reshape(-1)*100)
self.map_pub.publish(map_msg)
def creategraph(self, data):
nodes = []
edges = []
# Generate basic nodes and edges for our pose graph
for i in range(len(data)):
scan, pose = data[i]
nodes.append(pose)
self.pg.nodes.append(pose)
if i == 0:
continue
infm = np.array([[weight_trans_, 0, 0], [0, weight_trans_, 0],
[0, 0, weight_rot_]])
edge = [i - 1, i, get_motion_vector(nodes[i], nodes[i - 1]), infm]
edges.append(edge)
self.pg.edges.append(PoseEdge(*edge))
self.pg.nodes = np.array(self.pg.nodes)
# Detect the loop-closing
loop_count = 0
self.loop_candidates = []
for i in range(len(data)):
if i + min_loop_dist_ >= len(data):
continue
scan_i, pose_i = data[i]
if not check_loop_candidate(scan_i, hough_weight_):
continue
self.loop_candidates.append(i)
for j in range(i + min_loop_dist_, len(data)):
scan_j, pose_j = data[j]
dst = distance.euclidean(pose_i[0:2], pose_j[0:2])
if dst > max_dist_:
continue
#if not check_loop_candidate(scan_j, hough_weight_):
# continue
loop_count += 1
delta_odom_init = getDeltaM(v2t(pose_i), v2t(pose_j))
delta_rot_init, delta_trans_init = getRtFromM(delta_odom_init)
#start_time = time.time()
icp = ICP(scan_i, scan_j)
delta_rot, delta_trans, cost = icp.calculate(
200, delta_rot_init, delta_trans_init, max_dist_)
if (cost > first_cost_):
#print 'old cost:',cost
delta_rot, delta_trans, cost = icp.calculate(
200, delta_rot, delta_trans, 0.5)
#print 'new cost:',cost
#print '-------------'
if (use_cost_threshold_ and (cost > second_cost_)):
continue
#cost = cost/5
if icp_debug_:
new_scan_j = transpose(delta_rot, delta_trans.ravel(),
scan_j)
scan_j = transpose(delta_rot_init,
delta_trans_init.ravel(), scan_j)
show_icp_matching(
scan_i, new_scan_j, scan_j,
str(i) + '-->' + str(j) + 'cost:' + str(cost))
#plt.show()
infm = np.array([[weight_trans_ / cost**2, 0, 0],
[0, weight_trans_ / cost**2, 0],
[0, 0, weight_rot_ / cost**2]])
edge = [i, j, t2v(getMFromRt(delta_rot, delta_trans)), infm]
edges.append(edge)
self.pg.edges.append(PoseEdge(*edge))
print('{0} loop have been detected!'.format(loop_count))
'--number',
dest='pointNum',
help='number of points in the point cloud',
type='int',
default=256)
parser.add_option('-p',
'--plot',
dest='plot',
action='store_true',
help='visualize icp result',
default=False)
parser.add_option('-c',
'--core',
dest='coreNum',
help='number of threads used in the cuda',
type='int',
default=0)
(options, args) = parser.parse_args()
pc1 = PointCloud()
pc1.initFromRand(options.pointNum, 0, 1, 10, 20, 0, 1)
pc2 = PointCloud(pc1)
pc2.applyTransformation(None, None)
icpSolver = ICP(pc1, pc2, plot=options.plot)
icpSolver.solve()
print
icpParallelSolver = ICPParallel(pc1,
pc2,
numCore=options.coreNum,
plot=options.plot)
icpParallelSolver.solve()
class Localization():
def __init__(self):
self.icp = ICP()
self.ekf = EKF()
# 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.laser_pub = rospy.Publisher('/target_laser',
LaserScan,
queue_size=3)
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()
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):
# TODO
pass
def laserEstimation(self, msg, x):
# TODO
return data
def calc_map_observation(self, msg):
# TODO
return transform_acc
def calc_odometry(self, msg):
if self.isFirstScan:
self.tar_pc = self.laserToNumpy(
self.laserEstimation(msg, self.xEst))
# print("ddddddddddddddddddd ",self.tar_pc - self.laserToNumpy(msg))
self.isFirstScan = False
self.src_pc = self.laserToNumpy(msg)
transform_acc = self.icp.process(self.tar_pc, self.src_pc)
self.tar_pc = self.laserToNumpy(msg)
return transform_acc
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 publishResult(self):
# tf
s = self.xEst
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
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)