def build_and_publish_obstacle_point_clouds(self,
reachable_workspace_points):
"""
Builds and publishes obstacles point clouds for both the left and right Baxter arms.
Publishes obstacle clouds in the base from, as they are transformed and filtered points.
:param reachable_workspace_points: the overall environment points list, filtered and transformed to base frame,
in reachable workspace
"""
obstacle_cloud = PointCloud()
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
header.frame_id = 'base'
obstacle_cloud.header = header
left_filtered_pts = self.filter_out_arm(reachable_workspace_points,
"left")
# update collision checker obstacle list
self.left_cc.update_obstacles(left_filtered_pts)
for point in left_filtered_pts:
obstacle_cloud.points.append(Point32(point[0], point[1], point[2]))
print "publishing new left obstacle cloud!"
self.left_obs_pub.publish(obstacle_cloud)
obstacle_cloud = PointCloud()
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
header.frame_id = 'base'
obstacle_cloud.header = header
right_filtered_pts = self.filter_out_arm(reachable_workspace_points,
"right")
# update collision checker obstacle list
self.right_cc.update_obstacles(right_filtered_pts)
for point in right_filtered_pts:
obstacle_cloud.points.append(Point32(point[0], point[1], point[2]))
print "publishing new right obstacle cloud!"
self.right_obs_pub.publish(obstacle_cloud)
def match_detections():
distances = []
global legs
global faces
global pub
pc = PointCloud()
pc.header = Header(0, rospy.get_rostime(), '/map')
for m in range(len(faces)):
pc.points.append(Point32(faces[m].x, faces[m].y, 1.5))
for l in range(len(legs)):
pc.points.append(Point32(legs[l].x, legs[l].y, 1.0))
distancecol = []
for m in range(len(faces)):
# measure the distance between the detections and store them
dist = (sqrt((faces[m].x - legs[l].x)**2 +
(faces[m].y - legs[l].y)**2))
distancecol.append(dist)
distances.append(distancecol)
print "distances"
print distances
pub.publish(pc)
def lidarCallback(data):
global angle
global ekf
global p_acc
p = PointCloud()
p.points = []
p.header = data.header
p.header.frame_id = "map"
p_acc.header.stamp = data.header.stamp
p_acc.header.seq = data.header.seq
current_angle = data.angle_min
pitch = angle.data + ekf.pitch
roll = ekf.roll
for i in data.ranges:
if i < MAX_DISTANCE:
point = Point32()
yaw = current_angle+ekf.yaw
point.x = i * math.cos(yaw) * math.cos(pitch)
point.y = i * math.sin(yaw) * math.cos(pitch)
current_angle += data.angle_increment
point.z = i * math.sin(pitch)
p.points += [point]
p_acc.points += p.points
pub.publish(p)
pub_acc.publish(p_acc)
def getFrontiers(self):
if self.gmap is not None:
self.lookingForFrontiers = True
rospy.loginfo("[frontier_search] Looking for frontiers")
#First check to see if the cells are free and if we already decided they are not a frontier
cells = [x for x in range(0, len(self.gmap.data)) if self.gmap.data[x] == 0]
# and x not in self.notFrontier removed from previous line waay too slow!
#The check to see if the remaining are frontiers
frontiers = [cell for cell in cells if -1 in [self.gmap.data[val] for val in self.getNeighbors(cell)]]
#Add all cells that are not frontiers to our list of not frontiers waay too slow!
#self.notFrontier.extend([cell for cell in cells if cell not in frontiers])
#Check to see if the area around them is clear of obstacles
freeFrontiers = [frontier for frontier in frontiers if self.validFrontier(frontier)]
rospy.loginfo("[frontier_search] Found %s frontiers", len(freeFrontiers))
points = self.getPoints(freeFrontiers)
cloud = PointCloud()
h = std_msgs.msg.Header()
h.stamp = rospy.Time.now()
h.frame_id = 'map'
cloud.header = h
cloud.points = points
rospy.loginfo("[frontier_search] Publishing frontiers")
# Publish a point cloud for your viewing pleasure
self.publishFrontiers.publish(cloud)
self.lookingForFrontiers = False
return cloud
else:
rospy.loginfo("[frontier_search] No map available")
return None
def pt_cloud_setup(self, nx, ny, gx, gy):
pt_cloud = PointCloud()
pt_cloud.header = std_msgs.msg.Header()
pt_cloud.header.stamp = rospy.Time.now()
pt_cloud.header.frame_id = self.frameid
pt_cloud.points = [None] * (nx * ny)
colors = ['r', 'g', 'b']
for color in colors:
ch = Channel()
ch.name = color
ch.values = []
pt_cloud.channels.append(ch)
cnt = 0
for i1 in range(0, nx):
for i2 in range(0, ny):
pt_cloud.points[cnt] = Point(i1 * gx, i2 * gy, 0)
pt_cloud.channels[0].values.append(0.0)
pt_cloud.channels[1].values.append(0.0)
pt_cloud.channels[2].values.append(0.0)
cnt = cnt + 1
return pt_cloud
def publish_pc2(event=None):
global pc2data, ppl, p_angles, p_segs, p_segindex, laserpub, shiftscan
segid = rospy.get_param("/segid")
if pc2data == None:
pass
else:
ppl_seg = ppl[p_segindex[segid][0]:p_segindex[segid][1]]
pangle_seg = p_angles[p_segindex[segid][0]:p_segindex[segid][1]]
#print('segid ', segid, p_segindex[segid], ppl_seg)
pc2pub.publish(pc2data)
my_pc = PointCloud()
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
header.frame_id = pc2data.header.frame_id
my_pc.header = header
for pt in ppl_seg:
my_pc.points.append(pt)
print "happily publishing pointcloud"
pc_pub.publish(my_pc)
#/scan
#laserscan = laser_dummy(3)
laserscan, slopesample = convert_to_scan(ppl_seg, pangle_seg,
my_pc.header, shiftscan)
laserpub.publish(laserscan)
if slopesample > 20 or slopesample < -20:
print(' slope exceed 20 abnormal')
def get_data(self, var):
PC = PointCloud(
) # create the new object PointCloud to be published later
PC.header = var.header # copy the content of topic to another PointCloud in order to manipulate it
PC.channels = var.channels
PC.points = var.points
"""
Function to get the "3D" map
"""
for i in range((len(PC.points))):
PC.points[i].z = PC.channels[0].values[
i] / 100 # write in the z axis of the points the values of the "intensity" divided by 100 to make it readable on rviz
self.pub2.publish(PC) # publish it into the new topic
"""
Function to filtter the map and get only one point by degree
"""
index_max = PC.channels[0].values.index(max(PC.channels[0].values))
for i in range(len(PC.points)): #396
if i != index_max:
PC.points[i].x = PC.points[i].y = PC.points[i].z = 0
else:
PC.points[i].z = 0
self.pub1.publish(PC) # publish it into the new topic
def match_detections():
distances = []
global legs
global faces
global pub
pc = PointCloud()
pc.header = Header(0, rospy.get_rostime(), "/map")
for m in range(len(faces)):
pc.points.append(Point32(faces[m].x, faces[m].y, 1.5))
for l in range(len(legs)):
pc.points.append(Point32(legs[l].x, legs[l].y, 1.0))
distancecol = []
for m in range(len(faces)):
# measure the distance between the detections and store them
dist = sqrt((faces[m].x - legs[l].x) ** 2 + (faces[m].y - legs[l].y) ** 2)
distancecol.append(dist)
distances.append(distancecol)
print "distances"
print distances
pub.publish(pc)
def laser_callback(current_laser_scan):
real_angles = np.array(
[-90.0, -50.0, -30.0, -10.0, 10.0, 30.0, 50.0, 90.0])
real_angles = real_angles / 360.0 * 2 * np.pi
opening_angle = 40.0 / 360.0 * 2 * np.pi
sensor_angles = np.arange(
current_laser_scan.angle_min,
current_laser_scan.angle_max + current_laser_scan.angle_increment,
current_laser_scan.angle_increment)
sensor_ranges = np.array(current_laser_scan.ranges)
sonar_readings = np.zeros(len(real_angles))
for i in range(len(real_angles)):
sonar_readings[i] = np.min(sensor_ranges[np.where(
np.logical_and(
sensor_angles >= real_angles[i] - opening_angle / 2,
sensor_angles <= real_angles[i] + opening_angle / 2))])
pub = rospy.Publisher("robotic_games/sonar", PointCloud, queue_size=1)
output = PointCloud()
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = "robot"
output.header = header
for i in range(8):
#TODO add position of Sensor on Pioneer!
output.points.append((Point32(
np.sin(real_angles[i]) * sonar_readings[i],
np.cos(real_angles[i]) * sonar_readings[i], 0)))
pub.publish(output)
def callback(self, data):
#rospy.loginfo(rospy.get_caller_id() + "-Received %s,%s,%s,%s", data.num1, data.num2, data.num3, data.num4)
#convert data uints into 3D points
points = PointCloud()
points.header = Header()
points.header.frame_id = 'map'
point1 = Point()
point2 = Point()
point3 = Point()
point4 = Point()
point1.x = self.sensor1_x_from_origin
point1.y = self.sensor1_y_from_origin
point1.z = (self.sensor1_z_from_origin - data.num1)
point2.x = self.sensor2_x_from_origin
point2.y = self.sensor2_y_from_origin
point2.z = self.sensor2_z_from_origin - data.num2
point3.x = self.sensor3_x_from_origin
point3.y = self.sensor3_y_from_origin
point3.z = self.sensor3_z_from_origin - data.num3
point4.x = self.sensor4_x_from_origin
point4.y = self.sensor4_y_from_origin
point4.z = self.sensor4_z_from_origin - data.num4
point_list = [point1, point2, point3, point4]
points.points = point_list
self.sonar_pts.publish(points)
def aplicarFiltro(data):
global taps
global N
scan = PointCloud()
scan.header = data.header
for i in range(0, len(data.points)):
scan.points.append(
Point(data.points[i].x, data.points[i].y, data.points[i].z))
signal = [0.0] * 2 * len(data.points)
#x = [0.0]*(kFin-kIni+1)
# j = 0
for i in range(0, len(data.points)):
signal[i] = data.points[i].z
signal[i + len(data.points)] = data.points[len(data.points) - 1].z
# x[j+2*(kFin-kIni+1)] = data.ranges[i];
# j = j + 1
filtered_signal = lfilter(taps, 1.0, signal)
delay = int(0.5 * (N - 1))
for i in range(len(data.points)):
scan.points[i].z = filtered_signal[delay + i]
return scan
def cloud_build(self):
# add sonar readings (robot-local coordinate frame) to cloud
pl = Point32()
pm = Point32()
pr = Point32()
# Instanziiere leere PointCloud
cloud = PointCloud()
# filling pointcloud header
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
header.frame_id = 'base_link'
cloud.header = header
# Linke Seite
if (self.dist_left < 0.95 and self.dist_left > 0.05):
pl.x = self.dist_left + 0.05
pl.y = 0.03
pl.z = 0.0
cloud.points.append(pl)
pm.x = (self.dist_left + self.dist_right) / 2 + 0.05
pm.y = 0.0
pm.z = 0.0
cloud.points.append(pm)
# Rechte Seite punkt einfügen (x,y,z)
if (self.dist_right < 0.95 and self.dist_right > 0.05):
pr.x = self.dist_right + 0.05
pr.y = -0.03
pr.z = 0.0
cloud.points.append(pr)
# Senden
self.cloud_pub.publish(cloud)
def update_particles_with_laser(self, msg):
""" Updates the particle weights in response to the scan contained in the msg
Args:
msg (LaserScan): incoming message
"""
# Transform to cartesian coordinates
scan_points = PointCloud()
scan_points.header = msg.header
for i, range in enumerate(msg.ranges):
if range == 0:
continue
# Calculate point in laser coordinate frame
angle = msg.angle_min + i * msg.angle_increment
x = range * np.cos(angle)
y = range * np.sin(angle)
scan_points.points.append(Point32(x=x, y=y))
# Transform into base_link coordinates
scan_points = self.tf_listener.transformPointCloud('base_link', scan_points)
# For each particle...
for particle in self.particle_cloud:
# Create a 3x3 matrix that transforms points from the origin to the particle
rotmatrix = np.matrix([[np.cos(particle.theta), -np.sin(particle.theta), 0],
[np.sin(particle.theta), np.cos(particle.theta), 0],
[0, 0, 1]])
transmatrix = np.matrix([[1, 0, particle.x],
[0, 1, particle.y],
[0, 0, 1]])
mat33 = np.dot(transmatrix, rotmatrix)
# Iterate through the points in the laser scan
probabilities = []
for point in scan_points.points:
# Move the point onto the particle
xy = np.dot(mat33, np.array([point.x, point.y, 1]))
# Figure out the probability of that point
distToWall = self.occupancy_field.get_closest_obstacle_distance(xy.item(0), xy.item(1))
if np.isnan(distToWall):
continue
probabilities.append(self.laser_uncertainty_model(distToWall))
# Combine those into probability of this scan given hypothesized location
# This is the bullshit thing Paul showed
# TODO: exponent should be a rosparam
totalProb = np.sum([p ** 3 for p in probabilities]) / len(probabilities)
# Update the particle's probability with new info
particle.w *= totalProb
# Normalize particles
self.normalize_particles()
def calcularPosicaoTanque(data):
iMin,iMax, estado = calcularLimitesSolda(data)
if estado != 3:
return -1
dado = PointCloud()
dado.header = data.header
parteEsquerda = PointCloud()
parteEsquerda.header = data.header
for i in range (0,min(iMax,iMin)+1):
parteEsquerda.points.append(Point(data.points[i].x,0,data.points[i].z))
A,B = determinarCoeficientes(parteEsquerda)
C = calcularParametroTanque(A,B)
for i in range(0,len(parteEsquerda.points)):
ponto_x = data.points[i].x
ponto_z = C[0][0] + C[1][0]*data.points[i].x + C[2][0]*(data.points[i].x**2)
dado.points.append(Point(ponto_x,0,ponto_z))
if min(iMin,iMax)>4 and max(iMin,iMax) < len(data.points)-4:
a,b = estimarLinha(data.points[min(iMin,iMax)-4],data.points[max(iMin,iMax)+4])
else:
a,b = estimarLinha(data.points[min(iMin,iMax)],data.points[max(iMin,iMax)])
for i in range (min(iMin,iMax),max(iMin,iMax)):
dado.points.append(Point(data.points[i].x,0,a*data.points[i].x+b))
parteDireita = PointCloud()
parteDireita.header = data.header
for i in range (max(iMax,iMin),len(data.points)):
parteDireita.points.append(Point(data.points[i].x,0,data.points[i].z))
A,B = determinarCoeficientes(parteDireita)
C = calcularParametroTanque(A,B)
for i in range(max(iMax,iMin),len(data.points)):
ponto_x = data.points[i].x
ponto_z = C[0][0] + C[1][0]*data.points[i].x + C[2][0]*(data.points[i].x**2)
dado.points.append(Point(ponto_x,0,ponto_z))
return dado
def points3d_to_pcl(self, points3d):
pcl = PointCloud()
#transforms the points3d list into a pointcloud
pcl.header = self.recognition_header
pcl.points = points3d
return pcl
def testecalcularCentroSolda(scan):
iMin,iMax,estado = calcularLimitesSolda(scan)
if estado != 3:
return -1
centro = PointCloud()
centro.header = scan.header
centro.points.append(Point(scan.points[int(round((iMin+iMax)/2))].x,0,0))
return centro
def determinarSinal(scan,tanque):
dado = PointCloud()
dado.header = scan.header
for i in range(0,len(scan.points)):
ponto_x = scan.points[i].x
ponto_z = (scan.points[i].z - tanque.points[i].z)
dado.points.append(Point(ponto_x,0,ponto_z))
return dado
def points3d_to_pcl(self, points3d):
pcl = PointCloud()
#transforms the points3d list into a pointcloud
pcl.header = self.recognition_header
pcl.points = points3d
return pcl
def PointCloud2_to_PointCloud(pc):
assert isinstance(pc, PointCloud2)
xyz = pc2xyz(pc)
msg = PointCloud()
msg.header = pc.header
msg.points = [Point32(x,y,z) for (x,y,z) in xyz[0,:,:]]
return msg
def create_point_cloud(points):
point_cloud = PointCloud()
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
header.frame_id = 'map'
point_cloud.header = header
for point in points:
point_cloud.points.append(Point32(point[0], point[1], point[2]))
return point_cloud
def transform_polygon(self, polygon, frame_target):
""" Use TF Listener to transform the points in a
PolygonStamped into a different TF frame...via a PointCloud
msg, which is the output data type. """
cloud = PointCloud()
cloud.header = polygon.header
cloud.points = polygon.polygon.points
self.listener.waitForTransform(frame_target, cloud.header.frame_id, cloud.header.stamp, rospy.Duration(1.0))
return self.listener.transformPointCloud(frame_target, cloud)
def get_bounding_box(self, points):
""" simple axis-aligned bounding box"""
world_frame = "odom_combined"
# get pointcloud in world frame
if not self.listener.frameExists(points.header.frame_id):
rospy.logerr("point cloud header frame %s does not exist!",
points.header.frame_id)
return
if not self.listener.frameExists(world_frame):
rospy.logerr("world frame %s does not exist!", world_frame)
return
pc = PointCloud()
pc.header = points.header
iter_pt = pts.read_points(points)
for pt in iter_pt:
pc.points.append(Point(pt[0], pt[1], pt[2]))
# TODO: technically, want to transform at time image was taken, but thanks
# to waiting for human input, will be arbitrary delay ...
pc.header.stamp = self.listener.getLatestCommonTime(
world_frame, pc.header.frame_id)
# TODO: should be wrapped in try/except block
pc_world = self.listener.transformPointCloud(world_frame, pc)
# get world-axis-aligned bounding box
min_x = sys.float_info.max
max_x = sys.float_info.min
min_y = sys.float_info.max
max_y = sys.float_info.min
min_z = sys.float_info.max
max_z = sys.float_info.min
for pt in pc_world.points:
min_x = min(min_x, pt.x)
max_x = max(max_x, pt.x)
min_y = min(min_y, pt.y)
max_y = max(max_y, pt.y)
min_z = min(min_z, pt.z)
max_z = max(max_z, pt.z)
# testing whether fudging it helps
#min_z = min_z + 0.05
# add bounding box to the planning scene
obj_pose = PoseStamped()
obj_pose.pose.position.x = (min_x + max_x) / 2
obj_pose.pose.position.y = (min_y + max_y) / 2
obj_pose.pose.position.z = (min_z + max_z) / 2
obj_pose.pose.orientation.z = 1.0
obj_pose.header.frame_id = world_frame
obj_dims = Vector3()
obj_dims.x = max_x - min_x
obj_dims.y = max_y - min_y
obj_dims.z = max_z - min_z
return (obj_pose, obj_dims)
def GetLocalPC(self, data):
pc2_msg = self.lp.projectLaser(data)
point_generator = pc2.read_points(pc2_msg)
pc_msg = PointCloud()
pc_msg.header = pc2_msg.header
pc_msg.channels.append(ChannelFloat32())
for p in point_generator:
pc_msg.points.append(Point32(p[0], p[1], p[2]))
pc_msg.channels[0].values.append(0)
return pc_msg
def sub_cb(self, msg):
pnt = PointCloud()
pnt.header = msg.header
angle_step = 1.0 / (msg.range * 100.0)
angle = -msg.field_of_view
if msg.range < msg.max_range and msg.range > msg.min_range:
while angle < msg.field_of_view:
pnt.points.append(Point32(msg.range * cos(angle), msg.range * sin(angle), 0))
angle += angle_step
self.pub.publish(pnt)
def sub_cb(self, msg):
pnt = PointCloud()
pnt.header = msg.header
angle_step = 1.0/(msg.range*100.0)
angle = -msg.spread_angle
if msg.range < msg.range_max and msg.range > msg.range_min:
while angle < msg.spread_angle:
pnt.points.append(Point32(msg.range*cos(angle), msg.range*sin(angle), 0))
angle += angle_step
self.pub.publish(pnt)
def create_pointcload(points, res):
obscacles_cloud = PointCloud()
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
header.frame_id = '/simulator'
obscacles_cloud.header = header
for p in points:
obscacles_cloud.points.append(
Point32(p[0] * res, p[1] * res, p[2] * res))
return obscacles_cloud
def get_bounding_box(self, points):
""" simple axis-aligned bounding box"""
world_frame = "odom_combined"
# get pointcloud in world frame
if not self.listener.frameExists(points.header.frame_id):
rospy.logerr("point cloud header frame %s does not exist!", points.header.frame_id)
return
if not self.listener.frameExists(world_frame):
rospy.logerr("world frame %s does not exist!", world_frame)
return
pc = PointCloud()
pc.header = points.header
iter_pt = pts.read_points(points)
for pt in iter_pt:
pc.points.append(Point(pt[0], pt[1], pt[2]))
# TODO: technically, want to transform at time image was taken, but thanks
# to waiting for human input, will be arbitrary delay ...
pc.header.stamp = self.listener.getLatestCommonTime(world_frame, pc.header.frame_id)
# TODO: should be wrapped in try/except block
pc_world = self.listener.transformPointCloud(world_frame, pc)
# get world-axis-aligned bounding box
min_x = sys.float_info.max
max_x = sys.float_info.min
min_y = sys.float_info.max
max_y = sys.float_info.min
min_z = sys.float_info.max
max_z = sys.float_info.min
for pt in pc_world.points:
min_x = min(min_x, pt.x)
max_x = max(max_x, pt.x)
min_y = min(min_y, pt.y)
max_y = max(max_y, pt.y)
min_z = min(min_z, pt.z)
max_z = max(max_z, pt.z)
# testing whether fudging it helps
min_z = min_z + 0.05
# add bounding box to the planning scene
obj_pose = PoseStamped()
obj_pose.pose.position.x = (min_x + max_x)/2
obj_pose.pose.position.y = (min_y + max_y)/2
obj_pose.pose.position.z = (min_z + max_z)/2
obj_pose.pose.orientation.z = 1.0
obj_pose.header.frame_id = world_frame
obj_dims = Vector3()
obj_dims.x = max_x - min_x
obj_dims.y = max_y - min_y
obj_dims.z = max_z - min_z
return (obj_pose, obj_dims)
def process_scan(self, m):
pcloud = PointCloud()
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
header.frame_id = "map"
pcloud.header = header
lidar_range_rad = np.deg2rad(270)
step = 8
theta = np.deg2rad(self.current_angle)
for i in range(0, len(m.ranges), step):
d = m.ranges[i]
phi = np.deg2rad(Node.translate(i, 0.0, float(len(m.ranges)), 0.0, 270)) - 0.7843
xyz = Node.spherical_to_cartesian(d, theta, phi)
pcloud.points.append(Point32(xyz[0], xyz[1], xyz[2]))
if self.print_once:
if not self.printed:
time.sleep(2)
xyzs = []
xs = []
ys = []
zs = []
phis = []
for i in range(len(m.ranges)):
phi = np.deg2rad(Node.translate(i, 0.0, float(len(m.ranges)), 0.0, 270)) - 0.7843
phis.append(np.rad2deg(phi))
d = m.ranges[i]
xyz = Node.spherical_to_cartesian(d, theta, phi)
xyzs.append(xyz)
xs.append(xyz[0])
ys.append(xyz[1])
zs.append(xyz[2])
f, axarr = plt.subplots(2, 2)
axarr[0, 0].plot(phis, m.ranges)
axarr[0, 0].set_title("phi vs distance")
axarr[0, 1].plot(phis, xs)
axarr[0, 1].set_title("phi vs X values")
# axarr[0,1].text(0,0,"x = r * np.sin(theta) * np.cos(phi)",fontsize=12)
axarr[1, 0].plot(phis, zs)
axarr[1, 0].set_title("phi vs Z values")
plt.show()
self.printed = True
print("done plotting")
# print Node.pp_list(m.ranges)
try:
self.publisher.publish(pcloud)
except ROSException:
pass
def sub_cb(self, msg):
pnt = PointCloud()
pnt.header = msg.header
angle_step = 1.0 / (msg.range * 100.0)
angle = -msg.spread_angle
if msg.range < msg.range_max and msg.range > msg.range_min:
while angle < msg.spread_angle:
pnt.points.append(
Point32(msg.range * cos(angle), msg.range * sin(angle), 0))
angle += angle_step
self.pub.publish(pnt)
def sub_cb(self, msg):
pnt = PointCloud()
pnt.header = msg.header
angle_step = 1.0 / (msg.range * 100.0)
angle = -msg.field_of_view
if msg.range < msg.max_range and msg.range > msg.min_range:
while angle < msg.field_of_view:
pnt.points.append(
Point32(msg.range * cos(angle), msg.range * sin(angle), 0))
angle += angle_step
self.pub.publish(pnt)
def generatePointCloud(obstacleCart):
global cloud
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = "laser"
numberOfPoints = len(obstacleCart)
cloud = PointCloud()
cloud.header = h
point = []
for i in range(0,numberOfPoints):
cloud.points.append(Point32( obstacleCart[i][0], obstacleCart[i][1], 0 ))
return cloud
def publish_tracking(points, odom_tracker_key):
msg = PointCloud()
msg.points = []
msg.header = Header()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = 'odom'
for p in points:
msg.points.append(Point(p.x, p.y, 0))
odom_out_publishers[odom_tracker_key].publish(msg)
def GetUnifiedPointcloud(self, pointcloud_list):
pointcloudUnified = PointCloud()
pointcloudUnified.points = []
pointcloudUnified.channels = []
if len(pointcloud_list) > 0:
pointcloudUnified.header = pointcloud_list[0].header # They all have different headers, but just use the first one.
for pointcloud in pointcloud_list:
pointcloudUnified.points.extend(pointcloud.points)
pointcloudUnified.channels.extend(pointcloud.channels)
return pointcloudUnified
def pointToPointcloud(
self, x, y, z
): # FROM -> https://answers.ros.org/question/207071/how-to-fill-up-a-pointcloud-message-with-data-in-python/?answer=218951#post-id-218951
my_awesome_pointcloud = PointCloud() # declaring pointcloud
#filling pointcloud header
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = 'map'
my_awesome_pointcloud.header = header # assign header to pointcloud
my_awesome_pointcloud.points.append(Point32(x, y,
z)) #filling some points
self.pointcloud_publisher.publish(my_awesome_pointcloud) # publish
def determinarDerivada(scan):
derivada = PointCloud()
derivada.header = scan.header
derivada.points = [Point(0,0,0)]*(len(scan.points)-1)
for i in range(0,len(derivada.points)):
ponto_x = scan.points[i].x
ponto_y = 0
if math.fabs(scan.points[i+1].x-scan.points[i].x) > 0.000001:
ponto_z = (scan.points[i+1].z-scan.points[i].z)/(scan.points[i+1].x-scan.points[i].x)
else:
ponto_z = 0
derivada.points[i] = Point(ponto_x,ponto_y,ponto_z)
return derivada
def publish_frontiers(self):
point_cloud = PointCloud()
point_cloud.header = std_msgs.msg.Header()
point_cloud.header.stamp = rospy.Time.now()
point_cloud.header.frame_id = '/orb_slam/map'
off_x, off_y = self.grid.info.origin.position.x, self.grid.info.origin.position.y
point_cloud.points = [Point32(
(x+.5) * self.grid.info.resolution + off_x,
(y+.5) * self.grid.info.resolution + off_y, 0.0
) for (y, x) in self.frontiers
]
publisher = OneTimePublisher("exploration_frontiers", PointCloud, point_cloud)
publisher.start()
def lidarinfo(data):
p = PointCloud()
p.points = []
p.header = data.header
current_angle = data.angle_min
for i in data.ranges:
point = Point32()
point.x = i * math.cos(current_angle)
point.y = i * math.sin(current_angle)
current_angle += data.angle_increment
point.z = 0
p.points += [point]
pub.publish(p)
def __publishPointCloud(self, x, y, z, publ, step=100):
pointcloud = PointCloud()
header = std_msgs.msg.Header()
header.stamp = self.stamp_now #rospy.Time.now()
header.frame_id = 'camera_depth_optical_frame'
#header.frame_id = 'camera_link'
pointcloud.header = header
for i in range(0, len(x), step):
pointcloud.points.append(Point32(x[i], y[i], z[i]))
publ.publish(pointcloud)
def get_lidar_pcd(self, data):
lidar_pcd = PointCloud()
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
header.frame_id = 'lidar'
lidar_pcd.header = header
for idx in range(data.shape[0]):
lidar_pcd.points.append(
Point32(data[idx, 0], data[idx, 1], data[idx, 2]))
return lidar_pcd
def GetUnifiedPointcloud(self, pointcloud_list):
pointcloudUnified = PointCloud()
pointcloudUnified.points = []
pointcloudUnified.channels = []
if len(pointcloud_list) > 0:
pointcloudUnified.header = pointcloud_list[0].header # They all have different headers, but just use the first one.
pointcloudUnified.channels.append(ChannelFloat32(name='intensity', values=[]))
for pointcloud in pointcloud_list:
pointcloudUnified.points.extend(pointcloud.points)
pointcloudUnified.channels[0].values.extend(pointcloud.channels[0].values)
return pointcloudUnified
def get_data(self, var):
PC = PointCloud(
) # create the new object PointCloud to be published later
PC.header = var.header # copy the content of topic to another PointCloud in order to manipulate it
PC.channels = var.channels
PC.points = var.points
for i in range((len(PC.points))):
PC.points[i].z = PC.channels[0].values[
i] / 100 # write in the z axis of the points the values of the "intensity" divided by 100 to make it readable on rviz
self.pub.publish(PC) # publish it into the new topic
def publish_points(self, pcloud, seq):
pc = PointCloud()
pc.header = Header(seq, rospy.Time.now(), 'points')
for i in range(len(pcloud[:, 0])):
# Publishing as (z, x, y) b/c of the way that Z is forward in
# triangulation
pc.points.append(Point32(pcloud[i, 2],
pcloud[i, 0],
pcloud[i, 1]))
self.point_pub.publish(pc)
self.tf_broadcaster.sendTransform((0, 0, 0),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
'points',
'map')
def gmm_cloud_publish(self, data):
h = std_msgs.msg.Header()
h.stamp = rospy.Time.now()
h.frame_id = "map"
pcl = PointCloud()
pcl.header = h
pcl.points = []
pcl.channels = []
for i in range(0, data.shape[1]):
point = Point32()
point.x = data[0,i]
point.y = data[1,i]
point.z = data[2,i]
pcl.points.append(point)
self.gmmPub.publish(pcl)
def main():
viz_pub = rospy.Publisher('/cluster_boxes', Marker)
cloud_pub = rospy.Publisher('/cluster_points', PointCloud)
segmentation_service = rospy.ServiceProxy(SEGMENTATION_SERVICE, TabletopSegmentation)
rospy.loginfo('Waiting for object recognition service')
segmentation_service.wait_for_service()
rospy.loginfo('Found segmentation service!')
box_service = rospy.ServiceProxy('/find_cluster_bounding_box', FindClusterBoundingBox)
rospy.loginfo('Waiting for find cluster bounding box service')
box_service.wait_for_service()
rospy.loginfo('Found cluster bounding box service!')
req = TabletopSegmentationRequest()
resp = segmentation_service(req)
rospy.loginfo('Found %d clusters', len(resp.clusters))
rospy.loginfo('Table pose is:\n' + str(resp.table.pose))
table = marker_at(resp.table.pose, mtype = Marker.TRIANGLE_LIST, ns = 'table', sx = 1.0,
sy = 1.0, sz = 1.0, r = 0.0, g = 1.0, b = 0.0, a = 0.4)
for tri in resp.table.convex_hull.triangles:
for ind in tri.vertex_indices:
table.points.append(resp.table.convex_hull.vertices[ind])
viz_pub.publish(table)
pc = PointCloud()
pc.header = resp.table.pose.header
for (mid, cluster) in enumerate(resp.clusters):
box_resp = box_service.call(FindClusterBoundingBoxRequest(cluster = cluster))
marker = marker_at(
box_resp.pose, ns = str(mid), mid = mid, mtype = Marker.CUBE, sx = box_resp.box_dims.x, sy = box_resp.box_dims.y,
sz = box_resp.box_dims.z, a = 0.5)
viz_pub.publish(marker)
pose = PoseStamped()
pose.header = cluster.header
pose.pose.orientation.w = 1.0
marker = marker_at(pose, ns = 'cluster', mid = mid, mtype = Marker.POINTS, sx = 1.0,
sy = 1.0, sz = 1.0, r = 1.0, g = 0.0, b = 0.0, a = 1.0)
marker.points = cluster.points
#viz_pub.publish(marker)
pc.points += cluster.points
pc.channels += cluster.channels
rospy.loginfo('Cluster frame is ' + cluster.header.frame_id)
cloud_pub.publish(pc)
#!/usr/bin/env python
import rospy
import numpy as np
import tf
import mil_ros_tools
from sensor_msgs.msg import PointCloud
rospy.init_node("ground_finder")
pub = rospy.Publisher("ground_est", PointCloud, queue_size=1)
listener = tf.TransformListener()
pc = PointCloud()
pc.header = mil_ros_tools.make_header(frame="map")
pc.points = []
rate = rospy.Rate(1.0)
while not rospy.is_shutdown():
try:
t = listener.waitForTransform('/map', '/ground', rospy.Time.now(), rospy.Duration(1))
(trans, rot) = listener.lookupTransform('/map', '/ground', rospy.Time(0))
except (tf.Exception, tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.logwarn("TF waitForTransform timeout")
continue
pc.points.append(mil_ros_tools.numpy_to_point(np.array(trans)))
pub.publish(pc)
rate.sleep()
while not rospy.is_shutdown():
PC.header.seq = counter
PC.header.stamp = rospy.Time.now()
counter += 1
pub.publish(PC)
br.sendTransform((0, 0, 0),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
'points',
'map')
r.sleep()
if __name__ == '__main__':
rospy.init_node('talker', anonymous=True)
P = PointCloud()
P.header = Header(0, 0, 'points')
P.points.append(Point32(2, 0, 0)); P.points.append(Point32(3, 0, 0))
P.points.append(Point32(4, 0, 0)); P.points.append(Point32(5, 0, 0))
P.points.append(Point32(0, 3, 0)); P.points.append(Point32(0, 6, 0))
P.points.append(Point32(0, 6.5, 0)); P.points.append(Point32(0, 7, 0))
P.points.append(Point32(0, 0, 2)); P.points.append(Point32(0, 0, -2))
P.points.append(Point32(0, 0, 1)); P.points.append(Point32(0, 0, -1))
# for i in range(2):
# P.points.append(Point32(i, i, i))
talker(P)
