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 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")