class Laser2PC():
def __init__(self):
self.laserProj = LaserProjection()
self.pcPub = rospy.Publisher("/prometheus/sensors/pcl2",
pc2,
queue_size=1)
self.laserSub = rospy.Subscriber("/prometheus/sensors/2Dlidar_scan",
LaserScan, self.laserCallback)
def laserCallback(self, data):
# cc = LaserScan()
# cc.header = data.header
# cc.angle_max = data.angle_max
# cc.angle_min = data.angle_min
# cc.angle_increment = data.angle_increment
# cc.time_increment = data.time_increment
# cc.scan_time = data.scan_time
# cc.range_max = data.range_max
# cc.range_min = data.range_min
# cc.ranges = data.ranges
# cc.intensities = data.intensities
# a = len(data.ranges)
# for i in range(0,a):
# if data.ranges[i] != float("inf"):
# if data.ranges[i] < 0.4:
# cc.ranges[i] = 0.41
cloud_out = self.laserProj.projectLaser(data)
self.pcPub.publish(cloud_out)
class Laser2PC():
def __init__(self):
self.laserProj = LaserProjection()
self.pcPub = rospy.Publisher("/laserPointCloud",
PointCloud2,
queue_size=1)
self.laserSub = rospy.Subscriber("/scan",
LaserScan,
self.laserCallback,
queue_size=1)
def laserCallback(self, msg):
pc2_msg = self.laserProj.projectLaser(msg)
self.pcPub.publish(pc2_msg)
# Convert it to a generator of the individual points
point_generator = pc2.read_points(pc2_msg)
# Access a generator in a loop
sum_points = 0.0
num = 0
for point in point_generator:
if not np.isnan(point[2]):
sum_points += point[2]
num += 1
# Calculate the average z value for example
print("Average height: " + str(sum_points / num))
class Laser2PC():
#translate laser to pc and publish point cloud to /cloud_in
def __init__(self):
self._measurements = [float('inf')]
self.laserProj = LaserProjection()
self.pcPub = rospy.Publisher('/point_cloud2', pc2, queue_size=5)
self.laserSub = rospy.Subscriber('/scan', LaserScan,
self.laserCallBack)
def laserCallBack(self, data):
# rospy.loginfo(rospy.get_caller_id() + 'I heard %s', data)
cloud_out = self.laserProj.projectLaser(data)
cloud_out.header.seq = data.header.frame_id
cloud_out.header.stamp = data.header.stamp
cloud_out.header.frame_id = 'laser0_frame'
self._measurements = cloud_out
self.pcPub.publish(cloud_out)
@property
def ready(self):
return not np.isnan(self._measurements)
@property
def measurements(self):
return self._measurements
class Laser2PC():
def __init__(self):
self.laserProj = LaserProjection()
self.pcPub = rospy.Publisher("/laserPointCLoud",pc2, queue_size=1)
self.laserSub = rospy.Subscriber("scan",LaserScan,self.laserCallback)
def laserCallback(self,data):
cloud_out = self.laserProj.projectLaser(data)
self.pcPub.publish(cloud_out)
class LaserScanToPointCloud:
def __init__(self):
self.laserProj = LaserProjection()
self.pointCloudPublisher = rospy.Publisher("/points", PointCloud2, queue_size = 1)
self.laserScanSubscriber = rospy.Subscriber("/scan", LaserScan, self.laserScanCallback)
def laserScanCallback(self, data):
self.pointCloudPublisher.publish(self.laserProj.projectLaser(data))
class Laser2PC():
def __init__(self):
self.laserProj= LaserProjection()
self.pcPub = rospy.Publisher('Pointy_Cloud', PointCloud2, queue_size=10)
self.laserSub = rospy.Subscriber("lidar", LaserScan, self.laserCallback)
def laserCallback(self,msg):
point_cloud = self.laserProj.projectLaser(msg)
self.pcPub.publish(point_cloud)
class Laser2pc(object):
def __init__(self,robotname):
self.robotname= robotname
self.laserProj=LaserProjection()
self.pcPub= rospy.Publisher("/{}/LaserPointCloud".format(self.robotname),pc2, queue_size=1)
self.lsersub=rospy.Subscriber("/{}/laser_scan".format(self.robotname),LaserScan,self.laserCallback)
def laserCallback(self,data):
cloud_out=self.laserProj.projectLaser(data)
self.pcPub.publish(cloud_out)
def readscan(self):
laser_projector = LaserProjection()
for topic, msg, time_stamp in self.bag.read_messages(
topics=[self.scan_topic]):
cloud = laser_projector.projectLaser(msg)
frame_points = np.zeros([0, 2])
for p in pc2.read_points(cloud,
field_names=("x", "y", "z"),
skip_nans=True):
p2d = np.array([p[0], p[1]])
frame_points = np.vstack([frame_points, p2d])
self.points.append([time_stamp, frame_points])
class Laser2PC():
def __init__(self):
self.laserProj = LaserProjection()
self.pcPub = rospy.Publisher("/prometheus/sensors/pcl2",
pc2,
queue_size=1)
self.laserSub = rospy.Subscriber("/prometheus/sensors/2Dlidar_scan",
LaserScan, self.laserCallback)
def laserCallback(self, data):
cloud_out = self.laserProj.projectLaser(data)
self.pcPub.publish(cloud_out)
class LaserTurn:
def __init__(self,state):
rospy.loginfo("Scan done")
self.state = state
self.done = False
self.laser_msg = rospy.Subscriber("/scan", LaserScan, self.update_laser)
self.lp = LaserProjection()
pc = self.lp.projectLaser(scan)
pc.rangemin = targetdistance
def act(self):
spo = " "
class laser2pc():
def __init__(self):
self.laserProj = LaserProjection()
self.pcPub = rospy.Publisher("/laser_pointcloud", pc2, queue_size=1)
self.lasersub = rospy.Subscriber("/scan", LaserScan,
self.laserCallback)
def laserCallback(self, data):
cloud_out = self.laserProj.projectLaser(data)
#convert from point cloud 2 to PCL
#pc = ros_numpy.numpify(cloud_out)
#points=np.zeros((pc.shape[0],3))
#points[:,0]=pc['x']
#points[:,1]=pc['y']
#points[:,2]=pc['z']
#p = pcl.PointCloud(np.array(points, dtype=np.float32))
self.pcPub.publish(cloud_out)
class Laser2Cloud:
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 laser_callback(self, data):
cloud = self.projector.projectLaser(data)
self.publisher.publish(cloud)
def data_preprocess(self, msg):
laser_projector = LaserProjection()
cloud = laser_projector.projectLaser(msg.scan)
pts = list(
pc2.read_points(cloud, field_names=("x", "y", "z"),
skip_nans=True))
scan = np.array(pts)[:, :2]
qx = msg.odom.pose.pose.orientation.x
qy = msg.odom.pose.pose.orientation.y
qz = msg.odom.pose.pose.orientation.z
qw = msg.odom.pose.pose.orientation.w
odom = tf.transformations.quaternion_matrix((qx, qy, qz, qw))
odom[0, 3] = msg.odom.pose.pose.position.x
odom[1, 3] = msg.odom.pose.pose.position.y
odom[2, 3] = msg.odom.pose.pose.position.z
return scan, odom
def readscan(self):
laser_projector = LaserProjection()
for topic, msg, time_stamp in self.bag.read_messages(
topics=[self.scan_topic]):
cloud = laser_projector.projectLaser(msg)
frame_points = np.zeros([0, 2])
for p in pc2.read_points(cloud,
field_names=("x", "y", "z"),
skip_nans=True):
#theta = math.atan2(p[0] , p[1])
#laser_range = np.pi
#if theta > np.pi/2 + np.pi/3 + laser_range/2 or theta < np.pi/2 + np.pi/3 - laser_range/2:
# continue
#d = math.sqrt(p[0]*p[0] + p[1]*p[1])
#if d > 1.5 and theta > np.pi/2:
# continue
p2d = np.array([p[0], p[1]])
frame_points = np.vstack([frame_points, p2d])
self.points.append([time_stamp, frame_points])
class Lidar():
def __init__(self):
rospy.loginfo("Initializing Lidar class. Creating subscriber")
self.scan_sub = rospy.Subscriber('/base_scan', LaserScan, self.on_scan)
self.xyz_points = None
rospy.loginfo("Initializing Lidar class. Creating LaserProjection")
self.laser_projector = LaserProjection()
def on_scan(self, scan):
rospy.loginfo("Got scan, projecting")
cloud = self.laser_projector.projectLaser(scan)
gen = pc2.read_points(cloud, skip_nans=True, field_names=("x", "y", "z"))
self.xyz_points = np.array(list(gen))
rospy.sleep(1/Me.CMD_FREQ)
def show_point_plot(self):
x = self.xyz_points[:,0]
y = self.xyz_points[:,1]
plt.scatter(x, y)
plt.show()
class Laser2PC():
def __init__(self):
self.laserProj = LaserProjection()
self.pcPub = rospy.Publisher("/laserPointCloud", pc, queue_size=1)
self.laserSub = rospy.Subscriber("/scan",
LaserScan,
self.laserCallback,
queue_size=1)
def laserCallback(self, data):
#print(data)
cloud2_out = self.laserProj.projectLaser(data)
#print(cloud2_out)
transformed_cloud = pc()
transformed_cloud.header.seq = cloud2_out.header.seq
transformed_cloud.header.stamp = cloud2_out.header.stamp
transformed_cloud.header.frame_id = '/link_chassis'
points_from_cloud2 = point_cloud2.read_points(cloud2_out)
for p in points_from_cloud2:
laser_point_msg = PointStamped()
laser_point_msg.header.frame_id = '/laser_frame'
laser_point_msg.header.stamp = cloud2_out.header.stamp
laser_point_msg.header.seq = cloud2_out.header.seq
laser_point_msg.point = Point(p[0], p[1], p[2])
#print(laser_point_msg) #here it shows points
#print(point)
#print(type(laser_point_msg.point))
#listener = tf.TransformListener()
try:
transformedPoint = listener.transformPoint(
'/link_chassis', laser_point_msg)
#print(transformedPoint)
#print(transformed_cloud)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
transformed_cloud.points.append(transformedPoint.point)
#listener.waitForTransform('/link_chassis', '/laser_frame',)
#transformed_cloud.channels = cloud_out.channels
#print(transformed_cloud)
self.pcPub.publish(transformed_cloud)
class Laser2PC():
def __init__(self):
self.laserProj = LaserProjection()
# self.inMsg = rospy.get_param('~inMsg')
# self.outMsg = rospy.get_param('~outMsg')
self.inMsg = '/scan'
self.outMsg = '/converted_pc'
self.pcPub = rospy.Publisher(self.outMsg, pc2, queue_size=1)
self.laserSub = rospy.Subscriber(self.inMsg, LaserScan,
self.laserCallback)
self.count = 0
def laserCallback(self, data):
cloud_out = self.laserProj.projectLaser(data)
cloud_out.header = data.header
cloud_out.header.frame_id = self.outMsg
self.pcPub.publish(cloud_out)
self.count += 1
print("Publish complete!" + str(self.count))
class tb3_mapping_node():
def __init__(self):
# Variables
self.laserProj = LaserProjection()
# Publishers
self.pcWorldPub = rospy.Publisher("/world_pc", pc2, queue_size=1)
#self.pcWaterPub = rospy.Publisher("/water_pc", pc2, queue_size=1)
self.mkArrayPub = rospy.Publisher("/marker_array_water_recoloured",
MarkerArray,
queue_size=1)
# Subscribers
rospy.Subscriber("/scan", LaserScan, self.callback_lidar_scan)
rospy.Subscriber("/marker_array_water", MarkerArray,
self.callback_recolour)
rospy.spin()
# Publish pc2 of world for the 1st Octomap server to use for mapping.
def callback_lidar_scan(self, data):
cloud_out = self.laserProj.projectLaser(data)
self.pcWorldPub.publish(cloud_out)
# Listen to markerarray from 2nd Octomap server and modify to distinguish water from the world.
def callback_recolour(self, data):
tempMarker = data.markers[
len(list(data.markers)) -
1] # Only final marker element contains the markers.
tempMarker.header.stamp = rospy.Time(
) # Do not set Time.now() here, it can cause issues for marker being accepted.
tempMarker.color.r = 0.0
tempMarker.color.g = 0.9167
tempMarker.color.b = 1.0
tempMarker.color.a = 1.0
tempMarker.scale.z = 0.051 # Slightly larger than map resolution to make water voxels cover world voxels of same location.
tempMarker.colors.append(tempMarker.color)
self.mkArrayPub.publish([tempMarker
]) # Publish the marker as a MarkerArray
class Laser2PC():
def __init__(self):
self.laserProj = LaserProjection()
self.pcPub = rospy.Publisher("/laserPCL", pc2, queue_size=1)
self.laserSub = rospy.Subscriber("/scan", LaserScan,
self.laserCallback)
def laserCallback(self, data):
cloud_out = self.laserProj.projectLaser(data)
Xg = pc2c.read_points(cloud_out,
skip_nans=True,
field_names=("x", "y", "z"))
cloud_points = np.empty((cloud_out.width, 2))
a = 0
for p in Xg:
#rospy.loginfo(p)
#rospy.loginfo(p[0])
cloud_points[a, 0] = p[0]
cloud_points[a, 1] = p[1]
#cloud_points[a, 2] = p[2]
#rospy.loginfo(cloud_points[a,:])
a = a + 1
#for d in cloud_out.data:
# print(d<<4)
#rospy.loginfo(cloud_points)
# Cluster PCL:
#scaler = StandardScaler()
#scaler.fit(cloud_points)
dbscan = DBSCAN()
clusters = dbscan.fit_predict(cloud_points)
#print("Cluster Membership:\n{}".format(clusters))
self.pcPub.publish(cloud_out)
rospy.loginfo("Start Clusters")
rospy.loginfo(clusters)
rospy.loginfo("Start Clusters")
class Laser2pc():
#lst = []
def __init__(self):
pass
def subscriber(self):
rospy.Subscriber("/laserscan", LaserScan, self.callbac)
rospy.spin()
def callbac(self, msg):
lst = []
self.converter = LaserProjection()
cloud = self.converter.projectLaser(msg)
self.pub = rospy.Publisher("/pc", pc2, queue_size=1)
self.pub.publish(cloud)
for p in pc22.read_points(cloud,
field_names=("x", "y", "z"),
skip_nans=True):
lst.append((p[0], p[1], p[2]))
print(lst)
print("")
print("-------------------")
print("")
def callback_scan(self, data):
try:
transform = self.tfBuffer.lookup_transform("odom", "base_link",
rospy.Time())
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
return
trans = transform.transform.translation
rot = transform.transform.rotation
eular = tf.transformations.euler_from_quaternion(
(rot.x, rot.y, rot.z, rot.w), "szxy")
quat = tf.transformations.quaternion_from_euler(eular[0], 0, 0, "szxy")
transform.transform.translation.z = 0
transform.transform.rotation = Quaternion(quat[0], quat[1], quat[2],
quat[3])
self.tf_robot_position.sendTransform(
(trans.x, trans.y, 0), (quat[0], quat[1], quat[2], quat[3]),
rospy.Time.now(), 'robot_position', 'odom')
scan = LaserScan()
scan = data
projector = LaserProjection()
cloud = projector.projectLaser(scan)
fixed_frame_cloud = do_transform_cloud(cloud, transform)
points = pc2.read_points(fixed_frame_cloud,
field_names=['x', 'y', 'z'],
skip_nans=True)
pole = [[0, 0, 0], [0, 0, 0], [0, 0, 0]]
data = []
for x, y, z in points:
if x > 5 and x < 7 and y > -2 and y < 2:
data.append([x, y, z, 0xff0000])
pole[0][0] += x
pole[0][1] += y
pole[0][2] += 1
elif x > 2 and x < 4 and y > -2 and y < 0:
data.append([x, y, z, 0x00ff00])
pole[1][0] += x
pole[1][1] += y
pole[1][2] += 1
elif x > 2 and x < 4 and y > 0 and y < 2:
data.append([x, y, z, 0x0000ff])
pole[2][0] += x
pole[2][1] += y
pole[2][2] += 1
else:
data.append([x, y, z, 0xffff00])
for p in pole:
if p[2] != 0:
p[0] /= p[2]
p[1] /= p[2]
if pole[0][2] < 5 or (pole[1][2] < 5 and pole[2][2] < 5):
return
trans_diff = [6.0 - pole[0][0], 0.0 - pole[0][1]]
if pole[1][2] != 0:
rot_diff = math.atan2(-1, -3) - math.atan2(pole[1][1] - pole[0][1],
pole[1][0] - pole[0][0])
elif pole[2][2] != 0:
rot_diff = math.atan2(1, -3) - math.atan2(pole[2][1] - pole[0][1],
pole[2][0] - pole[0][0])
# data_pole = []
for x, y, z, color in data:
tx = x + trans_diff[0] - 6.0
ty = y + trans_diff[1]
rx = tx * math.cos(rot_diff) - ty * math.sin(rot_diff) + 6.0
ry = tx * math.sin(rot_diff) + ty * math.cos(rot_diff)
if random.random() < 0.01:
self.data_pole.append([rx, ry, z, color])
# print data_pole
HEADER = Header(frame_id='/odom')
FIELDS = [
PointField(name='x',
offset=0,
datatype=PointField.FLOAT32,
count=1),
PointField(name='y',
offset=4,
datatype=PointField.FLOAT32,
count=1),
PointField(name='z',
offset=8,
datatype=PointField.FLOAT32,
count=1),
PointField(name='rgb',
offset=12,
datatype=PointField.UINT32,
count=1),
]
filterd_cloud = pc2.create_cloud(HEADER, FIELDS, self.data_pole)
self.pc_pub.publish(filterd_cloud)
def compare_val(arr_1,arr_2):
# arr_1 = array1[:]
# arr_2 = array2[:]
obs_list = [] # contains list of points which are same objects
for i in range(len(arr_1)-1):
empty_list = [] # make a circular array
#s = arr_1[i] - arr_2[i] # compare 2 - 1
# print(s)
increment = 0.400000095367
j = 0
# print("arr2",arr_2)
# print("arr1",arr_1)
if arr_1[i] == "nan" or arr_2[i] == "nan":
continue
# print(len(arr_1) == len(arr_2))
# print(str(arr_2[i]-arr_1[i+1])[2])
# print(str(arr_2[i+1]-arr_1[i])[2])
# laser1 = LaserScan()
laser1.header = "laser1"
# laser2 = LaserScan()
laser2.header = "laser2"
x1 = []
x2 = []
for l1 in range(len(arr_2)):
x1.append(0)
x2.append(0) #making range entries equal to 0
laser1.ranges = x1[:]
laser2.ranges = x1[:]
while (str(arr_2[i]-arr_1[i+1])[2]) == '3' and (str(arr_2[i+1]-arr_1[i])[2]) == '4':
# print("in loop")
# empty_list.append(arr_1[i]) #this just makes list
# empty_list.append(arr_2[i])
x1[i] = arr_1[i]
x2[i] = arr_2[i]
i = i + 1
if i-2 > range(len(arr_1)):
break
if arr_1[i] == "nan" or arr_2[i] == 'nan':
i = i + 1
# if len(empty_list) != 0:
# obs_list.append(empty_list)
laser1.ranges = x1[:]
laser2.ranges = x2[:]
# print(x1)
# print(x2)
# print(laser1)
# print(laser2)
laserproj1 = LaserProjection()
laserproj2 = LaserProjection()
cloud1 = laserproj1.projectLaser(laser1)
cloud2 = laserproj2.projectLaser(laser2)
# print(cloud1)
# print(cloud2)
xyz_array1 = list(point_cloud2.read_points(cloud1, skip_nans=True, field_names = ("x", "y", "z")))
xyz_array2 = list(point_cloud2.read_points(cloud2, skip_nans=True, field_names = ("x", "y", "z")))
if len(xyz_array1) != 0:
print("1 ")
print(xyz_array1)
##
# print("obs_list")
# print(obs_list)
rate = rospy.Rate(1000)
rate.sleep()
class baseScan:
def __init__(self, verbose=False):
self.rangeData = LaserScan()
self.scan_sub = rospy.Subscriber("/scan", LaserScan,
self.callback_laser)
rospy.sleep(1)
self.listener = tf.TransformListener()
self.laser_projector = LaserProjection()
self.pc = []
self.leg1 = []
self.leg2 = []
self.br = tf.TransformBroadcaster()
self.rate = rospy.Rate(10.0)
self.rate_measurements = rospy.Rate(0.5)
self.priorOri_in_base = []
self.priorLeft_in_base = []
self.priorRight_in_base = []
self.odomL = []
self.odomR = []
self.priorAvailable = False
self.newObsWeight = 0.2
self.offsetXY = [0.0, 0.0]
self.binOffset = 0.02
#self.pubShelfSep = rospy.Publisher('pubShelfSep', PoseStamped, queue_size=10)
self.reCalibrationCount = 4
self.tolerance = 0.1
self.updateRounds = 100
self.asyncRate = 20
self.limitInitX = True
self.xLimit = 0.1
self.rp = rospkg.RosPack()
self.writeFile = False
self.calculateTF = False
self.sendTF = False
self._ready = False
self.radius_th_leg = 0.1
self.n_samples = 5
self.sleep_between_scans = 2
while not rospy.is_shutdown():
try:
self._shelfHeight = rospy.get_param(
"/apc/shelf_calibration/height")
self._shelfRoll = rospy.get_param(
"/apc/shelf_calibration/roll")
self._shelfPitch = rospy.get_param(
"/apc/shelf_calibration/pitch")
break
except:
rospy.sleep(1.0)
continue
self.start_srv = rospy.Service(rospy.get_name() + '/calibrate', Empty,
self.start_srv_callback)
def start_srv_callback(self, req):
while not self._ready:
rospy.logwarn('Shelf published waiting for scan callback')
rospy.sleep(1.0)
rospy.loginfo('[shelf_publisher]: starting shelf publisher')
self.calculateTF = True
return EmptyResponse()
def callback_laser(self, data):
self._ready = True
self.rangeData = data
def getCloud(self):
cloud2 = self.laser_projector.projectLaser(self.rangeData)
xyz = pc2.read_points(cloud2,
skip_nans=True,
field_names=("x", "y", "z"))
self.pc = []
while not rospy.is_shutdown():
try:
self.pc.append(xyz.next())
except:
break
return self.pc
def findLegsOnce(self):
pc = self.getCloud()
x = []
y = []
for i in range(len(pc)):
x.append(pc[i][0])
y.append(pc[i][1])
radius = []
if self.limitInitX:
y = [y[i] for i in range(len(y)) if x[i] >= self.xLimit]
x = [x[i] for i in range(len(x)) if x[i] >= self.xLimit]
for i in range(len(x)):
radius.append(math.sqrt(x[i]**2 + y[i]**2))
n = radius.index(min(radius))
x2 = [
x[i] for i in range(len(x))
if math.sqrt((x[i] - x[n])**2 + (y[i] - y[n])**2) > 0.7
]
y2 = [
y[i] for i in range(len(y))
if math.sqrt((x[i] - x[n])**2 + (y[i] - y[n])**2) > 0.7
]
radius2 = []
for i in range(len(x2)):
radius2.append(math.sqrt(x2[i]**2 + y2[i]**2))
n2 = radius2.index(min(radius2))
leg1_p = [x[n], y[n]]
leg2_p = [x2[n2], y2[n2]]
x1_avg = [
x[i] for i in range(len(x))
if math.sqrt((x[i] - x[n])**2 +
(y[i] - y[n])**2) < self.radius_th_leg
]
y1_avg = [
y[i] for i in range(len(y))
if math.sqrt((x[i] - x[n])**2 +
(y[i] - y[n])**2) < self.radius_th_leg
]
x2_avg = [
x[i] for i in range(len(x))
if math.sqrt((x[i] - x[n2])**2 +
(y[i] - y[n2])**2) < self.radius_th_leg
]
y2_avg = [
y[i] for i in range(len(y))
if math.sqrt((x[i] - x[n2])**2 +
(y[i] - y[n2])**2) < self.radius_th_leg
]
leg1 = [
numpy.mean(x1_avg) + self.offsetXY[0],
numpy.mean(y1_avg) + self.offsetXY[1]
]
leg2 = [
numpy.mean(x2_avg) + self.offsetXY[0],
numpy.mean(y2_avg) + self.offsetXY[1]
]
#leg1 = [leg1[0] + self.offsetXY[0], leg1[1] + self.offsetXY[1]]
#leg2 = [leg2[0] + self.offsetXY[0], leg2[1] + self.offsetXY[1]]
return [leg1, leg2] # left, right
def findLegs(self):
leg1_x = numpy.zeros(self.n_samples)
leg1_y = numpy.zeros(self.n_samples)
leg2_x = numpy.zeros(self.n_samples)
leg2_y = numpy.zeros(self.n_samples)
for i in range(0, self.n_samples):
rospy.loginfo('taking laser scan: ')
rospy.loginfo(i)
legs_sample = self.findLegsOnce()
leg1_x[i] = legs_sample[0][0]
leg1_y[i] = legs_sample[0][1]
leg2_x[i] = legs_sample[1][0]
leg2_y[i] = legs_sample[1][1]
time.sleep(self.sleep_between_scans)
#self.rate_measurements.sleep()
leg1_m = [numpy.mean(leg1_x), numpy.mean(leg1_y)]
leg2_m = [numpy.mean(leg2_x), numpy.mean(leg2_y)]
rospy.loginfo('done scan')
rospy.loginfo(leg1_m)
rospy.loginfo(leg2_m)
legs = [leg1_m, leg2_m]
if legs[0][1] < legs[1][1]:
legs[0], legs[1] = legs[1], legs[0]
self.leg1 = legs[0]
self.leg2 = legs[1]
return [self.leg1, self.leg2] # left, right
def getShelfFrame(self):
legs = self.findLegs()
ori_x = (legs[0][0] + legs[1][0]) / 2.
ori_y = (legs[0][1] + legs[1][1]) / 2.
left_leg = legs[0]
if legs[0][1] < legs[1][1]:
left_leg = legs[1]
rotAngle = atan2(ori_x - left_leg[0], left_leg[1] - ori_y)
return [ori_x, ori_y], rotAngle, legs
def tf2PoseStamped(self, xy, ori):
shelfPoseMsg = PoseStamped()
shelfPoseMsg.pose.position.x = xy[0]
shelfPoseMsg.pose.position.y = xy[1]
shelfPoseMsg.pose.position.z = 0.0
shelfPoseMsg.pose.orientation.x = ori[0]
shelfPoseMsg.pose.orientation.y = ori[1]
shelfPoseMsg.pose.orientation.z = ori[2]
shelfPoseMsg.pose.orientation.w = ori[3]
shelfPoseMsg.header.frame_id = 'base'
shelfPoseMsg.header.stamp = rospy.Time.now()
return shelfPoseMsg
def estimateShelfFrame(self):
while not rospy.is_shutdown():
self.rate.sleep()
t_init = rospy.Time.now()
if self.calculateTF:
try:
shelfOri, shelfRot, legs = self.getShelfFrame()
except:
rospy.logerr("Shelf calibration is not getting points")
continue
self.calculateTF = False
self.sendTF = True
self.writeFile = True
if self.sendTF:
# F1 = kdl.Frame(kdl.Rotation.RPY(0, 0, shelfRot),
# kdl.Vector(shelfOri[0], shelfOri[0], self._shelfHeight))
# F2 = kdl.Frame(kdl.Rotation.RPY(self._shelfRoll, self._shelfPitch, 0), kdl.Vector(0,0,0))
# F3 = F1*F2
# position_xyz3 = F3.p
# orientation_xyzw3 = F3.M.GetQuaternion()
# self.br.sendTransform((position_xyz3[0], position_xyz3[1], position_xyz3[2]),
# orientation_xyzw3,
# rospy.Time.now(),
# "/shelf_front", # child
# "/base" # parent
# )
self.br.sendTransform(
(shelfOri[0], shelfOri[1], self._shelfHeight),
tf.transformations.quaternion_from_euler(
self._shelfRoll, self._shelfPitch, shelfRot),
rospy.Time.now(),
"/shelf_front", # child
"/base" # parent
)
self.br.sendTransform((legs[0][0], legs[0][1], self._shelfHeight), \
tf.transformations.quaternion_from_euler(self._shelfRoll, self._shelfPitch, shelfRot),
rospy.Time.now(),
"/left_leg", # child
"/base" # parent
)
self.br.sendTransform((legs[1][0], legs[1][1], self._shelfHeight), \
tf.transformations.quaternion_from_euler(self._shelfRoll, self._shelfPitch, shelfRot),
rospy.Time.now(),
"/right_leg", # child
"/base" # parent
)
#self.pubShelfSep.publish(self.tf2PoseStamped(shelfOri, tf.transformations.quaternion_from_euler(0, 0, shelfRot)))
if self.writeFile:
file_path = self.rp.get_path('calibration_data')
f = open(file_path + "/extrinsics/shelf_front.txt", 'w')
f.write(str(shelfOri[0]) + '\t')
f.write(str(shelfOri[1]) + '\t')
f.write(str(0.0) + '\t')
quaternion_shelf = tf.transformations.quaternion_from_euler(
0, 0, shelfRot)
f.write(str(quaternion_shelf[0]) + '\t')
f.write(str(quaternion_shelf[1]) + '\t')
f.write(str(quaternion_shelf[2]) + '\t')
f.write(str(quaternion_shelf[3]) + '\n')
f.close()
self.writeFile = False
class baseScan:
def __init__(self, verbose=False):
self.rangeData = LaserScan()
self.scan_sub = rospy.Subscriber("/base_scan", LaserScan, self.callback)
self.listener = tf.TransformListener()
self.laser_projector = LaserProjection()
self.pc = []
self.leg1 = []
self.leg2 = []
self.br = tf.TransformBroadcaster()
self.rate = rospy.Rate(20.0)
self.calibrated = False
self.reCalibration = False
self.priorOri_in_base_laser_link = [] # in base_laser_link frame
self.priorLeft_in_base_laser_link = [] # in base_laser_link frame
self.priorRight_in_base_laser_link = [] # in base_laser_link frame
self.odomL = [] # in odom_combined frame
self.odomR = [] # in odom_combined frame
self.priorAvailable = False
self.newObsWeight = 0.2
self.offsetXY = [-0.044, 0]
self.binOffset = 0.02
self.pubShelfSep = rospy.Publisher('pubShelfSep', PoseStamped)
self.reCalibrationCount = 4
self.tolerance = 0.1
self.updateRounds = 100
self.asyncRate = 20
self.limitInitX = True
self.xLimit = 0.1
self.emergencyThreshold = 30*20
self.emergencyTimeWindow = 60
while not rospy.is_shutdown():
try:
self.offsetXY = rospy.get_param('/base_scan/offset')
break
except:
rospy.loginfo('[shelf publisher]: waiting for base scan offset param')
rospy.sleep(random.uniform(0,1))
continue
self.start_srv = rospy.Service(rospy.get_name() + '/start', Empty, self.start_srv_callback)
self._start = False
# backup human supervised info for emergency
rp = rospkg.RosPack()
try:
dict_fp = rp.get_path('amazon_challenge_grasping')
except:
rospy.logerr('[shelf_publisher]: emergency file not found!')
sys.exit(1)
self.fileName = dict_fp + '/config/shelf_emergency.dat'
self.emergency = {}
def start_srv_callback(self, req):
rospy.loginfo('[shelf_publisher]: starting shelf publisher!')
if self._start:
rospy.logwarn('[shelf_publisher]: already started!!!!')
self._start = True
return EmptyResponse()
def raw_input_with_timeout(prompt, timeout=1.0):
print prompt,
timer = threading.Timer(timeout, thread.interrupt_main)
astring = None
try:
timer.start()
astring = raw_input(prompt)
except KeyboardInterrupt:
pass
timer.cancel()
return astring
def callback(self,data):
self.rangeData = data
def refreshRangeData(self):
self.rangeData = LaserScan() # flush
while len(self.rangeData.ranges) == 0 and not rospy.is_shutdown():
rospy.sleep(0.04)
def getStatus(self):
return self.calibrated
def getCloud(self):
# self.refreshRangeData()
cloud2 = self.laser_projector.projectLaser(self.rangeData)
xyz = pc2.read_points(cloud2, skip_nans=True, field_names=("x", "y", "z"))
self.pc = []
while not rospy.is_shutdown():
try:
self.pc.append(xyz.next())
except:
break
return self.pc
def findLegsOnce(self):
pc = self.getCloud()
x = []
y= []
for i in range(len(pc)):
x.append(pc[i][0])
y.append(pc[i][1])
radius = []
if self.calibrated or self.reCalibration: # use prior to find legs
for i in range(len(x)):
radius.append(math.sqrt((x[i]-self.priorLeft_in_base_laser_link[0])**2 + (y[i] - self.priorLeft_in_base_laser_link[1])**2))
n1 = radius.index(min(radius))
radius = []
for i in range(len(x)):
radius.append(math.sqrt((x[i]-self.priorRight_in_base_laser_link[0])**2 + (y[i] - self.priorRight_in_base_laser_link[1])**2))
n2 = radius.index(min(radius))
leg1 = [x[n1], y[n1]]
leg2 = [x[n2], y[n2]]
else:
# Assuming there is nothing between the robot and the shelf
if self.limitInitX:
y = [y[i] for i in range(len(y)) if x[i] >= self.xLimit]
x = [x[i] for i in range(len(x)) if x[i] >= self.xLimit]
for i in range(len(x)):
radius.append(math.sqrt(x[i]**2 + y[i]**2))
n = radius.index(min(radius))
x2 = [x[i] for i in range(len(x)) if math.sqrt( (x[i]-x[n])**2 + (y[i]-y[n])**2 ) > 0.7]
y2 = [y[i] for i in range(len(y)) if math.sqrt( (x[i]-x[n])**2 + (y[i]-y[n])**2 ) > 0.7]
radius2 = []
for i in range(len(x2)):
radius2.append(math.sqrt(x2[i]**2 + y2[i]**2))
n2 = radius2.index(min(radius2))
leg1 = [x[n], y[n]]
leg2 = [x2[n2], y2[n2]]
leg1 = [leg1[0] + self.offsetXY[0], leg1[1] + self.offsetXY[1]]
leg2 = [leg2[0] + self.offsetXY[0], leg2[1] + self.offsetXY[1]]
return [leg1, leg2] # left, right
def findLegs(self):
'''
accumulate new observations
'''
L1x = 0
L1y = 0
L2x = 0
L2y = 0
legs = self.findLegsOnce()
if legs[0][1] < legs[1][1]:
legs[0], legs[1] = legs[1], legs[0]
if self.calibrated:
self.leg1[0] = self.leg1[0] * (1-self.newObsWeight) + legs[0][0] * self.newObsWeight
self.leg1[1] = self.leg1[1] * (1-self.newObsWeight) + legs[0][1] * self.newObsWeight
self.leg2[0] = self.leg2[0] * (1-self.newObsWeight) + legs[1][0] * self.newObsWeight
self.leg2[1] = self.leg2[1] * (1-self.newObsWeight) + legs[1][1] * self.newObsWeight
else:
self.leg1 = legs[0]
self.leg2 = legs[1]
return [self.leg1, self.leg2] # left, right
def getShelfFrame(self):
# with respect to the frame of /base_scan
legs = self.findLegs()
ori_x = (legs[0][0] + legs[1][0]) / 2.
ori_y = (legs[0][1] + legs[1][1]) / 2.
left_leg = legs[0]
if legs[0][1] < legs[1][1]:
left_leg = legs[1]
rotAngle = atan2(ori_x - left_leg[0], left_leg[1] - ori_y)
return [ori_x, ori_y], rotAngle, legs
def tf2PoseStamped(self, xy, ori):
shelfPoseMsg = PoseStamped()
shelfPoseMsg.pose.position.x = xy[0]
shelfPoseMsg.pose.position.y = xy[1]
shelfPoseMsg.pose.position.z = 0.0
shelfPoseMsg.pose.orientation.x = ori[0]
shelfPoseMsg.pose.orientation.y = ori[1]
shelfPoseMsg.pose.orientation.z = ori[2]
shelfPoseMsg.pose.orientation.w = ori[3]
shelfPoseMsg.header.frame_id = 'base_laser_link'
shelfPoseMsg.header.stamp = rospy.Time.now()
return shelfPoseMsg
def saveEmergency(self):
with open(self.fileName, 'wb') as handle:
pickle.dump(self.emergency, handle)
def loadEmergency(self):
with open(self.fileName, 'rb') as handle:
self.emergency = pickle.load(handle)
def publish2TF(self):
answer = 'n'
ask = True
u = 0
shelfN = 0
recalibrateCount = 0
emergencyCount = 0
t_init = rospy.Time.now()
while not rospy.is_shutdown():
self.rate.sleep()
# check if human calibration is done
shelfOri, shelfRot, legs = self.getShelfFrame()
if self.reCalibration:
u = 0
if not self.calibrated:
self.br.sendTransform((shelfOri[0], shelfOri[1], 0),
tf.transformations.quaternion_from_euler(0, 0, shelfRot),
rospy.Time.now(),
"/shelf_frame", # child
"/base_laser_link" # parent
)
self.br.sendTransform((legs[0][0], legs[0][1], 0), \
tf.transformations.quaternion_from_euler(0, 0, shelfRot), \
rospy.Time.now(), \
"/left_leg", \
"/base_laser_link")
self.br.sendTransform((legs[1][0], legs[1][1], 0), \
tf.transformations.quaternion_from_euler(0, 0, shelfRot), \
rospy.Time.now(), \
"/right_leg", \
"/base_laser_link")
if self.priorAvailable:
while not rospy.is_shutdown():
try:
shelf_in_odom, shelf_rot_in_odom = self.listener.lookupTransform("/odom_combined", "/shelf_frame", rospy.Time(0))
self.priorLeft_in_base_laser_link, dummy = self.listener.lookupTransform("/base_laser_link", "/odomL", rospy.Time(0))
self.priorRight_in_base_laser_link, dummy = self.listener.lookupTransform("/base_laser_link", "/odomR", rospy.Time(0))
break
except:
continue
else:
try:
shelf_in_odom, shelf_rot_in_odom = self.listener.lookupTransform("/odom_combined", "/shelf_frame", rospy.Time(0))
except Exception, e:
# print e
continue
if self.reCalibration and math.sqrt((shelf_in_odom[0]-self.priorOri_in_odom[0]) **2 + (shelf_in_odom[1]-self.priorOri_in_odom[1]) **2) <= self.tolerance:
# rospy.sleep(2)
recalibrateCount += 1
if recalibrateCount == self.reCalibrationCount: # take recalibration only if it's stable
rospy.loginfo('reCalibrated!')
recalibrateCount = 0
u = 0
while not rospy.is_shutdown(): # make sure the odomL and odomR are updated
try:
######## self.priorOri_in_odom, self.priorRot_in_odom = self.listener.lookupTransform("/odom_combined", "/shelf_frame", rospy.Time(0))
######## self.odomL, self.odomL_rot = self.listener.lookupTransform("/odom_combined", "/left_leg", rospy.Time(0))
######## self.odomR, self.odomR_rot = self.listener.lookupTransform("/odom_combined", "/right_leg", rospy.Time(0))
self.calibrated = True
self.reCalibration = False
rospy.loginfo("Prior origin in odom_combined: X = %4f, Y = %4f" % (self.priorOri_in_odom[0], self.priorOri_in_odom[1]))
break
except:
continue
if not self.calibrated and ask:
emergencyCount += 1
if emergencyCount == self.emergencyThreshold:
self.loadEmergency()
self.odomL = self.emergency.odomL
self.odomL_rot = self.emergency.odomL_rot
self.odomR = self.emergency.odomR
self.odomR_rot = self.emergency.odomR_rot
emergency_timestamp = self.emergency.timestamp
if (rospy.Time.now() - emergency_timestamp).to_sec() > self.emergencyTimeWindow:
print ''
rospy.logwarn('***********************************************************************')
rospy.logwarn('[shelf_publisher] NOT ABLE TO RECOVER FROM CRASHING, GAME OVER')
rospy.logwarn('***********************************************************************')
continue
ask = False
self.calibrated = True
self.priorAvailable = True
self.priorOri_in_odom = shelf_in_odom
self.priorRot_in_odom = shelf_rot_in_odom
self.priorLeft_in_base_laser_link = legs[0]
self.priorRight_in_base_laser_link = legs[1]
print ''
rospy.logwarn('***********************************************************************')
rospy.logwarn('[shelf_publisher] EMERGENCY RECOVERY CALLED!!!!!!!!!!!!!!!!!!!!!!')
rospy.logwarn('***********************************************************************')
else:
sys.stdout.write("\r [ROS time: %s] Is the current shelf pose estimation good? (y/n)" % rospy.get_time() )
sys.stdout.flush()
if self._start:
answer = 'y'
if answer == 'y' or answer == 'yes':
self.calibrated = True
ask = False
self.priorAvailable = True
self.priorOri_in_odom = shelf_in_odom
self.priorRot_in_odom = shelf_rot_in_odom
self.priorLeft_in_base_laser_link = legs[0]
self.priorRight_in_base_laser_link = legs[1]
print ""
rospy.loginfo("Human calibration finished")
rospy.loginfo("Prior origin in /odom_combined: X = %4f, Y = %4f" % (self.priorOri_in_odom[0], self.priorOri_in_odom[1]))
while not rospy.is_shutdown():
try:
self.odomL, self.odomL_rot = self.listener.lookupTransform("/odom_combined", "/left_leg", rospy.Time(0))
self.odomR, self.odomR_rot = self.listener.lookupTransform("/odom_combined", "/right_leg", rospy.Time(0))
break
except:
pass
self.emergency = shelfInfo(self.odomL, self.odomL_rot, self.odomR, self.odomR_rot, rospy.Time.now())
self.saveEmergency()
else:
continue
# check in the odometry frame
if self.priorAvailable:
# print 'pub odom'
self.br.sendTransform(self.odomL, \
self.odomL_rot, \
rospy.Time.now(),\
"/odomL", \
"/odom_combined")
self.br.sendTransform(self.odomR, \
self.odomR_rot, \
rospy.Time.now(),\
"/odomR", \
"/odom_combined")
if self.calibrated:
u += 1
shelfN += 1
if math.sqrt((shelf_in_odom[0] - self.priorOri_in_odom[0]) **2 + (shelf_in_odom[1] - self.priorOri_in_odom[1]) **2) > self.tolerance:
rospy.logwarn('something is wrong with shelf pose estimation!!!!!!!!!! RECALIBRATING')
recalibrateCount = 0
u = 0
self.calibrated = False
self.reCalibration = True
continue
else:
self.br.sendTransform((shelfOri[0], shelfOri[1], 0),
tf.transformations.quaternion_from_euler(0, 0, shelfRot),
rospy.Time.now(),
"/shelf_frame", # child
"/base_laser_link" # parent
)
self.br.sendTransform((legs[0][0], legs[0][1], 0), \
tf.transformations.quaternion_from_euler(0, 0, shelfRot), \
rospy.Time.now(),\
"/left_leg", \
"/base_laser_link")
self.br.sendTransform((legs[1][0], legs[1][1], 0), \
tf.transformations.quaternion_from_euler(0, 0, shelfRot), \
rospy.Time.now(),\
"/right_leg", \
"/base_laser_link")
self.pubShelfSep.publish(self.tf2PoseStamped(shelfOri, tf.transformations.quaternion_from_euler(0, 0, shelfRot)))
if u == self.updateRounds:
while not rospy.is_shutdown(): # make sure the odomL and odomR are updated
try:
self.priorOri_in_odom, self.priorRot_in_odom = self.listener.lookupTransform("/odom_combined", "/shelf_frame", rospy.Time(0))
self.odomL, self.odomL_rot = self.listener.lookupTransform("/odom_combined", "/left_leg", rospy.Time(0))
self.odomR, self.odomR_rot = self.listener.lookupTransform("/odom_combined", "/right_leg", rospy.Time(0))
rospy.loginfo("Prior origin in /odom_combined: X = %4f, Y = %4f" % (self.priorOri_in_odom[0], self.priorOri_in_odom[1]))
u = 0
break
except:
continue
if (rospy.Time.now()-t_init).to_sec()>1.0:
t_init = rospy.Time.now()
self.emergency = shelfInfo(self.odomL, self.odomL_rot, self.odomR, self.odomR_rot, rospy.Time.now())
self.saveEmergency()
# print 'pub'
shelfN = 0
self.br.sendTransform((0, 0.1515 + self.binOffset, 1.21),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
"/shelf_bin_A", # child
"/shelf_frame" # parent
)
self.br.sendTransform((0, - 0.1515 + self.binOffset, 1.21),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
"/shelf_bin_B", # child
"/shelf_frame" # parent
)
self.br.sendTransform((0, - 0.4303 + self.binOffset, 1.21),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
"/shelf_bin_C", # child
"/shelf_frame" # parent
)
self.br.sendTransform((0, 0.1515 + self.binOffset, 1),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
"/shelf_bin_D", # child
"/shelf_frame" # parent
)
self.br.sendTransform((0, - 0.1515 + self.binOffset, 1),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
"/shelf_bin_E", # child
"/shelf_frame" # parent
)
self.br.sendTransform((0, - 0.4303 + self.binOffset, 1),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
"/shelf_bin_F", # child
"/shelf_frame" # parent
)
self.br.sendTransform((0, 0.1515 + self.binOffset, 0.78),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
"/shelf_bin_G", # child
"/shelf_frame" # parent
)
self.br.sendTransform((0, - 0.1515 + self.binOffset, 0.78),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
"/shelf_bin_H", # child
"/shelf_frame" # parent
)
self.br.sendTransform((0, - 0.4303 + self.binOffset, 0.78),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
"/shelf_bin_I", # child
"/shelf_frame" # parent
)
self.br.sendTransform((0, 0.1515 + self.binOffset, 0.51),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
"/shelf_bin_J", # child
"/shelf_frame" # parent
)
self.br.sendTransform((0, - 0.1515 + self.binOffset, 0.51),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
"/shelf_bin_K", # child
"/shelf_frame" # parent
)
self.br.sendTransform((0, - 0.4303 + self.binOffset, 0.51),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
"/shelf_bin_L", # child
"/shelf_frame" # parent
)
class Explore:
def __init__(self):
"""
Class constructor
"""
rospy.init_node('explore', anonymous=True)
self.laser = LaserScan()
self.laser_proj = LaserProjection()
self.pc_pub = rospy.Publisher('/laserPointCloud',
PointCloud2,
queue_size=1)
self.sacn_subscriber = rospy.Subscriber('/scan', LaserScan,
self.callback)
self.velocity_publisher = rospy.Publisher('/cmd_vel',
Twist,
queue_size=10)
self.front_thresh = 0.3
def callback(self, msg):
'inside callback'
ranges = list(msg.ranges)
cloud_out = self.laser_proj.projectLaser(msg)
# front_thresh = 0.27
side_thresh = 0.5
tolerance = 0.38
right_dist_average = 0
left_dist_average = 0
front_dist = 100
if len(ranges) != 0:
right_dist = ranges[45:134]
left_dist = ranges[225:314]
if ranges[0] < 100000:
front_dist_list = ranges[349:359]
front_dist_list.extend(ranges[0:10])
front_dist = sum(front_dist_list) / len(front_dist_list)
right_dist_average = sum(right_dist) / len(right_dist)
left_dist_average = sum(left_dist) / len(left_dist)
diff = abs(right_dist_average - left_dist_average)
right = False
left = False
if right_dist_average > left_dist_average and diff > tolerance:
left = True
if left_dist_average > right_dist_average and diff > tolerance:
right = True
if (right_dist_average > side_thresh) and (left_dist_average >
side_thresh):
left = True
right = True
#-------------------------------------------------------------------------------------------
if front_dist > self.front_thresh:
print 'drive straight', front_dist, self.front_thresh
if front_dist > 1:
self.front_thresh = 0.27
self.send_speed(1, 0)
else:
if left and (not right):
self.front_thresh = front_dist - 0.05
print 'turn left', front_dist, self.front_thresh
self.rotate(pi / 2, 1)
if right and (not left):
self.front_thresh = front_dist - 0.05
print 'turn right', front_dist, self.front_thresh
self.rotate(pi / 2, -1)
if (not right) and (not left):
self.front_thresh = front_dist - 0.05
print 'drive back', front_dist, self.front_thresh
self.rotate(pi, 1)
if left and right:
x = random.randint(0, 1)
if x == 0:
print 'turn left rand', front_dist, self.front_thresh
self.front_thresh = front_dist - 0.05
self.rotate(pi / 2, 1)
else:
print 'turn right rand', right_dist_average, left_dist_average, diff
self.front_thresh = front_dist - 0.05
self.rotate(pi / 2, -1)
def send_speed(self, linear_speed, angular_speed):
"""
Sends the speeds to the motors.
:param linear_speed [float] [m/s] The forward linear speed.
:param angular_speed [float] [rad/s] The angular speed for rotating around the body center.
"""
# REQUIRED CREDIT
# Make a new Twist message
# TODO
self.msg_cmd_vel = Twist()
#Linear velocity
self.msg_cmd_vel.linear.x = linear_speed
self.msg_cmd_vel.linear.y = 0.0
self.msg_cmd_vel.linear.z = 0.0
#Angular Velocity
self.msg_cmd_vel.angular.x = 0.0
self.msg_cmd_vel.angular.y = 0.0
if angular_speed < 0:
clockwise = True
else:
clockwise = False
if clockwise:
self.msg_cmd_vel.angular.z = -abs(angular_speed)
else:
self.msg_cmd_vel.angular.z = abs(angular_speed)
# Publish the message
# TODO
self.velocity_publisher.publish(self.msg_cmd_vel)
def rotate(self, angle, aspeed):
"""
Rotates the robot around the body center by the given angle.
:param angle [float] [rad] The distance to cover.
:param angular_speed [float] [rad/s] The angular speed.
"""
# REQUIRED CREDIT
init_time = rospy.Time.now().to_sec()
current_angle = 0
while current_angle < abs(angle):
self.send_speed(0, aspeed)
time = rospy.Time.now().to_sec()
if aspeed > 0:
current_angle = aspeed * (time - init_time)
else:
current_angle = -aspeed * (time - init_time)
self.msg_cmd_vel.angular.z = 0
self.velocity_publisher.publish(self.msg_cmd_vel)
# new_laser = LaserScan()
# self.callback(new_laser)
def smooth_drive(self, distance, linear_speed):
"""
Drives the robot in a straight line by changing the actual speed smoothly.
:param distance [float] [m] The distance to cover.
:param linear_speed [float] [m/s] The maximum forward linear speed.
"""
# rospy.loginfo("------- Smooth drive starting --------")
initial_pose = [self.odom_x, self.odom_y]
traveled = math.sqrt((initial_pose[0] - self.odom_x)**2 +
(initial_pose[1] - self.odom_y)**2)
tolerance = linear_speed / 50.0
coeff = 1
self.send_speed(0, 0)
while traveled <= distance - tolerance:
# accelerate linearly
while traveled < 0.2:
self.send_speed(traveled + .2, 0)
# decelerate with sin() curve
if distance - traveled < linear_speed + 3:
linear_speed = 1
coeff = math.sin(math.pi / 2 + (math.pi / 2 *
(traveled / distance)))
scaled_speed = linear_speed * coeff
if scaled_speed < 0.1:
scaled_speed = 0.1
self.send_speed(scaled_speed, 0)
rospy.sleep(.05)
traveled = math.sqrt((initial_pose[0] - self.odom_x)**2 +
(initial_pose[1] - self.odom_y)**2)
# rospy.loginfo("Scaled speed: " + str(scaled_speed))
# rospy.loginfo("Total distance traveled: " + str(traveled))
self.send_speed(0, 0)
# rospy.sleep(1)
stopped_at = traveled
traveled = math.sqrt((initial_pose[0] - self.odom_x)**2 +
(initial_pose[1] - self.odom_y)**2)
# rospy.loginfo("------- Smooth drive finished --------")
# rospy.loginfo("Desired distance: " + str(distance))
# rospy.loginfo("Total distance after stopping: " + str(traveled))
# rospy.loginfo("Additional distance after stopping: " + str(traveled - stopped_at))
rospy.loginfo("Distance error: " + str(traveled - distance))
def run(self):
self.callback(LaserScan())
rospy.spin()
class UWTeleop(object):
def __init__(self):
rospy.init_node("uw_teleop")
self.vel_pub1 = rospy.Publisher('/dataNavigator1',
Odometry,
queue_size=10)
self.vel_pub2 = rospy.Publisher('/dataNavigator2',
Odometry,
queue_size=10)
self.rate = rospy.Rate(20)
self.joy_sub = rospy.Subscriber("/joy", Joy, self.joy_msg_callback)
self.bf1_odom_sub = rospy.Subscriber("/uwsim/BlueFox1Odom", Odometry,
self.pose1_callback)
self.bf2_odom_sub = rospy.Subscriber("/uwsim/BlueFox2Odom", Odometry,
self.pose2_callback)
####### Make a lidar by stacking multibeam sensors ##################
self.bf1_laser_sub = rospy.Subscriber("/BlueFox1/multibeam", LaserScan,
self.laser_callback)
self.bf1_laser1_sub = rospy.Subscriber("/BlueFox1/multibeam1",
LaserScan, self.laser1_callback)
self.bf1_laser2_sub = rospy.Subscriber("/BlueFox1/multibeam2",
LaserScan, self.laser2_callback)
self.bf1_laser3_sub = rospy.Subscriber("/BlueFox1/multibeam3",
LaserScan, self.laser3_callback)
self.bf1_laser4_sub = rospy.Subscriber("/BlueFox1/multibeam4",
LaserScan, self.laser4_callback)
self.bf1_laser5_sub = rospy.Subscriber("/BlueFox1/multibeam5",
LaserScan, self.laser5_callback)
self.bf1_laser6_sub = rospy.Subscriber("/BlueFox1/multibeam6",
LaserScan, self.laser6_callback)
self.bf1_laser7_sub = rospy.Subscriber("/BlueFox1/multibeam7",
LaserScan, self.laser7_callback)
self.bf1_laser8_sub = rospy.Subscriber("/BlueFox1/multibeam8",
LaserScan, self.laser8_callback)
self.bf1_laser9_sub = rospy.Subscriber("/BlueFox1/multibeam9",
LaserScan, self.laser9_callback)
self.bf1_laser10_sub = rospy.Subscriber("/BlueFox1/multibeam10",
LaserScan,
self.laser10_callback)
self.bf1_laser11_sub = rospy.Subscriber("/BlueFox1/multibeam11",
LaserScan,
self.laser11_callback)
self.laser_data = {}
self.combined_laserscan = LaserScan()
#######################################################################
self.bf2_laser_sub = rospy.Subscriber("/BlueFox2/multibeam", LaserScan,
self.laser2_callback)
# messages required by /ndt_mapping
self.lidar_pc2_pub = rospy.Publisher('velodyne_points',
PointCloud2,
queue_size=10)
self.imu_pub = rospy.Publisher("/imu_raw", Imu, queue_size=10)
self.vehicle_odom_pub = rospy.Publisher('/odom_pose',
Odometry,
queue_size=10)
self.joy_data = Joy()
self.vel_cmd1 = Odometry()
self.vel_cmd2 = Odometry()
self.bf1_pose = None
self.bf2_pose = None
self.odom_hz = 10
self.bf1_vel = None
self.bf2_vel = None
self.bf1_laser = LaserScan()
self.bf2_laser = LaserScan()
self.joy_button = None
self.lidar_counter = 0 # this is a hack to make lidar spin
self.laser_projection = LaserProjection()
self.velodyne_mapped_laserscan = LaserScan()
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
def pose1_callback(self, data):
pose = data.pose.pose
self.bf1_pose = pose
self.bf1_vel = data.twist.twist
# publish a /odom_pose topic
odom = Odometry()
odom.pose = data.pose
odom.twist = self.vel_cmd1.twist
self.vehicle_odom_pub.publish(odom)
# Publish a simulated Imu topic
# imu = Imu()
# imu.orientation = data.pose.pose.orientation
def pose2_callback(self, data):
pose = data.pose.pose
self.bf2_pose = pose
self.bf2_vel = data.twist.twist
def laser2_callback(self, data):
self.bf2_laser = data
def get_relative_pose(self, source, target):
'''
returns a vector pointing from source to target
'''
source_pos = np.array(
[source.position.x, source.position.y, source.position.z])
target_pos = np.array(
[target.position.x, target.position.y, target.position.z])
source_ori = source.orientation
target_ori = target.orientation
source_quater = (source_ori.x, source_ori.y, source_ori.z,
source_ori.w)
T_source = tf.transformations.quaternion_matrix(source_quater)
T_source[:3, 3] = source_pos
target_quater = (target_ori.x, target_ori.y, target_ori.z,
target_ori.w)
T_target = tf.transformations.quaternion_matrix(target_quater)
T_target[:3, 3] = target_pos
T_rel = np.dot(np.linalg.inv(T_source), T_target)
return T_rel
def get_planar_pose_follow_cmd(self, source, target, desired_dist=3.0):
'''
source vehicle follows target vehicle keeping a distance of 3.0 meters and maintaining the elevation
'''
T_rel = self.get_relative_pose(source, target)
roll_rel, pitch_rel, yaw_rel = tf.transformations.euler_from_matrix(
T_rel, axes='sxyz')
rel_pos = T_rel[:3, 3]
current_dist = np.linalg.norm(rel_pos[:2])
unit_rel_pos_xy = rel_pos[:2] / np.linalg.norm(rel_pos[:2])
# q_rel = tf.transformations.quaternion_from_matrix(T_rel)
vel_cmd = Odometry()
p = 0.1
vel_cmd.twist.twist.linear.z = p * rel_pos[2]
if abs(np.linalg.norm(rel_pos[:2]) - desired_dist) < 0.01:
vel_cmd.twist.twist.linear.x = 0
vel_cmd.twist.twist.linear.y = 0
elif np.linalg.norm(rel_pos[:2]) > desired_dist:
vel_cmd.twist.twist.linear.x = p * unit_rel_pos_xy[0]
vel_cmd.twist.twist.linear.y = p * unit_rel_pos_xy[1]
else:
vel_cmd.twist.twist.linear.x = -p * unit_rel_pos_xy[0]
vel_cmd.twist.twist.linear.y = -p * unit_rel_pos_xy[1]
vel_cmd.twist.twist.angular.x = 2.0 * p * roll_rel
vel_cmd.twist.twist.angular.y = 2.0 * p * pitch_rel
vel_cmd.twist.twist.angular.z = 2.0 * p * yaw_rel
return vel_cmd
def get_planar_vel_follow_cmd(self, target_vel):
''' source vehicle copies target vehicle's velocity'''
vel_cmd = Odometry()
vel_cmd.twist.twist = target_vel
return vel_cmd
def get_obs_avoidance_cmd(self, source_id, scale=0.01):
if source_id == 1:
laser = self.bf1_laser
pose = self.bf1_pose
if source_id == 2:
laser = self.bf2_laser
pose = self.bf2_pose
angle = laser.angle_min
angle_inc = laser.angle_increment
for i, r in enumerate(laser.ranges):
r_vec = scale * np.array([np.cos(angle), np.sin(angle)]) / r
if i == 0:
vel_vec = r_vec
else:
vel_vec = np.vstack([vel_vec, r_vec])
vel_tot = np.sum(vel_vec, axis=0)
vel_cmd = Odometry()
vel_cmd.twist.twist.linear.x = -vel_tot[0]
vel_cmd.twist.twist.linear.y = -vel_tot[1]
return vel_cmd
def joy_msg_callback(self, data):
'''
joy_data.axes = [+left, +forward, +yaw, +up, +pitch, +roll]
joy_data.buttons = [take_pictures, ...]
'''
self.joy_data = data
if self.joy_data.buttons[1] == 0:
self.joy_button = 0
self.vel_cmd1.twist.twist.linear.x = self.joy_data.axes[1] * .125
self.vel_cmd1.twist.twist.linear.y = -self.joy_data.axes[0] * .125
self.vel_cmd1.twist.twist.linear.z = -self.joy_data.axes[3] * .125
self.vel_cmd1.twist.twist.angular.x = self.joy_data.axes[4] * .125
self.vel_cmd1.twist.twist.angular.y = self.joy_data.axes[5] * .125
self.vel_cmd1.twist.twist.angular.z = -self.joy_data.axes[2] * .125
if self.joy_data.buttons[1] == 1:
self.joy_button = 1
self.vel_cmd2.twist.twist.linear.x = self.joy_data.axes[1] * .125
self.vel_cmd2.twist.twist.linear.y = -self.joy_data.axes[0] * .125
self.vel_cmd2.twist.twist.linear.z = -self.joy_data.axes[3] * .125
self.vel_cmd2.twist.twist.angular.x = self.joy_data.axes[4] * .125
self.vel_cmd2.twist.twist.angular.y = self.joy_data.axes[5] * .125
self.vel_cmd2.twist.twist.angular.z = -self.joy_data.axes[2] * .125
def start_teleop(self):
while not rospy.is_shutdown():
if self.joy_button == 0: # user is controlling BF1, BF2 is following
follow_cmd = self.get_planar_pose_follow_cmd(
source=self.bf2_pose,
target=self.bf1_pose,
desired_dist=3.0)
obs_cmd = self.get_obs_avoidance_cmd(source_id=1, scale=0.01)
self.vel_cmd2 = follow_cmd
self.vel_cmd2.twist.twist.linear.x += obs_cmd.twist.twist.linear.x
self.vel_cmd2.twist.twist.linear.y += obs_cmd.twist.twist.linear.y
# self.vel_cmd2 = self.get_planar_vel_follow_cmd(target_vel=self.bf1_vel)
# if self.joy_button == 1: # user is controlling BF1, BF2 is following
# rel_pos = self.get_relative_pose(source=self.bf1_pose, target=self.bf2_pose)
# #print(rel_pos)
# source_pos = np.array([self.bf1_pose.position.x, self.bf1_pose.position.y, self.bf1_pose.position.z])
# target_pos = np.array([self.bf2_pose.position.x, self.bf2_pose.position.y, self.bf2_pose.position.z])
# self.vel_cmd1 = self.get_planar_pose_follow_cmd(source_pos=source_pos, target_pos=target_pos,rel_pos=rel_pos,desired_dist=3.0)
self.vel_pub1.publish(self.vel_cmd1)
self.vel_pub2.publish(self.vel_cmd2)
self.rate.sleep()
def lidar_callback(self, data):
T = self.tf_buffer.lookup_transform("lidar", "velodyne", rospy.Time())
data.intensities = [1] * len(data.ranges)
pc = self.laser_projection.projectLaser(
data,
channel_options=self.laser_projection.ChannelOption.INTENSITY)
pc_transformed = do_transform_cloud(pc, T)
pc_transformed.header.frame_id = "velodyne"
pc_transformed.is_dense = True
self.lidar_pc2_pub.publish(pc_transformed)
def laser1_callback(self, data):
self.bf1_laser = data
class Laser2PC():
def __init__(self):
self.laserProj = LaserProjection()
#self.pub = rospy.Publisher("/own/true/sonar_PC2",PointCloud2, queue_size=1)
#self.pub1 = rospy.Publisher("/own/simulated/sonar_PC2",PointCloud2, queue_size=1)
self.pub2 = rospy.Publisher("/sonar/PC2", PointCloud2, queue_size=1)
#self.sub = rospy.Subscriber("/desistek_saga/sonar",LaserScan, self.callback)
#self.sub1 = rospy.Subscriber("/own/simulated/sonar_LS",LaserScan, self.callback1)
self.sub2 = rospy.Subscriber("/sonar/LS", LaserScan, self.callback2)
"""
def callback(self,data):
laser = LaserScan()
laser.header = data.header
laser.angle_min = data.angle_min
laser.angle_max = data.angle_max
laser.angle_increment = data.angle_increment
laser.time_increment = data.time_increment
laser.scan_time = data.scan_time
laser.range_min = data.range_min
laser.range_max = data.range_max
laser.intensities = data.intensities
laser.ranges = data.ranges
cloud = self.laserProj.projectLaser(laser)
self.pub.publish(cloud)
def callback1(self,arg):
laser = LaserScan()
laser.header = arg.header
laser.angle_min = arg.angle_min
laser.angle_max = arg.angle_max
laser.angle_increment = arg.angle_increment
laser.time_increment = arg.time_increment
laser.scan_time = arg.scan_time
laser.range_min = arg.range_min
laser.range_max = arg.range_max
laser.intensities = arg.intensities
laser.ranges = arg.ranges
cloud = self.laserProj.projectLaser(laser)
self.pub1.publish(cloud)
"""
def callback2(self, arg):
laser = LaserScan()
laser.header = arg.header
laser.angle_min = arg.angle_min
laser.angle_max = arg.angle_max
laser.angle_increment = arg.angle_increment
laser.time_increment = arg.time_increment
laser.scan_time = arg.scan_time
laser.range_min = arg.range_min
laser.range_max = arg.range_max
laser.intensities = arg.intensities
laser.ranges = arg.ranges
cloud = self.laserProj.projectLaser(laser)
self.pub2.publish(cloud)
class Localization_quality_estimation():
def __init__(self):
rospy.init_node('map_to_pointcloud')
self.br = tf.TransformBroadcaster()
self.transform = TransformStamped()
self.laser_footprint_transform = TransformStamped()
self.tf_listener = tf.TransformListener()
self.ts_old = rospy.Time.now()
self.index_old = 0
self.tf_listener.waitForTransform("/pr1/base_footprint", "pr1/left_laser_link", rospy.Time(0), rospy.Duration(1))
(self.trans, self.rot) = self.tf_listener.lookupTransform("/pr1/base_footprint", "pr1/left_laser_link",rospy.Time(0))
self.laser_footprint_transform.transform.translation.x = self.trans[0]
self.laser_footprint_transform.transform.translation.y = self.trans[1]
self.laser_footprint_transform.transform.rotation.x = self.rot[0]
self.laser_footprint_transform.transform.rotation.y = self.rot[1]
self.laser_footprint_transform.transform.rotation.z = self.rot[2]
self.laser_footprint_transform.transform.rotation.w = self.rot[3]
#rospy.Subscriber("/pr1/amcl_pose", PoseWithCovarianceStamped, self.robot_pose_cb)
rospy.Subscriber("map", OccupancyGrid, self.map_cb)
rospy.sleep(1)
self.laser_projection = LaserProjection()
rospy.Subscriber("/pr1/l_scan", LaserScan, self.laser_cb)
self.pc2_pub = rospy.Publisher("pointcloud", pc2, queue_size=1)
rospy.spin()
def map_cb(self, msg = OccupancyGrid()):
map_metadata = msg.info
map_data = msg.data
map_width = map_metadata.width
map_resolution = map_metadata.resolution
map_origin = map_metadata.origin
self.obstacles_x = []
self.obstacles_y = []
for i in range(0,len(map_data)):
if map_data[i] == 100 and self.index_old != i-1:
x = (i % map_width) * map_resolution + map_origin.position.x
y = (i // map_width) * map_resolution + map_origin.position.y
self.obstacles_x.append(x)
self.obstacles_y.append(y)
self.index_old = i
# br = tf.TransformBroadcaster()
# br2= tf2_ros.TransformBroadcaster()
# t = TransformStamped()
# rospy.sleep(5)
# for i in range(0,len(self.obstacles_x),10):
# #print("index: ", i)
# status = br.sendTransform((self.obstacles_x[i], self.obstacles_y[i], 0),
# tf.transformations.quaternion_from_euler(0, 0, 0),
# rospy.Time.now(),
# "obstacle_" + str(i),
# "map")
# t.header.stamp = rospy.Time.now()
# t.header.frame_id = "map"
# t.child_frame_id = "obstacle_" + str(i)
# t.transform.translation.x = self.obstacles_x[0]
# t.transform.translation.y = self.obstacles_y[0]
# status2 = br2.sendTransform(t)
# print(status, status2)
def laser_cb(self, msg = LaserScan()):
now = rospy.Time.now()
if now - self.ts_old > rospy.Duration(0.05):
cloud_out = self.laser_projection.projectLaser(msg)
#print(rospy.Time.now())
(trans, rot) = self.tf_listener.lookupTransform("/map", "pr1/left_laser_link",rospy.Time(0))
self.transform.transform.translation.x = trans[0]
self.transform.transform.translation.y = trans[1]
self.transform.transform.rotation.x = rot[0]
self.transform.transform.rotation.y = rot[1]
self.transform.transform.rotation.z = rot[2]
self.transform.transform.rotation.w = rot[3]
cloud_transformed = do_transform_cloud(cloud_out, self.transform)
#self.pc2_pub.publish(cloud_transformed)
pc_np_transformed = ros_numpy.point_cloud2.pointcloud2_to_xyz_array(cloud_transformed, remove_nans=True)
# for i in range(0,len(pc_np_transformed),15):
# #print("index: ", i)
# status = self.br.sendTransform((pc_np_transformed[i][0], pc_np_transformed[i][1], 0),
# tf.transformations.quaternion_from_euler(0, 0, 0),
# rospy.Time.now(),
# "scan" + str(i),
# "map")
matches = 0
for i in range(0,len(pc_np_transformed),5):
for j in range(0,len(self.obstacles_x),6):
if (abs(pc_np_transformed[i][0] - self.obstacles_x[j]) < 0.15) and (abs(pc_np_transformed[i][1] - self.obstacles_y[j]) < 0.15):
matches += 1
# self.br.sendTransform((pc_np_transformed[i][0], pc_np_transformed[i][1], 0),
# tf.transformations.quaternion_from_euler(0, 0, 0),
# rospy.Time.now(),
# "match_" + str(i),
# "map")
break
print(matches)
#print(self.obstacles_x, self.obstacles_y)
now2 = rospy.Time.now()
print("rechenzeit: ", (now2 - now)* 0.000001)
self.ts_old = now
class baseScan:
def __init__(self, verbose=False):
self.rangeData = LaserScan()
self.scan_sub = rospy.Subscriber("/scan", LaserScan, self.callback_laser)
rospy.sleep(1)
self.listener = tf.TransformListener()
self.laser_projector = LaserProjection()
self.pc = []
self.leg1 = []
self.leg2 = []
self.br = tf.TransformBroadcaster()
self.rate = rospy.Rate(10.0)
self.rate_measurements = rospy.Rate(0.5)
self.priorOri_in_base = []
self.priorLeft_in_base = []
self.priorRight_in_base = []
self.odomL = []
self.odomR = []
self.priorAvailable = False
self.newObsWeight = 0.2
self.offsetXY = [0.0, 0.0]
self.binOffset = 0.02
#self.pubShelfSep = rospy.Publisher('pubShelfSep', PoseStamped, queue_size=10)
self.reCalibrationCount = 4
self.tolerance = 0.1
self.updateRounds = 100
self.asyncRate = 20
self.limitInitX = True
self.xLimit = 0.0
self.rp = rospkg.RosPack()
self.writeFile = False
self.calculateTF = False
self.sendTF = False
self._ready = False
self.radius_th_leg = 0.1
self.n_samples = 5
self.sleep_between_scans = 2
while not rospy.is_shutdown():
try:
self._shelfHeight = rospy.get_param("/apc/shelf_calibration/height")
self._shelfRoll = rospy.get_param("/apc/shelf_calibration/roll")
self._shelfPitch = rospy.get_param("/apc/shelf_calibration/pitch")
break
except:
rospy.sleep(1.0)
continue
self.start_srv = rospy.Service(rospy.get_name() + '/calibrate', Empty, self.start_srv_callback)
def start_srv_callback(self, req):
while not self._ready:
rospy.logwarn('Shelf published waiting for scan callback')
rospy.sleep(1.0)
rospy.loginfo('[shelf_publisher]: starting shelf publisher')
self.calculateTF = True
return EmptyResponse()
def callback_laser(self, data):
self._ready = True
self.rangeData = data
def getCloud(self):
cloud2 = self.laser_projector.projectLaser(self.rangeData)
xyz = pc2.read_points(cloud2, skip_nans=True, field_names=("x", "y", "z"))
self.pc = []
while not rospy.is_shutdown():
try:
self.pc.append(xyz.next())
except:
break
return self.pc
def findLegsOnce(self):
pc = self.getCloud()
x = []
y = []
for i in range(len(pc)):
x.append(pc[i][0])
y.append(pc[i][1])
radius = []
if self.limitInitX:
y = [y[i] for i in range(len(y)) if x[i] >= self.xLimit]
x = [x[i] for i in range(len(x)) if x[i] >= self.xLimit]
for i in range(len(x)):
radius.append(math.sqrt(x[i]**2 + y[i]**2))
n = radius.index(min(radius))
x1 = [x[i] for i in range(len(x)) if y[i] > 0.0]
y1 = [y[i] for i in range(len(y)) if y[i] > 0.0]
radius2 = []
x2 = [x[i] for i in range(len(x)) if y[i] < 0.0]
y2 = [y[i] for i in range(len(y)) if y[i] < 0.0]
for i in range(len(x2)):
radius2.append(math.sqrt(x2[i]**2 + y2[i]**2))
n2 = radius2.index(min(radius2))
leg1_p = [x[n], y[n]]
leg2_p = [x2[n2], y2[n2]]
x1_avg = [x[i] for i in range(len(x)) if math.sqrt( (x[i]-x[n])**2 + (y[i]-y[n])**2 ) < self.radius_th_leg]
y1_avg = [y[i] for i in range(len(y)) if math.sqrt( (x[i]-x[n])**2 + (y[i]-y[n])**2 ) < self.radius_th_leg]
x2_avg = [x[i] for i in range(len(x)) if math.sqrt( (x[i]-x[n2])**2 + (y[i]-y[n2])**2 ) < self.radius_th_leg]
y2_avg = [y[i] for i in range(len(y)) if math.sqrt( (x[i]-x[n2])**2 + (y[i]-y[n2])**2 ) < self.radius_th_leg]
leg1 = [numpy.mean(x1) + self.offsetXY[0], numpy.mean(y1) + self.offsetXY[1]]
leg2 = [numpy.mean(x2) + self.offsetXY[0], numpy.mean(y2) + self.offsetXY[1]]
#leg1 = [leg1[0] + self.offsetXY[0], leg1[1] + self.offsetXY[1]]
#leg2 = [leg2[0] + self.offsetXY[0], leg2[1] + self.offsetXY[1]]
return [leg1, leg2] # left, right
def findLegs(self):
leg1_x = numpy.zeros(self.n_samples)
leg1_y = numpy.zeros(self.n_samples)
leg2_x = numpy.zeros(self.n_samples)
leg2_y = numpy.zeros(self.n_samples)
for i in range(0, self.n_samples):
rospy.loginfo('taking laser scan: ')
rospy.loginfo(i)
legs_sample = self.findLegsOnce()
leg1_x[i] = legs_sample[0][0]
leg1_y[i] = legs_sample[0][1]
leg2_x[i] = legs_sample[1][0]
leg2_y[i] = legs_sample[1][1]
time.sleep(self.sleep_between_scans)
#self.rate_measurements.sleep()
leg1_m = [numpy.mean(leg1_x), numpy.mean(leg1_y)]
leg2_m = [numpy.mean(leg2_x), numpy.mean(leg2_y)]
rospy.loginfo('done scan')
rospy.loginfo(leg1_m)
rospy.loginfo(leg2_m)
legs = [leg1_m, leg2_m]
if legs[0][1] < legs[1][1]:
legs[0], legs[1] = legs[1], legs[0]
self.leg1 = legs[0]
self.leg2 = legs[1]
return [self.leg1, self.leg2] # left, right
def getShelfFrame(self):
legs = self.findLegs()
ori_x = (legs[0][0] + legs[1][0]) / 2.
ori_y = (legs[0][1] + legs[1][1]) / 2.
left_leg = legs[0]
if legs[0][1] < legs[1][1]:
left_leg = legs[1]
rotAngle = atan2(ori_x - left_leg[0], left_leg[1] - ori_y)
return [ori_x, ori_y], rotAngle, legs
def tf2PoseStamped(self, xy, ori):
shelfPoseMsg = PoseStamped()
shelfPoseMsg.pose.position.x = xy[0]
shelfPoseMsg.pose.position.y = xy[1]
shelfPoseMsg.pose.position.z = 0.0
shelfPoseMsg.pose.orientation.x = ori[0]
shelfPoseMsg.pose.orientation.y = ori[1]
shelfPoseMsg.pose.orientation.z = ori[2]
shelfPoseMsg.pose.orientation.w = ori[3]
shelfPoseMsg.header.frame_id = 'base'
shelfPoseMsg.header.stamp = rospy.Time.now()
return shelfPoseMsg
def estimateShelfFrame(self):
while not rospy.is_shutdown():
self.rate.sleep()
t_init = rospy.Time.now()
if self.calculateTF:
try:
shelfOri, shelfRot, legs = self.getShelfFrame()
except:
rospy.logerr("Shelf calibration is not getting points")
continue
self.calculateTF = False
self.sendTF = True
self.writeFile = True
if self.sendTF:
# F1 = kdl.Frame(kdl.Rotation.RPY(0, 0, shelfRot),
# kdl.Vector(shelfOri[0], shelfOri[0], self._shelfHeight))
# F2 = kdl.Frame(kdl.Rotation.RPY(self._shelfRoll, self._shelfPitch, 0), kdl.Vector(0,0,0))
# F3 = F1*F2
# position_xyz3 = F3.p
# orientation_xyzw3 = F3.M.GetQuaternion()
# self.br.sendTransform((position_xyz3[0], position_xyz3[1], position_xyz3[2]),
# orientation_xyzw3,
# rospy.Time.now(),
# "/shelf_front", # child
# "/base" # parent
# )
self.br.sendTransform((shelfOri[0], shelfOri[1], self._shelfHeight),
tf.transformations.quaternion_from_euler(self._shelfRoll, self._shelfPitch, shelfRot),
rospy.Time.now(),
"/shelf_front", # child
"/base" # parent
)
self.br.sendTransform((legs[0][0], legs[0][1], self._shelfHeight), \
tf.transformations.quaternion_from_euler(self._shelfRoll, self._shelfPitch, shelfRot),
rospy.Time.now(),
"/left_leg", # child
"/base" # parent
)
self.br.sendTransform((legs[1][0], legs[1][1], self._shelfHeight), \
tf.transformations.quaternion_from_euler(self._shelfRoll, self._shelfPitch, shelfRot),
rospy.Time.now(),
"/right_leg", # child
"/base" # parent
)
#self.pubShelfSep.publish(self.tf2PoseStamped(shelfOri, tf.transformations.quaternion_from_euler(0, 0, shelfRot)))
if self.writeFile:
file_path = self.rp.get_path('calibration_data')
f = open(file_path + "/extrinsics/shelf_front.txt", 'w')
f.write(str(shelfOri[0])+'\t')
f.write(str(shelfOri[1])+'\t')
f.write(str(0.0)+'\t')
quaternion_shelf = tf.transformations.quaternion_from_euler(0, 0, shelfRot)
f.write(str(quaternion_shelf[0])+'\t')
f.write(str(quaternion_shelf[1])+'\t')
f.write(str(quaternion_shelf[2])+'\t')
f.write(str(quaternion_shelf[3])+'\n')
f.close()
self.writeFile = False
class mbotSimpleBehavior(object):
'''
Exposes a behavior for the pioneer robot so that moves forward until
it has an obstacle at 1.0 m then stops rotates for some time to the
right and resumes motion.
'''
def __init__(self):
'''
Class constructor: will get executed at the moment
of object creation
'''
self.odom_msg_received = False
self.laser_msg_received = False
self.map_msg_received = False
# register node in ROS network
rospy.init_node('pioneer_smart_behavior', anonymous=False)
self.INITIALPOSE_x = -1.39379392735
self.INITIALPOSE_y = -1.33716776885
self.INITIALPOSE_teta = 0
# send map as point cloud
self.publish_map = rospy.Publisher("/wall_map", pc2, queue_size=1)
# subscribe our laser point cloud
self.laserProjection = LaserProjection()
# puclish pointcloudlaser
self.pcPub = rospy.Publisher("/laserPointCloud", pc2, queue_size=1)
#
self.matchinginfo = rospy.Publisher("/laserPointCloud",
pc2,
queue_size=1)
# print message in terminal
rospy.loginfo('Pioneer simple behavior started !')
# subscribe to pioneer laser scanner topic
rospy.Subscriber("/scan_combined",
LaserScan,
self.laserCallback,
queue_size=1)
# subscribe to pioneer odometry topic
rospy.Subscriber("/odom", Odometry, self.odomCallback, queue_size=1)
# subscribe to mbot map topic
rospy.Subscriber("/map", OccupancyGrid, self.mapCallback, queue_size=1)
rospy.Subscriber("/amcl_pose",
PoseWithCovarianceStamped,
self.amclposeCallback,
queue_size=1)
# setup publisher to later on move the pioneer base
#self.pub_cmd_vel = rospy.Publisher('robot_0/cmd_vel', Twist, queue_size=1)
# define member variable and initialize with a big value
# it will store the distance from the robot to the walls
#self.distance = 10.0
# Last two odometry readings
self.initpose_icp = np.array([[
np.cos(self.INITIALPOSE_teta), -np.sin(self.INITIALPOSE_teta),
self.INITIALPOSE_x
],
[
np.sin(self.INITIALPOSE_teta),
np.cos(self.INITIALPOSE_teta),
self.INITIALPOSE_y
], [0, 0, 1]])
self.varicp = 0
self.amcl = np.array([[self.INITIALPOSE_x], [self.INITIALPOSE_y]])
self.currentmsgodom = [
self.INITIALPOSE_x, self.INITIALPOSE_y, self.INITIALPOSE_teta
]
self.currentmsgamcl = [
self.INITIALPOSE_x, self.INITIALPOSE_y, self.INITIALPOSE_teta
]
self.odom = {
'x': self.INITIALPOSE_x,
'y': self.INITIALPOSE_y,
'teta': self.INITIALPOSE_teta,
'x_1': self.INITIALPOSE_x,
'y_1': self.INITIALPOSE_y,
'teta_1': self.INITIALPOSE_teta
}
self.odom_read = {
'x': self.INITIALPOSE_x,
'y': self.INITIALPOSE_y,
'teta': self.INITIALPOSE_teta,
'x_1': self.INITIALPOSE_x,
'y_1': self.INITIALPOSE_y,
'teta_1': self.INITIALPOSE_teta
}
# EKF pose predcition
self.pose_prediction = {
'x': self.INITIALPOSE_x,
'y': self.INITIALPOSE_y,
'teta': self.INITIALPOSE_teta
}
self.init_pose_prediction = {
'x': self.INITIALPOSE_x,
'y': self.INITIALPOSE_y,
'teta': self.INITIALPOSE_teta
}
self.pose_estimation = {
'x': self.INITIALPOSE_x,
'y': self.INITIALPOSE_y,
'teta': self.INITIALPOSE_teta
}
# self.pose_prediction = {'x': 0,'y': 0, 'teta': 0}
# Jacobian matrix g
self.matrix_G = np.identity(3)
self.matrix_H = np.identity(3)
self.matrix_R = np.array([[0.9, 0, 0], [0, 1.1, 0], [0, 0, 100]])
# covariance of noise observation matrix
self.matrix_Q = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
# Covariance of instance t-1
#self.cov_matrix = np.array([[0.5, 0, 0], [0, 0.5, 0], [0, 0, 0.5]])
# covariance of instance t (actual)
self.prev_cov = np.identity(3)
self.act_cov = np.identity(3)
# predicted covariance
self.pred_cov = np.identity(3)
self.pred_update = np.array([0, 0, 0], dtype=float)
self.z_measurements = np.array([0, 0, 0], dtype=float)
self.map = []
self.width = 0
self.height = 0
self.resolution = 0
self.cloudpointmap = []
self.currentlaserdata = []
self.usingpointlaser = []
# defines the range threshold bellow which the robot should stop moving foward and rotate insteadnp.array([[np.cos(0),-np.sin(0),-1.37660027342],[np.sin(0),np.cos(0.003139031004),4.01820473614],[0,0,1]])
# if rospy.has_param('distance_threshold'):
# # retrieves the threshold from the parameter server in the case where the parameter exists
# self.distance_threshold = rospy.get_param('distance_threshold')
# else:
# self.distance_threshold = 1.0
def odomCallback(self, msg):
'''
This function gets executed everytime a odomotry reading msg is received on the
topic: /robot_0/odometry
'''
self.odom_msg_received = True
# print("ANGULO", self.quaternion_to_euler(msg))
# print("x", msg.pose.pose.position.x, "y", msg.pose.pose.position.y)
self.currentmsgodom[
0] = msg.pose.pose.position.x + self.init_pose_prediction[
'x'] - 1.3108866698979635
self.currentmsgodom[
1] = msg.pose.pose.position.y + self.init_pose_prediction[
'y'] - 0.19804321874743439
self.currentmsgodom[2] = self.quaternion_to_euler(
msg) + self.init_pose_prediction['teta'] + 2.401482407814359
def amclposeCallback(self, msg):
#print('amcl pose x= ', msg.pose.pose.position.x, 'amcl pose y= ', msg.pose.pose.position.y, "amcl teta = ", self.quaternion_to_euler(msg))
self.amcl = np.hstack((self.amcl,
np.array([[msg.pose.pose.position.x],
[msg.pose.pose.position.y]])))
self.currentmsgamcl[0] = msg.pose.pose.position.x
self.currentmsgamcl[1] = msg.pose.pose.position.y
self.currentmsgamcl[2] = np.remainder(self.quaternion_to_euler(msg),
2 * np.pi)
#print("amcl angulo", self.currentmsgamcl[2])
def quaternion_to_euler(self, msg):
quaternion = (msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z, msg.pose.pose.orientation.w)
(roll, pitch,
yaw) = tf.transformations.euler_from_quaternion(quaternion)
return yaw
def laserCallback(self, msg):
'''
This function gets executed everytime a laser scanner msg is received on the
topic: /scan_combined
'''
self.laser_msg_received = True
temporaria = []
cloud_out = self.laserProjection.projectLaser(msg)
for p in pcmapping.read_points(cloud_out,
field_names=("x", "y", "z"),
skip_nans=True):
temporaria.append([p[0], p[1]])
self.currentlaserdata = temporaria
#print("laser len", len(self.currentlaserdata))
#print("point _ point_cloud_laser=", self.currentlaserdata)
def mapCallback(self, msg):
self.map = msg.data
self.width = msg.info.width
self.height = msg.info.height
self.resolution = msg.info.resolution
i = 0
j = 0
for i in range(self.width):
for j in range(self.height):
if (self.map[i + j * self.width] > 0.0):
self.cloudpointmap.append([
self.resolution * i + msg.info.origin.position.x,
self.resolution * j + msg.info.origin.position.y
])
self.map_msg_received = True
def ekf_predict(self):
'''
perform one EKF predict
'''
self.odom['x'] = self.currentmsgodom[0]
self.odom['y'] = self.currentmsgodom[1]
self.odom['teta'] = self.currentmsgodom[2]
dx = self.odom['x'] - self.odom['x_1']
dy = self.odom['y'] - self.odom['y_1']
dtheta = self.odom['teta'] - self.odom['teta_1']
delta_trans = math.sqrt(dx**2 + dy**2)
delta_rot1 = math.atan2(dy, dx) - self.odom['teta_1']
delta_rot2 = dtheta - delta_rot1
#self.prev_cov = self.act_cov
self.pose_prediction['x'] = self.pose_estimation[
'x'] + delta_trans * math.cos(self.pose_estimation['teta'] +
delta_rot1)
self.pose_prediction['y'] = self.pose_estimation[
'y'] + delta_trans * math.sin(self.pose_estimation['teta'] +
delta_rot1)
self.pose_prediction[
'teta'] = self.pose_estimation['teta'] + delta_rot1 + delta_rot2
self.pose_prediction['teta'] = np.remainder(
self.pose_estimation['teta'], 2 * np.pi)
#print(self.pose_prediction)
#print("pose_prediction x =" ,self.pose_prediction['x'],"pose_prediction y =" ,self.pose_prediction['y'],"pose_prediction theta=" ,self.pose_prediction['teta'], "\n", "odom x =", self.odom['x'], "odom y =", self.odom['y'], "odom theta =", self.odom['teta'])
self.matrix_G = np.array(
[[
1, 0, -delta_trans *
math.sin(self.pose_estimation['teta'] + delta_rot1)
],
[
0, 1, delta_trans *
math.cos(self.pose_estimation['teta'] + delta_rot1)
], [0, 0, 1]])
self.pred_cov = np.dot(
self.matrix_G, np.dot(self.act_cov,
self.matrix_G.transpose())) + self.matrix_R
self.odom['x_1'] = self.odom['x']
self.odom['y_1'] = self.odom['y']
self.odom['teta_1'] = self.odom['teta']
def ekf_update(self):
pose_estimation_update = np.array([0, 0, 0], dtype=float)
self.pred_update[0] = self.pose_prediction['x']
self.pred_update[1] = self.pose_prediction['y']
self.pred_update[2] = self.pose_prediction['teta']
#print(self.pred_update)
#print("pose_prediction x =" ,self.pose_prediction['x'],"pose_prediction y =" ,self.pose_prediction['y'],"pose_prediction theta=" ,self.pose_prediction['teta'], "\n", "odom x =", self.odom['x'], "odom y =", self.odom['y'], "odom theta =", self.odom['teta'])
#Este Z deveria vir do ICP
z = np.array([[self.currentmsgamcl[0]], [self.currentmsgamcl[1]],
[self.currentmsgamcl[2]]])
#kalman gain
k_gain = (self.pred_cov.dot(self.matrix_H.transpose())).dot(
LA.inv((self.matrix_H.dot(self.pred_cov)
).dot(self.matrix_H.transpose()) + self.matrix_Q))
self.updatemeasurements_icp()
pose_estimation_update = self.pred_update.reshape(3, 1) + k_gain.dot(
(self.z_measurements.reshape(3, 1) -
self.pred_update.reshape(3, 1)))
#print(k_gain)
#print(pose_estimation_update)
#print(pose_estimation_update)
self.pose_estimation['x'] = pose_estimation_update[0][0]
self.pose_estimation['y'] = pose_estimation_update[1][0]
self.pose_estimation['teta'] = pose_estimation_update[2][0]
self.act_cov = (np.identity(3) - k_gain.dot(self.matrix_H)).dot(
self.pred_cov)
def updatemeasurements_icp(self):
#print("pointcloudlaser" , self.point_cloud_laser)
# x_max_pointcloud_laser = max(self.point_cloud_laser,key=itemgetter(1))[0]
# y_max_pointcloud_laser = max(self.point_cloud_laser,key=itemgetter(1))[1]
# x_min_pointcloud_laser = min(self.point_cloud_laser,key=itemgetter(1))[0]
# y_min_pointcloud_laser = min(self.point_cloud_laser,key=itemgetter(1))[1]
#
#
# resizedmap = [x for x in self.cloudpointmap if (x_min_pointcloud_laser - 0.5 <= x[0] <= x_max_pointcloud_laser +0.5 and y_min_pointcloud_laser - 0.5 <= x[1] <= y_max_pointcloud_laser +0.5) ]
self.usingpointlaser = self.currentlaserdata
# x_max_pointcloud_laser = max(self.usingpointlaser,key=itemgetter(1))[0]
# y_max_pointcloud_laser = max(self.usingpointlaser,key=itemgetter(1))[1]
# x_min_pointcloud_laser = min(self.usingpointlaser,key=itemgetter(1))[0]
# y_min_pointcloud_laser = min(self.usingpointlaser,key=itemgetter(1))[1]
if (self.varicp == 0):
# resizedmap = [x for x in self.cloudpointmap if (-1.43967161853 + x_min_pointcloud_laser -1.2 <= x[0] <= -1.43967161853 + x_max_pointcloud_laser+1.2 or 4.02608497565 + y_min_pointcloud_laser - 1.2 <= x[1] <= 4.02608497565 + y_max_pointcloud_laser +1.2 ) ]
T, distances, i = icp(
np.array(self.usingpointlaser), np.array(self.cloudpointmap),
np.array([[
np.cos(self.INITIALPOSE_teta),
-np.sin(self.INITIALPOSE_teta), self.INITIALPOSE_x
],
[
np.sin(self.INITIALPOSE_teta),
np.cos(self.INITIALPOSE_teta), self.INITIALPOSE_y
], [0, 0, 1]]))
self.z_measurements = np.array([[T[0][2]], [T[1][2]],
[np.arctan2(T[1][0], T[0][0])]])
#print(T)
#print("foi aqui")
#print("mapa ",np.shape( np.array(self.cloudpointmap)))
#print("laser", np.shape( np.array(self.usingpointlaser)))
self.varicp = 1
else:
#resizedmap = [x for x in self.cloudpointmap if (self.pred_update[0] + x_min_pointcloud_laser <= x[0] <= self.pred_update[0] + x_max_pointcloud_laser or self.pred_update[1] + y_min_pointcloud_laser <= x[1] <= self.pred_update[1] + y_max_pointcloud_laser ) ]
#print("mapa ",np.shape( np.array(self.cloudpointmap)))
#print("laser", np.shape( np.array(self.usingpointlaser)))
T, distances, i = icp(
np.array(self.usingpointlaser), np.array(self.cloudpointmap),
np.array([[
np.cos(self.pred_update[2]), -np.sin(self.pred_update[2]),
self.pred_update[0]
],
[
np.sin(self.pred_update[2]),
np.cos(self.pred_update[2]), self.pred_update[1]
], [0, 0, 1]]))
#print(T)
self.z_measurements[0] = T[0][2]
self.z_measurements[1] = T[1][2]
self.z_measurements[2] = np.arctan2(T[1][0], T[0][0])
#print(T)
def run_behavior(self):
pred = np.array([[self.pose_prediction['x']],
[self.pose_prediction['y']],
[self.pose_prediction['teta']]])
odometry = np.array([[self.odom_read['x_1']], [self.odom_read['y_1']],
[self.odom_read['teta_1']]])
error = np.array([[0], [0], [0]])
while not rospy.is_shutdown():
# base needs this msg to be published constantly for the robot to keep moving so we publish in a loop
# while the distance from the robot to the walls is bigger than the defined threshold keep moving forward
##if self.distance > self.distance_threshold:
## self.move_forward()
##else:
# rotate for a certain angle
## self.rotate_right()
# EKF - Predict
r = rospy.Rate(5)
while len(self.currentlaserdata
) == 0 or self.map_msg_received == False:
continue
#print("MAIN LASER", self.point_cloud_laser)
if self.odom_msg_received == True:
#rospy.loginfo('odom msg received!')
self.odom_msg_received = False
if self.laser_msg_received == True:
#rospy.loginfo('laser msg received!')
self.laser_msg_received == True
total_time = 0
self.ekf_predict()
start = time.time()
self.ekf_update()
end = time.time()
#print(end-start)
arr = np.array([[self.pose_estimation['x']],
[self.pose_estimation['y']],
[self.pose_estimation['teta']]])
#print("PRED=", pred)
#print ("ARR = ", arr)
pred = np.hstack((pred, arr))
error_array = np.array(
[[self.pose_estimation['x'] - self.currentmsgamcl[0]],
[self.pose_estimation['y'] - self.currentmsgamcl[1]],
[self.pose_prediction['teta'] - self.currentmsgamcl[2]]])
error = np.hstack((error, error_array))
with open('pred.pkl', 'wb') as f:
pickle.dump(pred, f)
with open('amcl.pkl', 'wb') as f2:
pickle.dump(self.amcl, f2)
arr2 = np.array([[self.odom['x_1']], [self.odom['y_1']],
[self.odom['teta_1']]])
odometry = np.hstack((odometry, arr2))
#plt.plot(odometry[0, :].flatten(),
# odometry[1, :].flatten(), "-r")
with open('rosbagbom.pkl', 'wb') as f:
pickle.dump((error[0, :], error[1, :], error[2, :]), f)
plt.plot(pred[0, :].flatten(), pred[1, :].flatten(), "-b")
plt.plot(self.amcl[0, :].flatten(), self.amcl[1, :].flatten(),
"-g")
# plt.plot(error[0, :].flatten(), "-r")
# plt.plot(error[1, :].flatten(), '-b')
# sleep for a small amount of time
plt.grid(True)
plt.pause(0.00001)
r.sleep()
class lineScan:
def __init__(self, verbose=False):
self.rangeData = LaserScan()
self.scan_sub = rospy.Subscriber("/base_scan", LaserScan, self.callback)
self.listener = tf.TransformListener()
self.laser_projector = LaserProjection()
self.pc = []
self.leg1 = []
self.leg2 = []
self.br = tf.TransformBroadcaster()
self.rate = rospy.Rate(100.0)
self.calibrated = False
self.reCalibration = False
self.priorOri = []
self.priorLeft = []
self.priorRight = []
self.odomL = []
self.odomR = []
self.priorAvailable = False
self.newObsWeight = 0.1
self.offsetXY = [-0.044, 0]
def callback(self,data):
self.rangeData = data
def refreshRangeData(self):
self.rangeData = LaserScan() # flush
while len(self.rangeData.ranges) == 0:
rospy.sleep(0.04)
def getStatus(self):
return self.calibrated
def getCloud(self):
self.refreshRangeData()
cloud2 = self.laser_projector.projectLaser(self.rangeData)
xyz = pc2.read_points(cloud2, skip_nans=True, field_names=("x", "y", "z"))
self.pc = []
while True:
try:
self.pc.append(xyz.next())
except:
break
return self.pc
def segmentLength(self, m, i1, i2):
v1 = m[i1]
v2 = m[i2]
return np.linalg.norm(v1-v2)
def cloud2Input(self):
while not rospy.is_shutdown():
plt.clf()
pc = self.getCloud()
m = np.asarray(pc)
segs = []
r = split_and_merge(m, 0.1)
for i in range(1, len(r)):
if r[i] - r[i-1] < 20:
continue
l = self.segmentLength(m, r[i], r[i-1])
if l > 0.8 and l < 0.9:
print 'found'
print l
segs.append([r[i], r[i-1]])
# Plot found lines
plt.plot(m[:, 0], m[:, 1], '.r')
plt.plot(m[r, 0], m[r, 1], '-')
plt.plot(m[r, 0], m[r, 1], '+')
for s in segs:
plt.plot(m[s, 0], m[s, 1], 'g-+', linewidth=5)
plt.axis('equal')
plt.draw()
plt.title("Laser scan")
plt.show(block=False)
class occupancy():
def __init__(self):
## SETUP ROS NODES
rospy.init_node('occupancy', anonymous=False)
rospy.loginfo("To stop occupancy CTRL + C")
rospy.on_shutdown(self.shutdown)
self.r = rospy.Rate(10) # 10hz
# SETUP MAP
self.range = 10.0 # meters
self.pixels = 36
self.map = np.zeros((self.pixels, self.pixels))
self.pixwid = self.pixels / self.range
print("pixel width", self.pixwid)
self.offset = np.asarray([1.0, 5.0]) # control where 0,0 will map to
# ~ x = np.linspace(0-self.offsets[0],self.range-self.offsets[0], self.pixels)
# ~ y = np.linspace(0-self.offsets[1],self.range-self.offsets[1], self.pixels)
# ~ xx,yy = np.meshgrid(x,y)
self.points, self.edges = self.get_graph()
self.heuristic = np.ones(len(self.points))
# MAP WORLD POINT TO MAP COORDINATE
self.xmap = lambda a: np.minimum(
np.maximum(0, ((a + self.offset[0]) * self.pixwid).astype(int)),
self.pixels - 1)
self.ymap = lambda a: np.minimum(
np.maximum(0, ((a + self.offset[1]) * self.pixwid).astype(int)),
self.pixels - 1)
self.xinverse = lambda a: ((a + 0.0) / self.pixwid) - self.offset[0]
self.yinverse = lambda a: ((a + 0.0) / self.pixwid) - self.offset[1]
self.center = np.asarray([self.xmap(0), self.ymap(0)])
wy = range(0, self.pixels)
# FOR RAYCASTING (head of the 'ray')
behind = np.vstack([np.zeros_like(wy), wy]).transpose()
right = np.vstack([wy, np.zeros_like(wy)]).transpose()
left = np.vstack([wy,
(self.pixels - 1) * np.ones_like(wy)]).transpose()
front = np.vstack([(self.pixels - 1) * np.ones_like(wy),
wy]).transpose()
self.wp = np.vstack([left, right, front])
#SUBSCRIBER
#rospy.Subscriber('/converted_pc', PointCloud2, self.parse_command)
self.laser_projector = LaserProjection()
self.scan_sub = rospy.Subscriber('/scan', LaserScan, self.on_scan)
# ~ target_string = "/new_target"
target_string = "/tracked_target"
self.target_sub = rospy.Subscriber(target_string, Marker,
self.process_target)
# Placeholder
# ~ self.target = np.asarray( [28, 16] )
# ~ self.start = np.ravel_multi_index(self.center, (self.pixels,self.pixels) )
# ~ self.goal = np.ravel_multi_index(self.target, (self.pixels,self.pixels) )
# PUBLISHER
# self.pub = rospy.Publisher('/go_nogo', ObjectVector, queue_size=1)
# ~ self.pub = rospy.Publisher('/net_input', Float32MultiArray, queue_size=1)
self.pub_path = rospy.Publisher('/path_to_target',
Marker,
queue_size=1)
# ~ self.pub_new_target = rospy.Publisher('/new_target', Point, queue_size=1)
# ~ self.pub_new_target = rospy.Publisher('/new_target', Marker, queue_size=1)
rospy.sleep(0.01)
rospy.loginfo("Waiting for topics...")
try:
rospy.wait_for_message("/scan", LaserScan, timeout=10.0)
rospy.wait_for_message(target_string, Marker, timeout=10.0)
except rospy.ROSException as e:
rospy.logerr("Timeout while waiting for topics!")
raise e
print("Node started")
while not rospy.is_shutdown():
self.r.sleep()
self.update_map()
# TODO: get heuristic
self.start = np.ravel_multi_index(self.center,
(self.pixels, self.pixels))
self.goal = np.ravel_multi_index(self.target,
(self.pixels, self.pixels))
path = self.find_path(self.start,
self.goal,
heuristic=self.heuristic)
x, y = np.unravel_index(path, (self.pixels, self.pixels))
self.map[x, y] = 2
if VIS_MAP:
# ~ fig = plt.figure(1)
# ~ plt.clf()
# ~ #plt.plot(self.xy[:,0], self.xy[:,1], '.')
# ~ plt.plot(self.xmap(self.xy[:,0]), self.xmap(self.xy[:,1]), '.')
fig = plt.figure(2)
plt.clf()
plt.imshow(self.map)
plt.show(block=False)
plt.pause(0.0000000001)
path_message = self.get_path_message(x, y)
self.pub_path.publish(path_message)
def shutdown(self):
rospy.loginfo("Stop occupancy")
# rospy.sleep(1)
return 0
def process_target(self, data):
x = self.xmap(data.pose.position.x)
y = self.ymap(data.pose.position.y)
# ~ print("points", x,y)
self.target = np.asarray([x, y])
def on_scan(self, scan):
cloud = self.laser_projector.projectLaser(scan)
gen = pc2.read_points(cloud,
skip_nans=True,
field_names=("x", "y", "z"))
self.xyz_generator = gen
self.xyz = np.asarray([[p[0], p[1], p[2]] for p in gen])
def update_map(self):
# ~ https://www.redblobgames.com/articles/visibility/
# ~ 1. Calculate the angles where walls begin or end.
# ~ 2. Cast a ray from the center along each angle.
# ~ 3. Fill in the triangles generated by those rays.
self.xy = deepcopy(self.xyz[:, :2])
xs = self.xmap(self.xy[:, 0])
ys = self.ymap(self.xy[:, 1])
# ~ print(self.xy[:,1])
# ~ print(ys)
self.map = 0.5 * np.ones((self.pixels, self.pixels))
self.map[xs, ys] = 1
self.map[self.center[0], self.center[1]] = 0
# TODO: raytracing
self.map = raycast(self.map, self.center, self.wp)
self.map = np.maximum(self.map, gaussian_filter(self.map, sigma=2))
# https://raw.githubusercontent.com/pfirsich/Ray-casting-test/master/main.lua
def get_graph(self):
# GET GRAPH CORRESPONDING TO OCCUPANCY MAP
# GET POINTS [time, forward, lateral]
X = range(self.pixels)
Y = range(self.pixels)
Y, X = np.meshgrid(Y, X) # X,Y doesn't ravel correctly
points = np.vstack([X.ravel(), Y.ravel()]).transpose()
print("points shape", np.shape(points))
# GET EDGES
edges = []
cost_edges = []
# [t,x,y]-> [t+1,x:x+2,y-1:y+1] # x can only take big steps (x+0 x+2) early on
for p in range(len(points)):
x, y = np.unravel_index(
p, (self.pixels, self.pixels)) # self.x_num, self.y_num) )
# # CONNECTIVITY
if x == 0:
xs = [x, x + 1]
elif x == self.pixels - 1:
xs = [x - 1, x]
else:
xs = [x - 1, x, x + 1]
if y == 0:
ys = [y, y + 1]
elif y == self.pixels - 1:
ys = [y - 1, y]
else:
ys = [y - 1, y, y + 1]
xgrid, ygrid = np.meshgrid(xs, ys)
xlist = xgrid.ravel()
ylist = ygrid.ravel()
arr = np.array([xlist, ylist]).astype(int)
inds = np.ravel_multi_index(
arr, (self.pixels, self.pixels)) # order='F')
edges.append(inds)
return points, edges
def find_path(self, start, goal, heuristic=0):
# DIJKSTRA's ALGORITHM / A*
node_num = len(self.points)
nodedist = np.inf * np.ones(node_num)
nodedist[start] = 0
visited = np.zeros(node_num)
came_from = np.zeros(node_num)
big_num = 10000
v = start
for i in range(node_num):
# STARTING NODE (working backwards)
costs = nodedist + 10 * big_num * visited + heuristic
v = np.argmin(costs) # unvisited and closest
# print('v', v, np.min(costs))
if v == goal:
break
visited[v] = 1
# FIND NEIGHBORS
# k = 30 + 1 # 1st is always self
# dist, ind = self.tree.query([self.points[v,:]], k=k)
# ind = ind[0]
# dist = dist[0]
v_neighbor = self.edges[v]
# dist = self.weights[v] # WE USE THE DESTINATION COST (from occupancy map) AS THE EDGE COST
# COST: cost to current + current to new cost
for p in range(np.size(v_neighbor)):
# ~ line = np.asarray(np.hstack((self.points[v,:],self.points[v_neighbor[p],:])))
new_dist = nodedist[v] + self.map.ravel()[v_neighbor[p]]
# print('...', p, new_dist)
if new_dist < nodedist[v_neighbor[p]]:
nodedist[v_neighbor[p]] = new_dist
came_from[v_neighbor[p]] = v
# steps+=1
if v != goal: #>max_steps:
print("No path found")
return []
# print("costs",nodedist)
self.node_dist = nodedist
# RECOVER PATH
self.path = []
self.path.append(v)
# FIND NEXT SUBGOAL
# if self.subgoal!=len(self.points)-1: ################################### if not finished
self.subgoal = start
while came_from[v] != self.subgoal:
v = int(came_from[v])
self.path.append(v)
self.path.append(self.subgoal)
self.subgoal = v
return self.path
def get_path_message(self, x, y):
X = self.xinverse(x)
Y = self.yinverse(y)
m = default_marker()
m.lifetime = rospy.Duration(0.1)
for i in range(len(X)):
m.points.append(Point(x=X[i], y=Y[i], z=0))
return m
def test_project_laser(self):
tolerance = 6 # decimal places
projector = LaserProjection()
ranges = [-1.0, 1.0, 2.0, 3.0, 4.0, 5.0, 100.0]
intensities = np.arange(1.0, 6.0).tolist()
min_angles = -np.pi / np.array([1.0, 1.5, 2.0, 4.0, 8.0])
max_angles = -min_angles
angle_increments = np.pi / np.array([180., 360., 720.])
scan_times = [rospy.Duration(1./i) for i in [40, 20]]
for range_val, intensity_val, \
angle_min, angle_max, angle_increment, scan_time in \
product(ranges, intensities,
min_angles, max_angles, angle_increments, scan_times):
try:
scan = build_constant_scan(
range_val, intensity_val,
angle_min, angle_max, angle_increment, scan_time)
except BuildScanException:
if (angle_max - angle_min)/angle_increment > 0:
self.fail()
cloud_out = projector.projectLaser(scan, -1.0,
LaserProjection.ChannelOption.INDEX)
self.assertEquals(len(cloud_out.fields), 4,
"PointCloud2 with channel INDEX: fields size != 4")
cloud_out = projector.projectLaser(scan, -1.0,
LaserProjection.ChannelOption.INTENSITY)
self.assertEquals(len(cloud_out.fields), 4,
"PointCloud2 with channel INDEX: fields size != 4")
cloud_out = projector.projectLaser(scan, -1.0)
self.assertEquals(len(cloud_out.fields), 5,
"PointCloud2 with channel INDEX: fields size != 5")
cloud_out = projector.projectLaser(scan, -1.0,
LaserProjection.ChannelOption.INTENSITY |
LaserProjection.ChannelOption.INDEX)
self.assertEquals(len(cloud_out.fields), 5,
"PointCloud2 with channel INDEX: fields size != 5")
cloud_out = projector.projectLaser(scan, -1.0,
LaserProjection.ChannelOption.INTENSITY |
LaserProjection.ChannelOption.INDEX |
LaserProjection.ChannelOption.DISTANCE)
self.assertEquals(len(cloud_out.fields), 6,
"PointCloud2 with channel INDEX: fields size != 6")
cloud_out = projector.projectLaser(scan, -1.0,
LaserProjection.ChannelOption.INTENSITY |
LaserProjection.ChannelOption.INDEX |
LaserProjection.ChannelOption.DISTANCE |
LaserProjection.ChannelOption.TIMESTAMP)
self.assertEquals(len(cloud_out.fields), 7,
"PointCloud2 with channel INDEX: fields size != 7")
valid_points = 0
for i in range(len(scan.ranges)):
ri = scan.ranges[i]
if (PROJECTION_TEST_RANGE_MIN <= ri and
ri <= PROJECTION_TEST_RANGE_MAX):
valid_points += 1
self.assertEqual(valid_points, cloud_out.width,
"Valid points != PointCloud2 width")
idx_x = idx_y = idx_z = 0
idx_intensity = idx_index = 0
idx_distance = idx_stamps = 0
i = 0
for f in cloud_out.fields:
if f.name == "x":
idx_x = i
elif f.name == "y":
idx_y = i
elif f.name == "z":
idx_z = i
elif f.name == "intensity":
idx_intensity = i
elif f.name == "index":
idx_index = i
elif f.name == "distances":
idx_distance = i
elif f.name == "stamps":
idx_stamps = i
i += 1
i = 0
for point in pc2.read_points(cloud_out):
ri = scan.ranges[i]
ai = scan.angle_min + i * scan.angle_increment
self.assertAlmostEqual(point[idx_x], ri * np.cos(ai),
tolerance, "x not equal")
self.assertAlmostEqual(point[idx_y], ri * np.sin(ai),
tolerance, "y not equal")
self.assertAlmostEqual(point[idx_z], 0,
tolerance, "z not equal")
self.assertAlmostEqual(point[idx_intensity],
scan.intensities[i],
tolerance, "Intensity not equal")
self.assertAlmostEqual(point[idx_index], i,
tolerance, "Index not equal")
self.assertAlmostEqual(point[idx_distance], ri,
tolerance, "Distance not equal")
self.assertAlmostEqual(point[idx_stamps],
i * scan.time_increment,
tolerance, "Timestamp not equal")
i += 1
class baseScan:
def __init__(self, verbose=False):
self.rangeData = LaserScan()
self.scan_sub = rospy.Subscriber("/base_scan", LaserScan,
self.callback)
self.listener = tf.TransformListener()
self.laser_projector = LaserProjection()
self.pc = []
self.leg1 = []
self.leg2 = []
self.br = tf.TransformBroadcaster()
self.rate = rospy.Rate(20.0)
self.calibrated = False
self.reCalibration = False
self.priorOri_in_base_laser_link = [] # in base_laser_link frame
self.priorLeft_in_base_laser_link = [] # in base_laser_link frame
self.priorRight_in_base_laser_link = [] # in base_laser_link frame
self.odomL = [] # in odom_combined frame
self.odomR = [] # in odom_combined frame
self.priorAvailable = False
self.newObsWeight = 0.2
self.offsetXY = [-0.044, 0]
self.binOffset = 0.02
self.pubShelfSep = rospy.Publisher('pubShelfSep', PoseStamped)
self.reCalibrationCount = 4
self.tolerance = 0.1
self.updateRounds = 100
self.asyncRate = 20
self.limitInitX = True
self.xLimit = 0.1
self.emergencyThreshold = 30 * 20
self.emergencyTimeWindow = 60
while not rospy.is_shutdown():
try:
self.offsetXY = rospy.get_param('/base_scan/offset')
break
except:
rospy.loginfo(
'[shelf publisher]: waiting for base scan offset param')
rospy.sleep(random.uniform(0, 1))
continue
self.start_srv = rospy.Service(rospy.get_name() + '/start', Empty,
self.start_srv_callback)
self._start = False
# backup human supervised info for emergency
rp = rospkg.RosPack()
try:
dict_fp = rp.get_path('amazon_challenge_grasping')
except:
rospy.logerr('[shelf_publisher]: emergency file not found!')
sys.exit(1)
self.fileName = dict_fp + '/config/shelf_emergency.dat'
self.emergency = {}
def start_srv_callback(self, req):
rospy.loginfo('[shelf_publisher]: starting shelf publisher!')
if self._start:
rospy.logwarn('[shelf_publisher]: already started!!!!')
self._start = True
return EmptyResponse()
def raw_input_with_timeout(prompt, timeout=1.0):
print prompt,
timer = threading.Timer(timeout, thread.interrupt_main)
astring = None
try:
timer.start()
astring = raw_input(prompt)
except KeyboardInterrupt:
pass
timer.cancel()
return astring
def callback(self, data):
self.rangeData = data
def refreshRangeData(self):
self.rangeData = LaserScan() # flush
while len(self.rangeData.ranges) == 0 and not rospy.is_shutdown():
rospy.sleep(0.04)
def getStatus(self):
return self.calibrated
def getCloud(self):
# self.refreshRangeData()
cloud2 = self.laser_projector.projectLaser(self.rangeData)
xyz = pc2.read_points(cloud2,
skip_nans=True,
field_names=("x", "y", "z"))
self.pc = []
while not rospy.is_shutdown():
try:
self.pc.append(xyz.next())
except:
break
return self.pc
def findLegsOnce(self):
pc = self.getCloud()
x = []
y = []
for i in range(len(pc)):
x.append(pc[i][0])
y.append(pc[i][1])
radius = []
if self.calibrated or self.reCalibration: # use prior to find legs
for i in range(len(x)):
radius.append(
math.sqrt(
(x[i] - self.priorLeft_in_base_laser_link[0])**2 +
(y[i] - self.priorLeft_in_base_laser_link[1])**2))
n1 = radius.index(min(radius))
radius = []
for i in range(len(x)):
radius.append(
math.sqrt(
(x[i] - self.priorRight_in_base_laser_link[0])**2 +
(y[i] - self.priorRight_in_base_laser_link[1])**2))
n2 = radius.index(min(radius))
leg1 = [x[n1], y[n1]]
leg2 = [x[n2], y[n2]]
else:
# Assuming there is nothing between the robot and the shelf
if self.limitInitX:
y = [y[i] for i in range(len(y)) if x[i] >= self.xLimit]
x = [x[i] for i in range(len(x)) if x[i] >= self.xLimit]
for i in range(len(x)):
radius.append(math.sqrt(x[i]**2 + y[i]**2))
n = radius.index(min(radius))
x2 = [
x[i] for i in range(len(x))
if math.sqrt((x[i] - x[n])**2 + (y[i] - y[n])**2) > 0.7
]
y2 = [
y[i] for i in range(len(y))
if math.sqrt((x[i] - x[n])**2 + (y[i] - y[n])**2) > 0.7
]
radius2 = []
for i in range(len(x2)):
radius2.append(math.sqrt(x2[i]**2 + y2[i]**2))
n2 = radius2.index(min(radius2))
leg1 = [x[n], y[n]]
leg2 = [x2[n2], y2[n2]]
leg1 = [leg1[0] + self.offsetXY[0], leg1[1] + self.offsetXY[1]]
leg2 = [leg2[0] + self.offsetXY[0], leg2[1] + self.offsetXY[1]]
return [leg1, leg2] # left, right
def findLegs(self):
'''
accumulate new observations
'''
L1x = 0
L1y = 0
L2x = 0
L2y = 0
legs = self.findLegsOnce()
if legs[0][1] < legs[1][1]:
legs[0], legs[1] = legs[1], legs[0]
if self.calibrated:
self.leg1[0] = self.leg1[0] * (
1 - self.newObsWeight) + legs[0][0] * self.newObsWeight
self.leg1[1] = self.leg1[1] * (
1 - self.newObsWeight) + legs[0][1] * self.newObsWeight
self.leg2[0] = self.leg2[0] * (
1 - self.newObsWeight) + legs[1][0] * self.newObsWeight
self.leg2[1] = self.leg2[1] * (
1 - self.newObsWeight) + legs[1][1] * self.newObsWeight
else:
self.leg1 = legs[0]
self.leg2 = legs[1]
return [self.leg1, self.leg2] # left, right
def getShelfFrame(self):
# with respect to the frame of /base_scan
legs = self.findLegs()
ori_x = (legs[0][0] + legs[1][0]) / 2.
ori_y = (legs[0][1] + legs[1][1]) / 2.
left_leg = legs[0]
if legs[0][1] < legs[1][1]:
left_leg = legs[1]
rotAngle = atan2(ori_x - left_leg[0], left_leg[1] - ori_y)
return [ori_x, ori_y], rotAngle, legs
def tf2PoseStamped(self, xy, ori):
shelfPoseMsg = PoseStamped()
shelfPoseMsg.pose.position.x = xy[0]
shelfPoseMsg.pose.position.y = xy[1]
shelfPoseMsg.pose.position.z = 0.0
shelfPoseMsg.pose.orientation.x = ori[0]
shelfPoseMsg.pose.orientation.y = ori[1]
shelfPoseMsg.pose.orientation.z = ori[2]
shelfPoseMsg.pose.orientation.w = ori[3]
shelfPoseMsg.header.frame_id = 'base_laser_link'
shelfPoseMsg.header.stamp = rospy.Time.now()
return shelfPoseMsg
def saveEmergency(self):
with open(self.fileName, 'wb') as handle:
pickle.dump(self.emergency, handle)
def loadEmergency(self):
with open(self.fileName, 'rb') as handle:
self.emergency = pickle.load(handle)
def publish2TF(self):
answer = 'n'
ask = True
u = 0
shelfN = 0
recalibrateCount = 0
emergencyCount = 0
t_init = rospy.Time.now()
while not rospy.is_shutdown():
self.rate.sleep()
# check if human calibration is done
shelfOri, shelfRot, legs = self.getShelfFrame()
if self.reCalibration:
u = 0
if not self.calibrated:
self.br.sendTransform(
(shelfOri[0], shelfOri[1], 0),
tf.transformations.quaternion_from_euler(0, 0, shelfRot),
rospy.Time.now(),
"/shelf_frame", # child
"/base_laser_link" # parent
)
self.br.sendTransform((legs[0][0], legs[0][1], 0), \
tf.transformations.quaternion_from_euler(0, 0, shelfRot), \
rospy.Time.now(), \
"/left_leg", \
"/base_laser_link")
self.br.sendTransform((legs[1][0], legs[1][1], 0), \
tf.transformations.quaternion_from_euler(0, 0, shelfRot), \
rospy.Time.now(), \
"/right_leg", \
"/base_laser_link")
if self.priorAvailable:
while not rospy.is_shutdown():
try:
shelf_in_odom, shelf_rot_in_odom = self.listener.lookupTransform(
"/odom_combined", "/shelf_frame", rospy.Time(0))
self.priorLeft_in_base_laser_link, dummy = self.listener.lookupTransform(
"/base_laser_link", "/odomL", rospy.Time(0))
self.priorRight_in_base_laser_link, dummy = self.listener.lookupTransform(
"/base_laser_link", "/odomR", rospy.Time(0))
break
except:
continue
else:
try:
shelf_in_odom, shelf_rot_in_odom = self.listener.lookupTransform(
"/odom_combined", "/shelf_frame", rospy.Time(0))
except Exception, e:
# print e
continue
if self.reCalibration and math.sqrt(
(shelf_in_odom[0] - self.priorOri_in_odom[0])**2 +
(shelf_in_odom[1] -
self.priorOri_in_odom[1])**2) <= self.tolerance:
# rospy.sleep(2)
recalibrateCount += 1
if recalibrateCount == self.reCalibrationCount: # take recalibration only if it's stable
rospy.loginfo('reCalibrated!')
recalibrateCount = 0
u = 0
while not rospy.is_shutdown(
): # make sure the odomL and odomR are updated
try:
######## self.priorOri_in_odom, self.priorRot_in_odom = self.listener.lookupTransform("/odom_combined", "/shelf_frame", rospy.Time(0))
######## self.odomL, self.odomL_rot = self.listener.lookupTransform("/odom_combined", "/left_leg", rospy.Time(0))
######## self.odomR, self.odomR_rot = self.listener.lookupTransform("/odom_combined", "/right_leg", rospy.Time(0))
self.calibrated = True
self.reCalibration = False
rospy.loginfo(
"Prior origin in odom_combined: X = %4f, Y = %4f"
% (self.priorOri_in_odom[0],
self.priorOri_in_odom[1]))
break
except:
continue
if not self.calibrated and ask:
emergencyCount += 1
if emergencyCount == self.emergencyThreshold:
self.loadEmergency()
self.odomL = self.emergency.odomL
self.odomL_rot = self.emergency.odomL_rot
self.odomR = self.emergency.odomR
self.odomR_rot = self.emergency.odomR_rot
emergency_timestamp = self.emergency.timestamp
if (rospy.Time.now() - emergency_timestamp
).to_sec() > self.emergencyTimeWindow:
print ''
rospy.logwarn(
'***********************************************************************'
)
rospy.logwarn(
'[shelf_publisher] NOT ABLE TO RECOVER FROM CRASHING, GAME OVER'
)
rospy.logwarn(
'***********************************************************************'
)
continue
ask = False
self.calibrated = True
self.priorAvailable = True
self.priorOri_in_odom = shelf_in_odom
self.priorRot_in_odom = shelf_rot_in_odom
self.priorLeft_in_base_laser_link = legs[0]
self.priorRight_in_base_laser_link = legs[1]
print ''
rospy.logwarn(
'***********************************************************************'
)
rospy.logwarn(
'[shelf_publisher] EMERGENCY RECOVERY CALLED!!!!!!!!!!!!!!!!!!!!!!'
)
rospy.logwarn(
'***********************************************************************'
)
else:
sys.stdout.write(
"\r [ROS time: %s] Is the current shelf pose estimation good? (y/n)"
% rospy.get_time())
sys.stdout.flush()
if self._start:
answer = 'y'
if answer == 'y' or answer == 'yes':
self.calibrated = True
ask = False
self.priorAvailable = True
self.priorOri_in_odom = shelf_in_odom
self.priorRot_in_odom = shelf_rot_in_odom
self.priorLeft_in_base_laser_link = legs[0]
self.priorRight_in_base_laser_link = legs[1]
print ""
rospy.loginfo("Human calibration finished")
rospy.loginfo(
"Prior origin in /odom_combined: X = %4f, Y = %4f"
% (self.priorOri_in_odom[0],
self.priorOri_in_odom[1]))
while not rospy.is_shutdown():
try:
self.odomL, self.odomL_rot = self.listener.lookupTransform(
"/odom_combined", "/left_leg",
rospy.Time(0))
self.odomR, self.odomR_rot = self.listener.lookupTransform(
"/odom_combined", "/right_leg",
rospy.Time(0))
break
except:
pass
self.emergency = shelfInfo(self.odomL, self.odomL_rot,
self.odomR, self.odomR_rot,
rospy.Time.now())
self.saveEmergency()
else:
continue
# check in the odometry frame
if self.priorAvailable:
# print 'pub odom'
self.br.sendTransform(self.odomL, \
self.odomL_rot, \
rospy.Time.now(),\
"/odomL", \
"/odom_combined")
self.br.sendTransform(self.odomR, \
self.odomR_rot, \
rospy.Time.now(),\
"/odomR", \
"/odom_combined")
if self.calibrated:
u += 1
shelfN += 1
if math.sqrt((shelf_in_odom[0] - self.priorOri_in_odom[0])**2 +
(shelf_in_odom[1] -
self.priorOri_in_odom[1])**2) > self.tolerance:
rospy.logwarn(
'something is wrong with shelf pose estimation!!!!!!!!!! RECALIBRATING'
)
recalibrateCount = 0
u = 0
self.calibrated = False
self.reCalibration = True
continue
else:
self.br.sendTransform(
(shelfOri[0], shelfOri[1], 0),
tf.transformations.quaternion_from_euler(
0, 0, shelfRot),
rospy.Time.now(),
"/shelf_frame", # child
"/base_laser_link" # parent
)
self.br.sendTransform((legs[0][0], legs[0][1], 0), \
tf.transformations.quaternion_from_euler(0, 0, shelfRot), \
rospy.Time.now(),\
"/left_leg", \
"/base_laser_link")
self.br.sendTransform((legs[1][0], legs[1][1], 0), \
tf.transformations.quaternion_from_euler(0, 0, shelfRot), \
rospy.Time.now(),\
"/right_leg", \
"/base_laser_link")
self.pubShelfSep.publish(
self.tf2PoseStamped(
shelfOri,
tf.transformations.quaternion_from_euler(
0, 0, shelfRot)))
if u == self.updateRounds:
while not rospy.is_shutdown(
): # make sure the odomL and odomR are updated
try:
self.priorOri_in_odom, self.priorRot_in_odom = self.listener.lookupTransform(
"/odom_combined", "/shelf_frame",
rospy.Time(0))
self.odomL, self.odomL_rot = self.listener.lookupTransform(
"/odom_combined", "/left_leg", rospy.Time(0))
self.odomR, self.odomR_rot = self.listener.lookupTransform(
"/odom_combined", "/right_leg", rospy.Time(0))
rospy.loginfo(
"Prior origin in /odom_combined: X = %4f, Y = %4f"
% (self.priorOri_in_odom[0],
self.priorOri_in_odom[1]))
u = 0
break
except:
continue
if (rospy.Time.now() - t_init).to_sec() > 1.0:
t_init = rospy.Time.now()
self.emergency = shelfInfo(self.odomL, self.odomL_rot,
self.odomR, self.odomR_rot,
rospy.Time.now())
self.saveEmergency()
# print 'pub'
shelfN = 0
self.br.sendTransform(
(0, 0.1515 + self.binOffset, 1.21),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
"/shelf_bin_A", # child
"/shelf_frame" # parent
)
self.br.sendTransform(
(0, -0.1515 + self.binOffset, 1.21),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
"/shelf_bin_B", # child
"/shelf_frame" # parent
)
self.br.sendTransform(
(0, -0.4303 + self.binOffset, 1.21),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
"/shelf_bin_C", # child
"/shelf_frame" # parent
)
self.br.sendTransform(
(0, 0.1515 + self.binOffset, 1),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
"/shelf_bin_D", # child
"/shelf_frame" # parent
)
self.br.sendTransform(
(0, -0.1515 + self.binOffset, 1),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
"/shelf_bin_E", # child
"/shelf_frame" # parent
)
self.br.sendTransform(
(0, -0.4303 + self.binOffset, 1),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
"/shelf_bin_F", # child
"/shelf_frame" # parent
)
self.br.sendTransform(
(0, 0.1515 + self.binOffset, 0.78),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
"/shelf_bin_G", # child
"/shelf_frame" # parent
)
self.br.sendTransform(
(0, -0.1515 + self.binOffset, 0.78),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
"/shelf_bin_H", # child
"/shelf_frame" # parent
)
self.br.sendTransform(
(0, -0.4303 + self.binOffset, 0.78),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
"/shelf_bin_I", # child
"/shelf_frame" # parent
)
self.br.sendTransform(
(0, 0.1515 + self.binOffset, 0.51),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
"/shelf_bin_J", # child
"/shelf_frame" # parent
)
self.br.sendTransform(
(0, -0.1515 + self.binOffset, 0.51),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
"/shelf_bin_K", # child
"/shelf_frame" # parent
)
self.br.sendTransform(
(0, -0.4303 + self.binOffset, 0.51),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
"/shelf_bin_L", # child
"/shelf_frame" # parent
)
class lidar_processing():
def __init__(self):
## SETUP ROS NODES
rospy.init_node('lidar_processing', anonymous=False)
rospy.loginfo("To stop vecs_to_gonogo CTRL + C")
rospy.on_shutdown(self.shutdown)
self.r = rospy.Rate(10) # 10hz
self.coms = 0.0
self.vel = 0.0
new_pose = np.asarray([1, 0])
target_pose = np.asarray([1, 0])
#SUBSCRIBER
#rospy.Subscriber('/converted_pc', PointCloud2, self.parse_command)
self.laser_projector = LaserProjection()
self.scan_sub = rospy.Subscriber('/scan', LaserScan, self.on_scan)
# target pose from filter
# PUBLISHER
# self.pub = rospy.Publisher('/go_nogo', ObjectVector, queue_size=1)
# ~ self.pub = rospy.Publisher('/net_input', Float32MultiArray, queue_size=1)
self.pub_hulls = rospy.Publisher('/convex_hulls',
MarkerArray,
queue_size=1)
# ~ self.pub_new_target = rospy.Publisher('/new_target', Point, queue_size=1)
self.pub_new_target = rospy.Publisher('/new_target',
Marker,
queue_size=1)
rospy.sleep(0.01)
rospy.loginfo("Waiting for topics...")
try:
rospy.wait_for_message("/scan", LaserScan, timeout=10.0)
except rospy.ROSException as e:
rospy.logerr("Timeout while waiting for topics!")
raise e
print("Node started")
while not rospy.is_shutdown():
self.r.sleep()
#self.xyz_generator
# GET CLUSTERS
points, labels, n_clusters = self.get_clusters()
# GET HULLS and their COMs
hulls = []
coms = []
for n in range(n_clusters):
hull = ConvexHull(points[labels == n, :])
# ~ print("mean",np.mean(points[labels==n,:], 0) )
coms.append(np.mean(points[labels == n, :], 0))
# hull.vertices, hull.area
# print(n, 'area', hull.area, np.std(points[labels==n,:],0))
hulls.append(hull)
# GET TARGET POSE
dists = np.sum((np.vstack(coms) - new_pose)**2, 1)
self.new_id = np.argmin(dists)
# PUBLISH HULLS
marker = self.get_hull_markers(hulls, coms)
self.pub_hulls.publish(marker)
# PUBLISH NEW TARGET (for reset condition)
nt = coms[self.new_id]
# ~ matched = coms[self.cluster_id]
# ~ print("new target", nt, nt[0], nt[1])
# ~ self.pub_new_target.publish(Pose(Point(x=nt[0], y=nt[1],z=0) ) )
m = default_marker(x=nt[0], y=nt[1], z=0)
m.type = 2
m.color.r = 0
m.color.a = 1.0
m.scale.x = .30
m.scale.y = .30
m.scale.z = .30
self.pub_new_target.publish(m)
if VIS_HULL:
fig = plt.figure(1)
plt.clf()
plt.plot(points[:, 0], points[:, 1], 'o')
for n in range(n_clusters):
hull = hulls[n]
pts = points[labels == n, :]
for simplex in hull.simplices:
if n == self.cluster_id:
plt.plot(hull.points[simplex, 0],
hull.points[simplex, 1], 'r-')
else:
plt.plot(hull.points[simplex, 0],
hull.points[simplex, 1], 'k-')
plt.axis([-2, 8, -4, 4])
plt.show(block=False)
plt.pause(0.0000000001)
# ~ self.pub.publish(self.data) # numpy in ros only handles 1d arrays
def shutdown(self):
rospy.loginfo("Stop lidar_processing")
# rospy.sleep(1)
return 0
def get_clusters(self):
# ~ print(data.data)
#for p in gen:
# print " x : %f y: %f z: %f" %(p[0],p[1],p[2])
self.xy = deepcopy(self.xyz[:, :2])
db = DBSCAN(eps=0.15, min_samples=3).fit(self.xy)
core_samples_mask = np.zeros_like(db.labels_, dtype=bool)
core_samples_mask[db.core_sample_indices_] = True
labels = db.labels_
# Number of clusters in labels, ignoring noise if present.
n_clusters = len(set(labels)) - (1 if -1 in labels else 0)
n_noise = list(labels).count(-1)
self.labels = labels
if VIS_CLUSTER:
fig = plt.figure(1)
plt.clf()
for c in range(n_clusters):
col = np.random.uniform(0, 1, (3))
# ~ print(np.shape(self.xy[labels==c,0]))
plt.plot(self.xy[self.labels == c, 0],
self.xy[self.labels == c, 1],
'.',
color=col)
# ~ plt.plot(self.vel_hist, '.r')
plt.plot(self.xy[self.labels == -1, 0],
self.xy[self.labels == -1, 1],
'.',
color=[0, 0, 0])
plt.xlabel("Time")
plt.ylabel("Speed (m/s)")
plt.show(block=False)
plt.pause(0.0000000001)
# ~ print("clusters:",n_clusters)
return self.xy, self.labels, n_clusters
def on_scan(self, scan):
#print scan
#rospy.loginfo("Got scan, projecting")
cloud = self.laser_projector.projectLaser(scan)
#print cloud
#rospy.loginfo("Printed cloud")
gen = pc2.read_points(cloud,
skip_nans=True,
field_names=("x", "y", "z"))
self.xyz_generator = gen
self.xyz = np.asarray([[p[0], p[1], p[2]] for p in gen])
# ~ print(np.shape(self.xyz))
#for p in gen:
# print " x : %f y: %f z: %f" %(p[0],p[1],p[2])
def get_hull_markers(self, hulls, coms):
marker = MarkerArray()
ind = 0
for h in range(len(hulls)):
m = default_marker()
m.id = ind #car.id -- all car.ids are 0
m.lifetime = rospy.Duration(0.1)
# ~ m.header = car.header
# m.color.g = 0.72
# m.color.a = (car.id+0.0)/t
# m.scale.x = 1.0 + 5*(car.id+0.0)/t
# m.scale.y = 1.0 + 5*(car.id+0.0)/t
# ~ print("hull",hull)
m.points.append(Point(x=coms[h][0], y=coms[h][1], z=0))
for v in hulls[h].vertices:
m.points.append(
Point(x=hulls[h].points[v, 0],
y=hulls[h].points[v, 1],
z=0))
marker.markers.append(m)
ind += 1
return marker
