Spring Clean (Part 1)

.gitmodules

@@ -1,3 +0,0 @@
-[submodule "common/third_party/aruco_ros"]
-	path = common/third_party/aruco_ros
-	url = [email protected]:pal-robotics/aruco_ros.git

legacy/lasr_web_server/CMakeLists.txt → common/helpers/cv2_pcl/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.0.2)
-project(lasr_web_server)
+project(cv2_pcl)
 
 ## 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 lasr_web_server
+#  LIBRARIES cv2_pcl
 #  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}/lasr_web_server.cpp
+#   src/${PROJECT_NAME}/cv2_pcl.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/lasr_web_server_node.cpp)
+# add_executable(${PROJECT_NAME}_node src/cv2_pcl_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_lasr_web_server.cpp)
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_cv2_pcl.cpp)
 # if(TARGET ${PROJECT_NAME}-test)
 #   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
 # endif()

legacy/lasr_dialogflow/package.xml → common/helpers/cv2_pcl/package.xml

@@ -1,25 +1,25 @@
 <?xml version="1.0"?>
 <package format="2">
-  <name>lasr_dialogflow</name>
+  <name>cv2_pcl</name>
   <version>0.0.0</version>
-  <description>The lasr_dialogflow package</description>
+  <description>The cv2_pcl package</description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag -->
   <!-- Example:  -->
   <!-- <maintainer email="[email protected]">Jane Doe</maintainer> -->
-  <maintainer email="jared@todo.todo">jared</maintainer>
-
+  <maintainer email="jared[email protected]">Jared Swift</maintainer>
+a
 
   <!-- 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>TODO</license>
+  <license>MIT</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/lasr_dialogflow</url> -->
+  <!-- <url type="website">http://wiki.ros.org/cv2_pcl</url> -->
 
 
   <!-- Author tags are optional, multiple are allowed, one per tag -->
@@ -49,6 +49,7 @@
   <!-- Use doc_depend for packages you need only for building documentation: -->
   <!--   <doc_depend>doxygen</doc_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
+  <depend>sensor_msgs</depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->

legacy/people_detection/train_dataset_model/setup.py → common/helpers/cv2_pcl/setup.py

@@ -3,9 +3,6 @@
 from distutils.core import setup
 from catkin_pkg.python_setup import generate_distutils_setup
 
-setup_args = generate_distutils_setup(
-    packages=['train_dataset_model'],
-    package_dir={'': 'src'}
-)
+setup_args = generate_distutils_setup(packages=["cv2_pcl"], package_dir={"": "src"})
 
 setup(**setup_args)

legacy/common_math/src/common_math/math_.py → common/helpers/cv2_pcl/src/cv2_pcl/__init__.py

@@ -1,34 +1,19 @@
-#!/usr/bin/python3
-import numpy as np
-import rospy
 import numpy as np
+from sensor_msgs.msg import PointCloud2
 import ros_numpy as rnp
-# import cv2
-# from geometry_msgs.msg import PointStamped, Point
-# from std_msgs.msg import Header
-# from robocup_receptionist.srv import TfTransform, TfTransformRequest
-import math
 import cv2
 
+Mat = np.ndarray
 
 
-def pcl_msg_to_cv2(pcl_msg):
+def pcl_to_cv2(pcl: PointCloud2) -> Mat:
     """
-    Constructs a cv2 image from a PointCloud2 message.
-
-    Parameters
-    ----------
-    pcl_msg : sensor_msgs/PointCloud2
-        Input pointcloud (organised)
-
-    Returns
-    -------
-    np.array : cv2 image
+    Convert a given PointCloud2 message to a 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 = np.fromstring(pcl.data, dtype=np.uint8)
+    frame = frame.reshape(pcl.height, pcl.width, 32)
     frame = frame[:, :, 16:19]
 
     # Ensure array is contiguous
@@ -37,12 +22,16 @@ def pcl_msg_to_cv2(pcl_msg):
     return frame
 
 
-def seg_to_centroid(pcl_msg, xyseg):
+def seg_to_centroid(pcl: PointCloud2, xyseg: np.ndarray) -> np.ndarray:
+    """
+    Computes the centroid of a given segment in a pointcloud
+    """
+
     # Convert xyseg to contours
     contours = xyseg.reshape(-1, 2)
 
     # cv2 image
-    cv_im = pcl_msg_to_cv2(pcl_msg)
+    cv_im = pcl_to_cv2(pcl)
     # Compute mask from contours
     mask = np.zeros(shape=cv_im.shape[:2])
     cv2.fillPoly(mask, pts=[contours], color=(255, 255, 255))
@@ -54,20 +43,24 @@ def seg_to_centroid(pcl_msg, xyseg):
         return np.full(3, np.nan)
 
     # Unpack pointcloud into xyz array
-    pcl_xyz = rnp.point_cloud2.pointcloud2_to_xyz_array(pcl_msg, remove_nans=False)
+    pcl_xyz = rnp.point_cloud2.pointcloud2_to_xyz_array(pcl, remove_nans=False)
 
     # Extract points of interest
     xyz_points = [pcl_xyz[x][y] for x, y in indices]
 
-    return np.nanmean(xyz_points, axis=0)
+    return np.mean(xyz_points, axis=0)
+
 
+def bb_to_centroid(pcl: PointCloud2, x: int, y: int, w: int, h: int) -> np.ndarray:
+    """
+    Computes the centroid of a given bounding box in a pointcloud
+    """
 
-def bb_to_centroid(pcl_msg, x, y, w, h):
     # Convert xywh to bounding box coordinates.
     x1, y1, x2, y2 = x, y, x + w, y + h
 
     # cv2 image
-    frame = pcl_msg_to_cv2(pcl_msg)
+    frame = pcl_to_cv2(pcl)
     # Compute mask from bounding box
     mask = np.zeros(shape=frame.shape[:2])
     mask[y1:y2, x1:x2] = 255  # bounding box dimensions
@@ -78,27 +71,10 @@ def bb_to_centroid(pcl_msg, x, y, w, h):
         return np.full(3, np.nan)
 
     # Unpack pointcloud into xyz array
-    pcl_xyz = rnp.point_cloud2.pointcloud2_to_xyz_array(pcl_msg, remove_nans=False)
+    pcl_xyz = rnp.point_cloud2.pointcloud2_to_xyz_array(pcl, remove_nans=False)
 
     # Extract points of interest
     xyz_points = [pcl_xyz[x][y] for x, y in indices]
 
     # Get mean (centroid) point.
     return np.nanmean(xyz_points, axis=0)
-
-
-def euclidian(a, b):
-    '''
-    Get the Euclidean distance between two positions
-    '''
-    return math.sqrt(
-        math.pow(a.x - b.x, 2) +
-        math.pow(a.y - b.y, 2) +
-        math.pow(a.z - b.z, 2)
-    )
-def euclidian_distance(p1, p2):
-        x1, y1 = p1
-        x2, y2 = p2
-        a = np.array((x1, y1))
-        b = np.array((x2, y2))
-        return np.linalg.norm(a - b)

legacy/markers/CMakeLists.txt → common/helpers/markers/CMakeLists.txt

@@ -8,11 +8,9 @@ project(markers)
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS
-  geometry_msgs
-  message_generation
-  roscpp
   rospy
-  std_msgs
+  visualization_msgs
+  geometry_msgs
 )
 
 ## System dependencies are found with CMake's conventions
@@ -22,7 +20,7 @@ find_package(catkin REQUIRED COMPONENTS
 ## Uncomment this if the package has a setup.py. This macro ensures
 ## modules and global scripts declared therein get installed
 ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
- catkin_python_setup()
+# catkin_python_setup()
 
 ################################################
 ## Declare ROS messages, services and actions ##
@@ -72,7 +70,7 @@ find_package(catkin REQUIRED COMPONENTS
 ## Generate added messages and services with any dependencies listed here
 # generate_messages(
 #   DEPENDENCIES
-#   geometry_msgs#   std_msgs
+#   visualization_msgs
 # )
 
 ################################################
@@ -107,7 +105,7 @@ find_package(catkin REQUIRED COMPONENTS
 catkin_package(
 #  INCLUDE_DIRS include
 #  LIBRARIES markers
-#  CATKIN_DEPENDS geometry_msgs message_generation roscpp rospy std_msgs
+#  CATKIN_DEPENDS rospy visualization_msgs
 #  DEPENDS system_lib
 )
 

legacy/markers/package.xml → common/helpers/markers/package.xml

@@ -7,13 +7,13 @@
   <!-- One maintainer tag required, multiple allowed, one person per tag -->
   <!-- Example:  -->
   <!-- <maintainer email="[email protected]">Jane Doe</maintainer> -->
-  <maintainer email="[email protected]">nicole</maintainer>
+  <maintainer email="[email protected]">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>TODO</license>
+  <license>MIT</license>
 
 
   <!-- Url tags are optional, but multiple are allowed, one per tag -->
@@ -49,19 +49,15 @@
   <!-- Use doc_depend for packages you need only for building documentation: -->
   <!--   <doc_depend>doxygen</doc_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
-  <build_depend>geometry_msgs</build_depend>
-  <build_depend>message_generation</build_depend>
-  <build_depend>roscpp</build_depend>
   <build_depend>rospy</build_depend>
-  <build_depend>std_msgs</build_depend>
-  <build_export_depend>geometry_msgs</build_export_depend>
-  <build_export_depend>roscpp</build_export_depend>
+  <build_depend>visualization_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
   <build_export_depend>rospy</build_export_depend>
-  <build_export_depend>std_msgs</build_export_depend>
-  <exec_depend>geometry_msgs</exec_depend>
-  <exec_depend>roscpp</exec_depend>
+  <build_export_depend>visualization_msgs</build_export_depend>
+  <build_export_depend>geometry_msgs</build_export_depend>
   <exec_depend>rospy</exec_depend>
-  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>visualization_msgs</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->

common/helpers/tf_module/setup.py → common/helpers/markers/setup.py

@@ -3,10 +3,6 @@
 from distutils.core import setup
 from catkin_pkg.python_setup import generate_distutils_setup
 
-# fetch values from package.xml
-setup_args = generate_distutils_setup(
-  packages=['tf_module'],
-  package_dir={'': 'src'}
-)
+setup_args = generate_distutils_setup(packages=["markers"], package_dir={"": "src"})
 
 setup(**setup_args)

common/helpers/markers/src/markers/__init__.py

@@ -0,0 +1,50 @@
+import rospy
+
+from visualization_msgs.msg import Marker
+from geometry_msgs.msg import PointStamped
+
+from collections import defaultdict
+
+publisher_counts = defaultdict(int)
+
+
+def create_marker(
+    point_stamped: PointStamped,
+    idx: int,
+    r: float = 0.0,
+    g: float = 1.0,
+    b: float = 0.0,
+):
+    marker_msg = Marker()
+    marker_msg.header.frame_id = point_stamped.header.frame_id
+    marker_msg.header.stamp = point_stamped.header.stamp
+    marker_msg.id = idx
+    marker_msg.type = Marker.SPHERE
+    marker_msg.action = Marker.ADD
+    marker_msg.pose.position = point_stamped.point
+    marker_msg.pose.orientation.w = 1.0
+    marker_msg.scale.x = 0.1
+    marker_msg.scale.y = 0.1
+    marker_msg.scale.z = 0.1
+    marker_msg.color.a = 1.0
+    marker_msg.color.r = r
+    marker_msg.color.g = g
+    marker_msg.color.b = b
+    return marker_msg
+
+
+def create_and_publish_marker(
+    publisher: rospy.Publisher,
+    point_stamped: PointStamped,
+    idx: int | None = None,
+    r: float = 0.0,
+    g: float = 1.0,
+    b: float = 0.0,
+):
+    if idx is None:
+        global publisher_counts
+        idx = publisher_counts[publisher]
+        publisher_counts[publisher] += 1
+
+    marker_msg = create_marker(point_stamped, idx, r, g, b)
+    publisher.publish(marker_msg)