def scan_points_cb(self, points):
self.broadcast_transform()
# Turn laser data into 2d points, republish for debugging purposes
# http://answers.ros.org/question/115136/python-pointcloud2-read_points-problem/
projector = laser_geometry.LaserProjection()
points2 = projector.projectLaser(points, 2.0)
data_out = pc2.read_points(points2, field_names=None, skip_nans=True)
self.cloud_pub.publish(points2)
# We can only transfrom the laser scan into 2d points in the same frame,
# so we need to bring the GameZoneBox to us. This should never be changing,
# So only do the calculation once and save it.
if not self.shifted_points:
try:
now = rospy.Time.now()
older = now - rospy.Duration(.01)
self._listener.waitForTransform("/laser", "/GameZoneBox", now,
rospy.Duration(1.0))
except Exception as e:
print e
print "F**K F**K F**K F**K F**K"
stamped_points = [
PointStamped(header=Header(stamp=now, frame_id="/GameZoneBox"),
point=pt) for pt in self._points
]
self.shifted_points = [
self._listener.transformPoint("/laser", pt)
for pt in stamped_points
]
# Are there any points in the box?
# http://stackoverflow.com/q/8833950
box_path = path.Path([(p.point.x, p.point.y)
for p in self.shifted_points])
pts = [(p[0], p[1]) for p in data_out]
pts_sum = sum(box_path.contains_points(pts))
if pts_sum > 10 and not self.person_found:
self.person_found = True
self.person_pub.publish(Bool(True))
elif pts_sum == 0 and self.person_found:
self.person_found = False
self.person_pub.publish(Bool(False))
def make_PC_from_Laser_display(laser_in):
# Initialize a point cloud object
pc_out = PointCloud()
# Converts the message from a LaserScan to a Point Cloud
projection = lp.LaserProjection()
cloud = projection.projectLaser(laser_in) # ,channel_options = 0x04)
# Convert it to individual points
cloud = pc.read_points(cloud,
field_names=("x", "y", "z")) # ,"distances"))
cloud = list(cloud)
pc_out.header = copy.deepcopy(laser_in.header)
pc_out.channels.append(ChannelFloat32())
pc_out.channels[0].name = "intensity"
# Format each Point Cloud into a x,y,z coordinates
for cc in cloud:
pc_out.points.append(Point(cc[0], cc[1], cc[2]))
pc_out.channels[0].values.append(.99)
return pc_out
emptyVelocity.x = 0
emptyVelocity.y = 0
emptyVelocity.z = 0
arr = [0 for _ in range(9)]
pubVal.orientation_covariance = arr
pubVal.angular_velocity_covariance = arr
pubVal.linear_acceleration_covariance = arr
pubVal.angular_velocity = emptyVelocity
pubVal.linear_acceleration = emptyVelocity
br = tf.TransformBroadcaster()
lp = lg.LaserProjection()
lines = []
rospy.init_node("yahyeet", anonymous=True)
rate = rospy.Rate(100)
pub = rospy.Publisher('/sync_scan_cloud_filtered',
PointCloud2,
queue_size=10)
pub2 = rospy.Publisher('/imu/data', Imu, queue_size=1)
print("here1")
hebiForLidar = hebi_control.HebiForLidar(2.5, 3.14159265 / 2,
-3.14159265 / 2, -1.5,
'D8:80:39:EE:C1:CD')
def callback(laser_scan_msg):
point = laser_geometry.LaserProjection()
pc2 = point.projectLaser(laser_scan_msg, -1.0, point.ChannelOption.DEFAULT)
pub = rospy.Publisher('~output', PointCloud2, queue_size=10)
pub.publish(pc2)