plemeri / ros_vision_pkg Goto Github PK

Hi,

  max_deg = int(360 // sample_rate)
   arr = pointcloud2_to_xyz_array(msg)

   angle = np.arctan2(arr[:, 1], arr[:, 0])
   angle = np.rad2deg(angle) + 180
   angle /= sample_rate
   angle = np.round(angle)
   angle[angle > max_deg - 1] = max_deg - 1

   dist = np.sqrt(arr[:, 0] ** 2 + arr[:, 1] ** 2)
   min_dist = np.ones(max_deg) * max_dist
   for i, ang in enumerate(angle):
       if min_dist[int(ang)] > dist[i]:
           min_dist[int(ang)] = dist[i]
   # -- make angle based min_distance data -- end

   # -- to pointcloud for visualization
   out_angle = np.deg2rad(np.linspace(0, 359, max_deg))
   out_x = -np.cos(out_angle) * min_dist
   out_y = -np.sin(out_angle) * min_dist
   out_z = np.zeros(max_deg)
   out_cloud = np.stack([out_x, out_y, out_z], axis=-1)

   print(out_z)

output:
[0. 0. 0. 0. 0. 0. 0.] [0. 0. 0. 0. 0. 0. 0.] [0. 0. 0. 0. 0. 0. 0.] [0. 0. 0. 0. 0. 0. 0.] [0. 0. 0. 0. 0. 0. 0.] [0. 0. 0. 0. 0. 0. 0.] [0. 0. 0. 0. 0. 0. 0.] [0. 0. 0. 0. 0. 0. 0.] [0. 0. 0. 0. 0. 0. 0.] [0. 0. 0. 0. 0. 0. 0.] [0. 0. 0. 0. 0. 0. 0.] [0. 0. 0. 0. 0. 0. 0.] [0. 0. 0. 0. 0. 0. 0.] [0. 0. 0. 0. 0. 0. 0.] [0. 0. 0. 0. 0. 0. 0.]

print(out_x)

output:
[-0. -0.30125702 0.30265708 0.99996192 0. -0.30468984 -0. ] [-0. -0.30637342 0.28919702 0.99996192 0. -0.30468984 -0. ] [-0. -0.31355883 0.30099976 0.99996192 0. -0.30612661 -0. ] [-0. -0.30135081 0.28919702 0.99996192 0. -0.29824787 -0. ]

ı use ouster os1. Thanks in advance

Do you plan to extend this script to install the latest ROS3 platform?

Dose this source codes aork on ROS2??

For example, night time or rainy situation while driving.