def echoes_callback(echo): echo['data'] = echo['data'][np.bitwise_and( echo['data']['flags'], 0x01).astype(np.bool)] #keep valid echoes only indices, flags, distances, amplitudes, x, y, z = [ echo['data'][x] for x in ['indices', 'flags', 'distances', 'amplitudes', 'x', 'y', 'z'] ] stamp = rospy.Time.now() #rospy.loginfo('pixell: echoes callback') exportation_echoes.add_sample(echo) if pub_raw.get_num_connections() > 0: pub_raw.publish( ros_numpy.msgify(PointCloud2, echo['data'], stamp, frame_id)) if pub_cloud.get_num_connections() > 0: struct_cloud = np.empty(indices.size, dtype=[('x', np.float32), ('y', np.float32), ('z', np.float32), ('intensity', np.float32)]) struct_cloud['x'] = x struct_cloud['y'] = y struct_cloud['z'] = z struct_cloud['intensity'] = amplitudes pub_cloud.publish( ros_numpy.msgify(PointCloud2, struct_cloud, stamp, frame_id))
def offset_goal(pose, offset): position = numpify(pose.pose.position) orientation = numpify(pose.pose.orientation) position += qv_mult(orientation, np.array([offset, 0.0, 0.0])) pose.pose.position = msgify(Point, position) pose.pose.orientation = msgify(Quaternion, orientation) return pose
def sub_image(self, im_msg, image_info): """ Take a thresholded image and output marker position metrics. """ print("sub_image in marker_locator") # Announce that we have received a packet self.pub_packet_entered_node.publish(im_msg.header) # Extract the image im = numpify(im_msg) height, width = im.shape[:2] # Label each connected blob in the image. im2 = (im > 128).astype(bool) labels, num_labels = scipy.ndimage.measurements.label(im2) # no blobs? if num_labels == 0: self.pub_marker.publish(PointStamped( header=Header(frame_id="base_link", stamp=im_msg.header.stamp), point=Point(x=np.nan, y=np.nan, z=0) )) return # Calculate the size of each blob. blob_sizes = scipy.ndimage.measurements.sum(im2, labels=labels, index=xrange(0,num_labels+1)) # Find the biggest blob. biggest_blob = np.argmax(blob_sizes) - 1 pp = scipy.ndimage.measurements.find_objects(labels)[biggest_blob] bbox = BoundingBox(pp[1].start, pp[0].start, pp[1].stop, pp[0].stop) # Calculate centroid (units of pixels). cx, cy = np.mean([bbox.maxx, bbox.minx]), np.mean([bbox.maxy, bbox.miny]) p_matrix = np.array(image_info.P).reshape((3, 4)) marker_position = self.get_position(cx, cy, p_matrix, im_msg.header) if marker_position is None: return self.pub_marker.publish(PointStamped( header=Header(frame_id="base_link", stamp=im_msg.header.stamp), point=msgify(Point, marker_position) )) # Make overlay visualizations. debug_overlay = np.zeros((height, width, 3), dtype=np.uint8) debug_overlay[...,0] = im debug_overlay[...,1] = im debug_overlay[...,2] = im self.draw_bbox(debug_overlay, bbox) self.draw_point(debug_overlay, cx, cy) debug_overlay_msg = msgify(Image, debug_overlay, encoding='rgb8') debug_overlay_msg.header = im_msg.header self.pub_debug_overlay.publish(debug_overlay_msg) # Announce that a packet has left the node. self.pub_packet_left_node.publish(im_msg.header)
def offset_goal(): pose = PoseStamped() pose.header.frame_id = 'map' goal_pose = goal() # type: PoseStamped position = numpify(goal_pose.pose.position) orientation = numpify(goal_pose.pose.orientation) position += qv_mult(orientation, np.array([-offset, 0.0, 0.0])) pose.pose.position = msgify(Point, position) pose.pose.orientation = msgify(Quaternion, orientation) return pose
def test_point(self): p = Point(1, 2, 3) p_arr = ros_numpy.numpify(p) np.testing.assert_array_equal(p_arr, [1, 2, 3]) p_arrh = ros_numpy.numpify(p, hom=True) np.testing.assert_array_equal(p_arrh, [1, 2, 3, 1]) self.assertEqual(p, ros_numpy.msgify(Point, p_arr)) self.assertEqual(p, ros_numpy.msgify(Point, p_arrh)) self.assertEqual(p, ros_numpy.msgify(Point, p_arrh * 2))
def test_point(self): p = Point(1, 2, 3) p_arr = ros_numpy.numpify(p) np.testing.assert_array_equal(p_arr, [1, 2, 3]) p_arrh = ros_numpy.numpify(p, hom=True) np.testing.assert_array_equal(p_arrh, [1, 2, 3, 1]) self.assertEqual(p, ros_numpy.msgify(Point, p_arr)) self.assertEqual(p, ros_numpy.msgify(Point, p_arrh)) self.assertEqual(p, ros_numpy.msgify(Point, p_arrh * 2))
def on_enter(self, userdata): post = posts[userdata.leg_number] if not post.has_seen(): self._error = True target_pose = PoseStamped() point = numpify(post.position.point) normal = numpify(post.normal.vector) target_pose.header.frame_id = post.pose.header.frame_id target_pose.pose.position = msgify(Point, point + normal * 2) target_pose.pose.orientation = msgify(Quaternion, normal_to_quaternion(-normal)) userdata.target_pose = target_pose
def __init__(self, number): # int: (str) -> None pose_name = 'S{}'.format(number) position = np.array([float(x) for x in rospy.get_param('named_poses/{}/position'.format(pose_name))]) orientation = np.array([float(x) for x in rospy.get_param('named_poses/{}/orientation'.format(pose_name))]) self._pose = PoseStamped() self._pose.header.frame_id = 'map' self._pose.pose.position = msgify(Point, position) self._pose.pose.orientation = msgify(Quaternion, orientation) self._contains = set() self._number = number self._pose_publisher = rospy.Publisher('/viz/parking_square_{}'.format(number), PoseStamped, latch=True, queue_size=1) self._pose_publisher.publish(self.pose)
def test_vector3(self): v = Vector3(1, 2, 3) v_arr = ros_numpy.numpify(v) np.testing.assert_array_equal(v_arr, [1, 2, 3]) v_arrh = ros_numpy.numpify(v, hom=True) np.testing.assert_array_equal(v_arrh, [1, 2, 3, 0]) self.assertEqual(v, ros_numpy.msgify(Vector3, v_arr)) self.assertEqual(v, ros_numpy.msgify(Vector3, v_arrh)) with self.assertRaises(AssertionError): ros_numpy.msgify(Vector3, np.array([0, 0, 0, 1]))
def test_vector3(self): v = Vector3(1, 2, 3) v_arr = ros_numpy.numpify(v) np.testing.assert_array_equal(v_arr, [1, 2, 3]) v_arrh = ros_numpy.numpify(v, hom=True) np.testing.assert_array_equal(v_arrh, [1, 2, 3, 0]) self.assertEqual(v, ros_numpy.msgify(Vector3, v_arr)) self.assertEqual(v, ros_numpy.msgify(Vector3, v_arrh)) with self.assertRaises(AssertionError): ros_numpy.msgify(Vector3, np.array([0, 0, 0, 1]))
def on_enter(self, userdata): gate = gates[userdata.leg_number] if not gate.has_seen(): self._error = True return point = numpify(gate.position.point) normal = numpify(gate.normal.vector) target_pose = PoseStamped() target_pose.header.frame_id = gate.position.header.frame_id target_pose.pose.position = msgify(Point, point + normal * self._offset) target_pose.pose.orientation = msgify(Quaternion, normal_to_quaternion(-normal)) userdata.target_pose = target_pose
def echoes_callback(echo): echo['data'] = echo['data'][np.bitwise_and( echo['data']['flags'], 0x01).astype(np.bool)] #keep valid echoes only indices, flags, distances, amplitudes = [ echo['data'][x] for x in ['indices', 'flags', 'distances', 'amplitudes'] ] stamp = rospy.Time.now() if pub_raw.get_num_connections() > 0: pub_raw.publish( ros_numpy.msgify(PointCloud2, echo['data'], stamp, frame_id)) if pub_cloud.get_num_connections() > 0: struct_cloud = np.empty(indices.size, dtype=[('x', np.float32), ('y', np.float32), ('z', np.float32), ('intensity', np.float32)]) points = clouds.to_point_cloud(indices, distances, directions, np.float32) struct_cloud['x'] = points[:, 0] struct_cloud['y'] = points[:, 1] struct_cloud['z'] = points[:, 2] struct_cloud['intensity'] = amplitudes pub_cloud.publish( ros_numpy.msgify(PointCloud2, struct_cloud, stamp, frame_id)) if pub_triangles.get_num_connections() > 0: points, quad_amplitudes, quad_indices = clouds.to_quad_cloud( indices, distances, amplitudes, quad_directions, specs.v, specs.h, np.float32) stl_points = points[quad_indices] stl_amplitudes = quad_amplitudes[quad_indices] marker = Marker() marker.header.stamp = stamp marker.header.frame_id = frame_id marker.type = Marker.TRIANGLE_LIST marker.scale = Vector3(1, 1, 1) marker.points = [Point(x[0], x[1], x[2]) for x in stl_points] #TODO: optimize me marker.colors = [ColorRGBA(a, a, a, 1) for a in stl_amplitudes] #TODO: optimize me pub_triangles.publish(marker)
def main(): colorama.init(autoreset=True) parser = argparse.ArgumentParser(formatter_class=my_formatter) parser.add_argument("results_dir", type=pathlib.Path, help='directory containing metrics.json') parser.add_argument("--trial-idx", type=int, help='which plan to show', default=0) rospy.init_node("play_dragging_plan") action_srv = rospy.ServiceProxy("execute_dual_gripper_action", DualGripperTrajectory) args = parser.parse_args() with (args.results_dir / 'metadata.json').open('r') as metadata_file: metadata_str = metadata_file.read() metadata = json.loads(metadata_str) scenario = get_scenario(metadata['scenario']) with gzip.open(args.results_dir / f'{args.trial_idx}_metrics.json.gz', 'rb') as metrics_file: metrics_str = metrics_file.read() datum = json.loads(metrics_str.decode("utf-8")) all_actions = [] steps = datum['steps'] for step_idx, step in enumerate(steps): if step['type'] == 'executed_plan': actions = step['planning_result']['actions'] elif step['type'] == 'executed_recovery': actions = [step['recovery_action']] all_actions.extend(actions) for action in all_actions: target_gripper1_point = ros_numpy.msgify( Point, np.array(action['gripper_position'])) target_gripper1_point.z = -0.39 target_gripper2_point = ros_numpy.msgify(Point, np.array([0.45, -0.2, 0.08])) req = DualGripperTrajectoryRequest() req.gripper1_points.append(target_gripper1_point) req.gripper2_points.append(target_gripper2_point) action_srv(req)
def test_bad_encodings(self): mono_arr = np.random.randint(0, 256, size=(240, 360)).astype(np.uint8) mono_arrf = np.random.randint(0, 256, size=(240, 360)).astype(np.float32) rgb_arr = np.random.randint(0, 256, size=(240, 360, 3)).astype(np.uint8) rgb_arrf = np.random.randint(0, 256, size=(240, 360, 3)).astype(np.float32) with self.assertRaises(TypeError): msg = ros_numpy.msgify(Image, rgb_arr, encoding='mono8') with self.assertRaises(TypeError): msg = ros_numpy.msgify(Image, mono_arrf, encoding='mono8') with self.assertRaises(TypeError): msg = ros_numpy.msgify(Image, rgb_arrf, encoding='rgb8') with self.assertRaises(TypeError): msg = ros_numpy.msgify(Image, mono_arr, encoding='rgb8')
def camera_callback(rgb_image, depth_image, camera_info): global wgan global is_initialized global underwater_image_pub global underwater_camera_info_pub global max_depth if is_initialized: try: cv_rgb_image = CvBridge().imgmsg_to_cv2(rgb_image, "rgb8") data = ros_numpy.numpify(depth_image) # fill NaNs with maximum depth value # because simulated Kinect reports NaN where depth is higher than maximum hardware depth where_are_NaNs = np.isnan(data) data[where_are_NaNs] = max_depth #cv2.normalize(data, data, 0, 255, cv2.NORM_MINMAX) cv_depth_image = data except CvBridgeError as e: print(e) cv_underwater_image = wgan.predict(cv_rgb_image, cv_depth_image) #cv2.normalize(cv_underwater_image, cv_underwater_image, 0, 255, cv2.NORM_MINMAX) #cv_underwater_image = np.array(cv_underwater_image).astype(np.uint8) underwater_image = ros_numpy.msgify(Image, cv_underwater_image, "rgb8") underwater_image.header = rgb_image.header underwater_image_pub.publish(underwater_image) underwater_camera_info_pub.publish(camera_info)
def to_point_cloud(self, color_map=None): points = self.to_points(color_map) point_cloud = ros_numpy.msgify(PointCloud2, points, stamp=self.last_update_time, frame_id=self.voi.frame_id) return point_cloud
def test_roundtrip_little_endian(self): arr = np.random.randint(0, 256, size=(240, 360)).astype('<u2') msg = ros_numpy.msgify(Image, arr, encoding='mono16') self.assertEqual(msg.is_bigendian, False) arr2 = ros_numpy.numpify(msg) np.testing.assert_equal(arr, arr2)
def run(self): while True: #We process detection detections, img = detect.Imageprocessing(self.net, "./treated_current_pic_"+self.target+".png") #Send image to ROS topic glob.pub.MUT.acquire() glob.pub.image.publish(msgify(Image, jetson.utils.cudaToNumpy(img), "rgb8")) glob.pub.MUT.release() #if we detect something set flag to block ROS periodic sending, ROS thread clears it if (len(detections) != 0): print('size detections: ', len(detections)) glob.detection_flag.MUT.acquire() glob.detection_flag.value = 1 glob.detection_flag.MUT.release() glob.detection_flag.MUT.acquire() #Debug print("FLAG = ", glob.detection_flag.value) #Debug glob.detection_flag.MUT.release() #Debug #Enumerate all the detections for hurdles detection for detection in detections: print("\nWe detected a " + self.net.GetClassDesc(detection.ClassID) + "\n") glob.pub.MUT.acquire() glob.pub.value.publish(self.detection_to_ROS_number(detection.ClassID)) glob.pub.left.publish(detection.Left) glob.pub.right.publish(detection.Right) glob.pub.top.publish(detection.Top) glob.pub.bottom.publish(detection.Bottom) glob.pub.MUT.release()
def sub_image(self, im_msg): """ Take in an image, and publish a black and white image indicatng which pixels are cone-like """ if self.lookup is None: return old_message_header = deepcopy(im_msg.header) # Announce that we have received a packet self.pub_packet_entered_node.publish(old_message_header) # extract the image im = numpify(im_msg) # Downsample. if self.downsample < 1: im = scipy.misc.imresize(im, self.downsample) im_mask = self.lookup[self.lookup_idx(im)] if self.morph_open: open_size = self.morph_size open_structure = np.ones((open_size, open_size)) im_mask = scipy.ndimage.morphology.binary_opening( im_mask, iterations=self.morph_iter, structure=open_structure) im_mask = (im_mask * 255).astype(np.uint8) # build the message im_mask_msg = msgify(Image, im_mask, encoding='mono8') im_mask_msg.header = old_message_header self.pub_mask.publish(im_mask_msg) # Announce that a packet has left the node. self.pub_packet_left_node.publish(old_message_header)
def main(): rospy.init_node("visualization_node", anonymous=True) node = visualization_node() context_box = zmq.Context() socket_box = context_box.socket(zmq.SUB) socket_box.setsockopt(zmq.SUBSCRIBE, b"") print("Collecting bboxs...") socket_box.connect("tcp://localhost:5557") context_grid = zmq.Context() socket_grid = context_grid.socket(zmq.SUB) socket_grid.setsockopt(zmq.SUBSCRIBE, b"") print("Collecting occupancy grid...") socket_grid.connect("tcp://localhost:5558") while not rospy.is_shutdown(): if socket_box.poll(timeout=1) != 0: bbox = recv_array(socket_box) line_msg = make_box_msg(bbox[0:8, :]) node.bbox_pub.publish(line_msg) arrow_msg = make_arrow(bbox[16:18, :]) node.arrow_pub.publish(arrow_msg) if socket_grid.poll(timeout=1) != 0: grid_image = recv_array(socket_grid) grid_image_msg = ros_numpy.msgify(Image, grid_image, encoding='rgb8') node.grid_image_pub.publish(grid_image_msg)
def __init__(self, number): # int: (str) -> None pose_name = 'S{}'.format(number) position = np.array([ float(x) for x in rospy.get_param('named_poses/{}/position'.format( pose_name)) ]) orientation = np.array([ float(x) for x in rospy.get_param( 'named_poses/{}/orientation'.format(pose_name)) ]) self._pose = PoseStamped() self._pose.header.frame_id = 'map' self._pose.pose.position = msgify(Point, position) self._pose.pose.orientation = msgify(Quaternion, orientation) self._contains = set() self._number = number
def callback(data): data_np = ros_numpy.numpify(data) coords = np.zeros((2, data_np.shape[0])) coords[0] = data_np['x'] coords[1] = data_np['y'] z = data_np['z'] coords = np.floor( np.multiply(np.divide(np.add(coords, 100), 200), img_size - 1)).astype(int) img_np = np.zeros((img_size, img_size, 3), dtype=np.uint8) for i in range(data_np.shape[0]): img_np[coords[0][i], coords[1][i]] += (z[i] * gain).astype(np.uint8) top_left_bounds = np.ceil(img_size / 2).astype(int) - mask_size // 2 bottom_right_bounds = np.ceil(img_size / 2).astype(int) + mask_size // 2 img_mask = np.flip(img_np[top_left_bounds:bottom_right_bounds, top_left_bounds:bottom_right_bounds]) img_mask[25, 25] = (255, 0, 0) img_mask[24, 25] = (0, 255, 0) img_mask[23, 25] = (0, 255, 0) pubMask.publish(ros_numpy.msgify(Image, img_mask, encoding='rgb8'))
def calculate_joints(self, position): self.joint_angle_matricies = [] for angle in position: self.joint_angle_matricies.append( tf.transformations.euler_matrix(0, 0, angle)) T_c = [np.identity(4)] link_states = LinkStates() for index in xrange(len(self.urdf_tranform_matricies)): urdf_transform_matrix = self.urdf_tranform_matricies[index] joint_angle_matrix = self.joint_angle_matricies[index] transform_matrix = np.dot(urdf_transform_matrix, joint_angle_matrix) if index in (6, 7, 8): #sets parent of fingers to link6 index = 6 elif index in (9, 10, 11): #sets parent of fingertip index -= 2 T_c.append(np.dot(T_c[index], transform_matrix)) link_states.pose.append(msgify(Pose, T_c[-1])) link_states.name.append(self.urdf_transforms[index].child_frame_id) #print transforms.header #print link_states.name[-1] #print link_states.pose[-1] #print '-----------------------------------------------' self.link_states = link_states
def _video_thread(self): container = av.open(self._tello.get_udp_video_address()) frame_skip = 300 seq = 0 while not rospy.is_shutdown(): for frame in container.decode(video=0): if 0 < frame_skip: frame_skip = frame_skip - 1 continue start_time = time.time() image_msg_time = rospy.Time.now() image = np.array(frame.to_image()) image_msg = ros_numpy.msgify(Image, image, encoding='rgb8') image_msg.header.stamp = image_msg_time image_msg.header.seq = seq self._camera_raw_pub.publish(image_msg) image_msg = self._bridge.cv2_to_compressed_imgmsg(image) image_msg.header.stamp = image_msg_time image_msg.header.seq = seq self._camera_pub.publish(image_msg) if frame.time_base < 1.0 / 60: time_base = 1.0 / 60 else: time_base = frame.time_base frame_skip = int((time.time() - start_time) / time_base) seq += 1
def slam_callback(self, pose): self.slam_pose_raw = ros_numpy.numpify( pose.pose) #homogeneous transformation matrix from the origin self.slam_pose_np = np.dot(self.slam_pose_correction, self.slam_pose_raw) self.slam_pose_np = self.slam_pose_np * self.scale_factor_matrix self.slam_pose = ros_numpy.msgify(Pose, self.slam_pose_np) #bebop camera link correction and camera calibration correction ang_z = self.euler_from_pose(self.slam_pose)[2] self.slam_pose.position.x += -0.11 * np.cos(ang_z) self.slam_pose.position.y += -0.11 * np.sin(ang_z) self.slam_pose_np = ros_numpy.numpify(self.slam_pose) self.last_slam_time = time.time() if not self.odom_pose == None: #calculates the transfor matrix for the odom position to the modified slam coords system (assumed as true value) self.odom_pose_correction = np.dot( np.linalg.inv(self.odom_pose_raw_np), self.slam_pose_np) else: rospy.loginfo("Havent received any odom coord yet!") self.current_pose = self.slam_pose self.current_pose_np = self.slam_pose_np
def test_roundtrip_little_endian(self): arr = np.random.randint(0, 256, size=(240, 360)).astype('<u2') msg = ros_numpy.msgify(Image, arr, encoding='mono16') self.assertEqual(msg.is_bigendian, False) arr2 = ros_numpy.numpify(msg) np.testing.assert_equal(arr, arr2)
def _callback(self, goal): t_start = rospy.get_time() # Get image image = cv2.cvtColor(self.bridge.compressed_imgmsg_to_cv2(goal.image), cv2.COLOR_BGR2RGB) # Calculate mask with torch.no_grad(): image_tensor = torch.tensor( prepare_img(image).transpose(2, 0, 1)[None]).float() image_input = image_tensor.cuda(self.gpu) mask = self.model(image_input)[0].data.cpu().numpy().transpose( 1, 2, 0) mask = cv2.resize(mask, image.shape[:2][::-1], interpolation=cv2.INTER_CUBIC) mask = mask.argmax(axis=2).astype(np.uint8) # Visualize results if self.visualization_activated: image[:, :, 0][mask == 0] = 0 image[:, :, 1][mask == 0] = 0 image[:, :, 2][mask == 0] = 0 self.pub_visualization.publish( ros_numpy.msgify(Image, image, encoding='8UC3')) # Publish results print('Body detection successful. Current Hz-rate:\t' + str(1 / (rospy.get_time() - t_start))) self.server.set_succeeded( SemSegBodyActResult(mask=self.bridge.cv2_to_compressed_imgmsg( mask, dst_format='png')))
def test_image_encoding(self): if not rospy.get_node_uri(): rospy.init_node("topic_store_tests") client = MongoStorage(collection="python_tests") # Test ROS_Message Serialisation with common Image Types from topic_store.utils import _numpy_types as numpy_types for encoding, np_type in numpy_types.items(): size = (32, 32, np_type[1]) if np_type[1] != 1 else (32, 32) random_array = np.random.random(size) typed_array = (random_array * 255.0).astype(np_type[0]) message = ros_numpy.msgify(Image, typed_array, encoding=encoding) # Insert image message im_document = TopicStore({"image": message}) im_result = client.insert_one(im_document) # Get image message and check data is the same returned_im_document = client.find_by_id(im_result.inserted_id) assert returned_im_document.id == im_result.inserted_id retrieved_array = ros_numpy.numpify( returned_im_document.msgs["image"]) np.testing.assert_equal(typed_array, retrieved_array) # Delete image message client.delete_by_id(im_result.inserted_id)
def _callback(self, goal): t_start = rospy.get_time() # Get image image = cv2.cvtColor(self.bridge.compressed_imgmsg_to_cv2(goal.image), cv2.COLOR_BGR2RGB) # Calculate Mask mask = hand_segmentation(image, self.segmentation_module) # Visualize results if self.visualization_activated: image[:, :, 0][mask == 0] = 0 image[:, :, 1][mask == 0] = 0 image[:, :, 2][mask == 0] = 0 self.pub_visualization.publish( ros_numpy.msgify(Image, image, encoding='8UC3')) # Publish results print('Body detection successful. Current Hz-rate:\t' + str(1 / (rospy.get_time() - t_start))) self.server.set_succeeded( SemSegHandActResult(mask=self.bridge.cv2_to_compressed_imgmsg( mask, dst_format='png')))
def lidar_callback(self, msg): # time1 = time.time() pc = ros_numpy.numpify(msg)[:, self.ind_list].flatten() pc = pc[np.where((pc['x'] < -1.5) & (pc['y'] > -1.5) & (pc['y'] < 1.5))] pc_cropped = np.empty((3, pc['x'].shape[0])) pc_cropped[0, :] = pc['x'] pc_cropped[1, :] = pc['y'] pc_cropped[2, :] = pc['z'] pc_transformed = self.rotation(pc_cropped) pc_transformed = np.concatenate( (pc_transformed, np.expand_dims(pc['intensity'], axis=0)), axis=0) # print(pc_transformed.shape) pc['x'] = pc_transformed[0, :] pc['y'] = pc_transformed[1, :] pc['z'] = pc_transformed[2, :] msg_transformed = ros_numpy.msgify(PointCloud2, pc) msg_transformed.header.frame_id = '/os1_lidar' self.pc_pub.publish(msg_transformed) # print('elapsed time: ', time.time() - time1) # self.send_array(self.socket, pc_transformed) print(str(msg.header.stamp)) np.save(str(msg.header.stamp) + '.npy', pc_transformed) rospy.sleep(1)
def sub_image(self, im_msg): """ Take in an image, and publish a black and white image indicatng which pixels are cone-like """ if self.lookup is None: return old_message_header = deepcopy(im_msg.header) # Announce that we have received a packet self.pub_packet_entered_node.publish(old_message_header) # extract the image im = numpify(im_msg) # Downsample. if self.downsample < 1: im = scipy.misc.imresize(im, self.downsample) im_mask = self.lookup[self.lookup_idx(im)] if self.morph_open: open_size = self.morph_size open_structure = np.ones((open_size, open_size)) im_mask = scipy.ndimage.morphology.binary_opening( im_mask, iterations=self.morph_iter, structure=open_structure) im_mask = (im_mask * 255).astype(np.uint8) # build the message im_mask_msg = msgify(Image, im_mask, encoding='mono8') im_mask_msg.header = old_message_header self.pub_mask.publish(im_mask_msg) # Announce that a packet has left the node. self.pub_packet_left_node.publish(old_message_header)
def test_roundtrip_numpy(self): points_arr = self.makeArray(100) cloud_msg = ros_numpy.msgify(numpy_msg(PointCloud2), points_arr) new_points_arr = ros_numpy.numpify(cloud_msg) np.testing.assert_equal(points_arr, new_points_arr)
def calculate_joints(self, position): self.joint_angle_matricies = [] for angle in position: self.joint_angle_matricies.append(transformations.euler_matrix(0, 0, angle)) T_c = [np.identity(4)] tf_msg = TFMessage() for index in xrange(len(self.urdf_transform_matricies)): urdf_transform_matrix = self.urdf_transform_matricies[index] joint_angle_matrix = self.joint_angle_matricies[index] transform_matrix = np.dot(urdf_transform_matrix, joint_angle_matrix) tf_stamp = TransformStamped() tf_stamp.child_frame_id = self.urdf_transforms[index].child_frame_id tf_stamp.header.frame_id = self.urdf_transforms[index].header.frame_id if index in (8, 10): #sets parent of fingers to link6 index = 6 T_c.append(np.dot(T_c[index], transform_matrix)) tf_stamp.transform = msgify(Transform, T_c[-1]) #tf_stamp.transform = msgify(Transform, transform_matrix) tf_msg.transforms.append(tf_stamp) #print transforms.header #print link_states.name[-1] #print link_states.pose[-1] #print '-----------------------------------------------' return tf_msg
def merged_point_publish(self, point_center, point_left, point_right): print('start merging...') self.merhing_flag = True self.start_time = rospy.Time.now().to_sec() point_left = do_transform_cloud(point_left, self.tf_left) point_right = do_transform_cloud(point_right, self.tf_right) point_center = numpify(point_center) point_left = numpify(point_left) point_right = numpify(point_right) point_set = np.append(np.append(point_center, point_left), point_right) # point_set = np.append(point_right, point_left) merged_points = msgify(PointCloud2, point_set) pub1.publish(merged_points) self.elapsed = rospy.Time.now().to_sec() - self.start_time pub2.publish(self.elapsed) print('Merged PointCloud Data Set is Published') self.center_flag = False self.left_flag = False self.right_flag = False self.merhing_flag = False
def test_convert_dtype_inner(self): fields = [ PointField(name='x', offset=0, count=1, datatype=PointField.FLOAT32), PointField(name='y', offset=4, count=1, datatype=PointField.FLOAT32), PointField(name='vectors', offset=8, count=3, datatype=PointField.FLOAT32) ] dtype = np.dtype([('x', np.float32), ('y', np.float32), ('vectors', np.float32, (3, ))]) conv_fields = ros_numpy.msgify(PointField, dtype, plural=True) self.assertSequenceEqual(fields, conv_fields, 'dtype->Pointfield with inner dimensions') conv_dtype = ros_numpy.numpify(fields, point_step=8) self.assertEqual(dtype, conv_dtype, 'Pointfield->dtype with inner dimensions')
def publish_pc_bnds(self, pc_bnds_dict): pc_bnds = np.concatenate( ( pc_bnds_dict["frontal_low_pc"], #pc_bnds_dict["frontal_mid_pc"], pc_bnds_dict["fl_flipper_pc"], pc_bnds_dict["fr_flipper_pc"], pc_bnds_dict["rl_flipper_pc"], pc_bnds_dict["rr_flipper_pc"]), axis=1) pc_bnds_data = np.zeros(len(pc_bnds[0]), dtype=[('x', np.float32), ('y', np.float32), ('z', np.float32), ('vectors', np.float32, (3, ))]) pc_bnds_data['x'] = pc_bnds[0, :] pc_bnds_data['y'] = pc_bnds[1, :] pc_bnds_data['z'] = pc_bnds[2, :] pc_bnds_data['vectors'] = np.arange(len(pc_bnds[0]))[:, np.newaxis] msg = ros_numpy.msgify(PointCloud2, pc_bnds_data) msg.header.frame_id = self.config["robot_prefix"] + "/" + self.config[ "bl_zpr_frame_name"] msg.header.stamp = rospy.Time.now() self.pc_bnds_publisher.publish(msg)
def map_timer_callback(self, event): if self.debug_merged_map: map_msg = ros_numpy.msgify(ros_numpy.numpy_msg(OccupancyGrid), self.map.grid, info=self.map.info) map_msg.header = Header() map_msg.header.stamp = rospy.Time.now() map_msg.header.frame_id = self.map.frame self.pub_merged_map.publish(map_msg)
def _make_local_map(self, scan): #target frame is map_frame, source frame is laser_link trans = self.tf_buffer.lookup_transform( target_frame=self.map_frame, source_frame=scan.header.frame_id, time=scan.header.stamp, timeout=rospy.Duration(5) ) pos = trans.transform.translation orient = trans.transform.rotation # transform from base to map transform = np.dot( transformations.translation_matrix([pos.x, pos.y, pos.z]), transformations.quaternion_matrix([orient.x, orient.y, orient.z, orient.w]) ) self.map_info.origin.position.x = pos.x - self.map_info.resolution*self.width/2.0 self.map_info.origin.position.y = pos.y - self.map_info.resolution*self.height/2.0 self.ros_map.info = self.map_info self.ros_map.data[...] = -1 mcl_local_map = mcl.Map(self.ros_map) mask = (scan.ranges < scan.range_max) & (scan.ranges > scan.range_min) angles = np.arange(scan.angle_min,scan.angle_max,scan.angle_increment) x = scan.ranges[mask]*np.cos(angles[mask]) y = scan.ranges[mask]*np.sin(angles[mask]) # set the last component to zero to ignore translation ii, jj = mcl_local_map.index_at( np.vstack((x,y,np.zeros(np.sum(mask)),np.ones(np.sum(mask)))).T, world_to_map=transform ) ok = ( (jj >= 0) & (jj < self.width) & (ii >= 0) & (ii < self.height) ) ii = ii[ok] jj = jj[ok] mcl_local_map.grid[ii,jj] = 100 ### TODO: figure out faster serialization issue map_msg = msgify(numpy_msg(OccupancyGrid), mcl_local_map.grid, info=self.map_info) map_msg.header = scan.header map_msg.header.frame_id = 'map' return map_msg
def _make_local_map(self, point): #target frame is map_frame, source frame is laser_link self.map_info.origin.position = point.point self.ros_map.info = self.map_info self.ros_map.data[...] = -1 mcl_local_map = mcl.Map(self.ros_map) mcl_local_map.grid[:,:] = 100 ### TODO: figure out faster serialization issue map_msg = msgify(numpy_msg(OccupancyGrid), mcl_local_map.grid, info=self.map_info) map_msg.header = point.header map_msg.header.frame_id = 'map' return map_msg
def sub_image(self, im_msg): """ Take in an image, and publish a black and white image indicatng which pixels are cone-like """ old_message_header = deepcopy(im_msg.header) # Announce that we have received a packet self.pub_packet_entered_node.publish(old_message_header) # extract the image im = numpify(im_msg) # Downsample. if self.downsample < 1: im = scipy.misc.imresize(im, self.downsample) if self.mode == 'linear': is_cone = np.ones(im.shape[:-1], dtype=np.bool) im = im.astype(np.float32) # red channel for theta in thetas: is_cone = is_cone & (im.dot(theta[:-1]) + theta[-1] > 0) im_mask = np.uint8(is_cone * 255) elif self.mode == 'lookup': self._init_lookup() im_mask = self.lookup[self.lookup_idx(im)] else: raise ValueError("Unrecognized thresholding mode.") if self.morph_open: open_size = self.morph_size open_structure = np.ones((open_size, open_size)) im_mask = scipy.ndimage.morphology.binary_opening( im_mask, iterations=self.morph_iter, structure=open_structure) im_mask = (im_mask * 255).astype(np.uint8) # build the message im_mask_msg = msgify(Image, im_mask, encoding='mono8') im_mask_msg.header = old_message_header self.pub_mask.publish(im_mask_msg) # Announce that a packet has left the node. self.pub_packet_left_node.publish(old_message_header)
def test_pose(self): t = Pose( position=Point(1.0, 2.0, 3.0), orientation=Quaternion(*transformations.quaternion_from_euler(np.pi, 0, 0)) ) t_mat = ros_numpy.numpify(t) np.testing.assert_allclose(t_mat.dot([0, 0, 1, 1]), [1.0, 2.0, 2.0, 1.0]) msg = ros_numpy.msgify(Pose, t_mat) np.testing.assert_allclose(msg.position.x, t.position.x) np.testing.assert_allclose(msg.position.y, t.position.y) np.testing.assert_allclose(msg.position.z, t.position.z) np.testing.assert_allclose(msg.orientation.x, t.orientation.x) np.testing.assert_allclose(msg.orientation.y, t.orientation.y) np.testing.assert_allclose(msg.orientation.z, t.orientation.z) np.testing.assert_allclose(msg.orientation.w, t.orientation.w)
def test_transform(self): t = Transform( translation=Vector3(1, 2, 3), rotation=Quaternion(*transformations.quaternion_from_euler(np.pi, 0, 0)) ) t_mat = ros_numpy.numpify(t) np.testing.assert_allclose(t_mat.dot([0, 0, 1, 1]), [1.0, 2.0, 2.0, 1.0]) msg = ros_numpy.msgify(Transform, t_mat) np.testing.assert_allclose(msg.translation.x, t.translation.x) np.testing.assert_allclose(msg.translation.y, t.translation.y) np.testing.assert_allclose(msg.translation.z, t.translation.z) np.testing.assert_allclose(msg.rotation.x, t.rotation.x) np.testing.assert_allclose(msg.rotation.y, t.rotation.y) np.testing.assert_allclose(msg.rotation.z, t.rotation.z) np.testing.assert_allclose(msg.rotation.w, t.rotation.w)
def sub_marker(self, marker): marker_loc = numpify(marker.point, hom=True) if np.isnan(marker_loc).any(): self.marker_target_pose = None return trans = self.tf_buffer.lookup_transform( target_frame="base_link", source_frame=self.map_frame, time=marker.header.stamp, timeout=rospy.Duration(1) ) transform = numpify(trans.transform) marker_loc = marker_loc.dot(transform.T) marker_target_pose = PoseStamped() marker_target_pose.header = marker.header marker_target_pose.header.frame_id = 'map' marker_target_pose.pose = Pose( msgify(Point, marker_loc), Quaternion(0,0,0,1) ) self.marker_target_pose = marker_target_pose
def sub_image(self, im_msg, image_info): """ Take a thresholded image and output cone position metrics. """ # Announce that we have received a packet self.pub_packet_entered_node.publish(im_msg.header) # Extract the image im = numpify(im_msg) im2 = (im > 160).astype(bool) height, width = im.shape[:2] # Label each connected blob in the image. labels, num_labels = scipy.ndimage.measurements.label(im2) msg = CameraObjectsStamped(header=im_msg.header) if num_labels == 0: self.pub_obj.publish(msg) # No objects. return # Calculate the size of each blob. blob_sizes = scipy.ndimage.measurements.sum(im2, labels=labels, index=xrange(0,num_labels+1)) # Find the biggest blob. biggest_blob = np.argmax(blob_sizes) - 1 pp = scipy.ndimage.measurements.find_objects(labels)[biggest_blob] bbox = BoundingBox(pp[1].start, pp[0].start, pp[1].stop, pp[0].stop) # Calculate centroid (units of pixels). cx, cy = np.mean([bbox.maxx, bbox.minx]), np.mean([bbox.maxy, bbox.miny]) sx, sy = (bbox.maxx - bbox.minx), (bbox.maxy - bbox.miny) # geometric mean p_matrix = np.array(image_info.P).reshape((3, 4)) uvw = p_matrix.dot([self.WIDTH, self.HEIGHT, 0, 0]) u, v, w = uvw cz = np.min([(u / sx), (v / sy)]) # load the depth image message # d_im = numpify(d_im_msg) * 0.001 # TODO # cz = np.mean(d_im[(labels == biggest_blob) & (d_im != 0)]) # get_depth_for(biggest_blob) # print u / sx, v / sy, sx, sy, uvw #, np.max(d_im) # projection matrix # [px, py, 1] = P.dot([X, Y, Z, 1]) # solve the simultaneous equations # [cx, cy, 1] = P.dot([X, Y, Z, 1]) # cz = Z # => 0 = np.array([0, 0, 1, -cz]).dot([X, Y, Z, 1]) try: position = np.linalg.solve( np.vstack(( p_matrix, [0, 0, 1, -cz] )), np.array([cx, cy, 1, 0]) ) except np.linalg.LinAlgError: rospy.logwarn("Matrix was singular %s", np.vstack(( p_matrix, [0, 0, 1, -cz] ))) return # theses are homogeneous coordinates position /= position[-1] position = Point(*position[:3]) msg.objects = [ CameraObject( label='cone', center=position, size=Vector3( x=remap((bbox.maxx - bbox.minx), 0, width, 0, 1), y=remap((bbox.maxy - bbox.miny), 0, height, 0, 1), z=0 ) ) ] self.pub_obj.publish(msg) # Make overlay visualizations. debug_overlay = np.zeros((height, width, 3), dtype=np.uint8) debug_overlay[...,0] = im debug_overlay[...,1] = im debug_overlay[...,2] = im self.draw_bbox(debug_overlay, bbox) self.draw_point(debug_overlay, cx, cy) debug_overlay_msg = msgify(Image, debug_overlay, encoding='rgb8') debug_overlay_msg.header = im_msg.header self.pub_debug_overlay.publish(debug_overlay_msg) # Announce that a packet has left the node. self.pub_packet_left_node.publish(im_msg.header)
def test_roundtrip_rgb8(self): arr = np.random.randint(0, 256, size=(240, 360, 3)).astype(np.uint8) msg = ros_numpy.msgify(Image, arr, encoding='rgb8') arr2 = ros_numpy.numpify(msg) np.testing.assert_equal(arr, arr2)