def callback(data):
global left
global right
#global frame
img = bridge.imgmsg_to_cv2(data)
image = process_image(img)
image = bridge.cv2_to_imgmsg(image)
pub_image.publish(image)
left_points, right_points = pub_scatter(left, right)
left_msg = PointCloud()
left_msg.header.stamp = rospy.Time.now()
left_msg.header.frame_id = "footprint"
left_msg.points = left_points
left_msg_chan = ChannelFloat32()
left_msg_chan.name = 'value'
left_msg_chan.values = [float(3)]
left_msg.channels = [left_msg_chan]
right_msg = PointCloud()
right_msg.header.stamp = rospy.Time.now()
right_msg.header.frame_id = "footprint"
right_msg.points = right_points
right_msg_chan = ChannelFloat32()
right_msg_chan.name = 'value'
right_msg_chan.values = [float(3)]
right_msg.channels = [right_msg_chan]
pub_point_cloud_right.publish(right_msg)
pub_point_cloud_left.publish(left_msg)
def convert_pointcloud_to_ROS(pts3d, intensities = None, labels = None, colors=None, bgr=True):
ROS_pointcloud = PointCloud()
ROS_pointcloud.points = []
ROS_pointcloud.channels = []
(x,y,z) = (0,1,2)
for pt in np.asarray(pts3d.T):
ROS_pointcloud.points += [Point32(pt[x], pt[y], pt[z])]
intensity_channel = ChannelFloat32(name = 'intensities')
if intensities != None:
for value in intensities:
intensity_channel.values += [value]
ROS_pointcloud.channels += [intensity_channel]
label_channel = ChannelFloat32(name = 'labels')
if labels != None:
for value in labels:
label_channel.values += [value]
ROS_pointcloud.channels += [label_channel]
if colors != None:
r_ch = ChannelFloat32(name='r')
g_ch = ChannelFloat32(name='g')
b_ch = ChannelFloat32(name='b')
rgb_ch = ChannelFloat32(name='rgb')
for (r,g,b) in np.asarray(colors.T):
#NOTE: CV image is in bgr order. Ros expects rgb order
r_ch.values += [b/256]
g_ch.values += [g/256]
b_ch.values += [r/256]
rgb_color = int(b*0xFF*0xFF+ g*0xFF+r) #incorrect
rgb_ch.values += [rgb_color]
ROS_pointcloud.channels += [r_ch, g_ch, b_ch]
return ROS_pointcloud
def local_rand_cloud():
#Create a new cloud object and stuff it
ROS_pointcloud = PointCloud() #sensor_msgs.msg.PointCloud()
x = random()*5 #const offset easy to change.
#stuff point cloud with random points in a rectangle which drifts over time.
#Add an extra intensity channel as well.
intensity_channel = ChannelFloat32()
intensity_channel.name = 'intensities'
for i in range(3000):
intensity_channel.values += [random()]
for i in range(3000):
ROS_pointcloud.points += [Point32(random()*5-x,random(), intensity_channel.values[i])]
#rgb color has maximum value of 0xFFFFFF
rgb_channel = ChannelFloat32()
rgb_channel.name = 'rgb'
rgb_channel.values = normList(intensity_channel.values, 0xFF)
#Separate r,g,b channels range betweeon 0 and 1.0.
r, g, b = [], [], []
for pt in ROS_pointcloud.points:
b += [pt.y];
r += [pt.z];
g += [0];
r_channel = ChannelFloat32(name='r', values = normList(r) )
g_channel = ChannelFloat32(name='g', values = normList(g) )
b_channel = ChannelFloat32(name='b', values = b)
ROS_pointcloud.channels = (intensity_channel, rgb_channel, r_channel, g_channel, b_channel)
return ROS_pointcloud
def to_pointcloud(self, frame):
"""Returns a PointCloud message corresponding to slice.
Args:
frame: Frame ID.
Returns:
A sensor_msgs.msg.PointCloud.
"""
# Construct PointCloud message.
cloud = PointCloud()
cloud.header.frame_id = frame
cloud.header.stamp = self.timestamp
# Convert bins to list of Point32 messages.
nbins = self.config["nbins"]
r_step = self.range / nbins
x_unit = math.cos(self.heading) * r_step
y_unit = math.sin(self.heading) * r_step
cloud.points = [
Point32(x=x_unit * r, y=y_unit * r, z=0.00)
for r in range(1, nbins + 1)
]
# Set intensity channel.
channel = ChannelFloat32()
channel.name = "intensity"
channel.values = self.bins
cloud.channels = [channel]
return cloud
def local_rand_cloud():
#Create a new cloud object and stuff it
ROS_pointcloud = PointCloud() #sensor_msgs.msg.PointCloud()
x = random() * 5 #const offset easy to change.
#stuff point cloud with random points in a rectangle which drifts over time.
#Add an extra intensity channel as well.
intensity_channel = ChannelFloat32()
intensity_channel.name = 'intensities'
for i in range(3000):
intensity_channel.values += [random()]
for i in range(3000):
ROS_pointcloud.points += [
Point32(random() * 5 - x, random(), intensity_channel.values[i])
]
#rgb color has maximum value of 0xFFFFFF
rgb_channel = ChannelFloat32()
rgb_channel.name = 'rgb'
rgb_channel.values = normList(intensity_channel.values, 0xFF)
#Separate r,g,b channels range betweeon 0 and 1.0.
r, g, b = [], [], []
for pt in ROS_pointcloud.points:
b += [pt.y]
r += [pt.z]
g += [0]
r_channel = ChannelFloat32(name='r', values=normList(r))
g_channel = ChannelFloat32(name='g', values=normList(g))
b_channel = ChannelFloat32(name='b', values=b)
ROS_pointcloud.channels = (intensity_channel, rgb_channel, r_channel,
g_channel, b_channel)
return ROS_pointcloud
def create_point_cloud(self, sortedRewards, min_len):
c = PointCloud()
c.header.seq = 1
c.header.stamp = rospy.Time.now()
c.header.frame_id = '/map'
c.points = []
channel = ChannelFloat32()
channel.name = "Values"
channel.values = []
c.channels = [channel]
for element in sortedRewards[0:min_len]:
p = Point()
x = self.map_msg.info.origin.orientation.x
y = self.map_msg.info.origin.orientation.y
z = self.map_msg.info.origin.orientation.z
w = self.map_msg.info.origin.orientation.w
roll, pitch, yaw = euler_from_quaternion((x, y, z, w))
offset_x = self.map_msg.info.origin.position.x
offset_y = self.map_msg.info.origin.position.y
p.y = (element[0] * np.cos(yaw) + element[1] *
np.sin(yaw)) * self.map_msg.info.resolution + offset_y
p.x = (element[1] * np.cos(yaw) - element[0] *
np.sin(yaw)) * self.map_msg.info.resolution + offset_x
c.points.append(p)
channel.values.append(element[2])
return c
def create_point_cloud(self, xs, ys, vals):
c = PointCloud()
c.header.seq = 1
c.header.stamp = rospy.Time.now()
c.header.frame_id = '/map'
c.points = []
channel = ChannelFloat32()
channel.name = "Values"
channel.values = []
c.channels = [channel]
for i in range(len(xs)):
p = Point()
x = self.map_msg.info.origin.orientation.x
y = self.map_msg.info.origin.orientation.y
z = self.map_msg.info.origin.orientation.z
w = self.map_msg.info.origin.orientation.w
roll, pitch, yaw = euler_from_quaternion((x, y, z, w))
offset_x = self.map_msg.info.origin.position.x
offset_y = self.map_msg.info.origin.position.y
p.y = (xs[i] * np.cos(yaw) + ys[i] *
np.sin(yaw)) * self.map_msg.info.resolution + offset_y
p.x = (ys[i] * np.cos(yaw) - xs[i] *
np.sin(yaw)) * self.map_msg.info.resolution + offset_x
c.points.append(p)
channel.values.append(vals[i])
return c
def to_pointcloud(self, frame):
"""Returns a PointCloud message corresponding to slice.
Args:
frame: Frame ID.
Returns:
A sensor_msgs.msg.PointCloud.
"""
# Construct PointCloud message.
cloud = PointCloud()
cloud.header.frame_id = frame
cloud.header.stamp = self.timestamp
# Convert bins to list of Point32 messages.
nbins = self.config["nbins"]
r_step = self.range / nbins
x_unit = math.cos(self.heading) * r_step
y_unit = math.sin(self.heading) * r_step
cloud.points = [
Point32(x=x_unit * r, y=y_unit * r, z=0.00)
for r in range(1, nbins + 1)
]
# Set intensity channel.
channel = ChannelFloat32()
channel.name = "intensity"
channel.values = self.bins
cloud.channels = [channel]
return cloud
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 _callback(self, data):
self.transformer.waitForTransform('odom_combined', 'map', Time.now(),
rospy.Duration(2))
new_data = PointCloud()
new_data.header.stamp = Time.now()
new_data.header.frame_id = 'odom_combined'
new_data.points = [
self._map_to_odom_combined(p, data.header.stamp)
for p in data.points
]
new_data.channels = data.channels
self.publisher.publish(new_data)
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 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 fakeDetect(self):
pc = PointCloud()
pc.header.stamp = rospy.Time.now()
pc.header.frame_id = 'odom'
pc.channels = []
pc.points.append(
Point32(self.bagHandlePoint[0], self.bagHandlePoint[1],
self.bagHandlePoint[2]))
self.pub.publish(pc)
time.sleep(0.5)
return self.transform3dToImg([self.bagHandlePoint])
def cloud_pub(self):
#create point cloud object
cloud = PointCloud()
cloud.header.stamp = rospy.Time.now() #time stamp
cloud.header.frame_id = 'map'
cloud.points = self.points #Cartesian points
channel = ChannelFloat32()
channel.name = 'intensity'
channel.values = self.inty_vals #rcs intensities
cloud.channels = [channel]
self.cloud_topic.publish(cloud) #publish point cloud object
#reset points and intesnities
self.points = []
self.inty_vals = []
def np_points_to_ros(pts):
p_list = []
chlist = []
# p_list = [Point32(p[0,0], p[0,1], p[0,2]) for p in pts.T]
# chlist = np.zeros(pts.shape[1]).tolist()
for p in pts.T:
p_list.append(Point32(p[0,0],p[0,1],p[0,2]))
chlist.append(0.)
ch = ChannelFloat32('t',chlist)
pc = PointCloud()
pc.points = p_list
pc.channels = [ch]
return pc
def broadcaster_particles(self):
# Part for export point clound
message = PointCloud()
message.header.stamp = rospy.get_rostime()
message.header.frame_id = self.parent_frame
message.channels = []
message.channels.append(ChannelFloat32())
message.channels[0].name = "intensities"
if self.direct_to_parent:
message.points = array_to_point32(self.filter.particles)
else:
message.points = array_to_point32(self.filter.particles +
self.state)
message.channels[0].values = tuple(self.filter.weights)
self.pub_particle.publish(message)
def _make_ros_point_cloud(self):
ros_point_cloud = PointCloud()
ros_point_cloud.header.seq = 1
ros_point_cloud.header.frame_id = self.frame_id
ros_point_cloud.header.stamp = rospy.Time.now()
ros_point_cloud.points = []
ros_point_cloud.channels = [ChannelFloat32()]
ros_point_cloud.channels[0].name = "obstacle_id"
ros_point_cloud.channels[0].values = []
for point in self.points_set:
new_point32 = Point32()
new_point32.x = point[0]
new_point32.y = point[1]
ros_point_cloud.points.append(new_point32)
ros_point_cloud.channels[0].values.append(self.obstacle_id)
return ros_point_cloud
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 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 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 talker():
pub = rospy.Publisher('pc_test', PointCloud, queue_size=10)
rospy.init_node('publish_pc')
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
pc = PointCloud()
pc.header.stamp = rospy.Time.now()
pc.header.frame_id = 'map'
numPoints = 3
pc.channels = []
for i in range(numPoints):
pc.points.append(Point32(i, i, 0))
pub.publish(pc)
print pc
rate.sleep()
def calcPCD(self, depthImage): # returns a PointCloud object
self.ID += 1
print(self.ID)
#############################
# getting the RGB image from the scene topic
# imageScene = rospy.Subscriber(
# '/airsim_node/PhysXCar/front_left_bumblebee/Scene', Image, self.getRGB, queue_size=1) # queue size of length 1 i.e. hold only one object and process it
# decalring a point cloud variable
pcd = PointCloud()
# intializing the points list
pcd.points = []
# intializing the channels list
pcd.channels = []
# initializing rows
rows = np.zeros((self.h, self.w), dtype=float)
rows[:, :] = np.arange(0, self.w)
# initializing columns
cols = np.zeros((self.h, self.w), dtype=float)
cols[:, :] = np.array([np.arange(0, self.h)]).T
# the calculations
# for more about the equations, check this link: https://medium.com/yodayoda/from-depth-map-to-point-cloud-7473721d3f
z = depthImage / self.depthScale
y = (np.multiply((cols - self.cy), z)) / self.fy
x = (np.multiply((rows - self.cx), z)) / self.fx
for i in range(0, self.h):
for j in range(0, self.w):
# always declare a new object to avoid errors due to bad references
pcd.points.append(Point32(z[i][j], -x[i][j], -y[i][j]))
# pcd.channels.append(ChannelFloat32(
# "rgb", [self.rgb[i][j][0], self.rgb[i][j][1], self.rgb[i][j][2]]))
################################################################
# initializing the frame id, this is a mechanical frame
pcd.header.frame_id = "front_left_bumblebee_body"
# initializing the frame time stamp
# Note you need to call rospy.init_node() before it; to work properly
pcd.header.stamp = rospy.Time.now()
################################################################
# self.listener.waitForTransform(
# "/world_ned", "/PhysXCar/front_left_bumblebee_body", rospy.Time.now(), rospy.Duration(4.0))
# pcd_out = self.listener.transformPointCloud("/world_ned", pcd)
return pcd
def get_point_cloud_in_fov(self, current_pose):
current_position = (current_pose.pose.position.x,
current_pose.pose.position.y)
fov_radius = self.robot_metadata.fov_radius
fov_point_cloud = PointCloud()
fov_point_cloud.header.seq = 1
fov_point_cloud.header.frame_id = self.frame_id
fov_point_cloud.header.stamp = rospy.Time.now()
fov_point_cloud.points = []
fov_point_cloud.channels = [ChannelFloat32()]
fov_point_cloud.channels[0].name = "obstacle_id"
fov_point_cloud.channels[0].values = []
# For all obstacles that are close enough from current robot pose,
# evaluate if their points are in the field of vision (fov) of the robot
# and add them to the point cloud if they are.
for obstacle_id, obstacle in self.obstacles.items():
# If obstacle is characterized by more than four points, it is more
# interesting to first check if its boundaries are within the fov
# radius. If not, then we simply go on to estimating the next obstacle.
if len(obstacle.ros_point_cloud.points) > 4:
if not (self.is_in_fov(obstacle.envelope.exterior.coords[0],
current_position, fov_radius)
or self.is_in_fov(obstacle.envelope.exterior.coords[1],
current_position, fov_radius)
or self.is_in_fov(obstacle.envelope.exterior.coords[2],
current_position, fov_radius)
or self.is_in_fov(obstacle.envelope.exterior.coords[3],
current_position, fov_radius)):
continue
# Iterate over the points of the obstacle's point cloud and add them
# to the point cloud in fov
for point in obstacle.ros_point_cloud.points:
if self.is_in_fov((point.x, point.y), current_position,
fov_radius):
fov_point_cloud.points.append(point)
fov_point_cloud.channels[0].values.append(obstacle_id)
return fov_point_cloud
def publish_poops_to_cost_map(self, poops):
"""Always publishes 30 poops. Keeps unused ones at map position 25, 25."""
NUM_POOPS = 30
poops_with_z = [(p[0], p[1], 0) for p in poops]
unused_poops = [(25, 25, 25)] * (NUM_POOPS - len(poops))
poop_positions = poops_with_z + unused_poops
#print poop_positions
point_cloud = PointCloud()
# cost map doesn't like the map coordinate frame
point_cloud.header = Header(stamp=Time.now(), frame_id='odom_combined')
point_cloud.points = [self._map_to_odom_combined(pos)
for pos in poop_positions]
#print 'new points', point_cloud.points
point_cloud.channels = [
ChannelFloat32(name='intensities', values=[2000] * NUM_POOPS),
ChannelFloat32(name='index', values=[0] * NUM_POOPS), #values=range(NUM_POOPS)),
ChannelFloat32(name='distances', values=[2] * NUM_POOPS),
ChannelFloat32(name='stamps', values=[0.002] * NUM_POOPS),
]
self.publisher.publish(point_cloud)
def create_point_cloud(self, rewards):
# get map information fist
# x = self.map_msg.info.origin.orientation.x
# y = self.map_msg.info.origin.orientation.y
# z = self.map_msg.info.origin.orientation.z
# w = self.map_msg.info.origin.orientation.w
# roll, pitch, yaw = euler_from_quaternion((x,y,z,w))
# offset_x = self.map_msg.info.origin.position.x
# offset_y = self.map_msg.info.origin.position.y
c = PointCloud()
c.header.seq = 1
c.header.stamp = rospy.Time.now()
c.header.frame_id = '/map'
c.points = []
channel = ChannelFloat32()
channel.name = "Values"
channel.values = []
c.channels = [channel]
for element in rewards[0:self.num - 1]:
p = Point()
x = element[
0] #(element[0]*np.cos(yaw) + element[1]*np.sin(yaw))*self.map_msg.info.resolution + offset_x
y = element[
1] #(-element[0]*np.sin(yaw) + element[1]*np.cos(yaw))*self.map_msg.info.resolution + offset_y
val = element[2]
p.x = x
p.y = y
c.points.append(p)
channel.values.append(val)
return c
def update(self): pc = PointCloud() pc.header.stamp = rospy.Time.now() pc.header.frame_id = self.frame pc.channels = [] for particle in self.pf.particles: pc.points.append(Point32(particle.pos[0], particle.pos[1], particle.pos[2])) prediction = self.pf.predict() marker = MarkerMsg() marker.header.stamp = rospy.Time.now() marker.header.frame_id = self.frame marker.type = marker.SPHERE marker.action = marker.ADD marker.scale.x = 0.05 marker.scale.y = 0.05 marker.scale.z = 0.05 marker.color.a = 1 marker.color.r = 1 marker.color.g = 1 marker.color.b = 1 marker.pose.position.x = prediction[0] marker.pose.position.y = prediction[1] marker.pose.position.z = prediction[2] markerArray = MarkerArrayMsg() markerArray.markers.append(marker) id = 0 for marker in markerArray.markers: marker.id = id id = id + 1 self.pcPub.publish(pc) self.markerPub.publish(markerArray)
def convert_pointcloud_to_ROS(pts3d,
intensities=None,
labels=None,
colors=None,
bgr=True):
ROS_pointcloud = PointCloud()
ROS_pointcloud.points = []
ROS_pointcloud.channels = []
(x, y, z) = (0, 1, 2)
for pt in np.asarray(pts3d.T):
ROS_pointcloud.points += [Point32(pt[x], pt[y], pt[z])]
intensity_channel = ChannelFloat32(name='intensities')
if intensities != None:
for value in intensities:
intensity_channel.values += [value]
ROS_pointcloud.channels += [intensity_channel]
label_channel = ChannelFloat32(name='labels')
if labels != None:
for value in labels:
label_channel.values += [value]
ROS_pointcloud.channels += [label_channel]
if colors != None:
r_ch = ChannelFloat32(name='r')
g_ch = ChannelFloat32(name='g')
b_ch = ChannelFloat32(name='b')
rgb_ch = ChannelFloat32(name='rgb')
for (r, g, b) in np.asarray(colors.T):
#NOTE: CV image is in bgr order. Ros expects rgb order
r_ch.values += [b / 256]
g_ch.values += [g / 256]
b_ch.values += [r / 256]
rgb_color = int(b * 0xFF * 0xFF + g * 0xFF + r) #incorrect
rgb_ch.values += [rgb_color]
ROS_pointcloud.channels += [r_ch, g_ch, b_ch]
return ROS_pointcloud
def talker():
global points
pub = rospy.Publisher('pc_test', PointCloud, queue_size=10)
rospy.init_node('publish_pc')
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
pc = PointCloud()
pc.header.stamp = rospy.Time.now()
pc.header.frame_id = 'odom'
pc.channels = []
for point in points:
pc.points.append(Point32(point[0], point[1], point[2]))
pub.publish(pc)
print pc
newPoint = list(map(float, raw_input("Next point x,y,z: ").split(",")))
points.append(newPoint)
rate.sleep()
def sonarCallback(self, data):
sonarpc = PointCloud()
sonarpc.header.stamp = data.header.stamp
sonarpc.header.frame_id = 'base_link'
sonarpc.channels = [ChannelFloat32()]
sonarpc.channels[0].values = []
counter = 0
for r in data.ranges:
#transform the pt
tpc = Point32()
r = r + 0.24
tpc.x = cos(self.sonar_positions[counter]) * r
tpc.y = sin(self.sonar_positions[counter]) * r
tpc.z = 0.23
counter += 1
#add this to the cloud
sonarpc.points.append(tpc)
sonarpc.channels[0].values.append(100)
# Publish the new cmd message using publish
self.pub.publish(sonarpc)
def sonarCallback(self, data):
sonarpc = PointCloud()
sonarpc.header.stamp = data.header.stamp
sonarpc.header.frame_id = 'base_link'
sonarpc.channels = [ChannelFloat32()]
sonarpc.channels[0].values = []
counter = 0
for r in data.ranges:
#transform the pt
tpc = Point32()
r = r + 0.24
tpc.x = cos(self.sonar_positions[counter])*r
tpc.y = sin(self.sonar_positions[counter])*r
tpc.z = 0.23
counter += 1
#add this to the cloud
sonarpc.points.append(tpc)
sonarpc.channels[0].values.append(100)
# Publish the new cmd message using publish
self.pub.publish(sonarpc)
def cloud_compute(self):
rate = rospy.Rate(self.freq)
while not rospy.is_shutdown():
while not all(self.callback_ready):
time.sleep(1e-3)
t0 = time.time()
self.code_ready = False
if len(self.img_depth) > self.image_size:
k_depth = 1.0 * self.image_size / len(self.img_depth)
h_depth = self.image_size
w_depth = int(k_depth * len(self.img_depth[0]))
fx_depth = k_depth * self.fx_depth
fy_depth = k_depth * self.fy_depth
img_depth = cv2.resize(self.img_depth, (w_depth, h_depth),
interpolation=cv2.INTER_NEAREST)
else:
h_depth = len(self.img_depth)
w_depth = len(self.img_depth[0])
fx_depth = self.fx_depth
fy_depth = self.fy_depth
img_depth = self.img_depth
if len(self.img_color) > self.image_size:
k_color = 1.0 * self.image_size / len(self.img_color)
h_color = self.image_size
w_color = int(k_color * len(self.img_color[0]))
fx_color = k_color * self.fx_color
fy_color = k_color * self.fy_color
img_color = cv2.resize(self.img_color, (w_color, h_color),
interpolation=cv2.INTER_NEAREST)
else:
h_color = len(self.img_color)
w_color = len(self.img_color[0])
fx_color = self.fx_color
fy_color = self.fy_color
img_color = self.img_color
stamp = rospy.Time.from_sec(
max([self.stamp_color, self.stamp_depth]))
z_depth = 1e-3 * img_depth.flatten()
z_depth[z_depth == 0.0] = -1.0
i_depth = numpy.arange(0, h_depth * w_depth) / w_depth
j_depth = numpy.arange(0, h_depth * w_depth) % w_depth
u_depth = j_depth - w_depth / 2.0 - 0.5
v_depth = i_depth - h_depth / 2.0 - 0.5
x_depth = u_depth * z_depth / fx_depth
y_depth = v_depth * z_depth / fy_depth
u_color = fx_color * x_depth / z_depth
v_color = fy_color * y_depth / z_depth
i_color = (v_color + h_color / 2.0 + 1.0).astype(int)
j_color = (u_color + w_color / 2.0 + 1.0).astype(int)
idx = (i_color >= 0) & (i_color < h_color) & (j_color >= 0) & (j_color < w_color) & \
(z_depth > 1e-3*self.min_range) & (z_depth < 1e-3*self.max_range)
x = +z_depth[idx]
y = -x_depth[idx]
z = -y_depth[idx]
n = len(z)
rgb = img_color.reshape(h_color * w_color, 3)[idx].astype(int)
msg = PointCloud()
msg.header.seq += 1
msg.header.stamp = stamp
msg.header.frame_id = self.frame_id[:-13] + "frame"
msg.points = [array2point(x[k], y[k], z[k]) for k in range(n)]
msg.channels = [
ChannelFloat32(),
ChannelFloat32(),
ChannelFloat32()
]
msg.channels[0].name = "r"
msg.channels[1].name = "g"
msg.channels[2].name = "b"
msg.channels[0].values = 1.0 * rgb[:, 0] / 255
msg.channels[1].values = 1.0 * rgb[:, 1] / 255
msg.channels[2].values = 1.0 * rgb[:, 2] / 255
self.callback_ready = [False, False, False, False]
self.code_ready = True
self.time[1] = 1e3 * (time.time() - t0)
t0 = time.time()
self.pub.publish(msg)
self.time[2] = 1e3 * (time.time() - t0)
sys.stdout.write(
"[CLOUD]: %7d points | Subscribing: %4.0fms | Computing: %4.0fms | Publishing: %4.0fms\n"
% (n, sum(self.time[0]), self.time[1], self.time[2]))
sys.stdout.flush()
rate.sleep()
def on_scan(scan):
image = image_cache.getElemBeforeTime(scan.header.stamp)
if not image:
return
try:
cv_image = bridge.imgmsg_to_cv2(image, desired_encoding="rgb8")
height, width = cv_image.shape[:2]
except CvBridgeError as e:
rospy.logerr('Cannot convert image to OpenCV: ' + str(e))
return
point_cloud = laser_projector.projectLaser(
scan, channel_options=LaserProjection.ChannelOption.TIMESTAMP)
color_channel = ChannelFloat32()
color_channel.name = 'rgb'
points = []
try:
tf_listener.waitForTransform(image.header.frame_id,
scan.header.frame_id, scan.header.stamp,
rospy.Duration(1.0))
except tf.Exception as e:
rospy.logwarn('Cannot transform scan: ' + str(e))
return
for point in point_cloud2.read_points(point_cloud,
field_names=("x", "y", "z"),
skip_nans=True):
point_stamped = PointStamped()
point_stamped.header = scan.header
point_stamped.point.x = point[0]
point_stamped.point.y = point[1]
point_stamped.point.z = point[2]
try:
transformed_point = tf_listener.transformPoint(
image.header.frame_id, point_stamped)
except tf.Exception as e:
rospy.logwarn('Cannot transform scan point: ' + str(e))
return
point_3d = (transformed_point.point.z, transformed_point.point.y,
-transformed_point.point.x)
u, v = camera_model.project3dToPixel(point_3d)
u = int(round(u))
v = int(round(v))
if u < 0 or u >= height or v < 0 or v >= width:
continue
else:
color = cv_image[u, v]
rgb8 = struct.pack('BBBB', color[2], color[1], color[0], 0)
rgb_float32 = struct.unpack('f', rgb8)[0]
color_channel.values.append(rgb_float32)
point_32 = Point32()
point_32.x = transformed_point.point.x
point_32.y = transformed_point.point.y
point_32.z = transformed_point.point.z
points.append(point_32)
rgb_point_cloud = PointCloud()
rgb_point_cloud.header.frame_id = image.header.frame_id
rgb_point_cloud.header.stamp = scan.header.stamp
rgb_point_cloud.channels = [color_channel]
rgb_point_cloud.points = points
point_cloud_pub.publish(rgb_point_cloud)
ProjectionPlane(get_shelf_boundaries((-2, -4), -math.pi)),
# Shelf 7
ProjectionPlane(get_shelf_boundaries((-3, -3), math.pi / 2)),
# Shelf 8
ProjectionPlane(get_shelf_boundaries((-2, -1), 0))
]
# Stores trophy coords determined.
# Note: if a centre does not project onto a projection plane (i.e. one of the shevles), it will not be stored.
trophy_coords = []
if testing:
point_cloud = PointCloud()
point_cloud.header.frame_id = "odom"
point_cloud.points = []
point_cloud.channels = []
# Start tf buffer and listener
tf_buffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tf_buffer)
######### </globals> ################################################
# Message coming from trophy_detector. Will need:
# {(Camera pose, coordinates in frame (x, y)), ...}
#
# Queue size of one as this is currently operating on a request-response model - the robot
# will request that the vision system runs a trophy position estimation process, from image
# forwarding through to projection, and will block until it receives a message (on the topic
# '/trophy_image_robot_request_response') that the projection process is complete. Hence it's fine to have a queue_size of one,
# since we won't have another request (set of centres) coming in until the projection process
def projectLaserToPC1(self, scan_in, range_cutoff=-1.0, channel_options=ChannelOption.DEFAULT):
"""
Project a sensor_msgs::LaserScan into a sensor_msgs::PointCloud.
Project a single laser scan from a linear array into a 3D
point cloud. The generated cloud will be in the same frame
as the original laser scan.
Keyword arguments:
scan_in -- The input laser scan.
range_cutoff -- An additional range cutoff which can be
applied which is more limiting than max_range in the scan
(default -1.0).
channel_options -- An OR'd set of channels to include.
"""
N = len(scan_in.ranges)
ranges = np.array(scan_in.ranges)
if (self.__cos_sin_map.shape[1] != N or
self.__angle_min != scan_in.angle_min or
self.__angle_max != scan_in.angle_max):
rospy.logdebug("No precomputed map given. Computing one.")
self.__angle_min = scan_in.angle_min
self.__angle_max = scan_in.angle_max
angles = scan_in.angle_min + np.arange(N) * scan_in.angle_increment
self.__cos_sin_map = np.array([np.cos(angles), np.sin(angles)])
output = ranges * self.__cos_sin_map
# Set the output cloud accordingly
cloud_out = PointCloud()
channels = []
idx_intensity = idx_index = idx_distance = idx_timestamp = -1
idx_vpx = idx_vpy = idx_vpz = -1
if (channel_options & self.ChannelOption.INTENSITY and
len(scan_in.intensities) > 0):
channel_index = len(channels)
channels.append(ChannelFloat32())
channels[channel_index].name = "intensity"
idx_intensity = channel_index
if channel_options & self.ChannelOption.INDEX:
channel_index = len(channels)
channels.append(ChannelFloat32())
channels[channel_index].name = "index"
idx_index = channel_index
if channel_options & self.ChannelOption.DISTANCE:
channel_index = len(channels)
channels.append(ChannelFloat32())
channels[channel_index].name = "distances"
idx_distance = channel_index
if channel_options & self.ChannelOption.TIMESTAMP:
channel_index = len(channels)
channels.append(ChannelFloat32())
channels[channel_index].name = "stamps"
idx_timestamp = channel_index
if channel_options & self.ChannelOption.VIEWPOINT:
channel_index = len(channels)
channels.extend([ChannelFloat32() for _ in range(3)])
channels[channel_index].name = "vp_x"
idx_vpx = channel_index
channel_index += 1
channels[channel_index].name = "vp_y"
idx_vpy = channel_index
channel_index += 1
channels[channel_index].name = "vp_z"
idx_vpz = channel_index
if range_cutoff < 0:
range_cutoff = scan_in.range_max
else:
range_cutoff = min(range_cutoff, scan_in.range_max)
count = 0
for i in range(N):
ri = scan_in.ranges[i]
if ri < range_cutoff and ri >= scan_in.range_min:
point = output[:, i].tolist()
cloud_out.points.append(Point32())
cloud_out.points[count].x = point[0]
cloud_out.points[count].y = point[1]
cloud_out.points[count].z = 0
if idx_intensity != -1:
channels[idx_intensity].values.append(scan_in.intensities[i])
if idx_index != -1:
channels[idx_intensity].values.append(i)
if idx_distance != -1:
channels[idx_distance].values.append(scan_in.ranges[i])
if idx_timestamp != -1:
channels[idx_timestamp].values.append(i * scan_in.time_increment)
if idx_vpx != -1 and idx_vpy != -1 and idx_vpz != -1:
channels[idx_vpx] = 0.0
channels[idx_vpy] = 0.0
channels[idx_vpz] = 0.0
count += 1
cloud_out.channels = channels
return cloud_out
def map_compute(self):
msg = PointCloud()
msg.header.frame_id = self.odom_child_frame
msg.channels = [ChannelFloat32(),ChannelFloat32(),ChannelFloat32()]
msg.channels[0].name = "r"
msg.channels[1].name = "g"
msg.channels[2].name = "b"
msg_tf = tf.TransformBroadcaster()
T_ros = 1.0/self.freq
k_max = 2e3/(self.rate*self.freq)
while not rospy.is_shutdown():
t0 = time.time()
t_ros = rospy.Time.now().to_sec()
self.code_ready = False
if self.interp_ready:
p_odom = (self.odom_position.x, self.odom_position.y, self.odom_position.z)
q_odom = (self.odom_orientation.x, self.odom_orientation.y, self.odom_orientation.z, self.odom_orientation.w)
p_tf = tuple(self.tf[:3])
q_tf = tuple(self.tf[3:])
p_cloud = pp_sum(qp_mult(q_odom,p_tf),p_odom)
q_cloud = qq_mult(q_odom,q_tf)
self.map_points = [tuple2point(pp_sum(qp_mult(q_cloud,(p.x,p.y,p.z)),p_cloud)) for p in self.cloud_points]
msg.header.seq += 1
msg.header.stamp = self.cloud_stamp
msg.points += self.map_points
msg.channels[0].values += self.cloud_channels[0].values
msg.channels[1].values += self.cloud_channels[1].values
msg.channels[2].values += self.cloud_channels[2].values
self.cloud_ready = False
self.interp_ready = False
self.code_ready = True
self.time[1] = 1e3*(time.time()-t0)
t0 = time.time()
if self.publish_tf:
msg_tf.sendTransform( tuple(self.tf[:3]),
tuple(self.tf[3:]),
self.cloud_stamp,
self.cloud_frame,
self.odom_child_frame)
self.pub.publish(msg)
self.time[2] = 1e3*(time.time()-t0)
sys.stdout.write("[ MAP]: %7d points | Subscribing: %4.0fms | Computing: %4.0fms | Publishing: %4.0fms\n" %
(len(msg.points), sum(self.time[0]), self.time[1], self.time[2]))
sys.stdout.flush()
k = 0
while not rospy.is_shutdown() and rospy.Time.now().to_sec()-t_ros < T_ros and k < k_max:
k += 1
time.sleep(1e-3)
def main(argv):
thresh = 0.25
darknet_path = "/home/apsync/GitHub/darknet/"
config_path = darknet_path + "cfg/yolov4-tiny.cfg"
weight_path = darknet_path + "yolov4-tiny.weights"
meta_path = darknet_path + "cfg/coco.data"
svo_path = None
zed_id = 0
help_str = 'darknet_zed.py -c <config> -w <weight> -m <meta> -t <threshold> -s <svo_file> -z <zed_id>'
try:
opts, args = getopt.getopt(argv, "hc:w:m:t:s:z:", [
"config=", "weight=", "meta=", "threshold=", "svo_file=", "zed_id="
])
except getopt.GetoptError:
log.exception(help_str)
sys.exit(2)
for opt, arg in opts:
if opt == '-h':
log.info(help_str)
sys.exit()
elif opt in ("-c", "--config"):
config_path = arg
elif opt in ("-w", "--weight"):
weight_path = arg
elif opt in ("-m", "--meta"):
meta_path = arg
elif opt in ("-t", "--threshold"):
thresh = float(arg)
elif opt in ("-s", "--svo_file"):
svo_path = arg
elif opt in ("-z", "--zed_id"):
zed_id = int(arg)
input_type = sl.InputType()
if svo_path is not None:
log.info("SVO file : " + svo_path)
input_type.set_from_svo_file(svo_path)
else:
# Launch camera by id
input_type.set_from_camera_id(zed_id)
init = sl.InitParameters(input_t=input_type)
init.coordinate_units = sl.UNIT.METER
init.depth_mode = sl.DEPTH_MODE.PERFORMANCE # Use ULTRA depth mode
# Start the ROS nodes for the images and the depth information
rospy.init_node('darknet_ros')
rospy.loginfo('darknet yolo node started')
pub = rospy.Publisher('/darknet_ros/detection_image', Image, queue_size=10)
pub2 = rospy.Publisher('/darknet_ros/distance_array',
LaserScan,
queue_size=50)
pub3 = rospy.Publisher('/darknet_ros/color_image', Image, queue_size=10)
pub4 = rospy.Publisher('/darknet_ros/nine_sector_image',
Image,
queue_size=10)
pub5 = rospy.Publisher('/darknet_ros/9sectorarray',
PointCloud,
queue_size=50)
cam = sl.Camera()
if not cam.is_opened():
log.info("Opening ZED Camera...")
status = cam.open(init)
if status != sl.ERROR_CODE.SUCCESS:
log.error(repr(status))
exit()
runtime = sl.RuntimeParameters()
# Use STANDARD sensing mode
runtime.sensing_mode = sl.SENSING_MODE.FILL
mat = sl.Mat()
mat_lv = sl.Mat()
point_cloud_mat = sl.Mat()
mat_9_sec = sl.Mat()
# Import the global variables. This lets us instance Darknet once,
# then just call performDetect() again without instancing again
global metaMain, netMain, altNames # pylint: disable=W0603
assert 0 < thresh < 1, "Threshold should be a float between zero and one (non-inclusive)"
if not os.path.exists(config_path):
raise ValueError("Invalid config path `" +
os.path.abspath(config_path) + "`")
if not os.path.exists(weight_path):
raise ValueError("Invalid weight path `" +
os.path.abspath(weight_path) + "`")
if not os.path.exists(meta_path):
raise ValueError("Invalid data file path `" +
os.path.abspath(meta_path) + "`")
if netMain is None:
netMain = load_net_custom(config_path.encode("ascii"),
weight_path.encode("ascii"), 0,
1) # batch size = 1
if metaMain is None:
metaMain = load_meta(meta_path.encode("ascii"))
if altNames is None:
# In thon 3, the metafile default access craps out on Windows (but not Linux)
# Read the names file and create a list to feed to detect
try:
with open(meta_path) as meta_fh:
meta_contents = meta_fh.read()
import re
match = re.search("names *= *(.*)$", meta_contents,
re.IGNORECASE | re.MULTILINE)
if match:
result = match.group(1)
else:
result = None
try:
if os.path.exists(result):
with open(result) as names_fh:
names_list = names_fh.read().strip().split("\n")
altNames = [x.strip() for x in names_list]
except TypeError:
pass
except Exception:
pass
color_array = generate_color(meta_path)
# get parameters for depth sensing
width = round(cam.get_camera_information().camera_resolution.width / 2)
height = round(cam.get_camera_information().camera_resolution.height / 2)
res = sl.Resolution()
res.width = width
res.height = height
angle_offset = 0 - (depth_hfov_deg / 2)
increment_f = depth_hfov_deg / (distances_array_length)
#Initialize laserscan node
scan = LaserScan()
scan.header.frame_id = 'zed_horizontal_scan'
scan.angle_min = angle_offset
scan.angle_max = depth_hfov_deg / 2
scan.angle_increment = increment_f
scan.range_min = DEPTH_RANGE_M[0]
scan.range_max = DEPTH_RANGE_M[1]
scan.intensities = []
scan.time_increment = 0
fps = 1
#Initialize pointcloud node
channel = ChannelFloat32()
channel.name = "depth_range"
channel.values = [DEPTH_RANGE_M[0], DEPTH_RANGE_M[1]]
pointcloud = PointCloud()
pointcloud.header.frame_id = 'zed_9_sector_scan'
pointcloud.channels = [channel]
while not rospy.is_shutdown():
start_time = time.time() # start time of the loop
current_time = rospy.Time.now()
err = cam.grab(runtime)
if err == sl.ERROR_CODE.SUCCESS:
cam.retrieve_image(mat, sl.VIEW.LEFT)
image = mat.get_data()
cam.retrieve_image(mat_lv, sl.VIEW.LEFT)
image_lv = mat_lv.get_data()
cam.retrieve_image(mat_9_sec, sl.VIEW.DEPTH)
nine_sector_image = mat_9_sec.get_data()
scan.header.stamp = current_time
pointcloud.header.stamp = current_time
scan.scan_time = fps
cam.retrieve_measure(point_cloud_mat, sl.MEASURE.XYZRGBA)
depth = point_cloud_mat.get_data()
print("\033[0;0H")
distances_from_depth_image(point_cloud_mat, distances,
DEPTH_RANGE_M[0], DEPTH_RANGE_M[1],
width, height,
obstacle_line_thickness_pixel)
scan.ranges = distances
distance_center = np.mean(distances[34:38])
# Publish the distance information for the mavros node
pub2.publish(scan)
# provide distance info to video stream and create image with horizontal distance information
# Draw a horizontal line to visualize the obstacles' line
x1, y1 = int(0), int(height)
x2, y2 = int(width * 2), int(height)
line_thickness = obstacle_line_thickness_pixel
cv2.line(image_lv, (x1, y1), (x2, y2), (0, 255, 0),
thickness=line_thickness)
#print("Distance to Camera at position {},{} (image center): {:1.3} m".format(width, height, distance_center))
if distance_center > DEPTH_RANGE_M[1]:
cv2.circle(image_lv, (width, height), 20, (0, 255, 0), 2)
elif distance_center <= DEPTH_RANGE_M[1] and distance_center > 10:
cv2.putText(image_lv, ("{:1.3} m".format(distance_center)),
((width - 50), (height + 50)),
cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
cv2.circle(image_lv, (width, height), 20, (0, 255, 0), 2)
elif distance_center > 5 and distance_center <= 10:
cv2.putText(image_lv, ("{:1.3} m".format(distance_center)),
((width - 70), (height + 65)),
cv2.FONT_HERSHEY_SIMPLEX, 1.5, (0, 255, 255), 2)
cv2.circle(image_lv, (width, height), 20, (0, 255, 255), 4)
else:
cv2.putText(image_lv, ("{:1.3} m".format(distance_center)),
((width - 100), (height + 70)),
cv2.FONT_HERSHEY_DUPLEX, 2, (0, 0, 255), 2)
cv2.circle(image_lv, (width, height), 20, (0, 0, 255), -1)
cv2.rectangle(image_lv, (0, 100),
((width * 2 - 5), (height * 2)), (30, 30, 255),
3)
# create 9 sector image with distance information
# divide view into 3x3 matrix
box = np.empty(4, dtype=int)
pxstep = int(width * 2 / 3)
pystep = int(height * 2 / 3)
gx = 0
gy = 0
# draw sector lines on image
while gx < (width * 2):
cv2.line(nine_sector_image, (gx, 0), (gx, (height * 2)),
color=(0, 0, 255),
thickness=2)
gx += pxstep
while gy <= (height * 2):
cv2.line(nine_sector_image, (0, gy), ((width * 2), gy),
color=(0, 0, 255),
thickness=2)
gy += pystep
# measure sector depth and printout in sectors
gx = 0
gy = 0
i = 0
box[1] = pystep / 2 + gy
box[2] = pxstep
box[3] = pystep
while gx < (width * 2 - 2):
gy = 0
box[1] = pystep / 2 + gy
box[0] = pxstep / 2 + gx
# calculate depth of closest object in sector
x, y, z = get_object_depth(depth, box)
sector_obstacle_coordinates[i][0] = x
sector_obstacle_coordinates[i][1] = y
sector_obstacle_coordinates[i][2] = z
distance = math.sqrt(x * x + y * y + z * z)
distance = "{:.2f}".format(distance)
cv2.putText(nine_sector_image,
"Dist.: " + (str(distance) + " m"),
((gx + 10), (gy + pystep - 10)),
cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 2)
gy += pystep
i += 1
while gy < (height * 2):
box[1] = pystep / 2 + gy
# calculate depth of closest object in sector
x, y, z = get_object_depth(depth, box)
sector_obstacle_coordinates[i][0] = x
sector_obstacle_coordinates[i][1] = y
sector_obstacle_coordinates[i][2] = z
distance = math.sqrt(x * x + y * y + z * z)
distance = "{:.2f}".format(distance)
cv2.putText(nine_sector_image,
"Dist.: " + (str(distance) + " m"),
((gx + 10), (gy + pystep - 10)),
cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 2)
gy += pystep
i += 1
gx += pxstep
pointcloud.points = [
Point32(x=sector_obstacle_coordinates[j][0],
y=sector_obstacle_coordinates[j][1],
z=sector_obstacle_coordinates[j][2]) for j in range(9)
]
pub5.publish(pointcloud)
# Do the yolo object detection
detections = detect(netMain, metaMain, image, thresh)
print(
chr(27) + "[2J" + "**** " + str(len(detections)) +
" Detection Results ****")
for detection in detections:
label = detection[0]
confidence = detection[1]
pstring = label + ": " + str(np.rint(100 * confidence)) + "%"
print(pstring)
bounds = detection[2]
y_extent = int(bounds[3])
x_extent = int(bounds[2])
# Coordinates are around the center
x_coord = int(bounds[0] - bounds[2] / 2)
y_coord = int(bounds[1] - bounds[3] / 2)
thickness = 1
x, y, z = get_object_depth(depth, bounds)
distance = math.sqrt(x * x + y * y + z * z)
distance = "{:.2f}".format(distance)
cv2.rectangle(image,
(x_coord - thickness, y_coord - thickness),
(x_coord + x_extent + thickness, y_coord +
(18 + thickness * 4)),
color_array[detection[3]], -1)
cv2.putText(image, pstring + " " + (str(distance) + " m"),
(x_coord + (thickness * 4), y_coord +
(10 + thickness * 4)), cv2.FONT_HERSHEY_SIMPLEX,
0.5, (255, 255, 255), 2)
cv2.rectangle(image,
(x_coord - thickness, y_coord - thickness),
(x_coord + x_extent + thickness,
y_coord + y_extent + thickness),
color_array[detection[3]], int(thickness * 2))
# convert image to ros
imgMsg = bridge.cv2_to_imgmsg(image, encoding="bgra8")
imgMsg.header.stamp = rospy.Time.now()
imgMsg.header.frame_id = "color_image"
imgMsg_lv = bridge.cv2_to_imgmsg(image_lv, encoding="bgra8")
imgMsg_lv.header.stamp = rospy.Time.now()
imgMsg_lv.header.frame_id = "yolo_image"
imgMsg_9sec = bridge.cv2_to_imgmsg(nine_sector_image,
encoding="bgra8")
imgMsg_9sec.header.stamp = rospy.Time.now()
imgMsg_9sec.header.frame_id = "nine_sector_image"
# publish images to ros nodes
pub.publish(imgMsg)
pub3.publish(imgMsg_lv)
pub4.publish(imgMsg_9sec)
fps = (time.time() - start_time)
print("\033[0;0H" + "FPS: {:1.3}".format(1.0 / fps))
#rospy.Rate(1.0).sleep() # 1 Hz
#cv2.destroyAllWindows()
cam.close()
log.info("\nFINISH")
