The runnable demo for reception task.

common/helpers/cv2_img/src/cv2_img/__init__.py

@@ -1,5 +1,6 @@
 import rospy
 import numpy as np
+import ros_numpy as rnp
 
 from PIL import Image
 from sensor_msgs.msg import Image as SensorImage
@@ -64,9 +65,47 @@ def msg_to_cv2_img(msg: SensorImage):
     img = np.array(img)
     img = img[:, :, ::-1].copy()
 
+    rospy.logwarn('shape of the image: ' + str(img.shape))
+
     return img
 
 
+# pcl_xyz = rnp.point_cloud2.pointcloud2_to_xyz_array(pcl_msg, remove_nans=False)
+def pcl_msg_to_xyz(pcl_msg):
+    pcl_xyz = rnp.point_cloud2.pointcloud2_to_xyz_array(pcl_msg, remove_nans=False)
+
+    rospy.logwarn('shape of the image: ' + str(pcl_xyz.shape))
+
+    return pcl_xyz
+
+
+def pcl_msg_to_cv2(pcl_msg):
+    """
+    Constructs a cv2 image from a PointCloud2 message.
+
+    Parameters
+    ----------
+    pcl_msg : sensor_msgs/PointCloud2
+        Input pointcloud (organised)
+
+    Returns
+    -------
+    np.array : cv2 image
+    """
+
+    # Extract rgb image from pointcloud
+    frame = np.fromstring(pcl_msg.data, dtype=np.uint8)
+    frame = frame.reshape(pcl_msg.height, pcl_msg.width, 32)
+    frame = frame[:,:,16:19]
+
+    # Ensure array is contiguous
+    frame = np.ascontiguousarray(frame, dtype=np.uint8)
+
+    rospy.logwarn('shape of the image: ' + str(frame.shape))
+
+    return frame
+
+
 def extract_mask_region(frame, mask, expand_x=0.5, expand_y=0.5):
     """
     Extracts the face region from the image and expands the region by the specified amount.

common/helpers/colour_estimation/CMakeLists.txt → common/helpers/feature_extractor/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.0.2)
-project(colour_estimation)
+project(feature_extractor)
 
 ## Compile as C++11, supported in ROS Kinetic and newer
 # add_compile_options(-std=c++11)
@@ -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 feature_extractor
 #  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}/feature_extractor.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/feature_extractor_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_feature_extractor.cpp)
 # if(TARGET ${PROJECT_NAME}-test)
 #   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
 # endif()

common/helpers/colour_estimation/package.xml → common/helpers/feature_extractor/package.xml

@@ -1,8 +1,8 @@
 <?xml version="1.0"?>
 <package format="2">
-  <name>colour_estimation</name>
+  <name>feature_extractor</name>
   <version>0.0.0</version>
-  <description>Python utilities for estimating the name of given colours.</description>
+  <description>Various PyTorch helpers and utilties</description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag -->
   <!-- Example:  -->
@@ -19,13 +19,13 @@
   <!-- 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/feature_extractor</url> -->
 
 
   <!-- Author tags are optional, multiple are allowed, one per tag -->
   <!-- Authors do not have to be maintainers, but could be -->
   <!-- Example: -->
-  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+  <author email="benteng.ma@kcl.ac.uk">Benteng Ma</author>
 
 
   <!-- The *depend tags are used to specify dependencies -->

common/helpers/colour_estimation/setup.py → common/helpers/feature_extractor/setup.py

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