Merge branch 'main' of https://github.com/MBTMBTMBT/LASR_ben

.github/workflows/continuous.yml

@@ -0,0 +1,25 @@
+name: Continuous
+
+on:
+  push:
+    branches:
+    - 'main'
+jobs:
+  QC:
+    runs-on: ubuntu-20.04
+    steps:
+    - name: Checkout Repository and Submodules
+      uses: actions/checkout@v4
+    - name: Quality Control
+      uses: "./.github/workflows/qc"
+  Documentation:
+    runs-on: ubuntu-20.04
+    env:
+      DOCS_DEPLOY_KEY: ${{ secrets.DOCS_DEPLOY_KEY }}
+    steps:
+    - name: Checkout Repository and Submodules
+      uses: actions/checkout@v4
+      with:
+        path: "src/lasr-base"
+    - name: Build & Deploy Documentation
+      uses: "./src/lasr-base/.github/workflows/docs"

.github/workflows/docs/action.yml

@@ -0,0 +1,48 @@
+name: Build & Deploy Documentation
+
+runs:
+  using: "composite"
+  steps:
+
+    - name: Install ROS Noetic
+      shell: bash
+      run: |
+        sudo add-apt-repository universe
+        sudo add-apt-repository restricted
+        sudo add-apt-repository multiverse
+
+        sudo apt update
+
+        sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
+
+        sudo apt install curl # if you haven't already installed curl
+        curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add -
+
+        sudo apt update
+        sudo apt install ros-noetic-ros-base python3-catkin-tools
+
+    - name: Build documentation package
+      shell: bash
+      run: |
+        source /opt/ros/noetic/setup.bash
+        catkin build documentation
+
+    - name: Use Node.js
+      uses: actions/setup-node@v3
+      with:
+        node-version: 20
+
+    - name: Build documentation
+      shell: bash
+      run: |
+        source /opt/ros/noetic/setup.bash
+        source devel/setup.bash
+        rosrun documentation build.py
+
+    - name: Deploy to GitHub Pages
+      uses: peaceiris/actions-gh-pages@v3
+      if: github.event_name != 'pull_request' && github.ref_name == 'main'
+      with:
+        deploy_key: ${{ env.DOCS_DEPLOY_KEY }}
+        publish_dir: ./src/lasr-base/documentation/web/build
+        external_repository: lasr-at-home/docs

.github/workflows/pr.yml

@@ -0,0 +1,14 @@
+name: Pull Request
+
+on:
+  pull_request:
+    branches:
+    - '*'
+jobs:
+  QC:
+    runs-on: ubuntu-20.04
+    steps:
+    - name: Checkout
+      uses: actions/checkout@v4
+    - name: Quality Control
+      uses: "./.github/workflows/qc"

.github/workflows/qc/action.yml

@@ -0,0 +1,12 @@
+name: Quality Control
+
+runs:
+  using: "composite"
+  steps:
+
+  - name: Python Formatting
+    uses: psf/black@stable
+    with:
+      options: "--check --verbose"
+      jupyter: true
+      version: "24.3.0"

common/helpers/cv2_img/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=['cv2_img'],
-  package_dir={'': 'src'}
-)
+setup_args = generate_distutils_setup(packages=["cv2_img"], package_dir={"": "src"})
 
-setup(**setup_args)
+setup(**setup_args)

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

@@ -20,7 +20,7 @@ def cv2_img_to_msg(img, stamp=None):
     msg.header.stamp = rospy.Time.now() if stamp is None else stamp
     msg.width = width
     msg.height = height
-    msg.encoding = 'bgr8'
+    msg.encoding = "bgr8"
     msg.is_bigendian = 1
     msg.step = 3 * width
     msg.data = img.tobytes()
@@ -37,13 +37,13 @@ def msg_to_pillow_img(msg: SensorImage):
     """
 
     size = (msg.width, msg.height)
-    if msg.encoding in ['bgr8', '8UC3']:
-        img = Image.frombytes('RGB', size, msg.data, 'raw')
+    if msg.encoding in ["bgr8", "8UC3"]:
+        img = Image.frombytes("RGB", size, msg.data, "raw")
 
         # BGR => RGB
         img = Image.fromarray(np.array(img)[:, :, ::-1])
-    elif msg.encoding == 'rgb8':
-        img = Image.frombytes('RGB', size, msg.data, 'raw')
+    elif msg.encoding == "rgb8":
+        img = Image.frombytes("RGB", size, msg.data, "raw")
     else:
         raise Exception("Unsupported format.")
 
@@ -80,8 +80,7 @@ def extract_mask_region(frame, mask, expand_x=0.5, expand_y=0.5):
     # only requiring cv2 if we need it
     import cv2
 
-    contours, _ = cv2.findContours(
-        mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
+    contours, _ = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
 
     if contours:
         largest_contour = max(contours, key=cv2.contourArea)
@@ -99,6 +98,6 @@ def extract_mask_region(frame, mask, expand_x=0.5, expand_y=0.5):
         new_w = min(frame.shape[1] - x, new_w)
         new_h = min(frame.shape[0] - y, new_h)
 
-        face_region = frame[y:y+int(new_h), x:x+int(new_w)]
+        face_region = frame[y : y + int(new_h), x : x + int(new_w)]
         return face_region
     return None

common/helpers/numpy2message/setup.py

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

common/helpers/numpy2message/src/numpy2message/__init__.py

@@ -8,12 +8,11 @@ def numpy2message(np_array: np.ndarray) -> list:
     return data, shape, dtype
 
 
-def message2numpy(data:bytes, shape:list, dtype:str) -> np.ndarray:
+def message2numpy(data: bytes, shape: list, dtype: str) -> np.ndarray:
     array_shape = tuple(shape)
     array_dtype = np.dtype(dtype)
 
     deserialized_array = np.frombuffer(data, dtype=array_dtype)
     deserialized_array = deserialized_array.reshape(array_shape)
 
     return deserialized_array
-

common/navigation/tiago_controllers/setup.py

@@ -5,8 +5,7 @@
 
 # fetch values from package.xml
 setup_args = generate_distutils_setup(
-  packages=['tiago_controllers'],
-  package_dir={'': 'src'}
+    packages=["tiago_controllers"], package_dir={"": "src"}
 )
 
-setup(**setup_args)
+setup(**setup_args)

common/navigation/tiago_controllers/src/tiago_controllers/__init__.py

@@ -1,3 +1,2 @@
 from .controllers.base_controller import BaseController
 from .controllers.head_controller import HeadController
-

common/navigation/tiago_controllers/src/tiago_controllers/base_planner.py

@@ -2,7 +2,14 @@
 
 import rospy
 import rosservice
-from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, PoseStamped, Vector3
+from geometry_msgs.msg import (
+    Pose,
+    PoseWithCovarianceStamped,
+    Point,
+    Quaternion,
+    PoseStamped,
+    Vector3,
+)
 from std_msgs.msg import Header
 from nav_msgs.srv import GetPlan
 import numpy as np
@@ -11,17 +18,21 @@
 
 
 def make_plan(current_pose, x, y, tol=0.01, max_dist=None):
-
-    if '/move_base/NavfnROS/make_plan' in rosservice.get_service_list():
-        make_plan_service = '/move_base/NavfnROS/make_plan'
-    elif '/move_base/GlobalPlanner/make_plan' in rosservice.get_service_list():
-        make_plan_service = '/move_base/GlobalPlanner/make_plan'
+    if "/move_base/NavfnROS/make_plan" in rosservice.get_service_list():
+        make_plan_service = "/move_base/NavfnROS/make_plan"
+    elif "/move_base/GlobalPlanner/make_plan" in rosservice.get_service_list():
+        make_plan_service = "/move_base/GlobalPlanner/make_plan"
     else:
-        rospy.loginfo('Failed to find the make_plan server')
+        rospy.loginfo("Failed to find the make_plan server")
         return False, None, None
-
-    start = PoseStamped(header=Header(frame_id="map", stamp=rospy.Time.now()), pose=current_pose)
-    goal = PoseStamped(header=Header(frame_id="map", stamp=rospy.Time.now()), pose=Pose(position=Point(x, y, 0), orientation=current_pose.orientation))
+
+    start = PoseStamped(
+        header=Header(frame_id="map", stamp=rospy.Time.now()), pose=current_pose
+    )
+    goal = PoseStamped(
+        header=Header(frame_id="map", stamp=rospy.Time.now()),
+        pose=Pose(position=Point(x, y, 0), orientation=current_pose.orientation),
+    )
     rospy.wait_for_service(make_plan_service, timeout=5)
     make_plan_serv = rospy.ServiceProxy(make_plan_service, GetPlan)
 
@@ -31,9 +42,9 @@ def make_plan(current_pose, x, y, tol=0.01, max_dist=None):
             dist = 0
             # zips the poses like 1 and 2, 2 and 3, etc
             for current, next in zip(mp.poses, mp.poses[1:]):
-                dist+= euclidian_distance(
+                dist += euclidian_distance(
                     (current.pose.position.x, current.pose.position.y),
-                    (next.pose.position.x, next.pose.position.y)
+                    (next.pose.position.x, next.pose.position.y),
                 )
             if dist > max_dist:
                 return False, None, None
@@ -43,7 +54,9 @@ def make_plan(current_pose, x, y, tol=0.01, max_dist=None):
 
 def draw_points(points):
     for x, y in points:
-        marker_pub = rospy.Publisher.publish("/visualization_marker", Marker, queue_size=2)
+        marker_pub = rospy.Publisher.publish(
+            "/visualization_marker", Marker, queue_size=2
+        )
         marker = Marker()
         marker.header.stamp = rospy.Time.now()
         marker.header.frame_id = "/map"
@@ -77,8 +90,8 @@ def points_on_radius(x1, y1, radius=0.5):
     # print( "here is it: ",  (x1 + 0.0, y1 - radius), " " ,
     #     (x1 - radius, y1 + 0.0), " ",
     #     (x1 + radius, y1 + 0.0), " ",
-        # (x1 - radius, y1 - radius), " ",
-        # (x1 + radius, y1 - radius))
+    # (x1 - radius, y1 - radius), " ",
+    # (x1 + radius, y1 - radius))
     return [
         (x1 + 0.0, y1 - radius),
         (x1 + 0.0, y1 + radius),
@@ -90,37 +103,43 @@ def points_on_radius(x1, y1, radius=0.5):
         (x1 + radius, y1 - radius),
     ]
 
+
 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)
+    return np.linalg.norm(a - b)
+
 
 def plan_to_radius(current_pose, point, radius, tol=0.1):
     picked_points = []
     points = None
-    for x,y in points_on_radius(*point, radius=radius):
+    for x, y in points_on_radius(*point, radius=radius):
         # print(f"the points are {x} , and {y}")
         success, point, points = make_plan(current_pose, x, y, tol)
         if success:
-            dist = euclidian_distance((current_pose.position.x, current_pose.position.y), (x,y))
+            dist = euclidian_distance(
+                (current_pose.position.x, current_pose.position.y), (x, y)
+            )
             picked_points.append([point, dist])
 
     # draw_points(picked_points[0:])
-    print("-"*30)
+    print("-" * 30)
     # if points:
-        # print(f"points of journey {points}")
-        # print(f"middle point of journey {points[round((len(points) - 1) / 2)].pose}")
-    picked_points = sorted(picked_points, key = lambda x: x[1])
+    # print(f"points of journey {points}")
+    # print(f"middle point of journey {points[round((len(points) - 1) / 2)].pose}")
+    picked_points = sorted(picked_points, key=lambda x: x[1])
     return [point[0] for point in picked_points] if picked_points else []
 
+
 NUMBER = 3
 
+
 def get_journey_points(current_pose, x, y, tol=0.01):
     success, point, points = make_plan(current_pose, x, y, tol)
     if len(points) > NUMBER + 2:
-        mid_points = [points[i] for i in range(points/NUMBER)]
+        mid_points = [points[i] for i in range(points / NUMBER)]
     else:
         mid_points = points
     return mid_points