def fusion(self, pose, dt, msg):
u, v = lidar2img(pose.x, pose.y, pose.z)
coneTmp = cone_on_img(u, v)
jj = len(image_raw)
dist = []
if jj != 0:
for ii in range(0, jj):
dist.append(Euc_dist(coneTmp, image_raw[jj]))
ii = findMinI(dist)
cone = cone_pos()
cone.x = pose.x
cone.y = pose.y
cone.color = image_raw[ii].color
self.lidar_raw.append(cone)
def fusion(self, pose, dt):
num = len(self.image_raw) - 1
#dist = []
if num > -1:
u, v = lidar2img.lidar2img(pose.x, pose.y, pose.z)
jj = len(self.image_raw[num].image_points)
if jj != 0:
#for ii in range(0, jj):
#dist.append(self.Euc_dist(coneTmp, self.image_raw[num].image_points[jj - 1]))
#ii = self.findMinI(dist)
cone = cone_pos()
cone.x = pose.x
cone.y = pose.y
#cone.color = self.image_raw[num].image_points[ii].color
cone.color = self.match(u, v)
self.lidar_raw.append(cone)
def local2map(self,odom_x,odom_y,odom_yaw,time_begin,time_end,latest_control):
#Effects: transform the cone pose from local to map frame
# only those cones within the given time range [time_begin,time_end) will be transformed.
#print "time_begin == " + str(time_begin) + "time_end == " + str(time_end)
denominator = np.tan(math.radians(latest_control.steer_angle))
R = 1000000000000.0
if denominator != 0:
R = L / denominator
#R = L / denominator
i = 0
for i in range(0,len(self.local_cones)):
if self.local_cones[i].stamp < time_begin:
continue
if self.local_cones[i].stamp >= time_end:
i = i - 1
break
#print "timestamp == " + str(self.local_cones[i].stamp)
theta = latest_control.linear_velocity * (self.local_cones[i].stamp - time_begin) / R
x1 = odom_x + R*np.sin(theta)*np.cos(odom_yaw)-R*(1-np.cos(theta))*np.sin(odom_yaw)
y1 = odom_y + R*np.sin(theta)*np.sin(odom_yaw)+R*(1-np.cos(theta))*np.cos(odom_yaw)
yaw1 = odom_yaw + math.atan2(1-np.cos(theta),np.sin(theta))
for j in range(0, len(self.local_cones[i].cones)):
global_cone = cone_pos()
global_cone.x = x1 + (self.local_cones[i].cones[j].x+tf_lidar_base)*np.cos(yaw1) - self.local_cones[i].cones[j].y*np.sin(yaw1)
global_cone.y = y1 + (self.local_cones[i].cones[j].x+tf_lidar_base)*np.sin(yaw1) - self.local_cones[i].cones[j].y*np.cos(yaw1)
global_cone.color = self.local_cones[i].cones[j].color
'''
if global_cone.x < 0:
print "----------"
print "strange x == " + str(global_cone.x) + " y == " + str(global_cone.x)
print "R == " + str(R) + "theta == " + str(theta)
print "odom_x == " + str(odom_x) + "odom_y == " + str(odom_y) + "odom_yaw == " + str(odom_yaw)
print "timestamp == " + str(self.local_cones[i].stamp) + " time_begin == " + str(time_begin)
'''
self.cone_fusion.push(global_cone,self.local_cones[i].stamp)#the Cone_fusion container will generate the map and publish the data
del self.local_cones[0:i+1]
self.cone_fusion.update(time_end)
def lidar_callback(self, msg):
msgl = len(msg.lidar_points)
if msgl != 0:
for i in msg.lidar_points:
i.x += transf_x
i.y += transf_y
i.z += transf_z
time_stamp = msg.stamp
jj = len(self.image_raw) - 1
delta_t = 1.0
while jj >= 0:
if time_stamp > self.image_raw[jj].stamp:
delta_t = time_stamp - self.image_raw[jj].stamp
break
jj = jj - 1
del self.image_raw[
0:
jj] #the jjth item is not deleted, such that next time we can find at least one ealier time stamp
length_msg = len(msg.lidar_points)
for ii in range(0, length_msg):
self.fusion(msg.lidar_points[ii], delta_t)
kk = len(self.lidar_raw) #originally: kk = len(self.lidar_raw) - 1
length_prev = len(self.previous_points)
for jj in range(0, kk):
if self.lidar_raw[jj].color == "unknown":
for ll in range(0, length_prev):
if Euc_dist(
np.array([[self.lidar_raw[jj].x],
[self.lidar_raw[jj].y]]),
np.array([[self.previous_points[ll].x],
[self.previous_points[ll].y]
])) < Dist_move:
print "change current"
print self.lidar_raw[jj]
print "to"
print self.previous_points[ll]
self.lidar_raw[jj].color = self.previous_points[
ll].color
break
self.previous_points = []
for ii in range(0, kk):
self.previous_points.append(self.lidar_raw[ii])
if self.lidar_raw[ii].color == 'y':
self.stop = 1
cones_whole = cone_pos_whole(self.lidar_raw, time_stamp)
self.fusion_pub.publish(cones_whole)
print(cones_whole)
target_point = cone_pos()
target = get_target.get_target(self.lidar_raw)
print "target"
print target
if target[0] != -1:
self.use_previous = 0
self.previous_target = target
else:
self.use_previous = self.use_previous + 1
target = self.previous_target
target_point.x = target[0]
target_point.y = target[1]
target_point.color = "unknown"
self.target_pub.publish(target_point)
self.driving(target)
del self.lidar_raw[0:kk]
#!/usr/bin/env python
# -*- coding: UTF-8 -*-
import rospy
import lidar2img
import numpy as np
from numpy import linalg as la
from fusion.msg import lidar3_single
from fusion.msg import lidar3_whole
from fusion.msg import imagel_single
from fusion.msg import imagel_whole
from fusion.msg import cone_pos
from fusion.msg import cone_pos_whole
from fusion.msg import Srt_Control
import get_target
import math
if __name__ == "__main__":
cone1 = cone_pos()
cone2 = cone_pos()
cone1.x = 2.9945142746
cone1.y = -0.670192182064
cone1.color = 'r'
cone2.x = 3.68553667068
cone2.y = -2.91794991493
cone2.color = 'r'
cones = [cone1, cone2]
target = get_target.get_target(cones)
print target