cmower

import os
import sys
import importlib.util
from typing import Callable

def import_from_file(script_path: str, function_name: str) -> Callable:
  """
  Imports a function from a Python script file given the file path and the function name.
  
  Args:

cmower

from time import time_ns, sleep


class Rate(object):
    """
    Convenience class for sleeping in a loop at a specified rate
    """

    def __init__(self, hz, reset=True):
        """

cmower

import pybullet as p
import numpy as np

def draw_frame(T: np.ndarray) -> None:
  """ Draw a frame represented by the 4-by-4 homogenous transformation matrix T."""
  for i in range(3):
    color = [0, 0, 0]
    color[i] = 1
    p.addUserDebugLine(
      lineFromXYZ=T[:3, 3],

cmower

from rclpy.node import Node
from rcl_interfaces.srv import GetParameters

class MyNode(Node):
  
    def get_robot_description(self) -> str:
        client = self.create_client(
            GetParameters, "robot_state_publisher/get_parameters"
        )
        while not client.wait_for_service(timeout_sec=1.0):

cmower

import numpy as np

def quaternion_average(*quaternions):
  """Compute the average quaternion."""
  # Markley, F. Landis, Yang Cheng, John Lucas Crassidis, and Yaakov Oshman.
  # "Averaging quaternions." Journal of Guidance, Control, and Dynamics, 2007.
  # https://ntrs.nasa.gov/citations/20070017872
  Q = np.concatenate([q.flatten().reshape(-1, 1) for q in quaternions], axis=1)
  evals, evecs = np.linalg.eig(Q @ Q.T)
  return evecs[:, evals.argmax()]

cmower

class Debugger:
   
   """Simple class for debugging"""
   
   def __init__(self):
      self.it = 0
      
   def __call__(self, msg=''):
      print('-'*40, f'DEBUG {self.it} {msg}', end=(' ' if msg else '') +'-'*40)
      self.it += 1

cmower

What you need to do to install SDL is:

#install sdl2
sudo apt install libsdl2-dev libsdl2-2.0-0 -y;

#install sdl image  - if you want to display images
sudo apt install libjpeg-dev libwebp-dev libtiff5-dev libsdl2-image-dev libsdl2-image-2.0-0 -y;

#install sdl mixer - if you want sound

cmower

def print_type_layout(x, indent=0, incr=2):
    if isinstance(x, dict):
        print(" " * indent, "{")
        for k, v in x.items():
            print(" " * indent, f"'{k}':", end="")
            if isinstance(v, (dict, list)):
                print("")
                print_type_layout(v, indent=indent + incr)
            else:
                print(f" {type(v)}")

cmower

"""Base class for ROS nodes, a ROS package using this will always require geometry_msgs and tf2_ros as deps."""
import tf2_ros
from geometry_msg.msg import TransformStamped

class RosNode:
    
    def __init__(self, rospy):
        """Child-classes need to make a call to super().__init__(rospy)"""
        self.__rp = rospy
        self.__tf_broadcaster = tf2_ros.TransformBroadcaster()

cmower

import cv2

"""Assumes video is called myvideo.MOV in the current directory and there
exists a directory called images/ in the same directory."""

filename = 'myvideo.MOV'
cap = cv2.VideoCapture(filename)
frame_count = int(cap.get(cv2.CAP_PROP_FRAME_COUNT))

fc = 0