Merge branch 'main' into receptionist-look-at-person

Author: jws-1

common/helpers/colour_estimation/README.md

@@ -4,6 +4,8 @@
 import cv2
 from cv2_img import cv2_img_to_msg
 
+from typing import Tuple, Union
+
 Mat = np.ndarray
 
 def pcl_to_img_msg(pcl: PointCloud2) -> Mat:
@@ -17,14 +19,19 @@ def pcl_to_img_msg(pcl: PointCloud2) -> Mat:
 
 
 
-def pcl_to_cv2(pcl: PointCloud2) -> Mat:
+def pcl_to_cv2(
+    pcl: PointCloud2, height: Union[int, None] = None, width: Union[int, None] = None
+) -> Mat:
     """
     Convert a given PointCloud2 message to a cv2 image
     """
 
+    height = height or pcl.height
+    width = width or pcl.width
+
     # Extract rgb image from pointcloud
     frame = np.fromstring(pcl.data, dtype=np.uint8)
-    frame = frame.reshape(pcl.height, pcl.width, 32)
+    frame = frame.reshape(height, width, -1)
     frame = frame[:, :, 16:19]
 
     # Ensure array is contiguous
@@ -33,16 +40,25 @@ def pcl_to_cv2(pcl: PointCloud2) -> Mat:
     return frame
 
 
-def seg_to_centroid(pcl: PointCloud2, xyseg: np.ndarray) -> np.ndarray:
+def seg_to_centroid(
+    pcl: PointCloud2,
+    xyseg: np.ndarray,
+    height: Union[int, None] = None,
+    width: Union[int, None] = None,
+) -> np.ndarray:
     """
     Computes the centroid of a given segment in a pointcloud
     """
 
+    height = height or pcl.height
+    width = width or pcl.width
+
     # Convert xyseg to contours
     contours = xyseg.reshape(-1, 2)
 
     # cv2 image
-    cv_im = pcl_to_cv2(pcl)
+    cv_im = pcl_to_cv2(pcl, height, width)
+
     # Compute mask from contours
     mask = np.zeros(shape=cv_im.shape[:2])
     cv2.fillPoly(mask, pts=[contours], color=(255, 255, 255))
@@ -55,23 +71,35 @@ def seg_to_centroid(pcl: PointCloud2, xyseg: np.ndarray) -> np.ndarray:
 
     # Unpack pointcloud into xyz array
     pcl_xyz = rnp.point_cloud2.pointcloud2_to_xyz_array(pcl, remove_nans=False)
+    pcl_xyz = pcl_xyz.reshape(height, width, 3)
 
     # Extract points of interest
     xyz_points = [pcl_xyz[x][y] for x, y in indices]
 
     return np.nanmean(xyz_points, axis=0)
 
 
-def bb_to_centroid(pcl: PointCloud2, x: int, y: int, w: int, h: int) -> np.ndarray:
+def bb_to_centroid(
+    pcl: PointCloud2,
+    x: int,
+    y: int,
+    w: int,
+    h: int,
+    height: Union[int, None] = None,
+    width: Union[int, None] = None,
+) -> np.ndarray:
     """
     Computes the centroid of a given bounding box in a pointcloud
     """
 
+    height = height or pcl.height
+    width = width or pcl.width
+
     # Convert xywh to bounding box coordinates.
     x1, y1, x2, y2 = x, y, x + w, y + h
 
     # cv2 image
-    frame = pcl_to_cv2(pcl)
+    frame = pcl_to_cv2(pcl, height, width)
     # Compute mask from bounding box
     mask = np.zeros(shape=frame.shape[:2])
     mask[y1:y2, x1:x2] = 255  # bounding box dimensions
@@ -83,6 +111,7 @@ def bb_to_centroid(pcl: PointCloud2, x: int, y: int, w: int, h: int) -> np.ndarr
 
     # Unpack pointcloud into xyz array
     pcl_xyz = rnp.point_cloud2.pointcloud2_to_xyz_array(pcl, remove_nans=False)
+    pcl_xyz = pcl_xyz.reshape(height, width, 3)
 
     # Extract points of interest
     xyz_points = [pcl_xyz[x][y] for x, y in indices]

common/helpers/colour_estimation/doc/EXAMPLE.md

@@ -1,13 +1,13 @@
 cmake_minimum_required(VERSION 3.0.2)
-project(colour_estimation)
+project(navigation_helpers)
 
 ## Compile as C++11, supported in ROS Kinetic and newer
 # add_compile_options(-std=c++11)
 
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
-find_package(catkin REQUIRED)
+find_package(catkin REQUIRED geometry_msgs)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -100,7 +100,7 @@ catkin_python_setup()
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
 #  INCLUDE_DIRS include
-#  LIBRARIES colour_estimation
+#  LIBRARIES navigation_helpers
 #  CATKIN_DEPENDS other_catkin_pkg
 #  DEPENDS system_lib
 )
@@ -118,7 +118,7 @@ include_directories(
 
 ## Declare a C++ library
 # add_library(${PROJECT_NAME}
-#   src/${PROJECT_NAME}/colour_estimation.cpp
+#   src/${PROJECT_NAME}/navigation_helpers.cpp
 # )
 
 ## Add cmake target dependencies of the library
@@ -129,7 +129,7 @@ include_directories(
 ## Declare a C++ executable
 ## With catkin_make all packages are built within a single CMake context
 ## The recommended prefix ensures that target names across packages don't collide
-# add_executable(${PROJECT_NAME}_node src/colour_estimation_node.cpp)
+# add_executable(${PROJECT_NAME}_node src/navigation_helpers_node.cpp)
 
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
@@ -193,7 +193,7 @@ include_directories(
 #############
 
 ## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_colour_estimation.cpp)
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_navigation_helpers.cpp)
 # if(TARGET ${PROJECT_NAME}-test)
 #   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
 # endif()

common/helpers/colour_estimation/doc/PREREQUISITES.md

@@ -1,25 +1,25 @@
 <?xml version="1.0"?>
 <package format="2">
-  <name>colour_estimation</name>
+  <name>navigation_helpers</name>
   <version>0.0.0</version>
-  <description>Python utilities for estimating the name of given colours.</description>
+  <description>The navigation_helpers package</description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag -->
   <!-- Example:  -->
   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
-  <maintainer email="me@insrt.uk">Paul Makles</maintainer>
+  <maintainer email="jared.swift@kcl.ac.uk">Jared Swift</maintainer>
 
 
   <!-- One license tag required, multiple allowed, one license per tag -->
   <!-- Commonly used license strings: -->
   <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
-  <license>MIT</license>
+  <license>TODO</license>
 
 
   <!-- Url tags are optional, but multiple are allowed, one per tag -->
   <!-- Optional attribute type can be: website, bugtracker, or repository -->
   <!-- Example: -->
-  <!-- <url type="website">http://wiki.ros.org/colour_estimation</url> -->
+  <!-- <url type="website">http://wiki.ros.org/navigation_helpers</url> -->
 
 
   <!-- Author tags are optional, multiple are allowed, one per tag -->

common/helpers/colour_estimation/doc/USAGE.md

@@ -4,8 +4,7 @@
 from catkin_pkg.python_setup import generate_distutils_setup
 
 setup_args = generate_distutils_setup(
-    packages=['colour_estimation'],
-    package_dir={'': 'src'}
+    packages=["navigation_helpers"], package_dir={"": "src"}
 )
 
 setup(**setup_args)