diff --git a/rktl_dependencies/package.xml b/rktl_dependencies/package.xml deleted file mode 100644 index f1d3ba972..000000000 --- a/rktl_dependencies/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - rktl_dependencies - 1.0.0 - Contains external dependencies required for other packages. - https://www.purduearc.com - Adam Schmok - BSD 3 Clause - - rclpy - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - python-numpy - - - ament_python - - diff --git a/rktl_dependencies/resource/rktl_dependencies b/rktl_dependencies/resource/rktl_dependencies deleted file mode 100644 index e69de29bb..000000000 diff --git a/rktl_dependencies/rktl_dependencies/__init__.py b/rktl_dependencies/rktl_dependencies/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/rktl_dependencies/rktl_dependencies/angles.py b/rktl_dependencies/rktl_dependencies/angles.py deleted file mode 100644 index c5a20e371..000000000 --- a/rktl_dependencies/rktl_dependencies/angles.py +++ /dev/null @@ -1,238 +0,0 @@ -#********************************************************************* -# Software License Agreement (BSD License) -# -# Copyright (c) 2015, Bossa Nova Robotics -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the Bossa Nova Robotics nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -#********************************************************************/ - -from math import fmod, pi, fabs - -def normalize_angle_positive(angle): - """ Normalizes the angle to be 0 to 2*pi - It takes and returns radians. """ - return angle % (2.0*pi) - -def normalize_angle(angle): - """ Normalizes the angle to be -pi to +pi - It takes and returns radians.""" - a = normalize_angle_positive(angle) - if a > pi: - a -= 2.0 *pi - return a - -def shortest_angular_distance(from_angle, to_angle): - """ Given 2 angles, this returns the shortest angular - difference. The inputs and ouputs are of course radians. - - The result would always be -pi <= result <= pi. Adding the result - to "from" will always get you an equivelent angle to "to". - """ - return normalize_angle(to_angle-from_angle) - -def two_pi_complement(angle): - """ returns the angle in [-2*pi, 2*pi] going the other way along the unit circle. - \param angle The angle to which you want to turn in the range [-2*pi, 2*pi] - E.g. two_pi_complement(-pi/4) returns 7_pi/4 - two_pi_complement(pi/4) returns -7*pi/4 - """ - #check input conditions - if angle > 2*pi or angle < -2.0*pi: - angle = fmod(angle, 2.0*pi) - if angle < 0: - return 2*pi+angle - elif angle > 0: - return -2*pi+angle - - return 2*pi - -def _find_min_max_delta(from_angle, left_limit, right_limit): - """ This function is only intended for internal use and not intended for external use. - If you do use it, read the documentation very carefully. - - Returns the min and max amount (in radians) that can be moved - from "from" angle to "left_limit" and "right_limit". - - \param from - "from" angle - must lie in [-pi, pi) - \param left_limit - left limit of valid interval for angular position - - must lie in [-pi, pi], left and right limits are specified on - the unit circle w.r.t to a reference pointing inwards - \param right_limit - right limit of valid interval for angular position - - must lie in [-pi, pi], left and right limits are specified on - the unit circle w.r.t to a reference pointing inwards - \return (valid, min, max) - angle in radians that can be moved from "from" position before hitting the joint stop - valid is False if "from" angle does not lie in the interval [left_limit,right_limit] - """ - delta = [0]*4 - delta[0] = shortest_angular_distance(from_angle,left_limit) - delta[1] = shortest_angular_distance(from_angle,right_limit) - delta[2] = two_pi_complement(delta[0]) - delta[3] = two_pi_complement(delta[1]) - - if delta[0] == 0: - return True, delta[0], max(delta[1], delta[3]) - - if delta[1] == 0: - return True, min(delta[0], delta[2]), delta[1] - - delta_min = delta[0] - delta_min_2pi = delta[2] - if delta[2] < delta_min: - delta_min = delta[2] - delta_min_2pi = delta[0] - - delta_max = delta[1] - delta_max_2pi = delta[3] - if delta[3] > delta_max: - delta_max = delta[3] - delta_max_2pi = delta[1] - - # printf("%f %f %f %f\n",delta_min,delta_min_2pi,delta_max,delta_max_2pi) - if (delta_min <= delta_max_2pi) or (delta_max >= delta_min_2pi): - if left_limit == -pi and right_limit == pi: - return (True, delta_max_2pi, delta_min_2pi) - else: - return (False, delta_max_2pi, delta_min_2pi) - return True, delta_min, delta_max - -def shortest_angular_distance_with_limits(from_angle, to_angle, left_limit, right_limit): - """ Returns the delta from "from_angle" to "to_angle" making sure it does not violate limits specified by left_limit and right_limit. - The valid interval of angular positions is [left_limit,right_limit]. E.g., [-0.25,0.25] is a 0.5 radians wide interval that contains 0. - But [0.25,-0.25] is a 2*pi-0.5 wide interval that contains pi (but not 0). - The value of shortest_angle is the angular difference between "from" and "to" that lies within the defined valid interval. - - E.g. shortest_angular_distance_with_limits(-0.5,0.5,0.25,-0.25) returns 2*pi-1.0 - shortest_angular_distance_with_limits(-0.5,0.5,-0.25,0.25) returns None since -0.5 and 0.5 do not lie in the interval [-0.25,0.25] - - \param left_limit - left limit of valid interval for angular position - - must lie in [-pi, pi], left and right limits are specified on - the unit circle w.r.t to a reference pointing inwards - \param right_limit - right limit of valid interval for angular position - - must lie in [-pi, pi], left and right limits are specified on - the unit circle w.r.t to a reference pointing inwards - \returns valid_flag, shortest_angle - """ - min_delta = -2*pi - max_delta = 2*pi - min_delta_to = -2*pi - max_delta_to = 2*pi - flag, min_delta, max_delta = _find_min_max_delta(from_angle, left_limit, right_limit) - delta = shortest_angular_distance(from_angle,to_angle) - delta_mod_2pi = two_pi_complement(delta) - - if flag: #from position is within the limits - if delta >= min_delta and delta <= max_delta: - return True, delta - elif delta_mod_2pi >= min_delta and delta_mod_2pi <= max_delta: - return True, delta_mod_2pi - else: #to position is outside the limits - flag, min_delta_to, max_delta_to = _find_min_max_delta(to_angle,left_limit,right_limit) - if fabs(min_delta_to) < fabs(max_delta_to): - shortest_angle = max(delta, delta_mod_2pi) - elif fabs(min_delta_to) > fabs(max_delta_to): - shortest_angle = min(delta,delta_mod_2pi) - else: - if fabs(delta) < fabs(delta_mod_2pi): - shortest_angle = delta - else: - shortest_angle = delta_mod_2pi - return False, shortest_angle - else: # from position is outside the limits - flag, min_delta_to, max_delta_to = _find_min_max_delta(to_angle,left_limit,right_limit) - - if fabs(min_delta) < fabs(max_delta): - shortest_angle = min(delta,delta_mod_2pi) - elif fabs(min_delta) > fabs(max_delta): - shortest_angle = max(delta,delta_mod_2pi) - else: - if fabs(delta) < fabs(delta_mod_2pi): - shortest_angle = delta - else: - shortest_angle = delta_mod_2pi - return False, shortest_angle - -def shortest_angular_distance_with_large_limits(from_angle, to_angle, left_limit, right_limit): - """ Returns the delta from `from_angle` to `to_angle`, making sure it does not violate limits specified by `left_limit` and `right_limit`. - This function is similar to `shortest_angular_distance_with_limits()`, with the main difference that it accepts limits outside the `[-M_PI, M_PI]` range. - Even if this is quite uncommon, one could indeed consider revolute joints with large rotation limits, e.g., in the range `[-2*M_PI, 2*M_PI]`. - - In this case, a strict requirement is to have `left_limit` smaller than `right_limit`. - Note also that `from_angle` must lie inside the valid range, while `to_angle` does not need to. - In fact, this function will evaluate the shortest (valid) angle `shortest_angle` so that `from_angle+shortest_angle` equals `to_angle` up to an integer multiple of `2*M_PI`. - As an example, a call to `shortest_angular_distance_with_large_limits(0, 10.5*M_PI, -2*M_PI, 2*M_PI)` will return `true`, with `shortest_angle=0.5*M_PI`. - This is because `from_angle` and `from_angle+shortest_angle` are both inside the limits, and `fmod(to_angle+shortest_angle, 2*M_PI)` equals `fmod(to_angle, 2*M_PI)`. - On the other hand, `shortest_angular_distance_with_large_limits(10.5*M_PI, 0, -2*M_PI, 2*M_PI)` will return false, since `from_angle` is not in the valid range. - Finally, note that the call `shortest_angular_distance_with_large_limits(0, 10.5*M_PI, -2*M_PI, 0.1*M_PI)` will also return `true`. - However, `shortest_angle` in this case will be `-1.5*M_PI`. - - \return valid_flag, shortest_angle - valid_flag will be true if `left_limit < right_limit` and if "from_angle" and "from_angle+shortest_angle" positions are within the valid interval, false otherwise. - \param left_limit - left limit of valid interval, must be smaller than right_limit. - \param right_limit - right limit of valid interval, must be greater than left_limit. - """ - # Shortest steps in the two directions - delta = shortest_angular_distance(from_angle, to_angle) - delta_2pi = two_pi_complement(delta) - - # "sort" distances so that delta is shorter than delta_2pi - if fabs(delta) > fabs(delta_2pi): - delta, delta_2pi = delta_2pi, delta - - if left_limit > right_limit: - # If limits are something like [PI/2 , -PI/2] it actually means that we - # want rotations to be in the interval [-PI,PI/2] U [PI/2,PI], ie, the - # half unit circle not containing the 0. This is already gracefully - # handled by shortest_angular_distance_with_limits, and therefore this - # function should not be called at all. However, if one has limits that - # are larger than PI, the same rationale behind shortest_angular_distance_with_limits - # does not hold, ie, M_PI+x should not be directly equal to -M_PI+x. - # In this case, the correct way of getting the shortest solution is to - # properly set the limits, eg, by saying that the interval is either - # [PI/2, 3*PI/2] or [-3*M_PI/2, -M_PI/2]. For this reason, here we - # return false by default. - return False, delta - - - # Check in which direction we should turn (clockwise or counter-clockwise). - - # start by trying with the shortest angle (delta). - to2 = from_angle + delta - if left_limit <= to2 and to2 <= right_limit: - # we can move in this direction: return success if the "from" angle is inside limits - valid_flag = left_limit <= from_angle and from_angle <= right_limit - return valid_flag, delta - - # delta is not ok, try to move in the other direction (using its complement) - to2 = from_angle + delta_2pi - if left_limit <= to2 and to2 <= right_limit: - # we can move in this direction: return success if the "from" angle is inside limits - valid_flag = left_limit <= from_angle and from_angle <= right_limit - return valid_flag, delta_2pi - - # nothing works: we always go outside limits - return False, delta \ No newline at end of file diff --git a/rktl_dependencies/rktl_dependencies/transformations.py b/rktl_dependencies/rktl_dependencies/transformations.py deleted file mode 100644 index c7d778a15..000000000 --- a/rktl_dependencies/rktl_dependencies/transformations.py +++ /dev/null @@ -1,1705 +0,0 @@ -# -*- coding: utf-8 -*- -# transformations.py - -# Copyright (c) 2006, Christoph Gohlke -# Copyright (c) 2006-2009, The Regents of the University of California -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in the -# documentation and/or other materials provided with the distribution. -# * Neither the name of the copyright holders nor the names of any -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -"""Homogeneous Transformation Matrices and Quaternions. - -A library for calculating 4x4 matrices for translating, rotating, reflecting, -scaling, shearing, projecting, orthogonalizing, and superimposing arrays of -3D homogeneous coordinates as well as for converting between rotation matrices, -Euler angles, and quaternions. Also includes an Arcball control object and -functions to decompose transformation matrices. - -:Authors: - `Christoph Gohlke `__, - Laboratory for Fluorescence Dynamics, University of California, Irvine - -:Version: 20090418 - -Requirements ------------- - -* `Python 2.6 `__ -* `Numpy 1.3 `__ -* `transformations.c 20090418 `__ - (optional implementation of some functions in C) - -Notes ------ - -Matrices (M) can be inverted using numpy.linalg.inv(M), concatenated using -numpy.dot(M0, M1), or used to transform homogeneous coordinates (v) using -numpy.dot(M, v) for shape (4, \*) "point of arrays", respectively -numpy.dot(v, M.T) for shape (\*, 4) "array of points". - -Calculations are carried out with numpy.float64 precision. - -This Python implementation is not optimized for speed. - -Vector, point, quaternion, and matrix function arguments are expected to be -"array like", i.e. tuple, list, or numpy arrays. - -Return types are numpy arrays unless specified otherwise. - -Angles are in radians unless specified otherwise. - -Quaternions ix+jy+kz+w are represented as [x, y, z, w]. - -Use the transpose of transformation matrices for OpenGL glMultMatrixd(). - -A triple of Euler angles can be applied/interpreted in 24 ways, which can -be specified using a 4 character string or encoded 4-tuple: - - *Axes 4-string*: e.g. 'sxyz' or 'ryxy' - - - first character : rotations are applied to 's'tatic or 'r'otating frame - - remaining characters : successive rotation axis 'x', 'y', or 'z' - - *Axes 4-tuple*: e.g. (0, 0, 0, 0) or (1, 1, 1, 1) - - - inner axis: code of axis ('x':0, 'y':1, 'z':2) of rightmost matrix. - - parity : even (0) if inner axis 'x' is followed by 'y', 'y' is followed - by 'z', or 'z' is followed by 'x'. Otherwise odd (1). - - repetition : first and last axis are same (1) or different (0). - - frame : rotations are applied to static (0) or rotating (1) frame. - -References ----------- - -(1) Matrices and transformations. Ronald Goldman. - In "Graphics Gems I", pp 472-475. Morgan Kaufmann, 1990. -(2) More matrices and transformations: shear and pseudo-perspective. - Ronald Goldman. In "Graphics Gems II", pp 320-323. Morgan Kaufmann, 1991. -(3) Decomposing a matrix into simple transformations. Spencer Thomas. - In "Graphics Gems II", pp 320-323. Morgan Kaufmann, 1991. -(4) Recovering the data from the transformation matrix. Ronald Goldman. - In "Graphics Gems II", pp 324-331. Morgan Kaufmann, 1991. -(5) Euler angle conversion. Ken Shoemake. - In "Graphics Gems IV", pp 222-229. Morgan Kaufmann, 1994. -(6) Arcball rotation control. Ken Shoemake. - In "Graphics Gems IV", pp 175-192. Morgan Kaufmann, 1994. -(7) Representing attitude: Euler angles, unit quaternions, and rotation - vectors. James Diebel. 2006. -(8) A discussion of the solution for the best rotation to relate two sets - of vectors. W Kabsch. Acta Cryst. 1978. A34, 827-828. -(9) Closed-form solution of absolute orientation using unit quaternions. - BKP Horn. J Opt Soc Am A. 1987. 4(4), 629-642. -(10) Quaternions. Ken Shoemake. - http://www.sfu.ca/~jwa3/cmpt461/files/quatut.pdf -(11) From quaternion to matrix and back. JMP van Waveren. 2005. - http://www.intel.com/cd/ids/developer/asmo-na/eng/293748.htm -(12) Uniform random rotations. Ken Shoemake. - In "Graphics Gems III", pp 124-132. Morgan Kaufmann, 1992. - - -Examples --------- - ->>> alpha, beta, gamma = 0.123, -1.234, 2.345 ->>> origin, xaxis, yaxis, zaxis = (0, 0, 0), (1, 0, 0), (0, 1, 0), (0, 0, 1) ->>> I = identity_matrix() ->>> Rx = rotation_matrix(alpha, xaxis) ->>> Ry = rotation_matrix(beta, yaxis) ->>> Rz = rotation_matrix(gamma, zaxis) ->>> R = concatenate_matrices(Rx, Ry, Rz) ->>> euler = euler_from_matrix(R, 'rxyz') ->>> numpy.allclose([alpha, beta, gamma], euler) -True ->>> Re = euler_matrix(alpha, beta, gamma, 'rxyz') ->>> is_same_transform(R, Re) -True ->>> al, be, ga = euler_from_matrix(Re, 'rxyz') ->>> is_same_transform(Re, euler_matrix(al, be, ga, 'rxyz')) -True ->>> qx = quaternion_about_axis(alpha, xaxis) ->>> qy = quaternion_about_axis(beta, yaxis) ->>> qz = quaternion_about_axis(gamma, zaxis) ->>> q = quaternion_multiply(qx, qy) ->>> q = quaternion_multiply(q, qz) ->>> Rq = quaternion_matrix(q) ->>> is_same_transform(R, Rq) -True ->>> S = scale_matrix(1.23, origin) ->>> T = translation_matrix((1, 2, 3)) ->>> Z = shear_matrix(beta, xaxis, origin, zaxis) ->>> R = random_rotation_matrix(numpy.random.rand(3)) ->>> M = concatenate_matrices(T, R, Z, S) ->>> scale, shear, angles, trans, persp = decompose_matrix(M) ->>> numpy.allclose(scale, 1.23) -True ->>> numpy.allclose(trans, (1, 2, 3)) -True ->>> numpy.allclose(shear, (0, math.tan(beta), 0)) -True ->>> is_same_transform(R, euler_matrix(axes='sxyz', *angles)) -True ->>> M1 = compose_matrix(scale, shear, angles, trans, persp) ->>> is_same_transform(M, M1) -True - -""" - -from __future__ import division - -import warnings -import math - -import numpy - -# Documentation in HTML format can be generated with Epydoc -__docformat__ = "restructuredtext en" - - -def identity_matrix(): - """Return 4x4 identity/unit matrix. - - >>> I = identity_matrix() - >>> numpy.allclose(I, numpy.dot(I, I)) - True - >>> numpy.sum(I), numpy.trace(I) - (4.0, 4.0) - >>> numpy.allclose(I, numpy.identity(4, dtype=numpy.float64)) - True - - """ - return numpy.identity(4, dtype=numpy.float64) - - -def translation_matrix(direction): - """Return matrix to translate by direction vector. - - >>> v = numpy.random.random(3) - 0.5 - >>> numpy.allclose(v, translation_matrix(v)[:3, 3]) - True - - """ - M = numpy.identity(4) - M[:3, 3] = direction[:3] - return M - - -def translation_from_matrix(matrix): - """Return translation vector from translation matrix. - - >>> v0 = numpy.random.random(3) - 0.5 - >>> v1 = translation_from_matrix(translation_matrix(v0)) - >>> numpy.allclose(v0, v1) - True - - """ - return numpy.array(matrix, copy=False)[:3, 3].copy() - - -def reflection_matrix(point, normal): - """Return matrix to mirror at plane defined by point and normal vector. - - >>> v0 = numpy.random.random(4) - 0.5 - >>> v0[3] = 1.0 - >>> v1 = numpy.random.random(3) - 0.5 - >>> R = reflection_matrix(v0, v1) - >>> numpy.allclose(2., numpy.trace(R)) - True - >>> numpy.allclose(v0, numpy.dot(R, v0)) - True - >>> v2 = v0.copy() - >>> v2[:3] += v1 - >>> v3 = v0.copy() - >>> v2[:3] -= v1 - >>> numpy.allclose(v2, numpy.dot(R, v3)) - True - - """ - normal = unit_vector(normal[:3]) - M = numpy.identity(4) - M[:3, :3] -= 2.0 * numpy.outer(normal, normal) - M[:3, 3] = (2.0 * numpy.dot(point[:3], normal)) * normal - return M - - -def reflection_from_matrix(matrix): - """Return mirror plane point and normal vector from reflection matrix. - - >>> v0 = numpy.random.random(3) - 0.5 - >>> v1 = numpy.random.random(3) - 0.5 - >>> M0 = reflection_matrix(v0, v1) - >>> point, normal = reflection_from_matrix(M0) - >>> M1 = reflection_matrix(point, normal) - >>> is_same_transform(M0, M1) - True - - """ - M = numpy.array(matrix, dtype=numpy.float64, copy=False) - # normal: unit eigenvector corresponding to eigenvalue -1 - l, V = numpy.linalg.eig(M[:3, :3]) - i = numpy.where(abs(numpy.real(l) + 1.0) < 1e-8)[0] - if not len(i): - raise ValueError("no unit eigenvector corresponding to eigenvalue -1") - normal = numpy.real(V[:, i[0]]).squeeze() - # point: any unit eigenvector corresponding to eigenvalue 1 - l, V = numpy.linalg.eig(M) - i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0] - if not len(i): - raise ValueError("no unit eigenvector corresponding to eigenvalue 1") - point = numpy.real(V[:, i[-1]]).squeeze() - point /= point[3] - return point, normal - - -def rotation_matrix(angle, direction, point=None): - """Return matrix to rotate about axis defined by point and direction. - - >>> angle = (random.random() - 0.5) * (2*math.pi) - >>> direc = numpy.random.random(3) - 0.5 - >>> point = numpy.random.random(3) - 0.5 - >>> R0 = rotation_matrix(angle, direc, point) - >>> R1 = rotation_matrix(angle-2*math.pi, direc, point) - >>> is_same_transform(R0, R1) - True - >>> R0 = rotation_matrix(angle, direc, point) - >>> R1 = rotation_matrix(-angle, -direc, point) - >>> is_same_transform(R0, R1) - True - >>> I = numpy.identity(4, numpy.float64) - >>> numpy.allclose(I, rotation_matrix(math.pi*2, direc)) - True - >>> numpy.allclose(2., numpy.trace(rotation_matrix(math.pi/2, - ... direc, point))) - True - - """ - sina = math.sin(angle) - cosa = math.cos(angle) - direction = unit_vector(direction[:3]) - # rotation matrix around unit vector - R = numpy.array(((cosa, 0.0, 0.0), - (0.0, cosa, 0.0), - (0.0, 0.0, cosa)), dtype=numpy.float64) - R += numpy.outer(direction, direction) * (1.0 - cosa) - direction *= sina - R += numpy.array((( 0.0, -direction[2], direction[1]), - ( direction[2], 0.0, -direction[0]), - (-direction[1], direction[0], 0.0)), - dtype=numpy.float64) - M = numpy.identity(4) - M[:3, :3] = R - if point is not None: - # rotation not around origin - point = numpy.array(point[:3], dtype=numpy.float64, copy=False) - M[:3, 3] = point - numpy.dot(R, point) - return M - - -def rotation_from_matrix(matrix): - """Return rotation angle and axis from rotation matrix. - - >>> angle = (random.random() - 0.5) * (2*math.pi) - >>> direc = numpy.random.random(3) - 0.5 - >>> point = numpy.random.random(3) - 0.5 - >>> R0 = rotation_matrix(angle, direc, point) - >>> angle, direc, point = rotation_from_matrix(R0) - >>> R1 = rotation_matrix(angle, direc, point) - >>> is_same_transform(R0, R1) - True - - """ - R = numpy.array(matrix, dtype=numpy.float64, copy=False) - R33 = R[:3, :3] - # direction: unit eigenvector of R33 corresponding to eigenvalue of 1 - l, W = numpy.linalg.eig(R33.T) - i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0] - if not len(i): - raise ValueError("no unit eigenvector corresponding to eigenvalue 1") - direction = numpy.real(W[:, i[-1]]).squeeze() - # point: unit eigenvector of R33 corresponding to eigenvalue of 1 - l, Q = numpy.linalg.eig(R) - i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0] - if not len(i): - raise ValueError("no unit eigenvector corresponding to eigenvalue 1") - point = numpy.real(Q[:, i[-1]]).squeeze() - point /= point[3] - # rotation angle depending on direction - cosa = (numpy.trace(R33) - 1.0) / 2.0 - if abs(direction[2]) > 1e-8: - sina = (R[1, 0] + (cosa-1.0)*direction[0]*direction[1]) / direction[2] - elif abs(direction[1]) > 1e-8: - sina = (R[0, 2] + (cosa-1.0)*direction[0]*direction[2]) / direction[1] - else: - sina = (R[2, 1] + (cosa-1.0)*direction[1]*direction[2]) / direction[0] - angle = math.atan2(sina, cosa) - return angle, direction, point - - -def scale_matrix(factor, origin=None, direction=None): - """Return matrix to scale by factor around origin in direction. - - Use factor -1 for point symmetry. - - >>> v = (numpy.random.rand(4, 5) - 0.5) * 20.0 - >>> v[3] = 1.0 - >>> S = scale_matrix(-1.234) - >>> numpy.allclose(numpy.dot(S, v)[:3], -1.234*v[:3]) - True - >>> factor = random.random() * 10 - 5 - >>> origin = numpy.random.random(3) - 0.5 - >>> direct = numpy.random.random(3) - 0.5 - >>> S = scale_matrix(factor, origin) - >>> S = scale_matrix(factor, origin, direct) - - """ - if direction is None: - # uniform scaling - M = numpy.array(((factor, 0.0, 0.0, 0.0), - (0.0, factor, 0.0, 0.0), - (0.0, 0.0, factor, 0.0), - (0.0, 0.0, 0.0, 1.0)), dtype=numpy.float64) - if origin is not None: - M[:3, 3] = origin[:3] - M[:3, 3] *= 1.0 - factor - else: - # nonuniform scaling - direction = unit_vector(direction[:3]) - factor = 1.0 - factor - M = numpy.identity(4) - M[:3, :3] -= factor * numpy.outer(direction, direction) - if origin is not None: - M[:3, 3] = (factor * numpy.dot(origin[:3], direction)) * direction - return M - - -def scale_from_matrix(matrix): - """Return scaling factor, origin and direction from scaling matrix. - - >>> factor = random.random() * 10 - 5 - >>> origin = numpy.random.random(3) - 0.5 - >>> direct = numpy.random.random(3) - 0.5 - >>> S0 = scale_matrix(factor, origin) - >>> factor, origin, direction = scale_from_matrix(S0) - >>> S1 = scale_matrix(factor, origin, direction) - >>> is_same_transform(S0, S1) - True - >>> S0 = scale_matrix(factor, origin, direct) - >>> factor, origin, direction = scale_from_matrix(S0) - >>> S1 = scale_matrix(factor, origin, direction) - >>> is_same_transform(S0, S1) - True - - """ - M = numpy.array(matrix, dtype=numpy.float64, copy=False) - M33 = M[:3, :3] - factor = numpy.trace(M33) - 2.0 - try: - # direction: unit eigenvector corresponding to eigenvalue factor - l, V = numpy.linalg.eig(M33) - i = numpy.where(abs(numpy.real(l) - factor) < 1e-8)[0][0] - direction = numpy.real(V[:, i]).squeeze() - direction /= vector_norm(direction) - except IndexError: - # uniform scaling - factor = (factor + 2.0) / 3.0 - direction = None - # origin: any eigenvector corresponding to eigenvalue 1 - l, V = numpy.linalg.eig(M) - i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0] - if not len(i): - raise ValueError("no eigenvector corresponding to eigenvalue 1") - origin = numpy.real(V[:, i[-1]]).squeeze() - origin /= origin[3] - return factor, origin, direction - - -def projection_matrix(point, normal, direction=None, - perspective=None, pseudo=False): - """Return matrix to project onto plane defined by point and normal. - - Using either perspective point, projection direction, or none of both. - - If pseudo is True, perspective projections will preserve relative depth - such that Perspective = dot(Orthogonal, PseudoPerspective). - - >>> P = projection_matrix((0, 0, 0), (1, 0, 0)) - >>> numpy.allclose(P[1:, 1:], numpy.identity(4)[1:, 1:]) - True - >>> point = numpy.random.random(3) - 0.5 - >>> normal = numpy.random.random(3) - 0.5 - >>> direct = numpy.random.random(3) - 0.5 - >>> persp = numpy.random.random(3) - 0.5 - >>> P0 = projection_matrix(point, normal) - >>> P1 = projection_matrix(point, normal, direction=direct) - >>> P2 = projection_matrix(point, normal, perspective=persp) - >>> P3 = projection_matrix(point, normal, perspective=persp, pseudo=True) - >>> is_same_transform(P2, numpy.dot(P0, P3)) - True - >>> P = projection_matrix((3, 0, 0), (1, 1, 0), (1, 0, 0)) - >>> v0 = (numpy.random.rand(4, 5) - 0.5) * 20.0 - >>> v0[3] = 1.0 - >>> v1 = numpy.dot(P, v0) - >>> numpy.allclose(v1[1], v0[1]) - True - >>> numpy.allclose(v1[0], 3.0-v1[1]) - True - - """ - M = numpy.identity(4) - point = numpy.array(point[:3], dtype=numpy.float64, copy=False) - normal = unit_vector(normal[:3]) - if perspective is not None: - # perspective projection - perspective = numpy.array(perspective[:3], dtype=numpy.float64, - copy=False) - M[0, 0] = M[1, 1] = M[2, 2] = numpy.dot(perspective-point, normal) - M[:3, :3] -= numpy.outer(perspective, normal) - if pseudo: - # preserve relative depth - M[:3, :3] -= numpy.outer(normal, normal) - M[:3, 3] = numpy.dot(point, normal) * (perspective+normal) - else: - M[:3, 3] = numpy.dot(point, normal) * perspective - M[3, :3] = -normal - M[3, 3] = numpy.dot(perspective, normal) - elif direction is not None: - # parallel projection - direction = numpy.array(direction[:3], dtype=numpy.float64, copy=False) - scale = numpy.dot(direction, normal) - M[:3, :3] -= numpy.outer(direction, normal) / scale - M[:3, 3] = direction * (numpy.dot(point, normal) / scale) - else: - # orthogonal projection - M[:3, :3] -= numpy.outer(normal, normal) - M[:3, 3] = numpy.dot(point, normal) * normal - return M - - -def projection_from_matrix(matrix, pseudo=False): - """Return projection plane and perspective point from projection matrix. - - Return values are same as arguments for projection_matrix function: - point, normal, direction, perspective, and pseudo. - - >>> point = numpy.random.random(3) - 0.5 - >>> normal = numpy.random.random(3) - 0.5 - >>> direct = numpy.random.random(3) - 0.5 - >>> persp = numpy.random.random(3) - 0.5 - >>> P0 = projection_matrix(point, normal) - >>> result = projection_from_matrix(P0) - >>> P1 = projection_matrix(*result) - >>> is_same_transform(P0, P1) - True - >>> P0 = projection_matrix(point, normal, direct) - >>> result = projection_from_matrix(P0) - >>> P1 = projection_matrix(*result) - >>> is_same_transform(P0, P1) - True - >>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=False) - >>> result = projection_from_matrix(P0, pseudo=False) - >>> P1 = projection_matrix(*result) - >>> is_same_transform(P0, P1) - True - >>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=True) - >>> result = projection_from_matrix(P0, pseudo=True) - >>> P1 = projection_matrix(*result) - >>> is_same_transform(P0, P1) - True - - """ - M = numpy.array(matrix, dtype=numpy.float64, copy=False) - M33 = M[:3, :3] - l, V = numpy.linalg.eig(M) - i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0] - if not pseudo and len(i): - # point: any eigenvector corresponding to eigenvalue 1 - point = numpy.real(V[:, i[-1]]).squeeze() - point /= point[3] - # direction: unit eigenvector corresponding to eigenvalue 0 - l, V = numpy.linalg.eig(M33) - i = numpy.where(abs(numpy.real(l)) < 1e-8)[0] - if not len(i): - raise ValueError("no eigenvector corresponding to eigenvalue 0") - direction = numpy.real(V[:, i[0]]).squeeze() - direction /= vector_norm(direction) - # normal: unit eigenvector of M33.T corresponding to eigenvalue 0 - l, V = numpy.linalg.eig(M33.T) - i = numpy.where(abs(numpy.real(l)) < 1e-8)[0] - if len(i): - # parallel projection - normal = numpy.real(V[:, i[0]]).squeeze() - normal /= vector_norm(normal) - return point, normal, direction, None, False - else: - # orthogonal projection, where normal equals direction vector - return point, direction, None, None, False - else: - # perspective projection - i = numpy.where(abs(numpy.real(l)) > 1e-8)[0] - if not len(i): - raise ValueError( - "no eigenvector not corresponding to eigenvalue 0") - point = numpy.real(V[:, i[-1]]).squeeze() - point /= point[3] - normal = - M[3, :3] - perspective = M[:3, 3] / numpy.dot(point[:3], normal) - if pseudo: - perspective -= normal - return point, normal, None, perspective, pseudo - - -def clip_matrix(left, right, bottom, top, near, far, perspective=False): - """Return matrix to obtain normalized device coordinates from frustrum. - - The frustrum bounds are axis-aligned along x (left, right), - y (bottom, top) and z (near, far). - - Normalized device coordinates are in range [-1, 1] if coordinates are - inside the frustrum. - - If perspective is True the frustrum is a truncated pyramid with the - perspective point at origin and direction along z axis, otherwise an - orthographic canonical view volume (a box). - - Homogeneous coordinates transformed by the perspective clip matrix - need to be dehomogenized (devided by w coordinate). - - >>> frustrum = numpy.random.rand(6) - >>> frustrum[1] += frustrum[0] - >>> frustrum[3] += frustrum[2] - >>> frustrum[5] += frustrum[4] - >>> M = clip_matrix(*frustrum, perspective=False) - >>> numpy.dot(M, [frustrum[0], frustrum[2], frustrum[4], 1.0]) - array([-1., -1., -1., 1.]) - >>> numpy.dot(M, [frustrum[1], frustrum[3], frustrum[5], 1.0]) - array([ 1., 1., 1., 1.]) - >>> M = clip_matrix(*frustrum, perspective=True) - >>> v = numpy.dot(M, [frustrum[0], frustrum[2], frustrum[4], 1.0]) - >>> v / v[3] - array([-1., -1., -1., 1.]) - >>> v = numpy.dot(M, [frustrum[1], frustrum[3], frustrum[4], 1.0]) - >>> v / v[3] - array([ 1., 1., -1., 1.]) - - """ - if left >= right or bottom >= top or near >= far: - raise ValueError("invalid frustrum") - if perspective: - if near <= _EPS: - raise ValueError("invalid frustrum: near <= 0") - t = 2.0 * near - M = ((-t/(right-left), 0.0, (right+left)/(right-left), 0.0), - (0.0, -t/(top-bottom), (top+bottom)/(top-bottom), 0.0), - (0.0, 0.0, -(far+near)/(far-near), t*far/(far-near)), - (0.0, 0.0, -1.0, 0.0)) - else: - M = ((2.0/(right-left), 0.0, 0.0, (right+left)/(left-right)), - (0.0, 2.0/(top-bottom), 0.0, (top+bottom)/(bottom-top)), - (0.0, 0.0, 2.0/(far-near), (far+near)/(near-far)), - (0.0, 0.0, 0.0, 1.0)) - return numpy.array(M, dtype=numpy.float64) - - -def shear_matrix(angle, direction, point, normal): - """Return matrix to shear by angle along direction vector on shear plane. - - The shear plane is defined by a point and normal vector. The direction - vector must be orthogonal to the plane's normal vector. - - A point P is transformed by the shear matrix into P" such that - the vector P-P" is parallel to the direction vector and its extent is - given by the angle of P-P'-P", where P' is the orthogonal projection - of P onto the shear plane. - - >>> angle = (random.random() - 0.5) * 4*math.pi - >>> direct = numpy.random.random(3) - 0.5 - >>> point = numpy.random.random(3) - 0.5 - >>> normal = numpy.cross(direct, numpy.random.random(3)) - >>> S = shear_matrix(angle, direct, point, normal) - >>> numpy.allclose(1.0, numpy.linalg.det(S)) - True - - """ - normal = unit_vector(normal[:3]) - direction = unit_vector(direction[:3]) - if abs(numpy.dot(normal, direction)) > 1e-6: - raise ValueError("direction and normal vectors are not orthogonal") - angle = math.tan(angle) - M = numpy.identity(4) - M[:3, :3] += angle * numpy.outer(direction, normal) - M[:3, 3] = -angle * numpy.dot(point[:3], normal) * direction - return M - - -def shear_from_matrix(matrix): - """Return shear angle, direction and plane from shear matrix. - - >>> angle = (random.random() - 0.5) * 4*math.pi - >>> direct = numpy.random.random(3) - 0.5 - >>> point = numpy.random.random(3) - 0.5 - >>> normal = numpy.cross(direct, numpy.random.random(3)) - >>> S0 = shear_matrix(angle, direct, point, normal) - >>> angle, direct, point, normal = shear_from_matrix(S0) - >>> S1 = shear_matrix(angle, direct, point, normal) - >>> is_same_transform(S0, S1) - True - - """ - M = numpy.array(matrix, dtype=numpy.float64, copy=False) - M33 = M[:3, :3] - # normal: cross independent eigenvectors corresponding to the eigenvalue 1 - l, V = numpy.linalg.eig(M33) - i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-4)[0] - if len(i) < 2: - raise ValueError("No two linear independent eigenvectors found {}".format(l)) - V = numpy.real(V[:, i]).squeeze().T - lenorm = -1.0 - for i0, i1 in ((0, 1), (0, 2), (1, 2)): - n = numpy.cross(V[i0], V[i1]) - l = vector_norm(n) - if l > lenorm: - lenorm = l - normal = n - normal /= lenorm - # direction and angle - direction = numpy.dot(M33 - numpy.identity(3), normal) - angle = vector_norm(direction) - direction /= angle - angle = math.atan(angle) - # point: eigenvector corresponding to eigenvalue 1 - l, V = numpy.linalg.eig(M) - i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0] - if not len(i): - raise ValueError("no eigenvector corresponding to eigenvalue 1") - point = numpy.real(V[:, i[-1]]).squeeze() - point /= point[3] - return angle, direction, point, normal - - -def decompose_matrix(matrix): - """Return sequence of transformations from transformation matrix. - - matrix : array_like - Non-degenerative homogeneous transformation matrix - - Return tuple of: - scale : vector of 3 scaling factors - shear : list of shear factors for x-y, x-z, y-z axes - angles : list of Euler angles about static x, y, z axes - translate : translation vector along x, y, z axes - perspective : perspective partition of matrix - - Raise ValueError if matrix is of wrong type or degenerative. - - >>> T0 = translation_matrix((1, 2, 3)) - >>> scale, shear, angles, trans, persp = decompose_matrix(T0) - >>> T1 = translation_matrix(trans) - >>> numpy.allclose(T0, T1) - True - >>> S = scale_matrix(0.123) - >>> scale, shear, angles, trans, persp = decompose_matrix(S) - >>> scale[0] - 0.123 - >>> R0 = euler_matrix(1, 2, 3) - >>> scale, shear, angles, trans, persp = decompose_matrix(R0) - >>> R1 = euler_matrix(*angles) - >>> numpy.allclose(R0, R1) - True - - """ - M = numpy.array(matrix, dtype=numpy.float64, copy=True).T - if abs(M[3, 3]) < _EPS: - raise ValueError("M[3, 3] is zero") - M /= M[3, 3] - P = M.copy() - P[:, 3] = 0, 0, 0, 1 - if not numpy.linalg.det(P): - raise ValueError("Matrix is singular") - - scale = numpy.zeros((3, ), dtype=numpy.float64) - shear = [0, 0, 0] - angles = [0, 0, 0] - - if any(abs(M[:3, 3]) > _EPS): - perspective = numpy.dot(M[:, 3], numpy.linalg.inv(P.T)) - M[:, 3] = 0, 0, 0, 1 - else: - perspective = numpy.array((0, 0, 0, 1), dtype=numpy.float64) - - translate = M[3, :3].copy() - M[3, :3] = 0 - - row = M[:3, :3].copy() - scale[0] = vector_norm(row[0]) - row[0] /= scale[0] - shear[0] = numpy.dot(row[0], row[1]) - row[1] -= row[0] * shear[0] - scale[1] = vector_norm(row[1]) - row[1] /= scale[1] - shear[0] /= scale[1] - shear[1] = numpy.dot(row[0], row[2]) - row[2] -= row[0] * shear[1] - shear[2] = numpy.dot(row[1], row[2]) - row[2] -= row[1] * shear[2] - scale[2] = vector_norm(row[2]) - row[2] /= scale[2] - shear[1:] /= scale[2] - - if numpy.dot(row[0], numpy.cross(row[1], row[2])) < 0: - scale *= -1 - row *= -1 - - angles[1] = math.asin(-row[0, 2]) - if math.cos(angles[1]): - angles[0] = math.atan2(row[1, 2], row[2, 2]) - angles[2] = math.atan2(row[0, 1], row[0, 0]) - else: - #angles[0] = math.atan2(row[1, 0], row[1, 1]) - angles[0] = math.atan2(-row[2, 1], row[1, 1]) - angles[2] = 0.0 - - return scale, shear, angles, translate, perspective - - -def compose_matrix(scale=None, shear=None, angles=None, translate=None, - perspective=None): - """Return transformation matrix from sequence of transformations. - - This is the inverse of the decompose_matrix function. - - Sequence of transformations: - scale : vector of 3 scaling factors - shear : list of shear factors for x-y, x-z, y-z axes - angles : list of Euler angles about static x, y, z axes - translate : translation vector along x, y, z axes - perspective : perspective partition of matrix - - >>> scale = numpy.random.random(3) - 0.5 - >>> shear = numpy.random.random(3) - 0.5 - >>> angles = (numpy.random.random(3) - 0.5) * (2*math.pi) - >>> trans = numpy.random.random(3) - 0.5 - >>> persp = numpy.random.random(4) - 0.5 - >>> M0 = compose_matrix(scale, shear, angles, trans, persp) - >>> result = decompose_matrix(M0) - >>> M1 = compose_matrix(*result) - >>> is_same_transform(M0, M1) - True - - """ - M = numpy.identity(4) - if perspective is not None: - P = numpy.identity(4) - P[3, :] = perspective[:4] - M = numpy.dot(M, P) - if translate is not None: - T = numpy.identity(4) - T[:3, 3] = translate[:3] - M = numpy.dot(M, T) - if angles is not None: - R = euler_matrix(angles[0], angles[1], angles[2], 'sxyz') - M = numpy.dot(M, R) - if shear is not None: - Z = numpy.identity(4) - Z[1, 2] = shear[2] - Z[0, 2] = shear[1] - Z[0, 1] = shear[0] - M = numpy.dot(M, Z) - if scale is not None: - S = numpy.identity(4) - S[0, 0] = scale[0] - S[1, 1] = scale[1] - S[2, 2] = scale[2] - M = numpy.dot(M, S) - M /= M[3, 3] - return M - - -def orthogonalization_matrix(lengths, angles): - """Return orthogonalization matrix for crystallographic cell coordinates. - - Angles are expected in degrees. - - The de-orthogonalization matrix is the inverse. - - >>> O = orthogonalization_matrix((10., 10., 10.), (90., 90., 90.)) - >>> numpy.allclose(O[:3, :3], numpy.identity(3, float) * 10) - True - >>> O = orthogonalization_matrix([9.8, 12.0, 15.5], [87.2, 80.7, 69.7]) - >>> numpy.allclose(numpy.sum(O), 43.063229) - True - - """ - a, b, c = lengths - angles = numpy.radians(angles) - sina, sinb, _ = numpy.sin(angles) - cosa, cosb, cosg = numpy.cos(angles) - co = (cosa * cosb - cosg) / (sina * sinb) - return numpy.array(( - ( a*sinb*math.sqrt(1.0-co*co), 0.0, 0.0, 0.0), - (-a*sinb*co, b*sina, 0.0, 0.0), - ( a*cosb, b*cosa, c, 0.0), - ( 0.0, 0.0, 0.0, 1.0)), - dtype=numpy.float64) - - -def superimposition_matrix(v0, v1, scaling=False, usesvd=True): - """Return matrix to transform given vector set into second vector set. - - v0 and v1 are shape (3, \*) or (4, \*) arrays of at least 3 vectors. - - If usesvd is True, the weighted sum of squared deviations (RMSD) is - minimized according to the algorithm by W. Kabsch [8]. Otherwise the - quaternion based algorithm by B. Horn [9] is used (slower when using - this Python implementation). - - The returned matrix performs rotation, translation and uniform scaling - (if specified). - - >>> v0 = numpy.random.rand(3, 10) - >>> M = superimposition_matrix(v0, v0) - >>> numpy.allclose(M, numpy.identity(4)) - True - >>> R = random_rotation_matrix(numpy.random.random(3)) - >>> v0 = ((1,0,0), (0,1,0), (0,0,1), (1,1,1)) - >>> v1 = numpy.dot(R, v0) - >>> M = superimposition_matrix(v0, v1) - >>> numpy.allclose(v1, numpy.dot(M, v0)) - True - >>> v0 = (numpy.random.rand(4, 100) - 0.5) * 20.0 - >>> v0[3] = 1.0 - >>> v1 = numpy.dot(R, v0) - >>> M = superimposition_matrix(v0, v1) - >>> numpy.allclose(v1, numpy.dot(M, v0)) - True - >>> S = scale_matrix(random.random()) - >>> T = translation_matrix(numpy.random.random(3)-0.5) - >>> M = concatenate_matrices(T, R, S) - >>> v1 = numpy.dot(M, v0) - >>> v0[:3] += numpy.random.normal(0.0, 1e-9, 300).reshape(3, -1) - >>> M = superimposition_matrix(v0, v1, scaling=True) - >>> numpy.allclose(v1, numpy.dot(M, v0)) - True - >>> M = superimposition_matrix(v0, v1, scaling=True, usesvd=False) - >>> numpy.allclose(v1, numpy.dot(M, v0)) - True - >>> v = numpy.empty((4, 100, 3), dtype=numpy.float64) - >>> v[:, :, 0] = v0 - >>> M = superimposition_matrix(v0, v1, scaling=True, usesvd=False) - >>> numpy.allclose(v1, numpy.dot(M, v[:, :, 0])) - True - - """ - v0 = numpy.array(v0, dtype=numpy.float64, copy=False)[:3] - v1 = numpy.array(v1, dtype=numpy.float64, copy=False)[:3] - - if v0.shape != v1.shape or v0.shape[1] < 3: - raise ValueError("Vector sets are of wrong shape or type.") - - # move centroids to origin - t0 = numpy.mean(v0, axis=1) - t1 = numpy.mean(v1, axis=1) - v0 = v0 - t0.reshape(3, 1) - v1 = v1 - t1.reshape(3, 1) - - if usesvd: - # Singular Value Decomposition of covariance matrix - u, s, vh = numpy.linalg.svd(numpy.dot(v1, v0.T)) - # rotation matrix from SVD orthonormal bases - R = numpy.dot(u, vh) - if numpy.linalg.det(R) < 0.0: - # R does not constitute right handed system - R -= numpy.outer(u[:, 2], vh[2, :]*2.0) - s[-1] *= -1.0 - # homogeneous transformation matrix - M = numpy.identity(4) - M[:3, :3] = R - else: - # compute symmetric matrix N - xx, yy, zz = numpy.sum(v0 * v1, axis=1) - xy, yz, zx = numpy.sum(v0 * numpy.roll(v1, -1, axis=0), axis=1) - xz, yx, zy = numpy.sum(v0 * numpy.roll(v1, -2, axis=0), axis=1) - N = ((xx+yy+zz, yz-zy, zx-xz, xy-yx), - (yz-zy, xx-yy-zz, xy+yx, zx+xz), - (zx-xz, xy+yx, -xx+yy-zz, yz+zy), - (xy-yx, zx+xz, yz+zy, -xx-yy+zz)) - # quaternion: eigenvector corresponding to most positive eigenvalue - l, V = numpy.linalg.eig(N) - q = V[:, numpy.argmax(l)] - q /= vector_norm(q) # unit quaternion - q = numpy.roll(q, -1) # move w component to end - # homogeneous transformation matrix - M = quaternion_matrix(q) - - # scale: ratio of rms deviations from centroid - if scaling: - v0 *= v0 - v1 *= v1 - M[:3, :3] *= math.sqrt(numpy.sum(v1) / numpy.sum(v0)) - - # translation - M[:3, 3] = t1 - T = numpy.identity(4) - T[:3, 3] = -t0 - M = numpy.dot(M, T) - return M - - -def euler_matrix(ai, aj, ak, axes='sxyz'): - """Return homogeneous rotation matrix from Euler angles and axis sequence. - - ai, aj, ak : Euler's roll, pitch and yaw angles - axes : One of 24 axis sequences as string or encoded tuple - - >>> R = euler_matrix(1, 2, 3, 'syxz') - >>> numpy.allclose(numpy.sum(R[0]), -1.34786452) - True - >>> R = euler_matrix(1, 2, 3, (0, 1, 0, 1)) - >>> numpy.allclose(numpy.sum(R[0]), -0.383436184) - True - >>> ai, aj, ak = (4.0*math.pi) * (numpy.random.random(3) - 0.5) - >>> for axes in _AXES2TUPLE.keys(): - ... R = euler_matrix(ai, aj, ak, axes) - >>> for axes in _TUPLE2AXES.keys(): - ... R = euler_matrix(ai, aj, ak, axes) - - """ - try: - firstaxis, parity, repetition, frame = _AXES2TUPLE[axes] - except (AttributeError, KeyError): - _ = _TUPLE2AXES[axes] - firstaxis, parity, repetition, frame = axes - - i = firstaxis - j = _NEXT_AXIS[i+parity] - k = _NEXT_AXIS[i-parity+1] - - if frame: - ai, ak = ak, ai - if parity: - ai, aj, ak = -ai, -aj, -ak - - si, sj, sk = math.sin(ai), math.sin(aj), math.sin(ak) - ci, cj, ck = math.cos(ai), math.cos(aj), math.cos(ak) - cc, cs = ci*ck, ci*sk - sc, ss = si*ck, si*sk - - M = numpy.identity(4) - if repetition: - M[i, i] = cj - M[i, j] = sj*si - M[i, k] = sj*ci - M[j, i] = sj*sk - M[j, j] = -cj*ss+cc - M[j, k] = -cj*cs-sc - M[k, i] = -sj*ck - M[k, j] = cj*sc+cs - M[k, k] = cj*cc-ss - else: - M[i, i] = cj*ck - M[i, j] = sj*sc-cs - M[i, k] = sj*cc+ss - M[j, i] = cj*sk - M[j, j] = sj*ss+cc - M[j, k] = sj*cs-sc - M[k, i] = -sj - M[k, j] = cj*si - M[k, k] = cj*ci - return M - - -def euler_from_matrix(matrix, axes='sxyz'): - """Return Euler angles from rotation matrix for specified axis sequence. - - axes : One of 24 axis sequences as string or encoded tuple - - Note that many Euler angle triplets can describe one matrix. - - >>> R0 = euler_matrix(1, 2, 3, 'syxz') - >>> al, be, ga = euler_from_matrix(R0, 'syxz') - >>> R1 = euler_matrix(al, be, ga, 'syxz') - >>> numpy.allclose(R0, R1) - True - >>> angles = (4.0*math.pi) * (numpy.random.random(3) - 0.5) - >>> for axes in _AXES2TUPLE.keys(): - ... R0 = euler_matrix(axes=axes, *angles) - ... R1 = euler_matrix(axes=axes, *euler_from_matrix(R0, axes)) - ... if not numpy.allclose(R0, R1): print axes, "failed" - - """ - try: - firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()] - except (AttributeError, KeyError): - _ = _TUPLE2AXES[axes] - firstaxis, parity, repetition, frame = axes - - i = firstaxis - j = _NEXT_AXIS[i+parity] - k = _NEXT_AXIS[i-parity+1] - - M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:3, :3] - if repetition: - sy = math.sqrt(M[i, j]*M[i, j] + M[i, k]*M[i, k]) - if sy > _EPS: - ax = math.atan2( M[i, j], M[i, k]) - ay = math.atan2( sy, M[i, i]) - az = math.atan2( M[j, i], -M[k, i]) - else: - ax = math.atan2(-M[j, k], M[j, j]) - ay = math.atan2( sy, M[i, i]) - az = 0.0 - else: - cy = math.sqrt(M[i, i]*M[i, i] + M[j, i]*M[j, i]) - if cy > _EPS: - ax = math.atan2( M[k, j], M[k, k]) - ay = math.atan2(-M[k, i], cy) - az = math.atan2( M[j, i], M[i, i]) - else: - ax = math.atan2(-M[j, k], M[j, j]) - ay = math.atan2(-M[k, i], cy) - az = 0.0 - - if parity: - ax, ay, az = -ax, -ay, -az - if frame: - ax, az = az, ax - return ax, ay, az - - -def euler_from_quaternion(quaternion, axes='sxyz'): - """Return Euler angles from quaternion for specified axis sequence. - - >>> angles = euler_from_quaternion([0.06146124, 0, 0, 0.99810947]) - >>> numpy.allclose(angles, [0.123, 0, 0]) - True - - """ - return euler_from_matrix(quaternion_matrix(quaternion), axes) - - -def quaternion_from_euler(ai, aj, ak, axes='sxyz'): - """Return quaternion from Euler angles and axis sequence. - - ai, aj, ak : Euler's roll, pitch and yaw angles - axes : One of 24 axis sequences as string or encoded tuple - - >>> q = quaternion_from_euler(1, 2, 3, 'ryxz') - >>> numpy.allclose(q, [0.310622, -0.718287, 0.444435, 0.435953]) - True - - """ - try: - firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()] - except (AttributeError, KeyError): - _ = _TUPLE2AXES[axes] - firstaxis, parity, repetition, frame = axes - - i = firstaxis - j = _NEXT_AXIS[i+parity] - k = _NEXT_AXIS[i-parity+1] - - if frame: - ai, ak = ak, ai - if parity: - aj = -aj - - ai = ai / 2.0 - aj = aj / 2.0 - ak = ak / 2.0 - ci = math.cos(ai) - si = math.sin(ai) - cj = math.cos(aj) - sj = math.sin(aj) - ck = math.cos(ak) - sk = math.sin(ak) - cc = ci*ck - cs = ci*sk - sc = si*ck - ss = si*sk - - quaternion = numpy.empty((4, ), dtype=numpy.float64) - if repetition: - quaternion[i] = cj*(cs + sc) - quaternion[j] = sj*(cc + ss) - quaternion[k] = sj*(cs - sc) - quaternion[3] = cj*(cc - ss) - else: - quaternion[i] = cj*sc - sj*cs - quaternion[j] = cj*ss + sj*cc - quaternion[k] = cj*cs - sj*sc - quaternion[3] = cj*cc + sj*ss - if parity: - quaternion[j] *= -1 - - return quaternion - - -def quaternion_about_axis(angle, axis): - """Return quaternion for rotation about axis. - - >>> q = quaternion_about_axis(0.123, (1, 0, 0)) - >>> numpy.allclose(q, [0.06146124, 0, 0, 0.99810947]) - True - - """ - quaternion = numpy.zeros((4, ), dtype=numpy.float64) - quaternion[:3] = axis[:3] - qlen = vector_norm(quaternion) - if qlen > _EPS: - quaternion *= math.sin(angle/2.0) / qlen - quaternion[3] = math.cos(angle/2.0) - return quaternion - - -def quaternion_matrix(quaternion): - """Return homogeneous rotation matrix from quaternion. - - >>> R = quaternion_matrix([0.06146124, 0, 0, 0.99810947]) - >>> numpy.allclose(R, rotation_matrix(0.123, (1, 0, 0))) - True - - """ - q = numpy.array(quaternion[:4], dtype=numpy.float64, copy=True) - nq = numpy.dot(q, q) - if nq < _EPS: - return numpy.identity(4) - q *= math.sqrt(2.0 / nq) - q = numpy.outer(q, q) - return numpy.array(( - (1.0-q[1, 1]-q[2, 2], q[0, 1]-q[2, 3], q[0, 2]+q[1, 3], 0.0), - ( q[0, 1]+q[2, 3], 1.0-q[0, 0]-q[2, 2], q[1, 2]-q[0, 3], 0.0), - ( q[0, 2]-q[1, 3], q[1, 2]+q[0, 3], 1.0-q[0, 0]-q[1, 1], 0.0), - ( 0.0, 0.0, 0.0, 1.0) - ), dtype=numpy.float64) - - -def quaternion_from_matrix(matrix): - """Return quaternion from rotation matrix. - - >>> R = rotation_matrix(0.123, (1, 2, 3)) - >>> q = quaternion_from_matrix(R) - >>> numpy.allclose(q, [0.0164262, 0.0328524, 0.0492786, 0.9981095]) - True - - """ - q = numpy.empty((4, ), dtype=numpy.float64) - M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:4, :4] - t = numpy.trace(M) - if t > M[3, 3]: - q[3] = t - q[2] = M[1, 0] - M[0, 1] - q[1] = M[0, 2] - M[2, 0] - q[0] = M[2, 1] - M[1, 2] - else: - i, j, k = 0, 1, 2 - if M[1, 1] > M[0, 0]: - i, j, k = 1, 2, 0 - if M[2, 2] > M[i, i]: - i, j, k = 2, 0, 1 - t = M[i, i] - (M[j, j] + M[k, k]) + M[3, 3] - q[i] = t - q[j] = M[i, j] + M[j, i] - q[k] = M[k, i] + M[i, k] - q[3] = M[k, j] - M[j, k] - q *= 0.5 / math.sqrt(t * M[3, 3]) - return q - - -def quaternion_multiply(quaternion1, quaternion0): - """Return multiplication of two quaternions. - - >>> q = quaternion_multiply([1, -2, 3, 4], [-5, 6, 7, 8]) - >>> numpy.allclose(q, [-44, -14, 48, 28]) - True - - """ - x0, y0, z0, w0 = quaternion0 - x1, y1, z1, w1 = quaternion1 - return numpy.array(( - x1*w0 + y1*z0 - z1*y0 + w1*x0, - -x1*z0 + y1*w0 + z1*x0 + w1*y0, - x1*y0 - y1*x0 + z1*w0 + w1*z0, - -x1*x0 - y1*y0 - z1*z0 + w1*w0), dtype=numpy.float64) - - -def quaternion_conjugate(quaternion): - """Return conjugate of quaternion. - - >>> q0 = random_quaternion() - >>> q1 = quaternion_conjugate(q0) - >>> q1[3] == q0[3] and all(q1[:3] == -q0[:3]) - True - - """ - return numpy.array((-quaternion[0], -quaternion[1], - -quaternion[2], quaternion[3]), dtype=numpy.float64) - - -def quaternion_inverse(quaternion): - """Return inverse of quaternion. - - >>> q0 = random_quaternion() - >>> q1 = quaternion_inverse(q0) - >>> numpy.allclose(quaternion_multiply(q0, q1), [0, 0, 0, 1]) - True - - """ - return quaternion_conjugate(quaternion) / numpy.dot(quaternion, quaternion) - - -def quaternion_slerp(quat0, quat1, fraction, spin=0, shortestpath=True): - """Return spherical linear interpolation between two quaternions. - - >>> q0 = random_quaternion() - >>> q1 = random_quaternion() - >>> q = quaternion_slerp(q0, q1, 0.0) - >>> numpy.allclose(q, q0) - True - >>> q = quaternion_slerp(q0, q1, 1.0, 1) - >>> numpy.allclose(q, q1) - True - >>> q = quaternion_slerp(q0, q1, 0.5) - >>> angle = math.acos(numpy.dot(q0, q)) - >>> numpy.allclose(2.0, math.acos(numpy.dot(q0, q1)) / angle) or \ - numpy.allclose(2.0, math.acos(-numpy.dot(q0, q1)) / angle) - True - - """ - q0 = unit_vector(quat0[:4]) - q1 = unit_vector(quat1[:4]) - if fraction == 0.0: - return q0 - elif fraction == 1.0: - return q1 - d = numpy.dot(q0, q1) - if abs(abs(d) - 1.0) < _EPS: - return q0 - if shortestpath and d < 0.0: - # invert rotation - d = -d - q1 *= -1.0 - angle = math.acos(d) + spin * math.pi - if abs(angle) < _EPS: - return q0 - isin = 1.0 / math.sin(angle) - q0 *= math.sin((1.0 - fraction) * angle) * isin - q1 *= math.sin(fraction * angle) * isin - q0 += q1 - return q0 - - -def random_quaternion(rand=None): - """Return uniform random unit quaternion. - - rand: array like or None - Three independent random variables that are uniformly distributed - between 0 and 1. - - >>> q = random_quaternion() - >>> numpy.allclose(1.0, vector_norm(q)) - True - >>> q = random_quaternion(numpy.random.random(3)) - >>> q.shape - (4,) - - """ - if rand is None: - rand = numpy.random.rand(3) - else: - assert len(rand) == 3 - r1 = numpy.sqrt(1.0 - rand[0]) - r2 = numpy.sqrt(rand[0]) - pi2 = math.pi * 2.0 - t1 = pi2 * rand[1] - t2 = pi2 * rand[2] - return numpy.array((numpy.sin(t1)*r1, - numpy.cos(t1)*r1, - numpy.sin(t2)*r2, - numpy.cos(t2)*r2), dtype=numpy.float64) - - -def random_rotation_matrix(rand=None): - """Return uniform random rotation matrix. - - rnd: array like - Three independent random variables that are uniformly distributed - between 0 and 1 for each returned quaternion. - - >>> R = random_rotation_matrix() - >>> numpy.allclose(numpy.dot(R.T, R), numpy.identity(4)) - True - - """ - return quaternion_matrix(random_quaternion(rand)) - - -class Arcball(object): - """Virtual Trackball Control. - - >>> ball = Arcball() - >>> ball = Arcball(initial=numpy.identity(4)) - >>> ball.place([320, 320], 320) - >>> ball.down([500, 250]) - >>> ball.drag([475, 275]) - >>> R = ball.matrix() - >>> numpy.allclose(numpy.sum(R), 3.90583455) - True - >>> ball = Arcball(initial=[0, 0, 0, 1]) - >>> ball.place([320, 320], 320) - >>> ball.setaxes([1,1,0], [-1, 1, 0]) - >>> ball.setconstrain(True) - >>> ball.down([400, 200]) - >>> ball.drag([200, 400]) - >>> R = ball.matrix() - >>> numpy.allclose(numpy.sum(R), 0.2055924) - True - >>> ball.next() - - """ - - def __init__(self, initial=None): - """Initialize virtual trackball control. - - initial : quaternion or rotation matrix - - """ - self._axis = None - self._axes = None - self._radius = 1.0 - self._center = [0.0, 0.0] - self._vdown = numpy.array([0, 0, 1], dtype=numpy.float64) - self._constrain = False - - if initial is None: - self._qdown = numpy.array([0, 0, 0, 1], dtype=numpy.float64) - else: - initial = numpy.array(initial, dtype=numpy.float64) - if initial.shape == (4, 4): - self._qdown = quaternion_from_matrix(initial) - elif initial.shape == (4, ): - initial /= vector_norm(initial) - self._qdown = initial - else: - raise ValueError("initial not a quaternion or matrix.") - - self._qnow = self._qpre = self._qdown - - def place(self, center, radius): - """Place Arcball, e.g. when window size changes. - - center : sequence[2] - Window coordinates of trackball center. - radius : float - Radius of trackball in window coordinates. - - """ - self._radius = float(radius) - self._center[0] = center[0] - self._center[1] = center[1] - - def setaxes(self, *axes): - """Set axes to constrain rotations.""" - if axes is None: - self._axes = None - else: - self._axes = [unit_vector(axis) for axis in axes] - - def setconstrain(self, constrain): - """Set state of constrain to axis mode.""" - self._constrain = constrain == True - - def getconstrain(self): - """Return state of constrain to axis mode.""" - return self._constrain - - def down(self, point): - """Set initial cursor window coordinates and pick constrain-axis.""" - self._vdown = arcball_map_to_sphere(point, self._center, self._radius) - self._qdown = self._qpre = self._qnow - - if self._constrain and self._axes is not None: - self._axis = arcball_nearest_axis(self._vdown, self._axes) - self._vdown = arcball_constrain_to_axis(self._vdown, self._axis) - else: - self._axis = None - - def drag(self, point): - """Update current cursor window coordinates.""" - vnow = arcball_map_to_sphere(point, self._center, self._radius) - - if self._axis is not None: - vnow = arcball_constrain_to_axis(vnow, self._axis) - - self._qpre = self._qnow - - t = numpy.cross(self._vdown, vnow) - if numpy.dot(t, t) < _EPS: - self._qnow = self._qdown - else: - q = [t[0], t[1], t[2], numpy.dot(self._vdown, vnow)] - self._qnow = quaternion_multiply(q, self._qdown) - - def next(self, acceleration=0.0): - """Continue rotation in direction of last drag.""" - q = quaternion_slerp(self._qpre, self._qnow, 2.0+acceleration, False) - self._qpre, self._qnow = self._qnow, q - - def matrix(self): - """Return homogeneous rotation matrix.""" - return quaternion_matrix(self._qnow) - - -def arcball_map_to_sphere(point, center, radius): - """Return unit sphere coordinates from window coordinates.""" - v = numpy.array(((point[0] - center[0]) / radius, - (center[1] - point[1]) / radius, - 0.0), dtype=numpy.float64) - n = v[0]*v[0] + v[1]*v[1] - if n > 1.0: - v /= math.sqrt(n) # position outside of sphere - else: - v[2] = math.sqrt(1.0 - n) - return v - - -def arcball_constrain_to_axis(point, axis): - """Return sphere point perpendicular to axis.""" - v = numpy.array(point, dtype=numpy.float64, copy=True) - a = numpy.array(axis, dtype=numpy.float64, copy=True) - v -= a * numpy.dot(a, v) # on plane - n = vector_norm(v) - if n > _EPS: - if v[2] < 0.0: - v *= -1.0 - v /= n - return v - if a[2] == 1.0: - return numpy.array([1, 0, 0], dtype=numpy.float64) - return unit_vector([-a[1], a[0], 0]) - - -def arcball_nearest_axis(point, axes): - """Return axis, which arc is nearest to point.""" - point = numpy.array(point, dtype=numpy.float64, copy=False) - nearest = None - mx = -1.0 - for axis in axes: - t = numpy.dot(arcball_constrain_to_axis(point, axis), point) - if t > mx: - nearest = axis - mx = t - return nearest - - -# epsilon for testing whether a number is close to zero -_EPS = numpy.finfo(float).eps * 4.0 - -# axis sequences for Euler angles -_NEXT_AXIS = [1, 2, 0, 1] - -# map axes strings to/from tuples of inner axis, parity, repetition, frame -_AXES2TUPLE = { - 'sxyz': (0, 0, 0, 0), 'sxyx': (0, 0, 1, 0), 'sxzy': (0, 1, 0, 0), - 'sxzx': (0, 1, 1, 0), 'syzx': (1, 0, 0, 0), 'syzy': (1, 0, 1, 0), - 'syxz': (1, 1, 0, 0), 'syxy': (1, 1, 1, 0), 'szxy': (2, 0, 0, 0), - 'szxz': (2, 0, 1, 0), 'szyx': (2, 1, 0, 0), 'szyz': (2, 1, 1, 0), - 'rzyx': (0, 0, 0, 1), 'rxyx': (0, 0, 1, 1), 'ryzx': (0, 1, 0, 1), - 'rxzx': (0, 1, 1, 1), 'rxzy': (1, 0, 0, 1), 'ryzy': (1, 0, 1, 1), - 'rzxy': (1, 1, 0, 1), 'ryxy': (1, 1, 1, 1), 'ryxz': (2, 0, 0, 1), - 'rzxz': (2, 0, 1, 1), 'rxyz': (2, 1, 0, 1), 'rzyz': (2, 1, 1, 1)} - -_TUPLE2AXES = dict((v, k) for k, v in _AXES2TUPLE.items()) - -# helper functions - -def vector_norm(data, axis=None, out=None): - """Return length, i.e. eucledian norm, of ndarray along axis. - - >>> v = numpy.random.random(3) - >>> n = vector_norm(v) - >>> numpy.allclose(n, numpy.linalg.norm(v)) - True - >>> v = numpy.random.rand(6, 5, 3) - >>> n = vector_norm(v, axis=-1) - >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=2))) - True - >>> n = vector_norm(v, axis=1) - >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1))) - True - >>> v = numpy.random.rand(5, 4, 3) - >>> n = numpy.empty((5, 3), dtype=numpy.float64) - >>> vector_norm(v, axis=1, out=n) - >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1))) - True - >>> vector_norm([]) - 0.0 - >>> vector_norm([1.0]) - 1.0 - - """ - data = numpy.array(data, dtype=numpy.float64, copy=True) - if out is None: - if data.ndim == 1: - return math.sqrt(numpy.dot(data, data)) - data *= data - out = numpy.atleast_1d(numpy.sum(data, axis=axis)) - numpy.sqrt(out, out) - return out - else: - data *= data - numpy.sum(data, axis=axis, out=out) - numpy.sqrt(out, out) - - -def unit_vector(data, axis=None, out=None): - """Return ndarray normalized by length, i.e. eucledian norm, along axis. - - >>> v0 = numpy.random.random(3) - >>> v1 = unit_vector(v0) - >>> numpy.allclose(v1, v0 / numpy.linalg.norm(v0)) - True - >>> v0 = numpy.random.rand(5, 4, 3) - >>> v1 = unit_vector(v0, axis=-1) - >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=2)), 2) - >>> numpy.allclose(v1, v2) - True - >>> v1 = unit_vector(v0, axis=1) - >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=1)), 1) - >>> numpy.allclose(v1, v2) - True - >>> v1 = numpy.empty((5, 4, 3), dtype=numpy.float64) - >>> unit_vector(v0, axis=1, out=v1) - >>> numpy.allclose(v1, v2) - True - >>> list(unit_vector([])) - [] - >>> list(unit_vector([1.0])) - [1.0] - - """ - if out is None: - data = numpy.array(data, dtype=numpy.float64, copy=True) - if data.ndim == 1: - data /= math.sqrt(numpy.dot(data, data)) - return data - else: - if out is not data: - out[:] = numpy.array(data, copy=False) - data = out - length = numpy.atleast_1d(numpy.sum(data*data, axis)) - numpy.sqrt(length, length) - if axis is not None: - length = numpy.expand_dims(length, axis) - data /= length - if out is None: - return data - - -def random_vector(size): - """Return array of random doubles in the half-open interval [0.0, 1.0). - - >>> v = random_vector(10000) - >>> numpy.all(v >= 0.0) and numpy.all(v < 1.0) - True - >>> v0 = random_vector(10) - >>> v1 = random_vector(10) - >>> numpy.any(v0 == v1) - False - - """ - return numpy.random.random(size) - - -def inverse_matrix(matrix): - """Return inverse of square transformation matrix. - - >>> M0 = random_rotation_matrix() - >>> M1 = inverse_matrix(M0.T) - >>> numpy.allclose(M1, numpy.linalg.inv(M0.T)) - True - >>> for size in range(1, 7): - ... M0 = numpy.random.rand(size, size) - ... M1 = inverse_matrix(M0) - ... if not numpy.allclose(M1, numpy.linalg.inv(M0)): print size - - """ - return numpy.linalg.inv(matrix) - - -def concatenate_matrices(*matrices): - """Return concatenation of series of transformation matrices. - - >>> M = numpy.random.rand(16).reshape((4, 4)) - 0.5 - >>> numpy.allclose(M, concatenate_matrices(M)) - True - >>> numpy.allclose(numpy.dot(M, M.T), concatenate_matrices(M, M.T)) - True - - """ - M = numpy.identity(4) - for i in matrices: - M = numpy.dot(M, i) - return M - - -def is_same_transform(matrix0, matrix1): - """Return True if two matrices perform same transformation. - - >>> is_same_transform(numpy.identity(4), numpy.identity(4)) - True - >>> is_same_transform(numpy.identity(4), random_rotation_matrix()) - False - - """ - matrix0 = numpy.array(matrix0, dtype=numpy.float64, copy=True) - matrix0 /= matrix0[3, 3] - matrix1 = numpy.array(matrix1, dtype=numpy.float64, copy=True) - matrix1 /= matrix1[3, 3] - return numpy.allclose(matrix0, matrix1) - - -def _import_module(module_name, warn=True, prefix='_py_', ignore='_'): - """Try import all public attributes from module into global namespace. - - Existing attributes with name clashes are renamed with prefix. - Attributes starting with underscore are ignored by default. - - Return True on successful import. - - """ - try: - module = __import__(module_name) - except ImportError: - if warn: - warnings.warn("Failed to import module " + module_name) - else: - for attr in dir(module): - if ignore and attr.startswith(ignore): - continue - if prefix: - if attr in globals(): - globals()[prefix + attr] = globals()[attr] - elif warn: - warnings.warn("No Python implementation of " + attr) - globals()[attr] = getattr(module, attr) - return True \ No newline at end of file diff --git a/rktl_dependencies/setup.cfg b/rktl_dependencies/setup.cfg deleted file mode 100644 index 064c689bf..000000000 --- a/rktl_dependencies/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/rktl_dependencies -[install] -install_scripts=$base/lib/rktl_dependencies diff --git a/rktl_dependencies/setup.py b/rktl_dependencies/setup.py deleted file mode 100644 index 10167f7b1..000000000 --- a/rktl_dependencies/setup.py +++ /dev/null @@ -1,25 +0,0 @@ -from setuptools import find_packages, setup - -package_name = 'rktl_dependencies' - -setup( - name=package_name, - version='1.0.0', - packages=find_packages(exclude=['test']), - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - ], - install_requires=['setuptools'], - zip_safe=True, - maintainer='Adam Schmok', - maintainer_email='aschmok@purdue.edu', - description='Contains external dependencies required for other packages.', - license='BSD 3 Clause', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - ], - }, -) diff --git a/rktl_dependencies/test/test_copyright.py b/rktl_dependencies/test/test_copyright.py deleted file mode 100644 index 97a39196e..000000000 --- a/rktl_dependencies/test/test_copyright.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_copyright.main import main -import pytest - - -# Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' diff --git a/rktl_dependencies/test/test_flake8.py b/rktl_dependencies/test/test_flake8.py deleted file mode 100644 index 27ee1078f..000000000 --- a/rktl_dependencies/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_flake8.main import main_with_errors -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ - '\n'.join(errors) diff --git a/rktl_dependencies/test/test_pep257.py b/rktl_dependencies/test/test_pep257.py deleted file mode 100644 index b234a3840..000000000 --- a/rktl_dependencies/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' diff --git a/rktl_game/README.md b/rktl_game/README.md deleted file mode 100644 index da9f89335..000000000 --- a/rktl_game/README.md +++ /dev/null @@ -1,5 +0,0 @@ -# rktl_game - -This package contains code for keeping track of the game state. It also contains -a simple web interface to control the system and view the game state from -another computer, phone, or laptop on the same network. diff --git a/rktl_game/launch/game.launch b/rktl_game/launch/game.launch deleted file mode 100644 index 98e0ec108..000000000 --- a/rktl_game/launch/game.launch +++ /dev/null @@ -1,5 +0,0 @@ - - - - - \ No newline at end of file diff --git a/rktl_game/launch/game.launch.py b/rktl_game/launch/game.launch.py deleted file mode 100644 index 63aee2f20..000000000 --- a/rktl_game/launch/game.launch.py +++ /dev/null @@ -1,28 +0,0 @@ -import os -import sys - -import launch -import launch_ros.actions -from ament_index_python.packages import get_package_share_directory - - -def generate_launch_description(): - ld = launch.LaunchDescription([ - launch_ros.actions.Node( - package='rktl_game', - executable='game_manager', - name='game_manager', - output='screen' - ), - launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory( - 'rosbridge_server'), 'launch/rosbridge_websocket.launch.py') - ) - ) - ]) - return ld - - -if __name__ == '__main__': - generate_launch_description() diff --git a/rktl_game/package.xml b/rktl_game/package.xml deleted file mode 100644 index 0cc17a9cc..000000000 --- a/rktl_game/package.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - - rktl_game - 0.0.0 - Game manager for ARC Rocket League project - Autonomous Robotics Club of Purdue - BSD 3 Clause - http://www.purduearc.com - AJ Ernandes - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - rclpy - nodelet - rosbridge_server - nav_msgs - rktl_msgs - - - ament_python - - diff --git a/rktl_game/resource/rktl_game b/rktl_game/resource/rktl_game deleted file mode 100644 index e69de29bb..000000000 diff --git a/rktl_game/rktl_game/__init__.py b/rktl_game/rktl_game/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/rktl_game/rktl_game/nodes/README.md b/rktl_game/rktl_game/nodes/README.md deleted file mode 100644 index f9e6e2c1c..000000000 --- a/rktl_game/rktl_game/nodes/README.md +++ /dev/null @@ -1,56 +0,0 @@ -# ROS Nodes - -These are the [ROS Nodes](http://wiki.ros.org/Nodes) provided by this package. -Most likely, you will use a launch file to run these. You can also run them -by using `rosrun`: - -```bash -rosrun rktl_autonomy -``` - -```{contents} ROS Nodes in the package -:depth: 2 -:backlinks: top -:local: true -``` - ---- - -## game_manager - -This node tracks the score of a match played by two teams (orange and blue) and -updates the match status (playing, paused, finished) based on certain conditions. -It subscribes to the Odometry topic of the ball to detect goals and publishes -the match status and score to the match_status topic. The ScoreKeeper node also -provides three services (`reset_game`, `unpause_game`, `pause_game`) that enable -resetting the game, unpausing it, or pausing it, respectively. - -### Subscribed Topics: - -- `ball/odom` ([nav_msgs/Odometry.msg](https://docs.ros.org/en/noetic/api/nav_msgs/html/msg/Odometry.html)): - Odometry of the ball. -- `cars/enable`([std_msgs/Bool.msg](https://docs.ros.org/en/noetic/api/std_msgs/html/msg/Bool.html)): - Status of the cars (enabled/disabled). - -### Published Topics: - -- `match_status` ([rktl_msgs/MatchStatus](/rktl_msgs/html/msg/MatchStatus.html#http://)): - Publishes the current match status and score. - -### Services : - -- `reset_game` ([std_srvs/Empty.srv](https://docs.ros.org/en/api/std_srvs/html/srv/Empty.html)): - Resets the game and sets the scores to 0. -- `unpause_game` ([std_srvs/Empty.srv](https://docs.ros.org/en/api/std_srvs/html/srv/Empty.html)): - Unpauses the game and enables the cars. -- `pause_game` ([std_srvs/Empty.srv](https://docs.ros.org/en/api/std_srvs/html/srv/Empty.html)): - Pauses the game and disables the cars. - -### Parameters: - -- `/field/length` (`double`, default: `1.0`): The length of the playing field. -- `/game_length` (`int`, default: `90`): The length of the game in seconds. -- `manager_rate` (`int`, default: `10`): The rate at which the main loop is - executed. - ---- diff --git a/rktl_game/rktl_game/nodes/game_manager b/rktl_game/rktl_game/nodes/game_manager deleted file mode 100755 index 6cb07e106..000000000 --- a/rktl_game/rktl_game/nodes/game_manager +++ /dev/null @@ -1,110 +0,0 @@ -#!/usr/bin/env python3 -"""Use ball location to determine when a goal is scored. -License: - BSD 3-Clause License - Copyright (c) 2023, Autonomous Robotics Club of Purdue (Purdue ARC) - All rights reserved. -""" - - - -# ROS -import rclpy -from std_msgs.msg import Bool, Header -from nav_msgs.msg import Odometry -from rktl_msgs.msg import Score, MatchStatus -from std_srvs.srv import Empty, EmptyResponse - -class ScoreKeeper(): - def __init__(self): - rclpy.init() - global node - node = rclpy.create_node('score_keeper') - - self.orange_score = 0 - self.blue_score = 0 - self.orange_goal = (node.get_parameter('/field/length', 1) - 0.15) / 2 - self.blue_goal = (node.get_parameter('/field/length', 1) - 0.15) / -2 - self.enabled = False - self.match_status = 0 - self.game_clock = node.get_parameter('/game_length', 90) - self.manager_rate = node.get_parameter('manager_rate', 10) - - # Publishers - self.match_status_pub = node.create_publisher(MatchStatus, 'match_status', queue_size=1) - self.active_pub = node.create_publisher(Bool, 'cars/enable', queue_size=1) - - # Subscribers - node.create_subscription(Odometry, 'ball/odom', self.check_goal) - node.create_subscription(Bool, 'cars/enable', self.enable) - - # Services - node.create_service('reset_game', Empty, self.reset) - node.create_service('unpause_game', Empty, self.unpause) - node.create_service('pause_game', Empty, self.pause) - - # Counts loops and decrements game clock every second - self.timer_ctr = 0 - # main loop - rate = node.create_rate(self.manager_rate) - while rclpy.ok(): - if self.match_status == 1: - self.timer_ctr += 1 - if self.timer_ctr == self.manager_rate: - self.timer_ctr = 0 - self.game_clock -= 1 - self.update_status() - rate.sleep() - - def update_status(self): - if (self.match_status == 1 and self.game_clock == 0): - if (self.orange_score > self.blue_score): - self.match_status = 3 - else: - self.match_status = 4 - score = Score(self.orange_score, self.blue_score) - header = Header() - header.stamp = node.get_clock().now() - self.match_status_pub.publish(MatchStatus(header, self.match_status, self.game_clock, score)) - - def pause(self, _): - self.enabled = False - self.match_status = 0 - self.active_pub.publish(False) - return EmptyResponse() - - def reset(self, _): - self.orange_score = 0 - self.blue_score = 0 - self.enabled = False - self.match_status = 0 - self.game_clock = node.get_parameter('/game_length', 90) - self.active_pub.publish(False) - header = Header() - header.stamp = node.get_clock().now() - self.match_status_pub.publish(MatchStatus(header, 0, self.game_clock, Score(0, 0))) - return EmptyResponse() - - def unpause(self, _): - self.enabled = True - self.match_status = 1 - self.active_pub.publish(True) - return EmptyResponse() - - def check_goal(self, ball_pose: Odometry): - if self.enabled: - # Check if ball is in goal - if ball_pose.pose.pose.position.x >= self.orange_goal: - self.orange_score += 1 - self.pause(self) - elif ball_pose.pose.pose.position.x <= self.blue_goal: - self.blue_score += 1 - self.pause(self) - - def enable(self, data: Bool): - # enable after a goal is scored & cars are replaced - if data.data: - self.enabled = True - -if __name__ == "__main__": - ScoreKeeper() diff --git a/rktl_game/setup.cfg b/rktl_game/setup.cfg deleted file mode 100644 index 1140ea26f..000000000 --- a/rktl_game/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/rktl_game -[install] -install_scripts=$base/lib/rktl_game diff --git a/rktl_game/setup.py b/rktl_game/setup.py deleted file mode 100644 index 6b0dbe78b..000000000 --- a/rktl_game/setup.py +++ /dev/null @@ -1,25 +0,0 @@ -from setuptools import find_packages, setup - -package_name = 'rktl_game' - -setup( - name=package_name, - version='0.0.0', - packages=find_packages(exclude=['test']), - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - ], - install_requires=['setuptools'], - zip_safe=True, - maintainer='jcrm1', - maintainer_email='52137472+jcrm1@users.noreply.github.com', - description='TODO: Package description', - license='TODO: License declaration', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - ], - }, -) diff --git a/rktl_game/test/test_copyright.py b/rktl_game/test/test_copyright.py deleted file mode 100644 index 97a39196e..000000000 --- a/rktl_game/test/test_copyright.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_copyright.main import main -import pytest - - -# Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' diff --git a/rktl_game/test/test_flake8.py b/rktl_game/test/test_flake8.py deleted file mode 100644 index 27ee1078f..000000000 --- a/rktl_game/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_flake8.main import main_with_errors -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ - '\n'.join(errors) diff --git a/rktl_game/test/test_pep257.py b/rktl_game/test/test_pep257.py deleted file mode 100644 index b234a3840..000000000 --- a/rktl_game/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' diff --git a/rktl_game/web/index.html b/rktl_game/web/index.html deleted file mode 100644 index dc91da9ee..000000000 --- a/rktl_game/web/index.html +++ /dev/null @@ -1,102 +0,0 @@ - - - - - - - - - - - - - - Rocket League Game - - - - - - - - -
-
-
-

0

-
-
-

0:00

-
-
-

0

-
-
-
- - -
-
- - -
-
- - - -
-
- - -
- -
- - -
- -
-
-

Blue

-
- - -
-
- - -
-

Orange

-
- - -
-
-
- -
- - - - - \ No newline at end of file diff --git a/rktl_game/web/index.js b/rktl_game/web/index.js deleted file mode 100644 index d209512f2..000000000 --- a/rktl_game/web/index.js +++ /dev/null @@ -1,79 +0,0 @@ -// Connecting to ROS -// ----------------- - -var ros = new ROSLIB.Ros({ - url: 'ws://r2d2:9090' -}); - -ros.on('connection', function () { - console.log('Connected to websocket server.'); -}); - -ros.on('error', function (error) { - console.log('Error connecting to websocket server: ', error); -}); - -ros.on('close', function () { - console.log('Connection to websocket server closed.'); -}); - - -// Subscribe to the game manager node - -var statusListener = new ROSLIB.Topic({ - ros: ros, - name: '/match_status', - messageType: 'rktl_msgs/MatchStatus' -}); - -// Creating the service objects called by the buttons - -var pause_srv = new ROSLIB.Service({ - ros: ros, - name: 'pause_game', - serviceType: 'std_srvs/Empty' -}); - -var unpause_srv = new ROSLIB.Service({ - ros: ros, - name: 'unpause_game', - serviceType: 'std_srvs/Empty' -}); - -var reset_srv = new ROSLIB.Service({ - ros: ros, - name: 'reset_game', - serviceType: 'std_srvs/Empty' -}); - -// Game clock parameters - -var game_time_param = new ROSLIB.Param({ - ros: ros, - name: '/game_length' -}); - -function set_game_time() { - var time = document.getElementById('time-set').value; - time = parseInt(time); - game_time_param.set(time); - reset_srv.callService(); -} - -// updating the score on a timer - -const interval = setInterval(function () { - statusListener.subscribe(function (message) { - document.getElementById('blue-score').innerHTML = message.score.blue; - document.getElementById('orange-score').innerHTML = message.score.orange; - var minutes = Math.floor(message.clock / 60); - var seconds = message.clock - minutes * 60; - if (seconds < 10) { - seconds = '0' + seconds; - } - var time = minutes + ':' + seconds; - - document.getElementById('timer').innerHTML = time; - console.log(message.status); - }); -}, 1000); \ No newline at end of file diff --git a/rktl_game/web/stylesheet.css b/rktl_game/web/stylesheet.css deleted file mode 100644 index 5e8b2585d..000000000 --- a/rktl_game/web/stylesheet.css +++ /dev/null @@ -1,174 +0,0 @@ -html { - font-family: Arial, Helvetica, sans-serif; - background-color: white; -} - -body { - display: flex; - flex-direction: column; - justify-content: center; -} - -.scoreboard { - display: flex; - justify-content: center; - width: 100%; - height: auto; -} - -.box { - display: flex; - justify-content: center; - width: 40%; - box-shadow: rgba(50, 50, 93, 0.25) 0px 6px 12px -2px, rgba(0, 0, 0, 0.3) 0px 3px 7px -3px; - background-color: rgb(88, 88, 88); - color: white; - border-radius: 12px; -} - -.blue-score { - width: 33.3333%; - background-color:rgb(88, 191, 255); - text-align: center; - border-radius: 12px 0px 0px 12px; -} - -.time { - width: 33.3333%; - text-align: center; -} - -.orange-score { - width: 33.3333%; - background-color:rgb(255, 170, 58); - text-align: center; - border-radius: 0px 12px 12px 0px; -} - -.field { - left: 0; - right: 0; - margin: auto; - margin-top: .5em; - margin-bottom: 2em; - width: 50%; - height: 50%; - background-color: rgb(38, 182, 38); - box-shadow: rgba(0, 0, 0, 0.15) 2px 1px 8px 2px; - border-radius: 8px 8px 8px 8px; -} - -.team-selection { - display: flex; - display: none; /* Need to eventually impliment the controller selection */ - justify-content: center; - width: 100%; - height: auto; -} - -.blue-team { - width: 50%; - background-color: rgb(88, 191, 255); - border-radius: 12px 12px 12px 12px; - box-shadow: rgba(50, 50, 93, 0.25) 0px 6px 12px -2px, rgba(0, 0, 0, 0.3) 0px 3px 7px -3px; - margin-right: 1em; -} - -.orange-team { - width: 50%; - background-color: rgb(255, 170, 58); - border-radius: 12px 12px 12px 12px; - box-shadow: rgba(50, 50, 93, 0.25) 0px 6px 12px -2px, rgba(0, 0, 0, 0.3) 0px 3px 7px -3px; - margin-left: 1em; -} - -h1 { - text-align: center; -} - -.dropdown { - text-align: center; - margin: 1em; -} - -.box2 { - display: flex; - justify-content: center; - width: 40%; - background-color: white; - color: white; - border-radius: 12px; -} - -.control { - display: flex; - justify-content: center; - width: 100%; - height: auto; -} - -.box3 { - display: flex; - justify-content: center; - width: 100%; - background-color: rgba(255, 255, 255, 0); - color: white; - border-radius: 12px; -} - -.box4 { - margin-top: 10px; - display: flex; - justify-content: center; - width: 100%; - background-color: rgba(255, 255, 255, 0); - color: white; - border-radius: 12px; -} - -input { - width: 10%; - border-radius: 5px; - height: 50px; -} - -.box4 button { - width: 3%; - border-radius: 5px; -} - - -/* CSS */ -.button-13 { - background-color: #fff; - border: 1px solid #d5d9d9; - border-radius: 8px; - box-shadow: rgba(213, 217, 217, .5) 0 2px 5px 0; - box-sizing: border-box; - color: #0f1111; - cursor: pointer; - display: inline-block; - font-family: "Amazon Ember",sans-serif; - font-size: 18px; - line-height: 32px; - padding: 0 10px 0 11px; - position: relative; - text-align: center; - text-decoration: none; - user-select: none; - -webkit-user-select: none; - touch-action: manipulation; - vertical-align: middle; - width: 100px; - margin: 1em; -} - -.button-13:hover { - background-color: #f7fafa; -} - -.button-13:focus { - border-color: #008296; - box-shadow: rgba(213, 217, 217, .5) 0 2px 5px 0; - outline: 0; -} \ No newline at end of file diff --git a/rktl_launch/README.md b/rktl_launch/README.md deleted file mode 100644 index 4cebefaa1..000000000 --- a/rktl_launch/README.md +++ /dev/null @@ -1,4 +0,0 @@ -# rktl_launch - -This package contains various launch files that are not specific to any -particular package, or apply to multiple packages simultaneously. diff --git a/rktl_launch/config/global_params.yaml b/rktl_launch/config/global_params.yaml deleted file mode 100644 index 2053ccd0c..000000000 --- a/rktl_launch/config/global_params.yaml +++ /dev/null @@ -1,31 +0,0 @@ -# ALL UNITS IN SI - -# field dimensions -field: - width: 3 - # max: 3.5 - # min: 2.5 - length: 4.25 - # max: 5 - # min: 3.5 - wall_thickness: 0.25 - goal: - width: 1 - -# car dimensions and physical constants -ball: - radius: 0.0508 # (2 in) - -# car dimensions and physical constants -cars: - length: 0.12 # front to rear wheel center to center, meters - # min: 0.10 - # max: 0.30 - steering: - max_throw: 0.1826 # center to side, radians - # min: 0.16 - # max: 0.20 - rate: 0.9128 # rad/s - throttle: - max_speed: 2.3 # m/s - tau: 0.2 # time constant, seconds diff --git a/rktl_launch/launch/README.md b/rktl_launch/launch/README.md deleted file mode 100644 index c796ee155..000000000 --- a/rktl_launch/launch/README.md +++ /dev/null @@ -1,148 +0,0 @@ -# Launch Files - -These are [.launch files](https://wiki.ros.org/roslaunch/XML) that can be run -using [roslaunch](https://wiki.ros.org/roslaunch): - -```shell -roslaunch rktl_launch -``` - -```{contents} Launch Files in the package -:depth: 2 -:backlinks: top -:local: true -``` - ---- - -## c3po.launch - -This launch file includes two instances of the `camera.launch` file from the -[`rktl_perception`](../../rktl_perception/README.md) package with different -values for the `camera_name` argument. This is the launch file that will be run -on the computer named "c3po" on the Rocket League cart. - -### Launch Arguments - -This launch file does not define any arguments of its own. - -### Nodes - -This launch file does not define any nodes of its own. - -### Included Launch Files - -- `rktl_perception camera.launch` (Included twice, once with `camera_name` set - to `cam0` and once with `camera_name` set to `cam1`) - ---- - -## r2d2.launch - -This launch file includes two instances of the `camera.launch` file from the -[`rktl_perception`](../../rktl_perception/README.md) package with different -values for the `camera_name` argument, launches a node from the -[`rktl_control`](../../rktl_control/README.md) package, and includes the -`rocket_league.launch` file from the [`rktl_launch`](../README.md) package. -This is the launch file that will be run on the computer named "r2d2" on the -Rocket League cart. - -### Nodes - -- `rktl_control pose_sync_node` (parameters are loaded from - `rktl_control/config/pose_synchronizer.yaml`) - -### Included Launch Files - -- `rktl_perception camera.launch` (Included twice, once with `camera_name` set - to `cam2` and once with `camera_name` set to `cam3`) -- `rktl_launch rocket_league.launch` - ---- - -## rocket_league.launch - -This roslaunch file launches everything that is needed to run the system using -actual hardware. It can be run with different types of agents and includes -launch files for the game manager, control GUI, ball, cars, and agents based on -the selected agent type. - -### Launch Arguments - -- `render` (default: `true`): A boolean argument that determines whether to - enable the visualizer or not. -- `agent_type` (default: `patrol`): A string argument that specifies the type of - agent to use. Valid options are `planner`, `autonomy`, or `patrol`. -- autonomy_weights (default: `model`): A string argument that specifies the name - of the autonomy weights file to use. This argument is only used if - `agent_type` is set to `'autonomy'`. - -### Nodes - -- `rqt_gui`: `rqt_gui` node from the `rqt_gui` package - -### Included Launch Files - -- `rktl_sim visualizer.launch` (Only included if `render` is set to `true`) -- `rktl_game game.launch` -- `rktl_control ball.launch` -- `rktl_control car.launch` -- `rktl_control hardware_interface.launch` -- `rktl_planner simple_agent.launch` (Only included if `agent_type` is set to - `'planner'`) -- `rktl_autonomy rocket_league_agent.launch`: (Only included if `agent_type` is - set to `'autonomy'`) -- `rktl_planner patrol_agent.launch`: (Only included if `agent_type` is set to - `'patrol'`) - ---- - -## rocket_league_sim.launch - -This roslaunch file launches a simulation of the environment. It includes a -visualizer, a simulator, simulated perception delays, and control/filtering -stacks for the ball and cars. It also includes agents for controlling the cars, -with options for a planner, an autonomy (ML) agent, or a patrol agent. - -### Parameters - -This launch file loads parametes from the `config/global_params.yaml` file -in the [`rktl_launch`](../README.md) package. - -### Launch Arguments - -- `render` (default: `true`) - whether to launch the visualizer or not. -- `sim_mode` (default: `realistic`) - the simulation mode to use, either - realistic or ideal. -- `perception_delay` (default: `0.15`) - the delay time to use for perception - in the simulation. -- `agent_type` (default: `patrol`) - the type of agent to use for controlling - the car, either `planner`, `autonomy`, or `patrol`. -- `autonomy_weights` (default: `model`) - the weights to use for the autonomy - agent if agent_type is set to autonomy. - -### Nodes - -- `pose_sync_node`: A `pose_sync_node` node from the - [`rktl_control`](../../rktl_control/README.md) package -- `ball/pose_delay`: A `topic_delay` node from the - [`rktl_control`](../../rktl_control/README.md) package. Uses the - `perception_delay` argument. Only run if `sim_mode` is set to `'realistic'`. -- `cars/car0/pose_delay`: A `topic_delay` node from the - [`rktl_control`](../../rktl_control/README.md) package. Uses the - `perception_delay` argument. Only run if `sim_mode` is set to `'realistic'`. - -### Included Launch Files - -- `rktl_sim visualizer.launch` (Only included if `render` is set to `true`) -- `rktl_sim simulator.launch` (Passes the `sim_mode` argument) -- `rktl_control ball.launch` -- `rktl_control car.launch` -- `rktl_planner simple_agent.launch` (Only included if `agent_type` is set to - `'planner'`) -- `rktl_autonomy rocket_league_agent.launch`: (Only included if `agent_type` is - set to `'autonomy'`) -- `rktl_planner patrol_agent.launch`: (Only included if `agent_type` is set to - `'patrol'`) - ---- diff --git a/rktl_launch/launch/c3po.launch b/rktl_launch/launch/c3po.launch deleted file mode 100644 index 80e7a9e4d..000000000 --- a/rktl_launch/launch/c3po.launch +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - diff --git a/rktl_launch/launch/c3po.launch.py b/rktl_launch/launch/c3po.launch.py deleted file mode 100644 index 0722ed50f..000000000 --- a/rktl_launch/launch/c3po.launch.py +++ /dev/null @@ -1,34 +0,0 @@ -import os -import sys - -import launch -import launch_ros.actions -from ament_index_python.packages import get_package_share_directory - - -def generate_launch_description(): - ld = launch.LaunchDescription([ - launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory( - 'rktl_perception'), 'launch/camera.launch.py') - ), - launch_arguments={ - 'camera_name': 'cam0' - }.items() - ), - launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory( - 'rktl_perception'), 'launch/camera.launch.py') - ), - launch_arguments={ - 'camera_name': 'cam1' - }.items() - ) - ]) - return ld - - -if __name__ == '__main__': - generate_launch_description() diff --git a/rktl_launch/launch/r2d2.launch b/rktl_launch/launch/r2d2.launch deleted file mode 100644 index c5ae87d6d..000000000 --- a/rktl_launch/launch/r2d2.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/rktl_launch/launch/r2d2.launch.py b/rktl_launch/launch/r2d2.launch.py deleted file mode 100644 index 3e6e4a41d..000000000 --- a/rktl_launch/launch/r2d2.launch.py +++ /dev/null @@ -1,49 +0,0 @@ -import os -import sys - -import launch -import launch_ros.actions -from ament_index_python.packages import get_package_share_directory - - -def generate_launch_description(): - ld = launch.LaunchDescription([ - launch_ros.actions.Node( - package='rktl_control', - executable='pose_synchronizer', - name='pose_sync_node', - parameters=[ - get_package_share_directory( - 'rktl_control') + '/config/pose_synchronizer.yaml' - ] - ), - launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory( - 'rktl_perception'), 'launch/camera.launch.py') - ), - launch_arguments={ - 'camera_name': 'cam2' - }.items() - ), - launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory( - 'rktl_perception'), 'launch/camera.launch.py') - ), - launch_arguments={ - 'camera_name': 'cam3' - }.items() - ), - launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory( - 'rktl_launch'), 'launch/rocket_league.launch.py') - ) - ) - ]) - return ld - - -if __name__ == '__main__': - generate_launch_description() diff --git a/rktl_launch/launch/rocket_league.launch b/rktl_launch/launch/rocket_league.launch deleted file mode 100644 index 32c2828dd..000000000 --- a/rktl_launch/launch/rocket_league.launch +++ /dev/null @@ -1,37 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/rktl_launch/launch/rocket_league.launch.py b/rktl_launch/launch/rocket_league.launch.py deleted file mode 100644 index bf5ed9f81..000000000 --- a/rktl_launch/launch/rocket_league.launch.py +++ /dev/null @@ -1,103 +0,0 @@ -import os - -import launch -import launch_ros.actions -from ament_index_python.packages import get_package_share_directory - - -def generate_launch_description(): - ld = launch.LaunchDescription([ - launch.actions.DeclareLaunchArgument( - name='render', - default_value='true' - ), - launch.actions.DeclareLaunchArgument( - name='agent_type', # Either planner, autonomy, or patrol - default_value='patrol' - ), - launch.actions.DeclareLaunchArgument( - name='autonomy_weights', - default_value='model' - ), - launch_ros.actions.Node( - package='rqt_gui', - executable='rqt_gui', - name='rqt_gui', - arguments='--perspective-file ' + os.path.join(get_package_share_directory( - 'rktl_launch'), 'rqt','rktl.perspective') - ), - launch_ros.actions.SetParametersFromFile( - filename=os.path.join(get_package_share_directory( - 'rktl_launch'), 'config', 'global_params.yaml') - ), - launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory( - 'rktl_sim'), 'launch/visualizer.launch.py') - ), - condition=launch.conditions.LaunchConfigurationEquals('render', 'true') - ), - launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory( - 'rktl_game'), 'launch/game.launch.py') - ) - ), - launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory( - 'rktl_control'), 'launch/ball.launch.py') - ) - ), - launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory( - 'rktl_control'), 'launch/car.launch.py') - ), - launch_arguments={ - 'car_name': 'car0' - }.items() - ), - launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory( - 'rktl_control'), 'launch/hardware_interface.launch.py') - ) - ), - launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory( - 'rktl_planner'), 'launch/simple_agent.launch.py') - ), - launch_arguments={ - 'agent_name': 'agent0', - 'car_name': 'car0' - }.items(), - condition=launch.conditions.LaunchConfigurationEquals('agent_type', 'planner') - ), - launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory( - 'rktl_autonomy'), 'launch/rocket_league/rocket_league_agent.launch.py') - ), - launch_arguments={ - 'weights_name': launch.substitutions.LaunchConfiguration('autonomy_weights') - }.items(), - condition=launch.conditions.LaunchConfigurationEquals('agent_type', 'autonomy') - ), - launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory( - 'rktl_planner'), 'launch/patrol_agent.launch.py') - ), - launch_arguments={ - 'car_name': 'car0' - }.items(), - condition=launch.conditions.LaunchConfigurationEquals('agent_type', 'patrol') - ) - ]) - return ld - - -if __name__ == '__main__': - generate_launch_description() diff --git a/rktl_launch/launch/rocket_league_sim.launch b/rktl_launch/launch/rocket_league_sim.launch deleted file mode 100644 index 83614a149..000000000 --- a/rktl_launch/launch/rocket_league_sim.launch +++ /dev/null @@ -1,67 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/rktl_launch/launch/rocket_league_sim.launch.py b/rktl_launch/launch/rocket_league_sim.launch.py deleted file mode 100644 index 20c82b458..000000000 --- a/rktl_launch/launch/rocket_league_sim.launch.py +++ /dev/null @@ -1,127 +0,0 @@ -import os -import sys - -import launch -import launch_ros.actions -from ament_index_python.packages import get_package_share_directory - - -def generate_launch_description(): - ld = launch.LaunchDescription([ - launch.actions.DeclareLaunchArgument( - name='render', - default_value='true' - ), - launch.actions.DeclareLaunchArgument( - name='sim_mode', - default_value='realistic' # Either realistic or ideal (check docs) - ), - launch.actions.DeclareLaunchArgument( - name='perception_delay', - default_value='0.15' - ), - launch.actions.DeclareLaunchArgument( - name='agent_type', - default_value='patrol' # Either planner, autonomy, or patrol - ), - launch.actions.DeclareLaunchArgument( - name='autonomy_weights', - default_value='model' - ), - launch_ros.actions.SetRemap( - src="/cars/car0/odom", - dst="/cars/car0/odom_truth", - namespace="truth", - condition=launch.conditions.LaunchConfigurationEquals('sim_mode', 'realistic') - ), - launch_ros.actions.SetRemap( - src="/ball/odom", - dst="/ball/odom_truth", - namespace="truth", - condition=launch.conditions.LaunchConfigurationEquals('sim_mode', 'realistic') - ), - launch_ros.actions.SetParametersFromFile( - filename=os.path.join(get_package_share_directory( - 'rktl_launch'), 'config', 'global_params.yaml') - ), - launch_ros.actions.SetParameter( - name="window_name", - value="Ground Truth", - namespace="visualizer", - condition=launch.conditions.LaunchConfigurationEquals('sim_mode', 'realistic') - ), - launch_ros.actions.Node( - namespace='ball', - package='rktl_control', - executable='topic_delay', - name='pose_delay', - condition=launch.conditions.LaunchConfigurationEquals('sim_mode', 'realistic'), - arguments="pose_sync_early pose_sync geometry_msgs/PoseWithCovarianceStamped " + str(launch.substitutions.LaunchConfiguration('perception_delay')) - ), - launch_ros.actions.Node( - namespace='cars/car0', - package='rktl_control', - executable='topic_delay', - name='pose_delay', - condition=launch.conditions.LaunchConfigurationEquals('sim_mode', 'realistic'), - arguments="pose_sync_early pose_sync geometry_msgs/PoseWithCovarianceStamped " + str(launch.substitutions.LaunchConfiguration('perception_delay')) - ), - launch_ros.actions.Node( - package='rqt_gui', - executable='rqt_gui', - name='rqt_gui', - condition=launch.conditions.LaunchConfigurationEquals('render', 'true'), - arguments='--perspective-file ' + os.path.join(get_package_share_directory( - 'rktl_launch'), 'rqt','rktl.perspective') - ), - launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory( - 'rktl_sim'), 'launch', 'visualizer.launch.py') - ), - namespace='truth', - condition=launch.conditions.LaunchConfigurationEquals('render', 'true') - ), - launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory( - 'rktl_sim'), 'launch/simulator.launch.py') - ), - launch_arguments={ - 'sim_mode': launch.substitutions.LaunchConfiguration('sim_mode') - }.items() - ), - launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory( - 'rktl_planner'), 'launch/simple_agent.launch.py') - ), - launch_arguments={ - 'agent_name': 'agent0', - 'car_name': 'car0' - }.items() - ), - launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory( - 'rktl_autonomy'), 'launch/rocket_league/rocket_league_agent.launch.py') - ), - launch_arguments={ - 'weights_name': launch.substitutions.LaunchConfiguration('autonomy_weights') - }.items() - ), - launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory( - 'rktl_planner'), 'launch/patrol_agent.launch.py') - ), - launch_arguments={ - 'car_name': 'car0' - }.items() - ) - ]) - return ld - - -if __name__ == '__main__': - generate_launch_description() diff --git a/rktl_launch/package.xml b/rktl_launch/package.xml deleted file mode 100644 index ccabe4d07..000000000 --- a/rktl_launch/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - rktl_launch - 1.0.0 - Launch files for the ARC Rocket League project - - Autonomous Robotics Club of Purdue - BSD 3 Clause - https://www.purduearc.com - James Baxter - Harrison McCarty - Arianna McNamara - Vincent Vu - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - rktl_perception - rktl_game - rktl_planner - rktl_sim - rktl_control - rqt_gui - - - ament_python - - diff --git a/rktl_launch/resource/rktl_launch b/rktl_launch/resource/rktl_launch deleted file mode 100644 index e69de29bb..000000000 diff --git a/rktl_launch/rktl_launch/__init__.py b/rktl_launch/rktl_launch/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/rktl_launch/rqt/rktl.perspective b/rktl_launch/rqt/rktl.perspective deleted file mode 100644 index a89eaaed8..000000000 --- a/rktl_launch/rqt/rktl.perspective +++ /dev/null @@ -1,173 +0,0 @@ -{ - "keys": {}, - "groups": { - "mainwindow": { - "keys": { - "geometry": { - "repr(QByteArray.hex)": "QtCore.QByteArray(b'01d9d0cb000300000000017b0000001c000006c6000002d7000001a100000057000006a0000002b100000000000000000780000001a100000057000006a0000002b1')", - "type": "repr(QByteArray.hex)", - "pretty-print": " { W W " - }, - "state": { - "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff00000000fd00000001000000030000050000000231fc0100000002fc00000000000002a40000023000fffffffc0200000002fb00000058007200710074005f007000750062006c00690073006800650072005f005f005000750062006c00690073006800650072005f005f0031005f005f005000750062006c006900730068006500720057006900640067006500740100000014000001070000008a00fffffffb00000072007200710074005f0073006500720076006900630065005f00630061006c006c00650072005f005f005300650072007600690063006500430061006c006c00650072005f005f0031005f005f005300650072007600690063006500430061006c006c00650072005700690064006700650074010000012100000124000000fe00fffffffb0000005a007200710074005f0069006d006100670065005f0076006900650077005f005f0049006d0061006700650056006900650077005f005f0031005f005f0049006d006100670065005600690065007700570069006400670065007401000002aa00000256000000bf00ffffff000005000000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')", - "type": "repr(QByteArray.hex)", - "pretty-print": " 0 ! $ V " - } - }, - "groups": { - "toolbar_areas": { - "keys": { - "MinimizedDockWidgetsToolbar": { - "repr": "8", - "type": "repr" - } - }, - "groups": {} - } - } - }, - "pluginmanager": { - "keys": { - "running-plugins": { - "repr": "{'rqt_image_view/ImageView': [1], 'rqt_publisher/Publisher': [1], 'rqt_service_caller/ServiceCaller': [1]}", - "type": "repr" - } - }, - "groups": { - "plugin__rqt_image_view__ImageView__1": { - "keys": {}, - "groups": { - "dock_widget__ImageViewWidget": { - "keys": { - "dock_widget_title": { - "repr": "'Image View'", - "type": "repr" - }, - "dockable": { - "repr": "True", - "type": "repr" - }, - "parent": { - "repr": "None", - "type": "repr" - } - }, - "groups": {} - }, - "plugin": { - "keys": { - "dynamic_range": { - "repr": "False", - "type": "repr" - }, - "max_range": { - "repr": "10.0", - "type": "repr" - }, - "mouse_pub_topic": { - "repr": "'/cams/cam0/image_raw_mouse_left'", - "type": "repr" - }, - "num_gridlines": { - "repr": "0", - "type": "repr" - }, - "publish_click_location": { - "repr": "False", - "type": "repr" - }, - "rotate": { - "repr": "0", - "type": "repr" - }, - "smooth_image": { - "repr": "False", - "type": "repr" - }, - "toolbar_hidden": { - "repr": "False", - "type": "repr" - }, - "topic": { - "repr": "'/cams/cam0/image_raw'", - "type": "repr" - }, - "zoom1": { - "repr": "False", - "type": "repr" - } - }, - "groups": {} - } - } - }, - "plugin__rqt_publisher__Publisher__1": { - "keys": {}, - "groups": { - "dock_widget__PublisherWidget": { - "keys": { - "dock_widget_title": { - "repr": "'Message Publisher'", - "type": "repr" - }, - "dockable": { - "repr": "True", - "type": "repr" - }, - "parent": { - "repr": "None", - "type": "repr" - } - }, - "groups": {} - }, - "plugin": { - "keys": { - "publishers": { - "repr": "\"[{'topic_name': '/cars/enable', 'type_name': 'std_msgs/Bool', 'rate': 10.0, 'enabled': False, 'publisher_id': 0, 'counter': 9501, 'expressions': {'/cars/enable/data': 'False'}}, {'topic_name': '/match_status', 'type_name': 'rktl_msgs/MatchStatus', 'rate': 20.0, 'enabled': False, 'publisher_id': 1, 'counter': 0, 'expressions': {}}]\"", - "type": "repr" - } - }, - "groups": {} - } - } - }, - "plugin__rqt_service_caller__ServiceCaller__1": { - "keys": {}, - "groups": { - "dock_widget__ServiceCallerWidget": { - "keys": { - "dock_widget_title": { - "repr": "'Service Caller'", - "type": "repr" - }, - "dockable": { - "repr": "True", - "type": "repr" - }, - "parent": { - "repr": "None", - "type": "repr" - } - }, - "groups": {} - }, - "plugin": { - "keys": { - "current_service_name": { - "repr": "'/cars/car1/controller_delay_mux/select'", - "type": "repr" - }, - "splitter_orientation": { - "repr": "2", - "type": "repr" - } - }, - "groups": {} - } - } - } - } - } - } -} \ No newline at end of file diff --git a/rktl_launch/setup.cfg b/rktl_launch/setup.cfg deleted file mode 100644 index b7e96ba37..000000000 --- a/rktl_launch/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/rktl_launch -[install] -install_scripts=$base/lib/rktl_launch diff --git a/rktl_launch/setup.py b/rktl_launch/setup.py deleted file mode 100644 index 521bfcbc0..000000000 --- a/rktl_launch/setup.py +++ /dev/null @@ -1,25 +0,0 @@ -from setuptools import find_packages, setup - -package_name = 'rktl_launch' - -setup( - name=package_name, - version='1.0.0', - packages=find_packages(exclude=['test']), - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - ], - install_requires=['setuptools'], - zip_safe=True, - maintainer='Autonomous Robotics Club of Purdue', - maintainer_email='autonomy@purdue.edu', - description='Launch files for the ARC Rocket League project', - license='BSD 3 Clause', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - ], - }, -) diff --git a/rktl_launch/test/test_copyright.py b/rktl_launch/test/test_copyright.py deleted file mode 100644 index 97a39196e..000000000 --- a/rktl_launch/test/test_copyright.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_copyright.main import main -import pytest - - -# Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' diff --git a/rktl_launch/test/test_flake8.py b/rktl_launch/test/test_flake8.py deleted file mode 100644 index 27ee1078f..000000000 --- a/rktl_launch/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_flake8.main import main_with_errors -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ - '\n'.join(errors) diff --git a/rktl_launch/test/test_pep257.py b/rktl_launch/test/test_pep257.py deleted file mode 100644 index b234a3840..000000000 --- a/rktl_launch/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' diff --git a/rktl_msgs/CMakeLists.txt b/rktl_msgs/CMakeLists.txt deleted file mode 100644 index 9f8365dd0..000000000 --- a/rktl_msgs/CMakeLists.txt +++ /dev/null @@ -1,43 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(rktl_msgs) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -find_package(std_msgs REQUIRED) -find_package(builtin_interfaces REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(rosidl_default_generators REQUIRED) - -rosidl_generate_interfaces(${PROJECT_NAME} - "msg/BezierPath.msg" - "msg/BezierPathList.msg" - "msg/ControlCommand.msg" - "msg/ControlEffort.msg" - "msg/MatchStatus.msg" - "msg/Path.msg" - "msg/Score.msg" - "msg/Waypoint.msg" - DEPENDENCIES geometry_msgs std_msgs builtin_interfaces # Add packages that above messages depend on, in this case geometry_msgs for Sphere.msg -) - -ament_package() diff --git a/rktl_msgs/README.md b/rktl_msgs/README.md deleted file mode 100644 index 2739a3f2d..000000000 --- a/rktl_msgs/README.md +++ /dev/null @@ -1,3 +0,0 @@ -# rktl_msgs - -This package contains various message files (.msg) that are used by many packages. diff --git a/rktl_msgs/msg/BezierPath.msg b/rktl_msgs/msg/BezierPath.msg deleted file mode 100644 index e04caa63f..000000000 --- a/rktl_msgs/msg/BezierPath.msg +++ /dev/null @@ -1,3 +0,0 @@ -int32 order -geometry_msgs/Point[] control_points -builtin_interfaces/Duration duration diff --git a/rktl_msgs/msg/BezierPathList.msg b/rktl_msgs/msg/BezierPathList.msg deleted file mode 100644 index b8d064965..000000000 --- a/rktl_msgs/msg/BezierPathList.msg +++ /dev/null @@ -1,3 +0,0 @@ -std_msgs/Header header -string child_frame_id -rktl_msgs/BezierPath[] paths \ No newline at end of file diff --git a/rktl_msgs/msg/ControlCommand.msg b/rktl_msgs/msg/ControlCommand.msg deleted file mode 100644 index e88049149..000000000 --- a/rktl_msgs/msg/ControlCommand.msg +++ /dev/null @@ -1,3 +0,0 @@ -std_msgs/Header header -float64 velocity -float64 curvature diff --git a/rktl_msgs/msg/ControlEffort.msg b/rktl_msgs/msg/ControlEffort.msg deleted file mode 100644 index 6cf958fd5..000000000 --- a/rktl_msgs/msg/ControlEffort.msg +++ /dev/null @@ -1,3 +0,0 @@ -std_msgs/Header header -float64 throttle -float64 steering diff --git a/rktl_msgs/msg/MatchStatus.msg b/rktl_msgs/msg/MatchStatus.msg deleted file mode 100644 index 4023e2c79..000000000 --- a/rktl_msgs/msg/MatchStatus.msg +++ /dev/null @@ -1,15 +0,0 @@ -std_msgs/Header header - -int8 PAUSED=0 # before the match is started -int8 ONGOING=1 # match is currently going on -int8 STOPPED=2 # match ends with no winning team -int8 VICTORY_TEAM_A=3 # match ends with team A winning -int8 VICTORY_TEAM_B=4 # match ends with team B winning - -# status of the match using above constants -int8 status - -# time left to play -int16 clock - -Score score \ No newline at end of file diff --git a/rktl_msgs/msg/Path.msg b/rktl_msgs/msg/Path.msg deleted file mode 100644 index 3ff593296..000000000 --- a/rktl_msgs/msg/Path.msg +++ /dev/null @@ -1,4 +0,0 @@ -std_msgs/Header header -string child_frame_id -float64 velocity -rktl_msgs/Waypoint[] waypoints diff --git a/rktl_msgs/msg/Score.msg b/rktl_msgs/msg/Score.msg deleted file mode 100644 index c523214a7..000000000 --- a/rktl_msgs/msg/Score.msg +++ /dev/null @@ -1,2 +0,0 @@ -int32 blue -int32 orange \ No newline at end of file diff --git a/rktl_msgs/msg/Waypoint.msg b/rktl_msgs/msg/Waypoint.msg deleted file mode 100644 index cced45368..000000000 --- a/rktl_msgs/msg/Waypoint.msg +++ /dev/null @@ -1,4 +0,0 @@ -std_msgs/Header header -std_msgs/Float32 delta_t -geometry_msgs/Pose pose -geometry_msgs/Twist twist \ No newline at end of file diff --git a/rktl_msgs/package.xml b/rktl_msgs/package.xml deleted file mode 100644 index 55761724d..000000000 --- a/rktl_msgs/package.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - - rktl_msgs - 0.0.0 - Custom messages for ARC Rocket League Project - Autonomous Robotics Club of Purdue - BSD 3 Clause - https://www.purduearc.com - Arianna McNamara - James Baxter - - ament_cmake - - ament_lint_auto - ament_lint_common - - - ament_cmake - - - geometry_msgs - std_msgs - builtin_interfaces - rosidl_default_generators - rosidl_default_runtime - rosidl_interface_packages - diff --git a/rktl_perception/CMakeLists.txt b/rktl_perception/CMakeLists.txt deleted file mode 100644 index cf9c2504c..000000000 --- a/rktl_perception/CMakeLists.txt +++ /dev/null @@ -1,147 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(rktl_perception) - -## Enable C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) - -# find dependencies added from previous -find_package(pointgrey_camera_driver REQUIRED) -find_package(image_proc REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(image_transport REQUIRED) -find_package(cv_bridge REQUIRED) -find_package(apriltag_ros REQUIRED) -find_package(nodelet REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(image_geometry REQUIRED) -find_package(rosidl_default_generators REQUIRED) - -# System dependencies -find_package(OpenCV REQUIRED COMPONENTS core highgui) -find_package(Eigen3 REQUIRED) - -# Add include directories -include_directories( - include - ${OpenCV_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIR} - ) - -# Build localizer node -add_executable(${PROJECT_NAME}_localizer_node - src/localizer_node.cpp - src/localizer.cpp - ) -set_target_properties(${PROJECT_NAME}_localizer_node PROPERTIES - OUTPUT_NAME localizer - PREFIX "" - ) -add_dependencies(${PROJECT_NAME}_localizer_node - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ) -target_link_libraries(${PROJECT_NAME}_localizer_node - Eigen3::Eigen - ) -ament_target_dependencies(${PROJECT_NAME}_localizer_node - rclcpp - std_msgs - ) - -# Build ball_detection node -add_executable(${PROJECT_NAME}_ball_detection_node - src/ball_detection.cpp - src/ball_detection_node.cpp - ) -set_target_properties(${PROJECT_NAME}_ball_detection_node PROPERTIES - OUTPUT_NAME ball_detection - PREFIX "" - ) -add_dependencies(${PROJECT_NAME}_ball_detection_node - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${OpenCV_EXPORTED_TARGETS} - ) -target_link_libraries(${PROJECT_NAME}_ball_detection_node - ${OpenCV_LIBRARIES} - ) -ament_target_dependencies(${PROJECT_NAME}_ball_detection_node - rclcpp - std_msgs - ) - -# Build ball_color node -add_executable(${PROJECT_NAME}_ball_color_node - src/ball_color.cpp - ) -set_target_properties(${PROJECT_NAME}_ball_color_node PROPERTIES - OUTPUT_NAME ball_color - PREFIX "" - ) -add_dependencies(${PROJECT_NAME}_ball_color_node - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${OpenCV_EXPORTED_TARGETS} - ) - target_link_libraries(${PROJECT_NAME}_ball_color_node - ${OpenCV_LIBRARIES} - ) - -ament_target_dependencies(${PROJECT_NAME}_ball_color_node - rclcpp - std_msgs - ) - -# vis node -add_executable(${PROJECT_NAME}_focus_vis_node - src/focus_vis.cpp - src/focus_vis_node.cpp - ) -set_target_properties(${PROJECT_NAME}_focus_vis_node PROPERTIES - OUTPUT_NAME focus_vis - PREFIX "" - ) -add_dependencies(${PROJECT_NAME}_focus_vis_node - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${OpenCV_EXPORTED_TARGETS} - ) -target_link_libraries(${PROJECT_NAME}_focus_vis_node - ${OpenCV_LIBRARIES} - ) -ament_target_dependencies(${PROJECT_NAME}_focus_vis_node - rclcpp - std_msgs - ) - - if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() - endif() - -ament_export_dependencies(rclcpp - pointgrey_camera_driver - image_proc - tf2 - tf2_ros - image_transport - cv_bridge - apriltag_ros - OpenCV - ) - -ament_export_include_directories(include) -ament_package() \ No newline at end of file diff --git a/rktl_perception/config/apriltag_settings.yaml b/rktl_perception/config/apriltag_settings.yaml deleted file mode 100644 index d971e57ec..000000000 --- a/rktl_perception/config/apriltag_settings.yaml +++ /dev/null @@ -1,10 +0,0 @@ -tag_family: 'tag25h9' -tag_border: 1 -tag_threads: 4 -tag_decimate: 1.0 -tag_blur: 0.0 -tag_refine_edges: 1 -tag_refine_decode: 0 -tag_refine_pose: 0 -tag_debug: 0 -publish_tf: false \ No newline at end of file diff --git a/rktl_perception/config/ball_settings.yaml b/rktl_perception/config/ball_settings.yaml deleted file mode 100644 index ef3cb0d9b..000000000 --- a/rktl_perception/config/ball_settings.yaml +++ /dev/null @@ -1,10 +0,0 @@ -publishThresh: true -min_hue: 50 -max_hue: 100 -min_sat: 70 -max_sat: 180 -min_vib: 40 -max_vib: 100 -min_size: 250 -erode: 2 -dilate: 8 \ No newline at end of file diff --git a/rktl_perception/config/cam0/calibration.yaml b/rktl_perception/config/cam0/calibration.yaml deleted file mode 100644 index 5b0dbcc46..000000000 --- a/rktl_perception/config/cam0/calibration.yaml +++ /dev/null @@ -1,20 +0,0 @@ -image_width: 1288 -image_height: 964 -camera_name: 15356310 -camera_matrix: - rows: 3 - cols: 3 - data: [610.4617133035794, 0, 648.8400790764666, 0, 612.8636563372359, 509.10981446149, 0, 0, 1] -distortion_model: plumb_bob -distortion_coefficients: - rows: 1 - cols: 5 - data: [-0.2432499228354474, 0.04650574966262223, -0.001403592205737197, -0.001846787629070493, 0] -rectification_matrix: - rows: 3 - cols: 3 - data: [1, 0, 0, 0, 1, 0, 0, 0, 1] -projection_matrix: - rows: 3 - cols: 4 - data: [437.3668518066406, 0, 642.4606144639438, 0, 0, 497.3519287109375, 519.6716765197198, 0, 0, 0, 1, 0] \ No newline at end of file diff --git a/rktl_perception/config/cam0/pointgrey.yaml b/rktl_perception/config/cam0/pointgrey.yaml deleted file mode 100644 index 8e515d209..000000000 --- a/rktl_perception/config/cam0/pointgrey.yaml +++ /dev/null @@ -1,6 +0,0 @@ -serial: 15356310 -frame_rate: 30 -auto_shutter: false -shutter_speed: 0.001 -auto_gain: false -gain: 25.0 diff --git a/rktl_perception/config/cam1/calibration.yaml b/rktl_perception/config/cam1/calibration.yaml deleted file mode 100644 index bb30a9a9a..000000000 --- a/rktl_perception/config/cam1/calibration.yaml +++ /dev/null @@ -1,20 +0,0 @@ -image_width: 1288 -image_height: 964 -camera_name: 15374749 -camera_matrix: - rows: 3 - cols: 3 - data: [614.57136917199, 0, 655.3078549706212, 0, 615.1667182505952, 527.5952225219263, 0, 0, 1] -distortion_model: plumb_bob -distortion_coefficients: - rows: 1 - cols: 5 - data: [-0.2139026302415871, 0.03446647780631319, 0.0003785624850231047, 0.001450676024114855, 0] -rectification_matrix: - rows: 3 - cols: 3 - data: [1, 0, 0, 0, 1, 0, 0, 0, 1] -projection_matrix: - rows: 3 - cols: 4 - data: [429.591796875, 0, 663.7421479902696, 0, 0, 510.9947814941406, 548.7979183761527, 0, 0, 0, 1, 0] \ No newline at end of file diff --git a/rktl_perception/config/cam1/pointgrey.yaml b/rktl_perception/config/cam1/pointgrey.yaml deleted file mode 100644 index 7b13e1a1d..000000000 --- a/rktl_perception/config/cam1/pointgrey.yaml +++ /dev/null @@ -1,6 +0,0 @@ -serial: 15374749 -frame_rate: 30 -auto_shutter: false -shutter_speed: 0.001 -auto_gain: false -gain: 25.0 diff --git a/rktl_perception/config/cam2/calibration.yaml b/rktl_perception/config/cam2/calibration.yaml deleted file mode 100644 index 561353f25..000000000 --- a/rktl_perception/config/cam2/calibration.yaml +++ /dev/null @@ -1,20 +0,0 @@ -image_width: 1288 -image_height: 964 -camera_name: 15374754 -camera_matrix: - rows: 3 - cols: 3 - data: [1126.08574591452, 0, 616.7375520125502, 0, 1116.255414571801, 525.7495338331287, 0, 0, 1] -distortion_model: plumb_bob -distortion_coefficients: - rows: 1 - cols: 5 - data: [-0.7942828618736387, 0.5381554120245144, 0.00207013009474492, 0.01022713459887638, 0] -rectification_matrix: - rows: 3 - cols: 3 - data: [1, 0, 0, 0, 1, 0, 0, 0, 1] -projection_matrix: - rows: 3 - cols: 4 - data: [827.6439208984375, 0, 634.490142294926, 0, 0, 908.0331420898438, 549.0852317258759, 0, 0, 0, 1, 0] \ No newline at end of file diff --git a/rktl_perception/config/cam2/pointgrey.yaml b/rktl_perception/config/cam2/pointgrey.yaml deleted file mode 100644 index f923d79ef..000000000 --- a/rktl_perception/config/cam2/pointgrey.yaml +++ /dev/null @@ -1,6 +0,0 @@ -serial: 15374754 -frame_rate: 30 -auto_shutter: false -shutter_speed: 0.001 -auto_gain: false -gain: 25.0 diff --git a/rktl_perception/config/cam3/calibration.yaml b/rktl_perception/config/cam3/calibration.yaml deleted file mode 100644 index 6bcbd4561..000000000 --- a/rktl_perception/config/cam3/calibration.yaml +++ /dev/null @@ -1,20 +0,0 @@ -image_width: 1288 -image_height: 964 -camera_name: 15374755 -camera_matrix: - rows: 3 - cols: 3 - data: [691.8297171319369, 0, 616.8642266971589, 0, 697.0148612025321, 517.5329039891342, 0, 0, 1] -distortion_model: plumb_bob -distortion_coefficients: - rows: 1 - cols: 5 - data: [-0.2678012034146836, 0.05194194294770087, 0.002831174538127116, 0.001537062305525107, 0] -rectification_matrix: - rows: 3 - cols: 3 - data: [1, 0, 0, 0, 1, 0, 0, 0, 1] -projection_matrix: - rows: 3 - cols: 4 - data: [478.2955932617188, 0, 611.6567255038535, 0, 0, 581.3926391601562, 538.9608447667997, 0, 0, 0, 1, 0] \ No newline at end of file diff --git a/rktl_perception/config/cam3/pointgrey.yaml b/rktl_perception/config/cam3/pointgrey.yaml deleted file mode 100644 index ad098a9ac..000000000 --- a/rktl_perception/config/cam3/pointgrey.yaml +++ /dev/null @@ -1,6 +0,0 @@ -serial: 15374755 -frame_rate: 30 -auto_shutter: false -shutter_speed: 0.001 -auto_gain: false -gain: 25.0 diff --git a/rktl_perception/config/tags.yaml b/rktl_perception/config/tags.yaml deleted file mode 100644 index 65097ca38..000000000 --- a/rktl_perception/config/tags.yaml +++ /dev/null @@ -1,24 +0,0 @@ -standalone_tags: - [ - { id: 10, size: 0.07 }, - { id: 11, size: 0.07 }, - { id: 12, size: 0.07 }, - { id: 13, size: 0.07 }, - ] -tag_bundles: - [ - { - name: 'field_origin', - layout: - [ - {id: 0, size: 0.2, x: -1.8081, y: 1.1742, z: -0.0308, qw: 0.9997, qx: 0.0099, qy: 0.0046, qz: 0.0197}, - {id: 1, size: 0.2, x: 0.0701, y: 1.3045, z: -0.0291, qw: 0.9998, qx: -0.0116, qy: -0.0007, qz: 0.0161}, - {id: 2, size: 0.2, x: 1.7377, y: 1.2590, z: -0.0244, qw: 0.9998, qx: 0.0085, qy: -0.0038, qz: 0.0152}, - {id: 3, size: 0.2, x: -1.1869, y: 0.0000, z: 0.0000, qw: 1.0000, qx: 0.0000, qy: 0.0000, qz: 0.0000}, - {id: 4, size: 0.2, x: 1.1869, y: 0.0138, z: -0.0036, qw: 0.9998, qx: 0.0047, qy: -0.0161, qz: 0.0089}, - {id: 5, size: 0.2, x: -1.7244, y: -1.2670, z: 0.0111, qw: 0.9998, qx: 0.0008, qy: -0.0024, qz: 0.0208}, - {id: 6, size: 0.2, x: 0.1233, y: -1.2090, z: 0.0012, qw: 0.9998, qx: 0.0023, qy: 0.0177, qz: 0.0000}, - {id: 7, size: 0.2, x: 1.8482, y: -1.0850, z: 0.0030, qw: 0.9999, qx: -0.0131, qy: -0.0009, qz: -0.0015} - ] - } - ] \ No newline at end of file diff --git a/rktl_perception/include/rktl_perception/ball_detection.h b/rktl_perception/include/rktl_perception/ball_detection.h deleted file mode 100644 index 03d42f4e9..000000000 --- a/rktl_perception/include/rktl_perception/ball_detection.h +++ /dev/null @@ -1,61 +0,0 @@ -/* Node to detect the ball's position on the field. - * License: - * BSD 3-Clause License - * Copyright (c) 2023, Autonomous Robotics Club of Purdue (Purdue ARC) - * All rights reserved. - */ - -#ifndef __RKTL_PERCEPTION_BALL_DETECTION_H__ -#define __RKTL_PERCEPTION_BALL_DETECTION_H__ - -/* ros */ -#include - - -/* image */ -#include -#include -#include - -/* opencv */ -#include -#include -#include -#include - -/* messages */ -#include -#include -#include - -/* math & vectors */ -#include -#include -#include - -namespace rktl_perception { - -class BallDetection { -public: - BallDetection(const std::shared_ptr& node); - ~BallDetection() = default; - -private: - void ballCallback(const sensor_msgs::ImageConstPtr& msg, - const sensor_msgs::CameraInfoConstPtr& info); - - std::shared_ptr _node; - image_transport::ImageTransport _it; - image_transport::CameraSubscriber _cameraSub; - image_transport::Publisher _imgPub; - rclcpp::Publisher _vecPub; - rclcpp::Subscribtion _detectionSub; - int _minHue, _minSat, _minVib, _maxHue, _maxSat, _maxVib, _minSize, _erodeAmnt, _dilateAmnt; - double _originX, _originY; - bool _publishThresh; - image_geometry::PinholeCameraModel _camera; -}; - -} // namespace rktl_perception - -#endif // __RKTL_PERCEPTION_BALL_DETECTION_H__ diff --git a/rktl_perception/include/rktl_perception/focus_vis.h b/rktl_perception/include/rktl_perception/focus_vis.h deleted file mode 100644 index 956c7f4dc..000000000 --- a/rktl_perception/include/rktl_perception/focus_vis.h +++ /dev/null @@ -1,57 +0,0 @@ -/* Node to help focus - * License: - * BSD 3-Clause License - * Copyright (c) 2023, Autonomous Robotics Club of Purdue (Purdue ARC) - * All rights reserved. - */ - -#ifndef __RKTL_PERCEPTION_FOCUS_VIS_H__ -#define __RKTL_PERCEPTION_FOCUS_VIS_H__ - -/* ros */ -#include - -/* image */ -#include -#include -#include - -/* opencv */ -#include -#include -#include -#include - -/* messages */ -#include -#include - -/* math & vectors */ -#include -#include - -/* memory */ -# include - -namespace rktl_perception { - -class FocusVis { -public: - FocusVis(const std::shared_ptr node); - ~FocusVis() = default; - -private: - void focusCallback(const sensor_msgs::ImageConstPtr& msg, - const sensor_msgs::CameraInfoConstPtr& info); - - std::shared_ptr _node; - image_transport::ImageTransport _it; - image_transport::CameraSubscriber _cameraSub; - image_transport::Publisher _imgPub; - rclcpp::Subscribtion _detectionSub; - image_geometry::PinholeCameraModel _camera; -}; - -} // namespace rktl_perception - -#endif // __CAMERA_TRACKING_FOCUS_VIS_H__ diff --git a/rktl_perception/include/rktl_perception/localizer.h b/rktl_perception/include/rktl_perception/localizer.h deleted file mode 100644 index 08949ec65..000000000 --- a/rktl_perception/include/rktl_perception/localizer.h +++ /dev/null @@ -1,49 +0,0 @@ -/* TODO - * License: - * BSD 3-Clause License - * Copyright (c) 2023, Autonomous Robotics Club of Purdue (Purdue ARC) - * All rights reserved. - */ - -#ifndef __RKTL_PERCEPTION_LOCALIZER_H__ -#define __RKTL_PERCEPTION_LOCALIZER_H__ - -#include -#include -#include -#include - -namespace rktl_perception { - -class Localizer { -public: - Localizer(const std::shared_ptr& node); - ~Localizer() = default; - - static std::string idsToString(std::vector ids); - -private: - void apriltagCallback(apriltag_ros::AprilTagDetectionArrayConstPtr msg); - void ballCallback(geometry_msgs::Vector3StampedConstPtr msg); - Eigen::Matrix4d combineMatrices(const Eigen::Matrix3d& rot, const Eigen::Vector3d& pos); - geometry_msgs::PoseWithCovarianceStamped toMsg(const Eigen::Matrix4d& transform, - rclcpp::Time stamp, - const std::string& frameId = "map"); - - std::shared_ptr _node; - rclcpp::Subscribtion _sub; - std::string _originId; - rclcpp::Publisher _pub; - std::map _pubs; - rclcpp::Subscribtion _ballSub; - rclcpp::Publisher _ballPub; - int _bufferSize; - int _bufferPos; - double _ballRadius; - std::vector> _buffer; - Eigen::Matrix4d _transform; -}; - -} // namespace rktl_perception - -#endif // __RKTL_PERCEPTION_LOCALIZER_H__ diff --git a/rktl_perception/launch/cal.launch.py b/rktl_perception/launch/cal.launch.py deleted file mode 100644 index 903cad8a1..000000000 --- a/rktl_perception/launch/cal.launch.py +++ /dev/null @@ -1,35 +0,0 @@ -import os -import sys - -import launch -import launch_ros.actions - - -def generate_launch_description(): - ld = launch.LaunchDescription([ - launch_ros.actions.Node( - package='camera_calibration', - executable='cameracalibrator.py', - name='calib' - ), - launch_ros.actions.Node( - package='camera_calibration', - executable='cameracalibrator.py', - name='calib' - ), - launch_ros.actions.Node( - package='camera_calibration', - executable='cameracalibrator.py', - name='calib' - ), - launch_ros.actions.Node( - package='camera_calibration', - executable='cameracalibrator.py', - name='calib' - ) - ]) - return ld - - -if __name__ == '__main__': - generate_launch_description() diff --git a/rktl_perception/launch/cal.py b/rktl_perception/launch/cal.py deleted file mode 100644 index cedee0b5c..000000000 --- a/rktl_perception/launch/cal.py +++ /dev/null @@ -1,35 +0,0 @@ -import launch -from launch import LaunchDescription -from launch_ros.actions import Node - -def generate_launch_description(): - return LaunchDescription([ - Node( - package='camera_calibration', - executable='cameracalibrator.py', - name='calib', - namespace='cams/cam0', - arguments=['--size', '8x6', '--square', '0.026', 'image:=/cams/cam0/image_raw', 'camera:=/cams/cam0'] - ), - Node( - package='camera_calibration', - executable='cameracalibrator.py', - name='calib', - namespace='cams/cam1', - arguments=['--size', '8x6', '--square', '0.026', 'image:=/cams/cam1/image_raw', 'camera:=/cams/cam1'] - ), - Node( - package='camera_calibration', - executable='cameracalibrator.py', - name='calib', - namespace='cams/cam2', - arguments=['--size', '8x6', '--square', '0.026', 'image:=/cams/cam2/image_raw', 'camera:=/cams/cam2'] - ), - Node( - package='camera_calibration', - executable='cameracalibrator.py', - name='calib', - namespace='cams/cam3', - arguments=['--size', '8x6', '--square', '0.026', 'image:=/cams/cam3/image_raw', 'camera:=/cams/cam3'] - ) - ]) \ No newline at end of file diff --git a/rktl_perception/launch/cal_launch.py b/rktl_perception/launch/cal_launch.py deleted file mode 100644 index a75f28297..000000000 --- a/rktl_perception/launch/cal_launch.py +++ /dev/null @@ -1,43 +0,0 @@ -import os -import sys - -import launch -import launch_ros.actions - - -def generate_launch_description(): - ld = launch.LaunchDescription([ - launch_ros.actions.Node( - package='camera_calibration', - executable='cameracalibrator.py', - name='calib', - namespace = 'cams/cam0', - arguments="--size 8x6 --square 0.026 image:=/cams/cam3/image_raw camera:=/cams/cam0" - ), - launch_ros.actions.Node( - package='camera_calibration', - executable='cameracalibrator.py', - name='calib', - namespace = 'cams/cam1', - arguments="--size 8x6 --square 0.026 image:=/cams/cam3/image_raw camera:=/cams/cam1" - ), - launch_ros.actions.Node( - package='camera_calibration', - executable='cameracalibrator.py', - name='calib', - namespace = 'cams/cam2', - arguments="--size 8x6 --square 0.026 image:=/cams/cam3/image_raw camera:=/cams/cam2" - ), - launch_ros.actions.Node( - package='camera_calibration', - executable='cameracalibrator.py', - name='calib', - namespace = 'cams/cam3', - arguments="--size 8x6 --square 0.026 image:=/cams/cam3/image_raw camera:=/cams/cam3" - ) - ]) - return ld - - -if __name__ == '__main__': - generate_launch_description() \ No newline at end of file diff --git a/rktl_perception/launch/camera.launch.py b/rktl_perception/launch/camera.launch.py deleted file mode 100644 index ae0d363d9..000000000 --- a/rktl_perception/launch/camera.launch.py +++ /dev/null @@ -1,31 +0,0 @@ -import os -import sys - -import launch -import launch_ros.actions - - -def generate_launch_description(): - ld = launch.LaunchDescription([ - launch.actions.DeclareLaunchArgument( - name='load_manager', - default_value='true' - ), - launch.actions.DeclareLaunchArgument( - name='manager_name', - default_value='camera_manager' - ), - launch.actions.DeclareLaunchArgument( - name='manager_threads', - default_value='4' - ), - launch.actions.DeclareLaunchArgument( - name='camera_name', - default_value='cam0' - ) - ]) - return ld - - -if __name__ == '__main__': - generate_launch_description() diff --git a/rktl_perception/launch/camera.py b/rktl_perception/launch/camera.py deleted file mode 100644 index 7a23ea403..000000000 --- a/rktl_perception/launch/camera.py +++ /dev/null @@ -1,78 +0,0 @@ -# original code at bottom - -import launch -from launch import LaunchDescription -from launch_ros.actions import Node -from launch.substitutions import LaunchConfiguration - -def generate_launch_description(): - load_manager = LaunchConfiguration('load_manager', default='true') - manager_name = LaunchConfiguration('manager_name', default='camera_manager') - manager_threads = LaunchConfiguration('manager_threads', default='4') - camera_name = LaunchConfiguration('camera_name', default='cam0') - - return LaunchDescription([ - launch.actions.DeclareLaunchArgument('load_manager', default_value='true', description='Load the nodelet manager'), - launch.actions.DeclareLaunchArgument('manager_name', default_value='camera_manager', description='Name for the nodelet manager'), - launch.actions.DeclareLaunchArgument('manager_threads', default_value='4', description='Number of worker threads for the nodelet manager'), - launch.actions.DeclareLaunchArgument('camera_name', default_value='cam0', description='Name for the camera'), - - launch.actions.Group( - namespace='cams/' + camera_name, - actions=[ - Node( - condition=launch.conditions.IfCondition(load_manager), - package='nodelet', - executable='nodelet', - name=manager_name, - arguments=['manager'], - parameters=[{'num_worker_threads': manager_threads}] - ), - Node( - package='nodelet', - executable='nodelet', - name=camera_name, - arguments=['load', 'pointgrey_camera_driver/PointGreyCameraNodelet', manager_name], - parameters=[{'frame_id': camera_name}, - {'camera_info_url': 'package://rktl_perception/config/' + camera_name + '/calibration.yaml'}], - remappings=[('/camera/image_raw', '/image_raw'), ('/camera/camera_info', '/camera_info')] - ), - launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.XmlLaunchFileSource( - '/path/to/image_proc_launch_file' - ), - launch_arguments={'manager': manager_name}.items() - ), - Node( - package='apriltag_ros', - executable='apriltag_ros_continuous_node', - name='apriltag', - parameters=[{'camera_frame': camera_name, 'publish_tag_detections_image': True}], - remappings=[('/tag_detections_image', '/tag_detections_image')], - ), - Node( - package='rktl_perception', - executable='localizer', - name='localizer', - parameters=[{'origin_id': '0,1,2,3,4,5,6,7', - 'buffer_size': 300, - 'ball_sub_topic': 'ball_vec', - 'ball_pub_topic': '/ball/pose', - 'ball_radius': 0.03, - 'pub_topic': 'pose'}], - ), - Node( - package='rktl_perception', - executable='pose_to_tf', - name='pose_to_tf', - parameters=[{'cam_frame_id': camera_name}], - ), - Node( - package='rktl_perception', - executable='ball_detection', - name='ball_detection', - ) - ] - ) - ]) - diff --git a/rktl_perception/launch/camera_launch.py b/rktl_perception/launch/camera_launch.py deleted file mode 100644 index cf0831fba..000000000 --- a/rktl_perception/launch/camera_launch.py +++ /dev/null @@ -1,92 +0,0 @@ -from ament_index_python import get_package_share_directory -import launch -from launch_ros.actions import Node, SetParametersFromFile -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -import os - -def generate_launch_description(): - load_manager = DeclareLaunchArgument('load_manager', default_value='true') - manager_name = DeclareLaunchArgument('manager_name', default_value='camera_manager') - manager_threads = DeclareLaunchArgument('manager_threads', default_value='4') - camera_name = DeclareLaunchArgument('camera_name', default_value='cam0') - - ns_group = launch.actions.GroupAction( - actions=[ - Node( - package='nodelet', - executable='nodelet', - name=[launch.substitutions.LaunchConfiguration('manager_name')], - namespace=['cams/', launch.substitutions.LaunchConfiguration('camera_name')], - arguments='manager', - parameters=[{'num_worker_threads': launch.substitutions.LaunchConfiguration('manager_threads')}], - condition=launch.conditions.IfCondition([launch.substitutions.LaunchConfiguration('load_manager')]) - ), - Node( - package='nodelet', - executable='nodelet', - name=[launch.substitutions.LaunchConfiguration('camera_name')], - namespace=['cams/', launch.substitutions.LaunchConfiguration('camera_name')], - arguments=['load', 'pointgrey_camera_driver/PointGreyCameraNodelet', launch.substitutions.LaunchConfiguration('manager_name')], - parameters=[ - {'frame_id': launch.substitutions.LaunchConfiguration('camera_name')}, - {'camera_info_url': 'package://rktl_perception/config/' + launch.substitutions.LaunchConfiguration('camera_name') + '/calibration.yaml'} - ] - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(get_package_share_directory('image_proc'), 'launch', 'image_proc.launch')), - launch_arguments={'manager': launch.substitutions.LaunchConfiguration('manager_name')}.items(), - ), - Node( - package='apriltag_ros', - executable='apriltag_ros_continuous_node', - name='apriltag', - namespace=['cams/', launch.substitutions.LaunchConfiguration('camera_name')], - parameters=[ - {'camera_frame': launch.substitutions.LaunchConfiguration('camera_name')}, - {'publish_tag_detections_image': True}, - os.path.join(get_package_share_directory('rktl_perception'), 'config', 'apriltag_settings.yaml'), - os.path.join(get_package_share_directory('rktl_perception'), 'config', 'tags.yaml') - ] - ), - SetParametersFromFile(filename=os.path.join(get_package_share_directory('rktl_perception'), 'config', launch.substitutions.LaunchConfiguration('camera_name'), 'pointgrey.yaml')), - - Node( - package='rktl_perception', - executable='localizer', - name='localizer', - namespace=['cams/', launch.substitutions.LaunchConfiguration('camera_name')], - parameters=[ - {'origin_id': '0,1,2,3,4,5,6,7'}, - {'buffer_size': 300}, - {'ball_sub_topic': 'ball_vec'}, - {'ball_pub_topic': '/ball/pose'}, - {'ball_radius': 0.03}, - {'pub_topic': 'pose'}, - {'pub_topics': { - '0,1,2,3,4,5,6,7': '/origin/pose', - '10': '/cars/car0/pose' - }} - ] - ), - Node( - package='rktl_perception', - executable='pose_to_tf', - name='pose_to_tf', - namespace=['cams/', launch.substitutions.LaunchConfiguration('camera_name')], - parameters=[ - {'cam_frame_id': launch.substitutions.LaunchConfiguration('camera_name')} - ] - ), - Node( - package='rktl_perception', - executable='ball_detection', - name='ball_detection', - namespace=['cams/', launch.substitutions.LaunchConfiguration('camera_name')], - parameters = os.path.join(get_package_share_directory('rktl_perception'), 'config', launch.substitutions.LaunchConfiguration('camera_name'), 'ball_settings.yaml') - ) - ], - namespace='cams/' + launch.substitutions.LaunchConfiguration('camera_name') - ) - - return launch.LaunchDescription([load_manager, manager_name, manager_threads, camera_name, ns_group]) \ No newline at end of file diff --git a/rktl_perception/launch/color_picker.launch.py b/rktl_perception/launch/color_picker.launch.py deleted file mode 100644 index b3c029db4..000000000 --- a/rktl_perception/launch/color_picker.launch.py +++ /dev/null @@ -1,21 +0,0 @@ -import os -import sys - -import launch -import launch_ros.actions - - -def generate_launch_description(): - ld = launch.LaunchDescription([ - launch_ros.actions.Node( - package='rktl_perception', - executable='ball_color', - name='ball_color', - output='screen' - ) - ]) - return ld - - -if __name__ == '__main__': - generate_launch_description() diff --git a/rktl_perception/launch/color_picker.py b/rktl_perception/launch/color_picker.py deleted file mode 100644 index ec7a6031d..000000000 --- a/rktl_perception/launch/color_picker.py +++ /dev/null @@ -1,42 +0,0 @@ -#original -# -# -# -# - -import launch -from launch import LaunchDescription -from launch_ros.actions import Node - -def generate_launch_description(): - return LaunchDescription([ - Node( - package='rktl_perception', - executable='ball_color', - name='ball_color', - namespace='cams/cam0', - output='screen' - ), - Node( - package='rktl_perception', - executable='ball_color', - name='ball_color', - namespace='cams/cam1', - output='screen' - ), - Node( - package='rktl_perception', - executable='ball_color', - name='ball_color', - namespace='cams/cam2', - output='screen' - ), - Node( - package='rktl_perception', - executable='ball_color', - name='ball_color', - namespace='cams/cam3', - output='screen' - ), - - ]) \ No newline at end of file diff --git a/rktl_perception/launch/color_picker_launch.py b/rktl_perception/launch/color_picker_launch.py deleted file mode 100644 index 3021866b7..000000000 --- a/rktl_perception/launch/color_picker_launch.py +++ /dev/null @@ -1,42 +0,0 @@ -import os -import sys - -import launch -import launch_ros.actions - -def generate_launch_description(): - ld = launch.LaunchDescription([ - launch_ros.actions.Node( - package='rktl_perception', - executable='ball_color', - name='ball_color', - namespace = 'cams/cam0', - output = 'screen' - ) - #, - # launch_ros.actions.Node( - # package='rktl_perception', - # executable='ball_color', - # name='ball_color', - # namespace = 'cams/cam1', - # output = 'screen' - # ), - # launch_ros.actions.Node( - # package='rktl_perception', - # executable='ball_color', - # name='ball_color', - # namespace = 'cams/cam2', - # output = 'screen' - # ), - # launch_ros.actions.Node( - # package='rktl_perception', - # executable='ball_color', - # name='ball_color', - # namespace = 'cams/cam3', - # output = 'screen' - # ), - ]) - return ld - -if __name__ == '__main__': - generate_launch_description() \ No newline at end of file diff --git a/rktl_perception/launch/field_view.launch.py b/rktl_perception/launch/field_view.launch.py deleted file mode 100644 index ee17f3c9a..000000000 --- a/rktl_perception/launch/field_view.launch.py +++ /dev/null @@ -1,60 +0,0 @@ -import os -import sys - -import launch -import launch_ros.actions - - -def generate_launch_description(): - ld = launch.LaunchDescription([ - launch_ros.actions.Node( - package='rktl_perception', - executable='projector', - name='projector', - parameters=[ - { - 'ground_height': '0.00' - } - ] - ), - launch_ros.actions.Node( - package='rktl_perception', - executable='projector', - name='projector', - parameters=[ - { - 'ground_height': '-0.02' - } - ] - ), - launch_ros.actions.Node( - package='rktl_perception', - executable='projector', - name='projector', - parameters=[ - { - 'ground_height': '-0.01' - } - ] - ), - launch_ros.actions.Node( - package='rktl_perception', - executable='projector', - name='projector', - parameters=[ - { - 'ground_height': '-0.03' - } - ] - ), - launch_ros.actions.Node( - package='rviz', - executable='rviz', - name='rviz' - ) - ]) - return ld - - -if __name__ == '__main__': - generate_launch_description() diff --git a/rktl_perception/launch/field_view.py b/rktl_perception/launch/field_view.py deleted file mode 100644 index 90eecc230..000000000 --- a/rktl_perception/launch/field_view.py +++ /dev/null @@ -1,48 +0,0 @@ -import launch -from launch import LaunchDescription -from launch_ros.actions import Node -from launch.substitutions import LaunchConfiguration -from ament_index_python.packages import get_package_share_directory - -def generate_launch_description(): - rktl_perception_share_dir = launch.substitutions.LaunchConfiguration('rktl_perception_share_dir', - default=get_package_share_directory('rktl_perception')) - - return LaunchDescription([ - Node( - namespace='cams/cam0', - package='rktl_perception', - executable='projector', - name='projector', - parameters=[{'ground_height': 0.00}] - ), - Node( - namespace='cams/cam1', - package='rktl_perception', - executable='projector', - name='projector', - parameters=[{'ground_height': -0.02}] - ), - Node( - namespace='cams/cam2', - package='rktl_perception', - executable='projector', - name='projector', - parameters=[{'ground_height': -0.01}] - ), - Node( - namespace='cams/cam3', - package='rktl_perception', - executable='projector', - name='projector', - parameters=[{'ground_height': -0.03}] - ), - Node( - package='rviz2', - executable='rviz2', - name='rviz', - output='screen', - arguments=['-d', rktl_perception_share_dir + '/rviz/field.rviz'] - ) - ]) - diff --git a/rktl_perception/launch/field_view_launch.py b/rktl_perception/launch/field_view_launch.py deleted file mode 100644 index 5f7aad83b..000000000 --- a/rktl_perception/launch/field_view_launch.py +++ /dev/null @@ -1,45 +0,0 @@ -import os -import sys - -import launch -from launch_ros.actions import Node - -def generate_launch_description(): - nodes = [ - Node( - package="rktl_perception", - executable="projector", - namespace="cams/cam0", - name="projector", - parameters=[{"ground_height": 0.00}] - ), - Node( - package="rktl_perception", - executable="projector", - namespace="cams/cam1", - name="projector", - parameters=[{"ground_height": -0.02}] - ), - Node( - package="rktl_perception", - executable="projector", - namespace="cams/cam2", - name="projector", - parameters=[{"ground_height": -0.01}] - ), - Node( - package="rktl_perception", - executable="projector", - namespace="cams/cam3", - name="projector", - parameters=[{"ground_height": -0.03}] - ), - Node( - package="rviz", - executable="rviz", - name="rviz", - arguments= "-d " + os.path.join(get_package_share_directory('rktl_perception'), 'rviz', 'field.rviz') - ) - ] - - return launch.LaunchDescription(nodes) \ No newline at end of file diff --git a/rktl_perception/launch/focus_assist.launch.py b/rktl_perception/launch/focus_assist.launch.py deleted file mode 100644 index ad8ec6ca4..000000000 --- a/rktl_perception/launch/focus_assist.launch.py +++ /dev/null @@ -1,35 +0,0 @@ -import os -import sys - -import launch -import launch_ros.actions - - -def generate_launch_description(): - ld = launch.LaunchDescription([ - launch_ros.actions.Node( - package='rktl_perception', - executable='focus_vis', - name='focus_vis' - ), - launch_ros.actions.Node( - package='rktl_perception', - executable='focus_vis', - name='focus_vis' - ), - launch_ros.actions.Node( - package='rktl_perception', - executable='focus_vis', - name='focus_vis' - ), - launch_ros.actions.Node( - package='rktl_perception', - executable='focus_vis', - name='focus_vis' - ) - ]) - return ld - - -if __name__ == '__main__': - generate_launch_description() diff --git a/rktl_perception/launch/focus_assist.py b/rktl_perception/launch/focus_assist.py deleted file mode 100644 index 86bb7854d..000000000 --- a/rktl_perception/launch/focus_assist.py +++ /dev/null @@ -1,17 +0,0 @@ -import launch -from launch import LaunchDescription -from launch_ros.actions import Node - -def generate_launch_description(): - nodes = [] - for i in range(4): - node = Node( - package='rktl_perception', - executable='focus_vis', - namespace=f'cams/cam{i}', - name='focus_vis' - ) - nodes.append(node) - - return LaunchDescription(nodes) - diff --git a/rktl_perception/launch/focus_assist_launch.py b/rktl_perception/launch/focus_assist_launch.py deleted file mode 100644 index 8c9c7f804..000000000 --- a/rktl_perception/launch/focus_assist_launch.py +++ /dev/null @@ -1,35 +0,0 @@ -import os -import sys - -import launch -from launch_ros.actions import Node - -def generate_launch_description(): - nodes = [ - Node( - package="rktl_perception", - executable="focus_vis", - namespace="cams/cam0", - name="focus_vis" - ), - Node( - package="rktl_perception", - executable="focus_vis", - namespace="cams/cam1", - name="focus_vis" - ), - Node( - package="rktl_perception", - executable="focus_vis", - namespace="cams/cam2", - name="focus_vis" - ), - Node( - package="rktl_perception", - executable="focus_vis", - namespace="cams/cam3", - name="focus_vis" - ) - ] - - return launch.LaunchDescription(nodes) \ No newline at end of file diff --git a/rktl_perception/nodes/pose_to_tf b/rktl_perception/nodes/pose_to_tf deleted file mode 100755 index 9990b2385..000000000 --- a/rktl_perception/nodes/pose_to_tf +++ /dev/null @@ -1,52 +0,0 @@ -#!/usr/bin/env python3 -"""publish info from a PoseWithCovarianceStamped to the TF tree -License: - BSD 3-Clause License - Copyright (c) 2023, Autonomous Robotics Club of Purdue (Purdue ARC) - All rights reserved. -""" - -# ROS -import rclpy -from geometry_msgs.msg import PoseWithCovarianceStamped, TransformStamped -from tf2_ros import TransformBroadcaster - - -class Publisher(rclpy.Node): - """Workaround class until TF is implemented""" - - def __init__(self): - #rospy.init_node('pose_to_tf') - rclpy.init_node('pose_to_tf') - - self.broadcaster = TransformBroadcaster() - #self.FRAME = rospy.get_param('~cam_frame_id', 'cam0') - self.FRAME = self.get_parameter_or('~cam_frame_id', 'cam0').get_parameter_value().string_value - - #rospy.Subscriber("pose", PoseWithCovarianceStamped, self.pose_cb) - self.subscription = self.create_subscription(PoseWithCovarianceStamped, "pose", self.pose_cb) - #rospy.spin() -> Moved into main function - - def pose_cb(self, pose_msg): - """Publish a pose corresponding to the TF.""" - tf = TransformStamped() - tf.header = pose_msg.header - tf.child_frame_id = self.FRAME - tf.transform.translation.x = pose_msg.pose.pose.position.x - tf.transform.translation.y = pose_msg.pose.pose.position.y - tf.transform.translation.z = pose_msg.pose.pose.position.z - tf.transform.rotation.x = pose_msg.pose.pose.orientation.x - tf.transform.rotation.y = pose_msg.pose.pose.orientation.y - tf.transform.rotation.z = pose_msg.pose.pose.orientation.z - tf.transform.rotation.w = pose_msg.pose.pose.orientation.w - self.broadcaster.sendTransform(tf) - -def main(args=None): - rclpy.init(arg = args) - publisher = Publisher() - rclpy.spin(publisher) - publisher.destroy_node() - rclpy.shutdown() - -if __name__ == "__main__": - main() diff --git a/rktl_perception/nodes/projector b/rktl_perception/nodes/projector deleted file mode 100755 index 1a290a4d6..000000000 --- a/rktl_perception/nodes/projector +++ /dev/null @@ -1,155 +0,0 @@ -#!/usr/bin/env python3 -"""Create a depth map to project camera data onto ground plane -License: - BSD 3-Clause License - Copyright (c) 2023, Autonomous Robotics Club of Purdue (Purdue ARC) - All rights reserved. -""" - -# ROS -import rclpy -from sensor_msgs.msg import Image, CameraInfo -from tf2_ros import Buffer, TransformListener -from tf.transformations import translation_matrix, quaternion_matrix - -import numpy as np - - -class Projector(rclpy.Node): - """Class to synchronize and buffer all poses.""" - - def __init__(self): - #self.init_node('projector') - super().init_node('projector') - #self.depth_pub = rospy.Publisher('depth_rect', Image, queue_size=1) - self.depth_pub = self.create_publisher(Image, 'depth_rect', queue_size=1) - #rospy.Subscriber('camera_info', CameraInfo, self.camera_info_cb) - self.subscription = self.create_subscription(CameraInfo, 'camera_info',self.camera_info_cb) - - # constants - #self.GROUND_HEIGHT = rospy.get_param('~ground_height', 0.0) - self.GROUND_HEIGHT = self.get_parameter_or('~ground_height', 0.0).get_parameter_value().float_value - - # used to access TF tree - buffer = Buffer() - TransformListener(buffer) - - # variables that are needed from camera_info - self.initialized = False - self.frame_id = None - self.width = None - self.height = None - self.cam_to_img = None - - # variables that are needed from main loop - self.img_msg = None - - # rate at which to re-compute depth map - #rate = rospy.Rate(1.0/rospy.get_param('~update_period', 1)) - #rate = rclpy.Rate(1.0/rclpy.get_parameter_or('~update_period', 1).get_parameter_value().integer_value) - self._loop_rate = self.create_rate(1.0/self.get_parameter_or('~update_period', 1).get_parameter_value().integer_value, self.get_clock()) - - # compute depth map in main loop and cache since it is slow changing - # publish latest in callback so that the stamps match (for rviz) - while not rclpy.ok(): - # check if camera_info callback has been run - if not self.initialized: - continue - - # get camera location via TF tree - #try: - # map_to_cam_msg = buffer.lookup_transform( - # self.frame_id, 'map', rospy.Time()) - #except: - try: - map_to_cam_msg = buffer.lookup_transform( - self.frame_id, 'map', self.get_clock().now().to_msg()) - except: - continue - - # pull translation and quaternion from pose msg - translation = np.array([ - map_to_cam_msg.transform.translation.x, - map_to_cam_msg.transform.translation.y, - map_to_cam_msg.transform.translation.z]) - quaternion = np.array([ - map_to_cam_msg.transform.rotation.x, - map_to_cam_msg.transform.rotation.y, - map_to_cam_msg.transform.rotation.z, - map_to_cam_msg.transform.rotation.w]) - # create transformation matrix - map_to_cam = translation_matrix( - translation) @ quaternion_matrix(quaternion) - - # ground plane in map space (X direction, Y direction, origin point) - # any [a,b,1].T input results in a point on the ground plane - ground_plane = np.array( - [[1, 0, 0, 0], - [0, 1, 0, 0], - [0, 0, self.GROUND_HEIGHT, 1]]).T - - # depth * [u,v,1].T = cam_to_img x map_to_cam x ground_plane x [a,b,1].T - # [u,v,1].T = cam_to_img x map_to_cam x ground_plane x [A,B,1/depth].T - - # create matrix of all pixels - w = self.width - h = self.height - u = np.arange(stop=w).reshape((1, w, 1, 1)) - v = np.arange(stop=h).reshape((h, 1, 1, 1)) - - u = np.broadcast_to(u, (h, w, 1, 1)) - v = np.broadcast_to(v, (h, w, 1, 1)) - one = np.broadcast_to(1, (h, w, 1, 1)) - - img = np.concatenate((u, v, one), axis=2) - - # reshape matrix - mat = (self.cam_to_img @ map_to_cam @ - ground_plane).reshape((1, 1, 3, 3)) - mat = np.broadcast_to(mat, (h, w, 3, 3)) - - # solve for all pixels at once - depth = np.linalg.solve(mat, img) - # extract depth - depth = 1.0/(depth[:, :, 2, 0].reshape((h, w))) - - # create message - if self.img_msg is None: - self.img_msg = Image() - self.img_msg.header.frame_id = self.frame_id - self.img_msg.height = h - self.img_msg.width = w - self.img_msg.encoding = '32FC1' - self.img_msg.step = 4*w - self.img_msg.data = depth.astype('float32').tobytes() - - # delay since making the depth map is expensive - try: - self._loop_rate.sleep() - #rate.sleep() - except: - pass - - def camera_info_cb(self, info_msg): - """Callback for publishing depth map""" - # extract relevant data if first time - if not self.initialized: - self.frame_id = info_msg.header.frame_id - self.width = info_msg.width - self.height = info_msg.height - self.cam_to_img = np.array(info_msg.P).reshape((3, 4)) - self.initialized = True - - # publish latest depth map - if self.img_msg is not None: - self.img_msg.header.stamp = info_msg.header.stamp - self.depth_pub.publish(self.img_msg) - -def main(args=None): - rclpy.init(args=args) - projector = Projector() - rclpy.spin(projector) - rclpy.shutdown() - -if __name__ == "__main__": - main() diff --git a/rktl_perception/package.xml b/rktl_perception/package.xml deleted file mode 100644 index fb4c76f8f..000000000 --- a/rktl_perception/package.xml +++ /dev/null @@ -1,41 +0,0 @@ - - - - rktl_perception - 0.0.0 - TODO: Package description - DinoSage - TODO: License declaration - - ament_cmake - - rclcpp - - pointgrey_camera_driver - image_proc - apriltag_ros - tf2 - tf2_ros - image_transport - cv_bridge - nodelet - - OpenCV - Eigen3 - geometry_msgs - image_geometry - - - camera_calibration - rviz - - - - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/rktl_perception/resource/rktl_perception b/rktl_perception/resource/rktl_perception deleted file mode 100644 index e69de29bb..000000000 diff --git a/rktl_perception/rktl_perception/__init__.py b/rktl_perception/rktl_perception/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/rktl_perception/rviz/field.rviz b/rktl_perception/rviz/field.rviz deleted file mode 100644 index 5099adfd2..000000000 --- a/rktl_perception/rviz/field.rviz +++ /dev/null @@ -1,402 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 128 - Name: Displays - Property Tree Widget: - Expanded: - - /DepthCloud1/Auto Size1 - - /DepthCloud2/Auto Size1 - - /DepthCloud3/Auto Size1 - - /DepthCloud4/Auto Size1 - - /PoseWithCovariance1 - Splitter Ratio: 0.5 - Tree Height: 675 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: DepthCloud -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Alpha: 1 - Class: rviz/Axes - Enabled: true - Length: 0.4000000059604645 - Name: Axes - Radius: 0.03999999910593033 - Reference Frame: - Show Trail: false - Value: true - - Class: rviz/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: true - cam0: - Value: true - cam2: - Value: true - map: - Value: true - Marker Alpha: 1 - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - map: - cam0: - {} - cam2: - {} - Update Interval: 0 - Value: true - - Alpha: 1 - Auto Size: - Auto Size Factor: 1 - Value: false - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 0.45774024724960327 - Min Value: 0.0676114559173584 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/DepthCloud - Color: 255; 255; 255 - Color Image Topic: /cams/cam0/image_rect - Color Transformer: RGB8 - Color Transport Hint: raw - Decay Time: 0 - Depth Map Topic: /cams/cam0/depth_rect - Depth Map Transport Hint: raw - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: DepthCloud - Occlusion Compensation: - Occlusion Time-Out: 30 - Value: false - Position Transformer: XYZ - Queue Size: 3 - Selectable: false - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Points - Topic Filter: true - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Auto Size: - Auto Size Factor: 1 - Value: false - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/DepthCloud - Color: 255; 255; 255 - Color Image Topic: /cams/cam1/image_rect - Color Transformer: RGB8 - Color Transport Hint: raw - Decay Time: 0 - Depth Map Topic: /cams/cam1/depth_rect - Depth Map Transport Hint: raw - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: DepthCloud - Occlusion Compensation: - Occlusion Time-Out: 30 - Value: false - Position Transformer: XYZ - Queue Size: 3 - Selectable: false - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Points - Topic Filter: true - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Auto Size: - Auto Size Factor: 1 - Value: false - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/DepthCloud - Color: 255; 255; 255 - Color Image Topic: /cams/cam2/image_rect - Color Transformer: RGB8 - Color Transport Hint: raw - Decay Time: 0 - Depth Map Topic: /cams/cam2/depth_rect - Depth Map Transport Hint: raw - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: DepthCloud - Occlusion Compensation: - Occlusion Time-Out: 30 - Value: false - Position Transformer: XYZ - Queue Size: 3 - Selectable: false - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Points - Topic Filter: true - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Auto Size: - Auto Size Factor: 1 - Value: false - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/DepthCloud - Color: 255; 255; 255 - Color Image Topic: /cams/cam3/image_rect - Color Transformer: RGB8 - Color Transport Hint: raw - Decay Time: 0 - Depth Map Topic: /cams/cam3/depth_rect - Depth Map Transport Hint: raw - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: DepthCloud - Occlusion Compensation: - Occlusion Time-Out: 30 - Value: false - Position Transformer: XYZ - Queue Size: 3 - Selectable: false - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Points - Topic Filter: true - Use Fixed Frame: true - Use rainbow: true - Value: true - - Angle Tolerance: 0.10000000149011612 - Class: rviz/Odometry - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: true - Keep: 10 - Name: Odometry - Position Tolerance: 0.10000000149011612 - Queue Size: 10 - Shape: - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Color: 255; 255; 255 - Head Length: 0.07500000298023224 - Head Radius: 0.07500000298023224 - Shaft Length: 0.15000000596046448 - Shaft Radius: 0.02500000037252903 - Value: Arrow - Topic: /cars/car0/odom - Unreliable: false - Value: true - - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz/PoseWithCovariance - Color: 255; 0; 0 - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: true - Head Length: 0.07500000298023224 - Head Radius: 0.07500000298023224 - Name: PoseWithCovariance - Queue Size: 10 - Shaft Length: 0.15000000596046448 - Shaft Radius: 0.02500000037252903 - Shape: Arrow - Topic: /cars/car0/pose_sync - Unreliable: false - Value: true - - Alpha: 1 - Axes Length: 0.02500000037252903 - Axes Radius: 0.15000000596046448 - Class: rviz/PoseWithCovariance - Color: 0; 170; 0 - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: true - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Name: PoseWithCovariance - Queue Size: 10 - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Shape: Axes - Topic: /ball/pose_sync - Unreliable: false - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: map - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 5.56512975692749 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Field of View: 0.7853981852531433 - Focal Point: - X: 0.11027099937200546 - Y: -0.3318149447441101 - Z: 0.03906717151403427 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.7747973203659058 - Target Frame: - Yaw: 3.850470542907715 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1016 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001560000035efc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000035e000000c700fffffffb0000000c00430061006d006500720061000000026a000001360000000000000000fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650200000598000000da000001c40000013a000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000005dc0000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1848 - X: 72 - Y: 27 diff --git a/rktl_perception/scripts/calibrate_bundle.m b/rktl_perception/scripts/calibrate_bundle.m deleted file mode 100644 index e09ccf9ba..000000000 --- a/rktl_perception/scripts/calibrate_bundle.m +++ /dev/null @@ -1,318 +0,0 @@ -% Copyright (c) 2023, California Institute of Technology. -% All rights reserved. -% -%%% calibrate_bundle.m %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -% -% Script to determine AprilTag bundle relative poses to a "master" tag. -% -% Instructions: -% Record a bagfile of the /tag_detections topic where you steadily -% point the camera at the AprilTag bundle such that all the bundle's -% individual tags are visible at least once at some point (the more the -% better). Run the script, then copy the printed output into the tag.yaml -% configuration file of apriltag_ros. -% -% $Revision: 1.0 $ -% $Date: 2017/12/17 13:37:34 $ -% $Author: dmalyuta $ -% -% Originator: Danylo Malyuta, JPL -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -% -% Ammended Instructions: -% Record a bag file as instructed above. Then, save the data to a csv -% by running "rostopic echo -b calibration.bag -p tag_detections > calibration.csv" -% -% WARNING: -% Make sure that apriltags was configured to detect lone tags, not bundles. -% Otherwise, the software will improperly parse the csv and produce incorrect results. -% -% Edited by: James Baxter -% Date: 2022/04/12 -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -%% User inputs - -% Relative directory of calibration bagfile -calibration_file = '../../../../data/bags/calibration.csv'; - -% Bundle name -bundle_name = 'field_origin'; - -% Master tag's ID -master_id = 3; - -%% Load the tag detections bagfile - -bag = readcell(calibration_file); - -clear tag_data; -N = size(bag,1) - 1; -max_detections = ceil((size(bag,2) - 5) / 48); -t0 = bag{2,3}; -for i = (1:N) - row = i+1; - tag_data.t(i) = bag{row,3} - t0; - - % get the number of detections - n_detections = 0; - for j = 1:max_detections - col = 5 + 48*(j-1); - if ismissing(bag{row,col}) - break - else - n_detections = n_detections + 1; - end - end - - % extract the relevant data - for j = 1:n_detections - col = 5 + 48*(j-1); - tag_data.detection(i).id(j) = bag{row,col}; - tag_data.detection(i).size(j) = bag{row,col+1}; - % Tag position with respect to camera frame - % [x;y;z] format - tag_data.detection(i).p(:,j) = cell2mat(bag(row,col+5:col+7)); - % Tag orientation with respect to camera frame - % [w;x;y;z] format - tag_data.detection(i).q(:,j) = cell2mat(bag(row,[col+11, col+8:col+10])); - end -end - -%% Compute the measured poses of each tag relative to the master tag - -master_size = []; % Size of the master tag - -% IDs, sizes, relative positions and orientations of detected tags other -% than master -other_ids = []; -other_sizes = []; -rel_p = {}; -rel_q = {}; - -createT = @(p,q) [quat2rotmat(q) p; zeros(1,3) 1]; -invertT = @(T) [T(1:3,1:3)' -T(1:3,1:3)'*T(1:3,4); zeros(1,3) 1]; - -N = numel(tag_data.detection); -for i = 1:N - this = tag_data.detection(i); - - mi = find(this.id == master_id); - if isempty(mi) - % Master not detected in this detection, so this particular - % detection is useless - continue; - end - - % Get the master tag's rigid body transform to the camera frame - T_cm = createT(this.p(:,mi), this.q(:,mi)); - - % Get the rigid body transform of every other tag to the camera frame - for j = 1:numel(this.id) - % Skip the master, but get its size first - if isempty(master_size) - master_size = this.size(j); - end - % We already have the rigid body transform from the master tag to - % the camera frame (T_cm) - if j == mi - continue; - end - - % Add ID to detected IDs, if not already there - id = this.id(j); - if ~ismember(id, other_ids) - other_ids(end+1) = id; - other_sizes(end+1) = this.size(j); - rel_p{end+1} = []; - rel_q{end+1} = []; - end - - % Find the index in other_ids corresponding to this tag - k = find(other_ids == id); - assert(numel(k) == 1, ... - 'Tag ID must appear exactly once in the other_ids array'); - - % Get this tag's rigid body transform to the camera frame - T_cj = createT(this.p(:,j), this.q(:,j)); - - % Deduce this tag's rigid body transform to the master tag's frame - T_mj = invertT(T_cm)*T_cj; - - % Save the relative position and orientation of this tag to the - % master tag - rel_p{k}(:,end+1) = T_mj(1:3,4); - rel_q{k}(:,end+1) = rotmat2quat(T_mj); - end -end - -assert(~isempty(master_size), ... - sprintf('Master tag with ID %d not found in detections', master_id)); - -%% Compute (geometric) median position of each tag in master tag frame - -geometricMedianCost = @(x,y) sum(sqrt(sum((x-y).^2))); - -options = optimset('MaxIter',1000,'MaxFunEvals',1000, ... - 'Algorithm','interior-point', ... - 'TolFun', 1e-6, 'TolX', 1e-6); - -M = numel(rel_p); -rel_p_median = nan(3, numel(other_ids)); -for i = 1:M - % Compute the mean position as the initial value for the minimization - % problem - p_0 = mean(rel_p{i},2); - - % Compute the geometric median - [rel_p_median(:,i),~,exitflag] = ... - fminsearch(@(x) geometricMedianCost(rel_p{i}, x), p_0, options); - assert(exitflag == 1, ... - sprintf(['Geometric median minimization did ' ... - 'not converge (exitflag %d)'], exitflag)); -end - -%% Compute the average orientation of each tag with respect to the master tag - -rel_q_mean = nan(4, numel(other_ids)); -for i = 1:M - % Use the method in Landis et al. "Averaging Quaternions", JGCD 2007 - - % Check the sufficient uniqueness condition - % TODO this is a computational bottleness - without this check, script - % returns much faster. Any way to speed up this double-for-loop? - error_angle{i} = []; - for j = 1:size(rel_q{i},2) - q_1 = rel_q{i}(:,j); - for k = 1:size(rel_q{i},2) - if j==k - continue; - end - q_2 = rel_q{i}(:,k); - q_error = quatmult(quatinv(q_1),q_2); - % Saturate to valid acos range, which prevents imaginary output - % from acos due to q_error_w being infinitesimaly (to numerical - % precision) outside of valid [-1,1] range - q_error_w = min(1,max(q_error(1),-1)); - error_angle{i}(end+1) = 2*acos(q_error_w); - if 2*acos(q_error_w) >= pi/2 - warning(['Quaternion pair q_%u and q_%u for tag ID %u ' ... - 'are more than 90 degrees apart!'], ... - j,k,other_ids(i)); - end - end - end - - % Average quaternion method - Q = rel_q{i}; - [V, D] = eig(Q*Q.'); - [~,imax] = max(diag(D)); % Get the largest eigenvalue - rel_q_mean(:,i) = V(:,imax); % Corresponding eigenvector - if rel_q_mean(1,i) < 0 - rel_q_mean(:,i) = -rel_q_mean(:,i); % Ensure w positive - end -end - -%% Print output to paste in tags.yaml - -% Head + master tag -fprintf([ ... -'tag_bundles:\n' ... -' [\n' ... -' {\n' ... -' name: ''%s'',\n' ... -' layout:\n' ... -' [\n'], bundle_name); - -% All other tags detected at least once together with master tag -for i = 0:numel(other_ids) - newline = ','; - if i == numel(other_ids) - newline = ''; - end - if i == 0 - fprintf(' {id: %d, size: %.2f, x: %.4f, y: %.4f, z: %.4f, qw: %.4f, qx: %.4f, qy: %.4f, qz: %.4f}%s\n', ... - master_id, master_size, 0, 0, 0, 1, 0, 0, 0, newline); - else - fprintf(' {id: %d, size: %.2f, x: %.4f, y: %.4f, z: %.4f, qw: %.4f, qx: %.4f, qy: %.4f, qz: %.4f}%s\n', ... - other_ids(i), other_sizes(i), rel_p_median(1,i), ... - rel_p_median(2,i), rel_p_median(3,i), rel_q_mean(1,i), ... - rel_q_mean(2,i), rel_q_mean(3,i), rel_q_mean(4,i), newline); - end -end - -% Tail -fprintf([ ... -' ]\n'... -' }\n'... -' ]\n']); - -%% Local functions - -function t = getBagTime(bagfile) - t = double(bagfile.header.stamp.sec)+ ... - double(bagfile.header.stamp.nsec)/1e9; -end - -function R = quat2rotmat(q) - % Creates an ACTIVE rotation matrix from a quaternion - w = q(1); - x = q(2); - y = q(3); - z = q(4); - - R = [1-2*(y^2+z^2) 2*(x*y-w*z) 2*(x*z+w*y) - 2*(x*y+w*z) 1-2*(x^2+z^2) 2*(y*z-w*x) - 2*(x*z-w*y) 2*(y*z+w*x) 1-2*(x^2+y^2)]; -end - -function q = rotmat2quat(R) - % Adapted for MATLAB from - % http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/ - tr = R(1,1) + R(2,2) + R(3,3); - - if tr > 0 - S = sqrt(tr+1.0) * 2; % S=4*qw - qw = 0.25 * S; - qx = (R(3,2) - R(2,3)) / S; - qy = (R(1,3) - R(3,1)) / S; - qz = (R(2,1) - R(1,2)) / S; - elseif (R(1,1) > R(2,2)) && (R(1,1) > R(3,3)) - S = sqrt(1.0 + R(1,1) - R(2,2) - R(3,3)) * 2; % S=4*qx - qw = (R(3,2) - R(2,3)) / S; - qx = 0.25 * S; - qy = (R(1,2) + R(2,1)) / S; - qz = (R(1,3) + R(3,1)) / S; - elseif (R(2,2) > R(3,3)) - S = sqrt(1.0 + R(2,2) - R(1,1) - R(3,3)) * 2; % S=4*qy - qw = (R(1,3) - R(3,1)) / S; - qx = (R(1,2) + R(2,1)) / S; - qy = 0.25 * S; - qz = (R(2,3) + R(3,2)) / S; - else - S = sqrt(1.0 + R(3,3) - R(1,1) - R(2,2)) * 2; % S=4*qz - qw = (R(2,1) - R(1,2)) / S; - qx = (R(1,3) + R(3,1)) / S; - qy = (R(2,3) + R(3,2)) / S; - qz = 0.25 * S; - end - - q = [qw qx qy qz]'; -end - -function c = quatmult(a,b) - % More humanly understandable version: - % Omegaa = [a((1)) -a((2):(4)).' - % a((2):(4)) a((1))*eye((3))-[0 -a((4)) a((3)); a((4)) 0 -a((2));-a((3)) a((2)) 0]]; - % c = Omegaa*b; - % More optimized version: - c_w = a(1)*b(1) - a(2)*b(2) - a(3)*b(3) - a(4)*b(4); - c_x = a(1)*b(2) + a(2)*b(1) - a(3)*b(4) + a(4)*b(3); - c_y = a(1)*b(3) + a(3)*b(1) + a(2)*b(4) - a(4)*b(2); - c_z = a(1)*b(4) - a(2)*b(3) + a(3)*b(2) + a(4)*b(1); - c = [c_w; c_x; c_y; c_z]; -end - -function qinv = quatinv(q) - qinv = [q(1); -q(2:4)]; -end diff --git a/rktl_perception/setup.cfg b/rktl_perception/setup.cfg deleted file mode 100644 index d2a7845d8..000000000 --- a/rktl_perception/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/rktl_perception -[install] -install_scripts=$base/lib/rktl_perception diff --git a/rktl_perception/setup.py b/rktl_perception/setup.py deleted file mode 100644 index 49ec35b50..000000000 --- a/rktl_perception/setup.py +++ /dev/null @@ -1,25 +0,0 @@ -from setuptools import find_packages, setup - -package_name = 'rktl_perception' - -setup( - name=package_name, - version='0.0.0', - packages=find_packages(exclude=['test']), - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - ], - install_requires=['setuptools'], - zip_safe=True, - maintainer='zakolano', - maintainer_email='zakolano@todo.todo', - description='TODO: Package description', - license='TODO: License declaration', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - ], - }, -) diff --git a/rktl_perception/src/ball_color.cpp b/rktl_perception/src/ball_color.cpp deleted file mode 100644 index 81e2720fa..000000000 --- a/rktl_perception/src/ball_color.cpp +++ /dev/null @@ -1,131 +0,0 @@ -/* - * License: - * BSD 3-Clause License - * Copyright (c) 2023, Autonomous Robotics Club of Purdue (Purdue ARC) - * All rights reserved. - */ - -/* ros */ -#include -#include - -/* image */ -#include -#include - -/* opencv */ -#include -#include -#include "opencv2/highgui.hpp" -#include -#include - -/* messages */ -#include - -#include -#include - -const int max_value_H = 360 / 2; -const int max_value = 255; -const std::string window_capture_name = "Video Capture"; -const std::string window_detection_name = "Object Detection"; -int low_H = 0, low_S = 0, low_V = 0, dilate_amnt = 0, erode_amnt = 0; -int high_H = max_value_H, high_S = max_value, high_V = max_value; - -static void on_low_H_thresh_trackbar(int, void*) { - low_H = std::min(high_H - 1, low_H); - cv::setTrackbarPos("Low H", window_detection_name, low_H); -} - -static void on_high_H_thresh_trackbar(int, void*) { - high_H = std::max(high_H, low_H + 1); - cv::setTrackbarPos("High H", window_detection_name, high_H); -} - -static void on_low_S_thresh_trackbar(int, void*) { - low_S = std::min(high_S - 1, low_S); - cv::setTrackbarPos("Low S", window_detection_name, low_S); -} - -static void on_high_S_thresh_trackbar(int, void*) { - high_S = std::max(high_S, low_S + 1); - cv::setTrackbarPos("High S", window_detection_name, high_S); -} - -static void on_low_V_thresh_trackbar(int, void*) { - low_V = std::min(high_V - 1, low_V); - cv::setTrackbarPos("Low V", window_detection_name, low_V); -} - -static void on_high_V_thresh_trackbar(int, void*) { - high_V = std::max(high_V, low_V + 1); - cv::setTrackbarPos("High V", window_detection_name, high_V); -} - -static void on_erode_thresh_trackbar(int, void*) { - erode_amnt = 0; - cv::setTrackbarPos("Erode", window_detection_name, erode_amnt); -} - -static void on_dilate_thresh_trackbar(int, void*) { - dilate_amnt = 0; - cv::setTrackbarPos("Dilate", window_detection_name, dilate_amnt); -} - -void imageCallback(const sensor_msgs::ImageConstPtr& msg) { - try { - /* Convert the ROS message into a cv_ptr & dereferencing the pointer to an OpenCV Mat */ - cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, "bgr8"); - cv::Mat current_frame = cv_ptr->image; - cv::Mat frame_HSV, frame_threshold; - // Convert from BGR to HSV colorspace - cv::cvtColor(current_frame, frame_HSV, cv::COLOR_BGR2HSV); - // Detect the object based on HSV Range Values - cv::inRange(frame_HSV, cv::Scalar(low_H, low_S, low_V), cv::Scalar(high_H, high_S, high_V), - frame_threshold); - cv::erode(frame_threshold, frame_threshold, cv::Mat(), cv::Point(-1, -1), erode_amnt, 1, 1); - cv::dilate(frame_threshold, frame_threshold, cv::Mat(), cv::Point(-1, -1), dilate_amnt, 1, - 1); - - // Show the frames - cv::imshow(window_detection_name, frame_threshold); - cv::waitKey(30); - } catch (cv_bridge::Exception& e) { - //ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str()); - RCLCPP_ERROR(node->get_logger(), "Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str()); - } -} - -int main(int argc, char* argv[]) { - //ros::init(argc, argv, "image_listener"); - //ros::NodeHandle nh; - rclcpp::init(argc, argv); - auto node = rclcpp:Node::make_shared("image_listener"); - - cv::namedWindow(window_detection_name); - - // Trackbars to set thresholds for HSV values - cv::createTrackbar("Low H", window_detection_name, &low_H, max_value_H, - on_low_H_thresh_trackbar); - cv::createTrackbar("High H", window_detection_name, &high_H, max_value_H, - on_high_H_thresh_trackbar); - cv::createTrackbar("Low S", window_detection_name, &low_S, max_value, on_low_S_thresh_trackbar); - cv::createTrackbar("High S", window_detection_name, &high_S, max_value, - on_high_S_thresh_trackbar); - cv::createTrackbar("Low V", window_detection_name, &low_V, max_value, on_low_V_thresh_trackbar); - cv::createTrackbar("High V", window_detection_name, &high_V, max_value, - on_high_V_thresh_trackbar); - cv::createTrackbar("Erode", window_detection_name, &erode_amnt, 25, on_high_V_thresh_trackbar); - cv::createTrackbar("Dilate", window_detection_name, &dilate_amnt, 25, - on_high_V_thresh_trackbar); - - cv::Mat current_frame, frame_HSV, frame_threshold; - image_transport::ImageTransport it(nh); - image_transport::Subscriber sub = it.subscribe("image_rect_color", 10, imageCallback); - - rclcpp::spin(); - cv::destroyWindow(window_detection_name); - - return 0; -} diff --git a/rktl_perception/src/ball_detection.cpp b/rktl_perception/src/ball_detection.cpp deleted file mode 100644 index 488688940..000000000 --- a/rktl_perception/src/ball_detection.cpp +++ /dev/null @@ -1,128 +0,0 @@ -/* Node to detect the ball's position on the field. - * License: - * BSD 3-Clause License - * Copyright (c) 2023, Autonomous Robotics Club of Purdue (Purdue ARC) - * All rights reserved. - */ - -#include "rktl_perception/ball_detection.h" - -namespace rktl_perception { - -int getMaxAreaContourId(std::vector> contours); - - -BallDetection::BallDetection(const std::shared_ptr& node) : _node(node), _it(node) { - _vecPub = _node->create_publisher("ball_vec", 10); - _imgPub = _it.advertise("threshold_img", 1); - _cameraSub = _it.subscribe_camera("image_rect_color", 10, std::bind(&BallDetection::ballCallback, this, std::placeholders::_1, std::placeholders::_2)); - - // future plan for this node is to make all of these adjustable with dynamic_reconfigure - _publishThresh = _node->declare_parameter("publishThresh", false); - _minHue = _node->declare_parameter("min_hue", 50); - _maxHue = _node->declare_parameter("max_hue", 100); - _minSat = _node->declare_parameter("min_sat", 75); - _maxSat = _node->declare_parameter("max_sat", 180); - _minVib = _node->declare_parameter("min_vib", 40); - _maxVib = _node->declare_parameter("max_vib", 100); - _minSize = _node->declare_parameter("min_size", 50); - _erodeAmnt = _node->declare_parameter("erode", 4); - _dilateAmnt = _node->declare_parameter("dilate", 5); -} - -void BallDetection::ballCallback(const sensor_msgs::ImageConstPtr& msg, - const sensor_msgs::CameraInfoConstPtr& info) { - try { - /* define published vector */ - geometry_msgs::Vector3Stamped vec; - - /* Convert the ROS message into a cv_ptr & dereferencing the pointer to an OpenCV Mat */ - cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, "bgr8"); - cv::Mat current_frame = cv_ptr->image; - cv::Mat frame_HSV, frame_threshold; - - /* Convert from BGR to HSV colorspace */ - cvtColor(current_frame, frame_HSV, cv::COLOR_BGR2HSV); - - /* get image size */ - int h = current_frame.size().height; - int w = current_frame.size().width; - - /* Detect the object based on HSV Range Values */ - inRange(frame_HSV, cv::Scalar(_minHue, _minSat, _minVib), - cv::Scalar(_maxHue, _maxSat, _maxVib), frame_threshold); - erode(frame_threshold, frame_threshold, cv::Mat(), cv::Point(-1, -1), _erodeAmnt, 1, 1); - dilate(frame_threshold, frame_threshold, cv::Mat(), cv::Point(-1, -1), _dilateAmnt, 1, 1); - - /* find all the contours */ - std::vector> contours; - std::vector hierarchy; - findContours(frame_threshold, contours, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_SIMPLE); - - /* only find the center if there are detections */ - if (contours.size() != 0) { - /* find largest contour */ - std::vector largestContour = contours.at(getMaxAreaContourId(contours)); - cv::Moments moment = cv::moments(largestContour); - double largestContourArea = cv::contourArea(largestContour); - /* do not include contours below a certain size */ - if (largestContourArea < _minSize) - return; - - /* calculates the center of the contour*/ - double centerX = moment.m10 / moment.m00; - double centerY = moment.m01 / moment.m00; - double centerdX = (w / 2) - centerX; - double centerdY = (h / 2) - centerY; - - /* create camera model and project the pixel to a vector */ - _camera.fromCameraInfo(info); - cv::Point3d cam = _camera.projectPixelTo3dRay(cv::Point2d(centerX, centerY)); - - /* create the vector to publish */ - vec.vector.x = cam.x; - vec.vector.y = cam.y; - vec.vector.z = largestContourArea; - - /* publish the location vector */ - vec.header = msg->header; - _vecPub.publish(vec); - - /* debug the size of the countour */ - //ROS_INFO("Size of the largest ball: %.0f\n", largestContourArea); - RCLCPP_INFO(node->get_logger(), "Size of the largest ball: %.0f\n", largestContourArea); - - /* publishes the threshold image */ - if (_publishThresh) { - sensor_msgs::Image threshImg; - std_msgs::Header header; - cv_bridge::CvImage img_bridge; - header.stamp = ros::Time::now(); - img_bridge = cv_bridge::CvImage(header, sensor_msgs::image_encodings::MONO8, - frame_threshold); - img_bridge.toImageMsg(threshImg); - _imgPub.publish(threshImg); - } - } - } catch (cv_bridge::Exception& e) { - - //ROS_ERROR("There was some error, likely with image conversion"); - RCLCPP_ERROR(node->get_logger(), "There was some error, likely with image conversion"); - } -} - -int getMaxAreaContourId(std::vector> contours) { - /* calculates the area of each contour and then returns the largest one */ - double maxArea = 0; - int maxAreaContourId = -1; - for (int j = 0; j < contours.size(); j++) { - double newArea = cv::contourArea(contours.at(j)); - if (newArea > maxArea) { - maxArea = newArea; - maxAreaContourId = j; - } - } - return maxAreaContourId; -} - -} // namespace rktl_perception diff --git a/rktl_perception/src/ball_detection_node.cpp b/rktl_perception/src/ball_detection_node.cpp deleted file mode 100644 index 5ea110c71..000000000 --- a/rktl_perception/src/ball_detection_node.cpp +++ /dev/null @@ -1,20 +0,0 @@ -/* Node to detect the ball's position on the field. - * License: - * BSD 3-Clause License - * Copyright (c) 2023, Autonomous Robotics Club of Purdue (Purdue ARC) - * All rights reserved. - */ - -#include "rclcpp/rclcpp.hpp" -#include "rktl_perception/ball_detection.h" - -int main(int argc, char* argv[]) { - rclcpp::init(argc, argv); - auto node = std::make_shared("ball_detection"); - - auto ballDetection = std::make_shared(node); - - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} \ No newline at end of file diff --git a/rktl_perception/src/focus_vis.cpp b/rktl_perception/src/focus_vis.cpp deleted file mode 100644 index d2d536296..000000000 --- a/rktl_perception/src/focus_vis.cpp +++ /dev/null @@ -1,48 +0,0 @@ -/* Node to help focus - * License: - * BSD 3-Clause License - * Copyright (c) 2023, Autonomous Robotics Club of Purdue (Purdue ARC) - * All rights reserved. - */ - -#include - -rclcpp::init(argc, argv); - -namespace rktl_perception { - -FocusVis::FocusVis(const std::shared_ptr& node) : _node(node), _it(_node) { - _imgPub = _it.advertise("edge_dectection", 1); - _cameraSub = _it.subscribeCamera("image_rect_color", 10, &FocusVis::focusCallback, this); -} - -void FocusVis::focusCallback(const sensor_msgs::ImageConstPtr& msg, - const sensor_msgs::CameraInfoConstPtr& info) { - try { - /* Convert the ROS message into a cv_ptr & dereferencing the pointer to an OpenCV Mat */ - cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, "bgr8"); - cv::Mat current_frame = cv_ptr->image; - cv::Mat frame_gray, frame_blurred, frame_edge; - - /* Convert from BGR to grey scale colorspace */ - cvtColor(current_frame, frame_gray, cv::COLOR_BGR2GRAY); - - cv::GaussianBlur(frame_gray, frame_blurred, cv::Size(5, 5), 1.5); - - cv::Canny(frame_blurred, frame_edge, 100, 200); - /* publishes the threshold image */ - - sensor_msgs::Image edgeImg; - std_msgs::Header header; - cv_bridge::CvImage img_bridge; - header.stamp = rclcpp::Time::now(); - img_bridge = cv_bridge::CvImage(header, sensor_msgs::image_encodings::MONO8, frame_edge); - img_bridge.toImageMsg(edgeImg); - _imgPub.publish(edgeImg); - - } catch (cv_bridge::Exception& e) { - ROS_ERROR("There was some error, likely with image conversion"); - } -} - -} // namespace rktl_perception diff --git a/rktl_perception/src/focus_vis_node.cpp b/rktl_perception/src/focus_vis_node.cpp deleted file mode 100644 index e1e2e7d82..000000000 --- a/rktl_perception/src/focus_vis_node.cpp +++ /dev/null @@ -1,17 +0,0 @@ -/* Node to help focus - * License: - * BSD 3-Clause License - * Copyright (c) 2023, Autonomous Robotics Club of Purdue (Purdue ARC) - * All rights reserved. - */ - -#include - -int main(int argc, char* argv[]) { - rclcpp::init(argc, argv); - auto node = rclcpp:Node::make_shared("focus_vis"); - rktl_perception::FocusVis focusVis(node); - - rclcpp::spin(); - return 0; -} diff --git a/rktl_perception/src/localizer.cpp b/rktl_perception/src/localizer.cpp deleted file mode 100644 index 388c12a47..000000000 --- a/rktl_perception/src/localizer.cpp +++ /dev/null @@ -1,156 +0,0 @@ -/* TODO - * License: - * BSD 3-Clause License - * Copyright (c) 2023, Autonomous Robotics Club of Purdue (Purdue ARC) - * All rights reserved. - */ - -#include - -namespace rktl_perception { - -Localizer::Localizer() : _node(node) { - std::string detectionTopic; - std::string pubTopic; - std::map pubTopics; - std::string ballSubTopic; - std::string ballPubTopic; - int queueSize; - - _node->param("dectection_topic", detectionTopic, "tag_detections"); - _node->param("origin_id", _originId, "0"); - _node->getParam("pub_topic", pubTopic); - _node->getParam("pub_topics", pubTopics); - _node->param("ball_sub_topic", ballSubTopic, "ball_vector"); - _node->param("ball_pub_topic", ballPubTopic, "ball_pos"); - _node->param("ball_radius", _ballRadius, 0.05); - _node->param("buffer_size", _bufferSize, 10); - _node->param("queue_size", queueSize, 100); - - _sub = _node->subscribe(detectionTopic, queueSize, &Localizer::apriltagCallback, this); - _pub = _node->advertise(pubTopic, queueSize); - _ballSub = _node->subscribe(ballSubTopic, queueSize, &Localizer::ballCallback, this); - _ballPub = _node->advertise(ballPubTopic, queueSize); - for (auto& kv : pubTopics) - _pubs[kv.first] = - _node->advertise(kv.second, queueSize); - - _bufferPos = 0; - _buffer.reserve(_bufferSize); -} - -std::string Localizer::idsToString(std::vector ids) { - if (ids.size() == 0) - return ""; - - std::sort(ids.begin(), ids.end()); - std::ostringstream oss; - oss << ids.front(); - for (auto it = ids.begin() + 1; it != ids.end(); ++it) - oss << ',' << *it; - return oss.str(); -} - -void Localizer::apriltagCallback(apriltag_ros::AprilTagDetectionArrayConstPtr msg) { - // Collect transforms and current camera transform - std::map transforms; - for (auto& detection : msg->detections) { - std::string idStr = idsToString(detection.id); - geometry_msgs::Quaternion rot = detection.pose.pose.pose.orientation; - geometry_msgs::Point pos = detection.pose.pose.pose.position; - Eigen::Quaterniond quat(rot.w, rot.x, rot.y, rot.z); - Eigen::Matrix3d mat = quat.toRotationMatrix(); - Eigen::Vector3d vec(pos.x, pos.y, pos.z); - - if (idStr == _originId) { - if (_buffer.size() == _bufferPos) - _buffer.emplace_back(); - _buffer[_bufferPos] = std::make_pair(mat, vec); - _bufferPos = (_bufferPos + 1) % _bufferSize; - } - if (_pubs.find(idStr) != _pubs.end()) { - transforms[idStr] = combineMatrices(mat, vec); - } - } - - if (_buffer.size() == 0) - return; - - // Naively average positions/rotations - Eigen::Matrix3d avgRot = Eigen::Matrix3d::Zero(); - Eigen::Vector3d avgPos = Eigen::Vector3d::Zero(); - for (auto& item : _buffer) { - avgRot += item.first; - avgPos += item.second; - } - avgRot /= _buffer.size(); - avgPos /= _buffer.size(); - avgPos *= -1; - - // Kabsch algorithm - Eigen::JacobiSVD svd(avgRot, Eigen::ComputeThinU | Eigen::ComputeThinV); - Eigen::Matrix3d ensureRHS = Eigen::Matrix3d::Identity(); - ensureRHS(2, 2) = ((svd.matrixV() * svd.matrixU().transpose()).determinant() > 0) ? 1.0 : -1.0; - Eigen::Matrix3d camRotate = svd.matrixV() * ensureRHS * svd.matrixU().transpose(); - _transform = combineMatrices(camRotate, Eigen::Vector3d::Zero()) * - combineMatrices(Eigen::Matrix3d::Identity(), avgPos); - _pub.publish(toMsg(_transform, msg->header.stamp)); - - // re-publish detections - for (auto& kv : transforms) { - auto& id = kv.first; - auto& tagTransform = kv.second; - _pubs[id].publish(toMsg(_transform * tagTransform, msg->header.stamp)); - } -} - -void Localizer::ballCallback(geometry_msgs::Vector3StampedConstPtr msg) { - Eigen::Vector4d camPos = _transform.col(3); - if (camPos.z() == 0) - return; - - // saves the confidence of the ball position's validity - double confidence = msg->vector.z; - - // set tge z to 1 to restore the vector's properties - Eigen::Vector4d camVec(msg->vector.x, msg->vector.y, 1, 0); - Eigen::Vector4d vec = _transform * camVec; - double t = (_ballRadius - camPos.z()) / vec.z(); - Eigen::Vector4d pos = camPos + t * vec; - - pos.z() = confidence; - /* set the z of the new pose to the confidence of the ball position's validity */ - - Eigen::Matrix4d pose = Eigen::Matrix4d::Identity(); - pose.col(3) << pos; - _ballPub.publish(toMsg(pose, msg->header.stamp)); -} - -Eigen::Matrix4d Localizer::combineMatrices(const Eigen::Matrix3d& rot, const Eigen::Vector3d& pos) { - Eigen::Matrix4d transform = Eigen::Matrix4d::Identity(); - transform.topLeftCorner<3, 3>() << rot; - transform.col(3).head<3>() << pos; - return transform; -} - -geometry_msgs::PoseWithCovarianceStamped Localizer::toMsg(const Eigen::Matrix4d& transform, - rclcpp::Time stamp, - const std::string& frameId) { - Eigen::Quaterniond rot(transform.topLeftCorner<3, 3>()); - Eigen::Vector3d pos(transform.col(3).head<3>()); - - geometry_msgs::PoseWithCovarianceStamped poseMsg; - poseMsg.header.stamp = stamp; - poseMsg.header.frame_id = frameId; - poseMsg.pose.pose.position.x = pos.x(); - poseMsg.pose.pose.position.y = pos.y(); - poseMsg.pose.pose.position.z = pos.z(); - poseMsg.pose.pose.orientation.w = rot.w(); - poseMsg.pose.pose.orientation.x = rot.x(); - poseMsg.pose.pose.orientation.y = rot.y(); - poseMsg.pose.pose.orientation.z = rot.z(); - - return poseMsg; -} - -} // namespace rktl_perception diff --git a/rktl_perception/src/localizer_node.cpp b/rktl_perception/src/localizer_node.cpp deleted file mode 100644 index d1d1a3c78..000000000 --- a/rktl_perception/src/localizer_node.cpp +++ /dev/null @@ -1,22 +0,0 @@ -/* TODO - * License: - * BSD 3-Clause License - * Copyright (c) 2023, Autonomous Robotics Club of Purdue (Purdue ARC) - * All rights reserved. - */ - -#include - -int main(int argc, char* argv[]) { - // ros::init(argc, argv, "localizer"); - // ros::NodeHandle nh; - // ros::NodeHandle pnh("~"); - - rclcpp::init(argc, argv); - auto node = rclcpp:Node::make_shared("localizer"); - - rktl_perception::Localizer localizer(node); - - ros::spin(); - return 0; -} diff --git a/rktl_perception/test/test_copyright.py b/rktl_perception/test/test_copyright.py deleted file mode 100644 index 97a39196e..000000000 --- a/rktl_perception/test/test_copyright.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_copyright.main import main -import pytest - - -# Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' diff --git a/rktl_perception/test/test_flake8.py b/rktl_perception/test/test_flake8.py deleted file mode 100644 index 27ee1078f..000000000 --- a/rktl_perception/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_flake8.main import main_with_errors -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ - '\n'.join(errors) diff --git a/rktl_perception/test/test_pep257.py b/rktl_perception/test/test_pep257.py deleted file mode 100644 index b234a3840..000000000 --- a/rktl_perception/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' diff --git a/rktl_planner/README.md b/rktl_planner/README.md deleted file mode 100644 index 31929cef8..000000000 --- a/rktl_planner/README.md +++ /dev/null @@ -1,11 +0,0 @@ -# rktl_planner - -This package provides classical algorithms to plan trajectories for the car, as -well as algorithms to control the car. Both a "patrolling" planner and a planner -based on generating bezier curves are included. In this package, path -generation is handled in two parts: planner nodes decide where a car should go, -and then call upon a [server](https://wiki.ros.org/Services) node to -decide how it will get there. Currently, only 2 nodes adhere to this schema -(path_planner and bezier_path_server, respectively), but the idea is that once -multiple of these nodes are created, they should be able to be switched out -seamlessly. diff --git a/rktl_planner/config/path_follower.yaml b/rktl_planner/config/path_follower.yaml deleted file mode 100644 index fb97b2333..000000000 --- a/rktl_planner/config/path_follower.yaml +++ /dev/null @@ -1,3 +0,0 @@ -lookahead_dist: 0.25 -lookahead_gain: 0.1 -lookahead_pnts: -1 diff --git a/rktl_planner/config/path_planner.yaml b/rktl_planner/config/path_planner.yaml deleted file mode 100644 index a07c92493..000000000 --- a/rktl_planner/config/path_planner.yaml +++ /dev/null @@ -1 +0,0 @@ -planner_type: 'complex' \ No newline at end of file diff --git a/rktl_planner/config/patrol_planner.yaml b/rktl_planner/config/patrol_planner.yaml deleted file mode 100644 index 10917f6df..000000000 --- a/rktl_planner/config/patrol_planner.yaml +++ /dev/null @@ -1,25 +0,0 @@ -# tuned params for no prediction at 0.15 delay -wall_avoidance: - reverse_time: 0.125 - distance_margin: 0.375 - heading_margin: 1.57 - -patrol_wall_dist: 0.75 - -curvature_gain: - kp: 2.0 - kd: 0.05 - falloff: 0.25 - -scoring: - car_lookahead_dist: 0.75 - goal_depth_target: 0.05 - -# X coordinate to switch to defense -defensive_line: -0.5 - -# Y coordinate to switch to reverse -reverse_line: 0.0 - -speed: 1.75 -attempt_timeout: 4.0 diff --git a/rktl_planner/launch/README.md b/rktl_planner/launch/README.md deleted file mode 100644 index e4cbc92c6..000000000 --- a/rktl_planner/launch/README.md +++ /dev/null @@ -1,61 +0,0 @@ -# Launch Files - -These are [.launch files](https://wiki.ros.org/roslaunch/XML) that can be run -using [roslaunch](https://wiki.ros.org/roslaunch): - -```shell -roslaunch rktl_planner -``` - -```{contents} Launch Files in the package -:depth: 2 -:backlinks: top -:local: true -``` - ---- - -## patrol_agent.launch - -This launches a [`patrol_planner`](../nodes/README.md#patrol-planner) node with -parameters supplied by `rktl_planner/config/patrol_planner.yaml`. - -### Launch Arguments - -- `car_name` (default: `car0`): Name of the car to launch the agent for. - -### Nodes - -- `cars/{car_name}/patrol_planner`: - [`patrol_planner`](../nodes/README.md#patrol-planner) node from the - [`rktl_planner`](../README.md) package. Parameters supplied by - `rktl_planner/config/patrol_planner.yaml`. - ---- - -## simple_agent.launch - -This launches nodes to generate and follow paths using bezier curve-based -methods. It loads parameters from `rktl_planner/config/path_follower.yaml` and -`rktl_planner/config/path_planner.yaml`. - -### Launch Arguments - -- `agent_name` (default: `agent0`): Name of the agent to run. -- `car_name` (default: `car0`): Name of the car to launch the agent for. - -### Nodes - -- `agents/{agent_name}/path_follower`: - [`path_follower`](../nodes/README.md#path-follower) node from the - [`rktl_planner`](../README.md) package. Parameters supplied by - `rktl_planner/config/path_follower.yaml`. -- `agents/{agent_name}/bezier_path_server`: - [`bezier_path_server`](../nodes/README.md#bezier-path-server) node from the - [`rktl_planner`](../README.md) package. -- `agents/{agent_name}/path_planner`: - [`path_planner`](../nodes/README.md#path-planner) node from the - [`rktl_planner`](../README.md) package. Parameters supplied by - `rktl_planner/config/path_planner.yaml`. - ---- diff --git a/rktl_planner/launch/patrol_agent.launch b/rktl_planner/launch/patrol_agent.launch deleted file mode 100644 index 14b8ece94..000000000 --- a/rktl_planner/launch/patrol_agent.launch +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - diff --git a/rktl_planner/launch/patrol_agent.launch.py b/rktl_planner/launch/patrol_agent.launch.py deleted file mode 100644 index 8bc2064ff..000000000 --- a/rktl_planner/launch/patrol_agent.launch.py +++ /dev/null @@ -1,30 +0,0 @@ -import os -import sys - -import launch -import launch_ros.actions -from ament_index_python.packages import get_package_share_directory - - -def generate_launch_description(): - ld = launch.LaunchDescription([ - launch.actions.DeclareLaunchArgument( - name='car_name', - default_value='car0' - ), - launch_ros.actions.Node( - package='rktl_planner', - namespace="cars/"+launch.substitutions.LaunchConfiguration("car_name"), - executable='patrol_planner', - name='patrol_planner', - output='screen', - parameters=[ - get_package_share_directory('rktl_planner') + '/config/patrol_planner.yaml' - ] - ) - ]) - return ld - - -if __name__ == '__main__': - generate_launch_description() diff --git a/rktl_planner/launch/simple_agent.launch b/rktl_planner/launch/simple_agent.launch deleted file mode 100644 index ea5cb361a..000000000 --- a/rktl_planner/launch/simple_agent.launch +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - - - - - - - - - - - - diff --git a/rktl_planner/launch/simple_agent.launch.py b/rktl_planner/launch/simple_agent.launch.py deleted file mode 100644 index 5c39924e2..000000000 --- a/rktl_planner/launch/simple_agent.launch.py +++ /dev/null @@ -1,60 +0,0 @@ -import os -import sys - -import launch -import launch_ros.actions - -def generate_launch_description(): - ld = launch.LaunchDescription([ - launch.actions.DeclareLaunchArgument( - name='agent_name', - default_value='agent0' - ), - launch.actions.DeclareLaunchArgument( - name='car_name', - default_value='car0' - ), - launch.actions.GroupAction( - actions=[ - launch_ros.actions.PushRosNamespace("agents/" + launch.substitutions.LaunchConfiguration("agent_name")), - launch_ros.actions.Node( - package="rktl_planner", - executable="path_follower", - name="path_follower", - output="screen", - parameters=[ - { - launch.substitutions.PathJoinSubstitution(launch_ros.substitutions.FindPackageShare('rktl_planner'), '/config/path_follower.yaml') - }, - { - "car_name": launch.substitutions.LaunchConfiguration("car_name") - } - ] - ), - launch_ros.actions.Node( - package="rktl_planner", - executable="bezier_path_server", - name="bezier_path_server", - output="screen" - ), - launch_ros.actions.Node( - package="rktl_planner", - executable="path_planner", - name="path_planner", - output="screen", - parameters=[ - { - launch.substitutions.PathJoinSubstitution(launch_ros.substitutions.FindPackageShare('rktl_planner'), '/config/path_planner.yaml') - }, - { - "car_name": launch.substitutions.LaunchConfiguration("car_name") - } - ] - ) - ] - ) - ]) - return ld - -if __name__ == '__main__': - generate_launch_description() diff --git a/rktl_planner/package.xml b/rktl_planner/package.xml deleted file mode 100644 index 7de7a9e81..000000000 --- a/rktl_planner/package.xml +++ /dev/null @@ -1,37 +0,0 @@ - - - - rktl_planner - 1.0.0 - Handles localized actions to plan and follow trajectories. - Autonomous Robotics Club of Purdue - BSD 3 Clause - https://www.purduearc.com - Harrison McCarty - Arianna McNamara - Vincent Vu - Adrian Teigen - Daniel Gerblick - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - geometry_msgs - rclpy - std_msgs - nav_msgs - geometry_msgs - std_srvs - tf - python-numpy - rktl_msgs - rktl_dependencies - rktl_msgs - message_generation - message_runtime - - - ament_python - - diff --git a/rktl_planner/resource/rktl_planner b/rktl_planner/resource/rktl_planner deleted file mode 100644 index e69de29bb..000000000 diff --git a/rktl_planner/rktl_planner/__init__.py b/rktl_planner/rktl_planner/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/rktl_planner/rktl_planner/bezier_curve.py b/rktl_planner/rktl_planner/bezier_curve.py deleted file mode 100644 index a585ff199..000000000 --- a/rktl_planner/rktl_planner/bezier_curve.py +++ /dev/null @@ -1,154 +0,0 @@ -""" -This module implements a Bezier curve class with various methods. A Bezier -curve is defined by its order and control points. - -License: - BSD 3-Clause License - Copyright (c) 2023, Autonomous Robotics Club of Purdue (Purdue ARC) - All rights reserved. -""" - -from geometry_msgs.msg import Point -from math import pow - - -class BezierCurve: - """ - A class representing a Bezier Curve and a duration. If `order` and `control_points` are given in - kwargs, then it is used to initialize the instance. Otherwise, args is used to initialize the - instance. When not using kwargs, 4 options exist. Either a list of control points - (geometry_msgs.msg.Points), the order of the bezier curve, both the order and the list of - control points, or the control points as indiviudal arguments can be given. - - For example, All examples below give an instance of a `BezierCurve` of order 3. When points - are specified, they are the control points used for the curve. - - ```python - curve = BezierCurve(3) - curve = BezierCurve(3, [Point(0, 0, 0), Point(1, 2, 3), Point(3, 2, 1), Point(4, 2, 0)]) - curve = BezierCurve([Point(0, 0, 0), Point(1, 2, 3), Point(3, 2, 1), Point(4, 2, 0)])) - curve = BezierCurve(Point(0, 0, 0), Point(1, 2, 3), Point(3, 2, 1), Point(4, 2, 0))) - curve = BezierCurve(order=3, control_points=[Point(0, 0, 0), Point(1, 2, 3), Point(3, 2, 1), Point(4, 2, 0)])) - curve = BezierCurve(control_points=[Point(0, 0, 0), Point(1, 2, 3), Point(3, 2, 1), Point(4, 2, 0)]))``` - """ - - _coefficients = [[1]] - """Table used internally to generate the coefficients of the bezier curve faster""" - - def __init__(self, *args, **kwargs): - self.control_points = None - self.order = None - - if args: - if len(args) == 1 and type(args[0]) is list: - self.control_points = args[0] - elif len(args) == 1 and type(args[0]) is int: - self.order = args[0] - elif len(args) == 2 and type(args[0]) is int and type(args[1]) is list: - self.order = args[0] - self.control_points = args[1] - else: - self.control_points = args - if kwargs: - for k, v in kwargs.items(): - if k == 'order': - if type(v) is not int: - raise ValueError(f'{k!r} must be {int}, got {type(v)}') - self.order = v - elif k == 'control_points': - if type(v) is not list: - raise ValueError( - f'{k!r} must be {list}, got {type(v)}') - self.control_points = v - else: - raise ValueError(f'Unknown keyword argument {k!r}') - - if self.order is not None and self.control_points is None: - self.control_points = [Point()] * (self.order + 1) - elif self.control_points is not None and self.order is None: - self.order = len(self.control_points) - 1 - elif self.order is None and self.control_points is None: - raise ValueError( - 'Neither \'order\' nor \'control_points\' was specified') - elif self.order and self.control_points and self.order != len(self.control_points) - 1: - raise ValueError( - 'Value of \'order\' and length of \'control_points\' contridict each other') - - for i, p in enumerate(self.control_points): - if type(p) is not Point: - raise ValueError( - f'Element {i} of \'control_points\' must be {Point}, got {type(p)}') - - self.my_hodograph = self if self.order == 0 else None - self.coefficients = BezierCurve.calc_coefficients(self.order + 1) - - def __repr__(self): - """Returns a string representation of the instance.""" - def point_str(p): return f'({p.x:.2f}, {p.y:.2f}, {p.z:.2f})' - strs = [point_str(x) for x in self.control_points[:2]] - if len(self.control_points) > 5: - strs += ['...'] + [point_str(x) for x in self.control_points[-2:]] - else: - strs += [point_str(x) for x in self.control_points[2:]] - points = ', '.join(strs) - return f'{self.__class__.__name__}({self.order}, [{points}])' - - def __str__(self): - """Returns a string representation of the instance.""" - return f'{self.__class__.__name__}: order {self.order}' - - def calc_coefficients(n): - """Calculates and returns the list of binomial coefficients for a given n.""" - if len(BezierCurve._coefficients) >= n: - return BezierCurve._coefficients[n - 1] - prevRow = [0] + BezierCurve.calc_coefficients(n - 1) + [0] - row = [prevRow[i] + prevRow[i + 1] for i in range(n)] - BezierCurve._coefficients += [row] - return row - - def at(self, t): - """Returns a Point object representing the location of the curve at the given parameter value `t`.""" - point = Point() - basis = [c * pow(t, i) * pow(1 - t, self.order - i) - for i, c in enumerate(self.coefficients)] - for b, p in zip(basis, self.control_points): - point.x += b * p.x - point.y += b * p.y - point.z += b * p.z - return point - - def hodograph(self): - """Returns a `BezierCurve` object representing the hodograph (derivative) of the curve.""" - if not self.my_hodograph: - control_points = [] - for i in range(self.order): - p = Point() - p1 = self.control_points[i + 1] - p2 = self.control_points[i] - p.x = self.order * (p1.x - p2.x) - p.y = self.order * (p1.y - p2.y) - p.z = self.order * (p1.z - p2.z) - control_points.append(p) - self.my_hodograph = BezierCurve( - order=self.order - 1, control_points=control_points) - return self.my_hodograph - - def deriv(self, t): - """Returns a `geometry_msgs/Point` object representing the derivative of the curve at the given parameter value `t`.""" - return self.hodograph().at(t) - - def de_casteljau(self, t): - """Splits this curve into 2 curves at the the point specified by `t`. Returns the two new `BezierCurve` objects.""" - # b[j][i] = b_i^(j) - t0 = float(t) - t1 = 1 - t0 - b = [self.control_points] - for j in range(self.order): - b.append([]) - for i in range(self.order - j): - b[j + 1].append(Point()) - b[j + 1][i].x = b[j][i].x * t1 + b[j][i + 1].x * t0 - b[j + 1][i].y = b[j][i].y * t1 + b[j][i + 1].y * t0 - points1 = [b[i][0] for i in range(self.order + 1)] - points2 = [b[self.order - i][i] for i in range(self.order + 1)] - return BezierCurve(points1), BezierCurve(points2) diff --git a/rktl_planner/rktl_planner/bezier_path.py b/rktl_planner/rktl_planner/bezier_path.py deleted file mode 100644 index d31eb1461..000000000 --- a/rktl_planner/rktl_planner/bezier_path.py +++ /dev/null @@ -1,146 +0,0 @@ -""" -A module for handling Bezier curves and paths. - -License: - BSD 3-Clause License - Copyright (c) 2023, Autonomous Robotics Club of Purdue (Purdue ARC) - All rights reserved. -""" - -import math -from rrktl_planner import BezierCurve -from rktl_msgs.msg import BezierPath as BezierPathMsg -from rclpy import Duration -from geometry_msgs.msg import Vector3 -from std_msgs.msg import Duration as DurationMsg - - -class BezierPath: - """ - A class representing a Bezier curve path, which is a `BezierCurve` and a duration. If `msg`, - `bezier_curve`, and/or `duration` is given as kwargs, they are used to convert from - `rktl_msgs/BezierPathMsg`, used as the bezier curve for this path, and/or used for as the - duration path; respectively. Otherwise, args is used to initalize this object. If one argument - is given, it is expected to be of type `rktl_msgs.msgs.BezierPathMsg`. If two arguments are - given, the first should be either a `BezierCurve` object or a list of control points to generate - a `BezierCurve` object. The second argument should be a `std_msgs.msg.Duration` or a `float` - dictating how long this segment should last. - """ - - def __init__(self, *args, **kwargs): - self.bezier_curve = None - self.duration = None - - if args: - if len(args) == 1 and type(args[0]) is BezierPathMsg: - self.bezier_curve = BezierCurve( - args[0].order, args[0].control_points) - self.duration = args[0].duration.data - elif len(args) == 2: - if type(args[0]) is BezierCurve: - self.bezier_curve = args[0] - elif type(args[0]) is list: - self.bezier_curve = BezierCurve(args[0]) - else: - raise ValueError(f'Unknown argument {args[0]!r}') - if type(args[1]) is Duration: - self.duration = args[1] - elif type(args[1]) is float: - self.duration = Duration(args[1]) - else: - raise ValueError(f'Unknown argument {args[1]!r}') - else: - raise ValueError(f'Unknown arguments {args!r}') - if kwargs: - for k, v in kwargs.items(): - if k == 'msg': - if type(v) is not BezierPathMsg: - raise ValueError( - f'{k!r} must be {BezierPathMsg}, got {type(v)}') - self.bezier_curve = v.bezier_curve - self.duration = v.duration - elif k == 'bezier_curve': - if type(v) is not BezierCurve: - raise ValueError( - f'{k!r} must be {BezierCurve}, got {type(v)}') - self.bezier_curve = v - elif k == 'duration': - if type(v) is not Duration: - raise ValueError( - f'{k!r} must be {Duration}, got {type(v)}') - self.duration = v - else: - raise ValueError(f'Unknown keyword argument {k!r}') - - def __repr__(self): - """Returns a string representation of the instance.""" - return f'{self.__class__.__name__}({self.bezier_curve!r}, [{self.duration!r}])' - - def __str__(self): - """Returns a string representation of the instance.""" - return f'{self.__class__.__name__}[{self.bezier_curve!s}, {self.duration!s}ns]' - - def to_param(self, secs): - """Returns the parameter value corresponding to a time in seconds.""" - return (secs.to_sec() if type(secs) is Duration else float(secs)) / self.duration.to_sec() - - def from_param(self, vec): - """Returns a `Vector3` object corresponding to a parameter value.""" - dt = 1.0 / self.duration.to_sec() - return Vector3(vec.x * dt, vec.y * dt, vec.z * dt) - - def at(self, secs): - """Returns a `Vector3` object representing the position on the path at a given time in seconds.""" - t = self.to_param(secs) - return self.bezier_curve.at(t) - - def vel_at(self, secs): - """Returns a `Vector3` object representing the velocity on the path at a given time in seconds.""" - t = self.to_param(secs) - vec = self.bezier_curve.deriv(t) - return self.from_param(vec) - - def speed_at(self, secs): - """Returns the (tangential) speed on the path at a given time in seconds.""" - vel = self.vel_at(secs) - return math.sqrt(vel.x ** 2 + vel.y ** 2 + vel.z ** 2) - - def accel_at(self, secs): - """Returns a Vector3 object representing the acceleration on the path at a given time in seconds.""" - t = self.to_param(secs) - dv = self.bezier_curve.hodograph().hodograph().at(t) - dt = self.duration.to_sec() - return Vector3(dv.x/(dt**2), dv.y/(dt**2), dv.z/(dt**2)) - - def angle_at(self, secs): - """Returns the angle of the tangent vector of the curve at a given time in seconds.""" - vel = self.vel_at(secs) - if vel.x == 0 and vel.y == 0: - vel = self.accel_at(secs) - return math.atan2(vel.y, vel.x) - - def angular_vel_at(self, secs): - """Returns the angular velocity of the curve at a given time in seconds.""" - vel = self.vel_at(secs) - accel = self.accel_at(secs) - return (vel.x * accel.x - vel.y * accel.y) / (vel.x ** 2 + vel.y ** 2) - - def to_msg(self): - """Returns a `rktl_msg/BezierPathMsg` object representing the `BezierPath` object.""" - duration_msg = DurationMsg(self.duration) - msg = BezierPathMsg( - order=self.bezier_curve.order, - control_points=self.bezier_curve.control_points, - duration=duration_msg - ) - return msg - - def split(self, secs): - """Splits this path into 2 paths at a given time in seconds. Returns the two new `BezierPath` objects.""" - t = self.to_param(secs) - curve1, curve2 = self.bezier_curve.de_casteljau(t) - duration1 = Duration(secs) - duration2 = Duration(self.duration.to_sec() - secs) - path1 = BezierPath(bezier_curve=curve1, duration=duration1) - path2 = BezierPath(bezier_curve=curve2, duration=duration2) - return path1, path2 diff --git a/rktl_planner/rktl_planner/convert.py b/rktl_planner/rktl_planner/convert.py deleted file mode 100755 index 7228d3ff6..000000000 --- a/rktl_planner/rktl_planner/convert.py +++ /dev/null @@ -1,56 +0,0 @@ -"""Contains ROS message conversions. -License: - BSD 3-Clause License - Copyright (c) 2023, Autonomous Robotics Club of Purdue (Purdue ARC) - All rights reserved. -""" - -import numpy as np - - -def odom_to_array(msg): - """Convert odom msg to numpy arrays.""" - - p, q, pc = pose_covar_to_array(msg.pose) - l, q, tc = twist_covar_to_array(msg.twist) - - return p, q, pc, l, q, tc - - -def pose_covar_to_array(msg): - """Convert pose with covariance msg to numpy arrays.""" - - p, q = pose_to_array(msg.pose) - c = np.array(msg.covariance) - c = np.reshape(c, (6, 6)) - - return p, q, c - - -def twist_covar_to_array(msg): - """Convert twist with covariance msg to numpy arrays.""" - - l, a = twist_to_array(msg.twist) - c = np.array(msg.covariance) - c = np.reshape(c, (6, 6)) - - return l, a, c - - -def pose_to_array(msg): - """Convert pose msg to numpy arrays.""" - - p = np.array([msg.position.x, msg.position.y, msg.position.z]) - q = np.array([msg.orientation.x, msg.orientation.y, - msg.orientation.z, msg.orientation.w]) - - return p, q - - -def twist_to_array(msg): - """Convert twist msg to numpy arrays.""" - - l = np.array([msg.linear.x, msg.linear.y, msg.linear.z]) - a = np.array([msg.angular.x, msg.angular.y, msg.angular.z]) - - return l, a diff --git a/rktl_planner/rktl_planner/nodes/README.md b/rktl_planner/rktl_planner/nodes/README.md deleted file mode 100644 index 7b68e59fe..000000000 --- a/rktl_planner/rktl_planner/nodes/README.md +++ /dev/null @@ -1,154 +0,0 @@ -# ROS Nodes - -These are the [ROS Nodes](http://wiki.ros.org/Nodes) provided by this package. -Most likely, you will use a launch file to run these. You can also run them -by using `rosrun`: - -```bash -rosrun rktl_planner -``` - -```{contents} ROS Nodes in the package -:depth: 2 -:backlinks: top -:local: true -``` - ---- - -## bezier_path_server - -This node is a ROS service that creates a Bezier path using a series of input -poses, target durations, and segment durations. The output includes the Bezier -path (which is a series of connected Bezier curves) and a linear path (which is -a series of linear segments that approximate the Bezier path). The Bezier path -is designed to allow for smooth turns and gradual acceleration and deceleration, -while the linear path is used for easier computation and execution. - - -### Services: - -- `create_bezier_path` ([rktl_planner/CreateBezierPath](/rktl_planner/html/msg/CreateBezierPath.html#http://)): - A service that creates a Bezier path using the input poses and durations. - ---- - -## path_follower - -This node ensures the car follows a given trajectory using the pure pursuit -algorithm. It subscribes to the `/cars/{car_name}/odom` topic to obtain the -car's odometry information and the `/linear_path` topic to receive the desired -path. It then publishes the control commands to the `/cars/{car_name}/command` -topic. - -### Subscribed Topics: - -- `/cars/{car_name}/odom` ([nav_msgs/Odometry](/nav_msgs/html/msg/Odometry.html#http://)): - The car's odometry information containing its position, orientation, and velocities. -- `linear_path` ([rktl_msgs/Path](/rktl_msgs/html/msg/Path.html#http://)): - The desired path that the car should follow. - -### Published Topics: - -- `/cars/{car_name}/command` ([rktl_msgs/ControlCommand](/rktl_msgs/html/msg/ControlCommand.html#http://)): - The control commands to be sent to the car to follow the desired path - -### Parameters: - -- `~frame_id` (str, default: 'map'): The frame ID for the path and pose messages. -- `~max_speed` (float, default: 0.1): The maximum speed at which the car should - travel along the path. -- `~lookahead_dist` (float, default: 0.15): The distance ahead of the car to - look for the next point on the path. -- `~lookahead_gain` (float, default: 0.035): The coefficient used to adjust the - lookahead distance based on the car's current speed. -- `~lookahead_pnts` (int, default: -1): The number of points to search along the - path for the next valid intersection. If set to -1, the entire path is searched. -- `~car_name` (str): The name of the car to which this node belongs. - ---- - -## path_planner - -This node uses `bezier_path_server` to generate bezier paths between a car and -the ball in order to hit the ball into a goal. Options exist for generating a -'simple' or 'complex' path to the ball. A 'simple' path is a direct path between -the current position of the car and the ball's position, while a 'complex' path -takes into account the orientation of the car and decides whether to take a -direct path or to generate a path in reverse. - -### Subscribed Topics: - -- `/cars/{car_name}/odom` ([nav_msgs/Odometry](/nav_msgs/html/msg/Odometry.html#http://)): - The car's odometry information containing its position, orientation, and velocities. - -### Published Topics: - -- `linear_path` ([rktl_msgs/Path](/rktl_msgs/html/msg/Path.html#http://)): - The desired path that the car should follow, split into linear segments -- `bezier_path` ([rktl_msgs/BezierPath](/rktl_msgs/html/msg/BezierPath.html#http://)): - The desired path that the car should follow, split into bezier segments - -### Services: - -- `reset_planner` ([std_srvs/Empty](/std_srvs/html/msg/Empty.html#http://)): - An empty service that is used to trigger the generation/regeneration of the - path based on the odemetry of the car and ball. The new path is published on - the topics above. - -### Parameters: - -- `~frame_id` (str, default: 'map'): The frame ID for the path and pose messages. -- `~max_speed` (float, default: 0.1): The maximum speed at which the car should - travel along the path. -- `~lookahead_dist` (float, default: 0.15): The distance ahead of the car to - look for the next point on the path. -- `~lookahead_gain` (float, default: 0.035): The coefficient used to adjust the - lookahead distance based on the car's current speed. -- `~lookahead_pnts` (int, default: -1): The number of points to search along the - path for the next valid intersection. If set to -1, the entire path is searched. -- `~car_name` (str): The name of the car to which this node belongs. - ---- - -## patrol_planner - -This node implements a "patrol"-based algorithm to control the car. The car -"patrols" around the field in a circle and makes a beeline towards the ball if -it seems like it would result in a goal. Logic is in place for both offensive -and defensive modes. A Proportional Derivative (PD) controller is used to -determine how much to turn. - -### Subscribed Topics: -- `odom` ([nav_msgs/Odometry](/nav_msgs/html/msg/Odometry.html#http://)): - The car's odometry information, containing its position, orientation, and velocities. -- `/ball/odom` ([nav_msgs/Odometry](/nav_msgs/html/msg/Odometry.html#http://)): - The ball's odometry information, containing its position, orientation, and velocities. - -### Published Topics: -- `command` ([rktl_msgs/ControlCommand](/rktl_msgs/html/msg/ControlCommand.html#http://)): - The control commands to be sent to the car to perform the patrol algorithm - -### Parameters: -- `/field/width` (float): Physical constant, width of the field -- `/field/length` (float): Physical constant, length of the field -- `/cars/steering/max_throw` (float): Physical constant, maximum steering throw -- `/cars/length` (float): Physical constant, car length -- `~speed` (float, default: 1.0): Maximum speed of the car -- `~curvature_gain/kp` (float, default: 1.0): Parameter for PD Controller -- `~curvature_gain/kd` (float, default: 0.0): Parameter for PD Controller -- `~curvature_gain/falloff` (float, default: 1e-9): Parameter for PD Controller -- `~patrol_wall_dist` (float, default: 0.5) -- `~wall_avoidance/reverse_time` (float, default: 0.25) -- `~wall_avoidance/distance_margin` (float, default: 0.5) -- `~wall_avoidance/heading_margin` (float, default: π/4) -- `~attempt_timeout` (float, default: 5.0) -- `~defensive_line` (float: 0.0) -- `~reverse_line` (float: 0.0) -- `~scoring/heading_margin` (float, default: π/8) -- `~scoring/car_lookahead_dist` (float, default: 1.0) -- `~scoring/ball_lookahead_time` (float, default: 0.0) -- `~scoring/goal_depth_target` (float, default: 0.0) -- `~defense/reverse_time_gain` (float, default: 0.5) - ---- diff --git a/rktl_planner/rktl_planner/nodes/bezier_path_server.py b/rktl_planner/rktl_planner/nodes/bezier_path_server.py deleted file mode 100755 index fed4f55c7..000000000 --- a/rktl_planner/rktl_planner/nodes/bezier_path_server.py +++ /dev/null @@ -1,103 +0,0 @@ -#!/usr/bin/env python3 - -#import rospy -import rclpy -import math -from rktl_dependencies.transformations import euler_from_quaternion, quaternion_from_euler -from rktl_planner.srv import CreateBezierPath, CreateBezierPathRequest, CreateBezierPathResponse -from rktl_planner import BezierPath -from rktl_msgs.msg import Path as PathMsg, Waypoint as WaypointMsg -from geometry_msgs.msg import Pose, Point, Vector3 - - -def vel(pose: Pose, speed: float): - q = pose.orientation - angle = euler_from_quaternion([q.x, q.y, q.z, q.w])[2] - head = Vector3() - head.x = math.cos(angle) - head.y = math.sin(angle) - vel = Vector3() - vel.x = speed * head.x - vel.y = speed * head.y - return vel, head - - -def line(p: Point, v: Vector3, t: float): - point = Point() - point.x = p.x + t * v.x - point.y = p.y + t * v.y - point.z = p.z + t * v.z - return point - - -def create_segment(start_pose: Pose, end_pose: Pose, velocity: float, - # duration: rospy.Duration, segment_duration: rospy.Duration): - duration: rclpy.duration.Duration, segment_duration: rclpy.duration.Duration): - control_points = [start_pose.position, end_pose.position] - start_vel, start_head = vel(start_pose, velocity) - end_vel, end_head = vel(end_pose, velocity) - - t_val = duration.to_sec() / 3 - - control_points.insert(1, line(control_points[0], start_vel, t_val)) - control_points.insert(len(control_points) - 1, line(control_points[-1], end_vel, -t_val)) - path = BezierPath(control_points, duration.to_sec()) - num_segments = math.ceil(duration.to_sec() / segment_duration.to_sec()) - segment_length = duration.to_sec() / num_segments - segments = [] - for i in range(num_segments - 1): - path0, path1 = path.split(segment_length) - segments.append(path0) - path = path1 - segments.append(path) - return segments - - -def bezier_path_server(req: CreateBezierPathRequest): - velocity = req.velocity - bezier_segments = [] - durations = [] - for i in range(len(req.target_durations)): - start_pose = req.target_poses[i] - end_pose = req.target_poses[i + 1] - duration = req.target_durations[i].data - segments = create_segment(start_pose, end_pose, velocity, - duration, req.bezier_segment_duration.data) - bezier_segments.extend(segments) - for segment in segments: - if len(durations) > 0: - durations.append(durations[-1] + segment.duration.to_sec()) - else: - durations.append(segment.duration.to_sec()) - - res = CreateBezierPathResponse() - res.bezier_paths = [x.to_msg() for x in bezier_segments] - res.linear_path = PathMsg() - res.linear_path.velocity = velocity - - duration = durations[-1] - segment_length = req.linear_segment_duration.data.to_sec() - num_segments = math.floor(duration / segment_length) - idx = 0 - t = 0. - for i in range(num_segments): - heading = bezier_segments[idx].angle_at(t) - quat = quaternion_from_euler(0, 0, heading) - wp = WaypointMsg() - wp.pose.position = bezier_segments[idx].at(t) - wp.pose.orientation.x = quat[0] - wp.pose.orientation.y = quat[1] - wp.pose.orientation.z = quat[2] - wp.pose.orientation.w = quat[3] - wp.twist.linear = bezier_segments[idx].vel_at(t) - res.linear_path.waypoints.append(wp) - t += segment_length - if t > durations[idx]: - idx += 1 - return res - -if __name__ == '__main__': - rclpy.init() - node = rclpy.create_node('bezier_path_server') - service = node.create_service(CreateBezierPath, 'create_bezier_path', bezier_path_server) - rclpy.spin() \ No newline at end of file diff --git a/rktl_planner/rktl_planner/nodes/path_follower b/rktl_planner/rktl_planner/nodes/path_follower deleted file mode 100755 index fcf895bd3..000000000 --- a/rktl_planner/rktl_planner/nodes/path_follower +++ /dev/null @@ -1,143 +0,0 @@ -#!/usr/bin/env python3 -"""Contains the PathPlanner Node. -License: - BSD 3-Clause License - Copyright (c) 2023, Autonomous Robotics Club of Purdue (Purdue ARC) - All rights reserved. -""" - -# 3rd party modules -import rclpy -#import rospy -import numpy as np -import math -import time -from nav_msgs.msg import Odometry - -# Local modules -import rktl_planner.convert as convert -import rktl_planner.pure_pursuit as pursuit -from rktl_msgs.msg import Path, ControlCommand - - -class PathFollower(object): - """A node to ensure the car follows a given trajectory.""" - - def __init__(self): - rclpy.init() - global node - node = rclpy.create_node('path_follower') - - self.path_start_time = None - self.path = None - self.last_pose_idx = None - self.start_time = None - self.max_speed = None - - self.frame_id = node.declare_parameter('~frame_id', 'map') - - # Max speed to travel path - self.max_speed = node.declare_parameter('~max_speed', 0.1) - - # Radius to search for intersections - self.lookahead_dist = node.declare_parameter('~lookahead_dist', 0.15) - - # Coeffient to adjust lookahead distance by speed - self.lookahead_gain = node.declare_parameter('~lookahead_gain', 0.035) - - # Number of waypoints to search per pass (-1 is full path) - self.lookahead_pnts = node.declare_parameter('~lookahead_pnts', -1) - - # Publishers - car_name = node.declare_parameter('~car_name') - self.bot_velocity_cmd = node.create_publisher(ControlCommand, f'/cars/{car_name}/command', 1) - - # Subscribers - node.create_subscription(Odometry, f'/cars/{car_name}/odom', self.odom_cb, rclpy.qos.QoSProfile()) - node.create_subscription(Path, 'linear_path', self.path_cb) - - rclpy.spin() - - def path_cb(self, path_msg: Path): - """Creates path using waypoints in Path message.""" - self.path_start_time = path_msg.header.stamp - self.goal_vel = path_msg.velocity - self.path = path_msg.waypoints - self.last_pnt_idx = 0 - self.start_time = time.time() - - def odom_cb(self, odom_msg: Odometry): - """Updates car odometry and follows current path.""" - if self.path: - if self.last_pose_idx == None: - self.last_pose_idx = 0 - - # Converting odom msg to numpy arrays - bot_pos, bot_orient, _ = convert.pose_covar_to_array(odom_msg.pose) - bot_linear, bot_angular, _ = convert.twist_covar_to_array( - odom_msg.twist) - - # Set lookahead dist by lookahead gain and current speed - lookahead_boost = np.linalg.norm(bot_linear) \ - * self.lookahead_gain - lookahead_dist = self.lookahead_dist + lookahead_boost - - # Set number of waypoints to check - if self.lookahead_pnts == -1: - lookahead_pnts = len(self.path) - else: - lookahead_pnts = self.lookahead_pnts - - # Find next valid intersection along path - intersect = None - goal_vel = self.goal_vel - i = self.last_pose_idx - pnts_checked = 0 - while pnts_checked < lookahead_pnts: - if i == len(self.path) - 1: - i = 0 - - start_pos, _ = convert.pose_to_array(self.path[i].pose) - end_pos, _ = convert.pose_to_array(self.path[i + 1].pose) - - path_seg = end_pos - start_pos - bot_path = start_pos - bot_pos - - intersect = pursuit.find_intersection(path_seg, bot_path, - lookahead_dist) - - if intersect is not None: - self.last_pose_idx = i - intersect += start_pos - - # Check if intersection is behind vehicle - d_angle = pursuit.calculate_angle(intersect, bot_pos, - bot_orient, lookahead_dist, goal_vel < 0) - if abs(d_angle) > math.pi/2: - intersect = None - else: - break - - i += 1 - pnts_checked += 1 - - # If no intersection found, stop moving - if intersect is None: - self.bot_velocity_cmd.publish(ControlCommand()) - node.get_logger().warn("No intersection could be found.") - #rospy.logwarn("No intersection could be found.") - return - - # Calculate curvature - turn_rad = pursuit.calculate_turn_rad(intersect, bot_pos, - bot_orient, lookahead_dist, goal_vel < 0) - - # Publish command data - cmd = ControlCommand() - cmd.velocity = goal_vel - cmd.curvature = -(1.0 / turn_rad) - self.bot_velocity_cmd.publish(cmd) - - -if __name__ == "__main__": - PathFollower() diff --git a/rktl_planner/rktl_planner/nodes/path_planner b/rktl_planner/rktl_planner/nodes/path_planner deleted file mode 100755 index 99edb73b0..000000000 --- a/rktl_planner/rktl_planner/nodes/path_planner +++ /dev/null @@ -1,152 +0,0 @@ -#!/usr/bin/env python3 - -import math -import rclpy -import numpy as np -from rktl_dependencies.transformations import quaternion_from_euler, euler_from_quaternion -from std_msgs.msg import Duration -from nav_msgs.msg import Odometry -from geometry_msgs.msg import Pose -from rktl_msgs.msg import BezierPathList, Path -from std_srvs.srv import Empty, EmptyRequest, EmptyResponse -from rktl_planner.srv import CreateBezierPath, CreateBezierPathRequest - -def create_simple_path_req(car_odom, ball_odom, goal_pos): - req = CreateBezierPathRequest() - - req.velocity = 0.5 - req.bezier_segment_duration.data = node.create_rate(0.5) - req.linear_segment_duration.data = node.create_rate(0.01) - - # Target 0 (car pos) - pose0 = Pose() - pose0.position.x = car_odom.pose.pose.position.x - pose0.position.y = car_odom.pose.pose.position.y - pose0.position.z = 0.0 - pose0.orientation = car_odom.pose.pose.orientation - req.target_poses.append(pose0) - req.target_durations.append(Duration(data=node.create_rate(5.0))) - - # Target 1 (ball pos) - final_vec_x = goal_pos[0] - ball_odom.pose.pose.position.x - final_vec_y = goal_pos[1] - ball_odom.pose.pose.position.y - final_quat = quaternion_from_euler( - 0., 0., math.atan2(final_vec_y, final_vec_x)) - final_vec_len = math.sqrt(final_vec_x ** 2 + final_vec_y ** 2) - pose1 = Pose() - pose1.position.x = ball_odom.pose.pose.position.x - pose1.position.y = ball_odom.pose.pose.position.y - pose1.position.z = 0.0 - pose1.orientation.x = final_quat[0] - pose1.orientation.y = final_quat[1] - pose1.orientation.z = final_quat[2] - pose1.orientation.w = final_quat[3] - req.target_poses.append(pose1) - req.target_durations.append(Duration(data=node.create_rate(1.0))) - - # Target 2 (stop) - pose2 = Pose() - pose2.position.x = pose1.position.x + final_vec_x / final_vec_len / 2 - pose2.position.y = pose1.position.y + final_vec_y / final_vec_len / 2 - pose2.position.z = 0.0 - pose2.orientation = pose2.orientation - req.target_poses.append(pose2) - - return req - -def create_backup_path_req(car_odom, ball_odom, goal_pos): - req = CreateBezierPathRequest() - req.velocity = -0.5 - req.bezier_segment_duration.data = node.create_rate(0.5) - req.linear_segment_duration.data = node.create_rate(0.01) - - # Target 0 (car pos) - pose0 = Pose() - pose0.position.x = car_odom.pose.pose.position.x - pose0.position.y = car_odom.pose.pose.position.y - pose0.position.z = 0.0 - pose0.orientation = car_odom.pose.pose.orientation - req.target_poses.append(pose0) - req.target_durations.append(Duration(data=node.create_rate(1.0))) - - # Target 1 (in front of ball) - final_vec_x = goal_pos[0] - ball_odom.pose.pose.position.x - final_vec_y = goal_pos[1] - ball_odom.pose.pose.position.y - final_quat = quaternion_from_euler( - 0., 0., math.atan2(final_vec_y, final_vec_x)) - pose1 = Pose() - pose1.position.x = ball_odom.pose.pose.position.x - final_vec_x - pose1.position.y = ball_odom.pose.pose.position.y - final_vec_y - pose1.position.z = 0.0 - pose1.orientation.x = final_quat[0] - pose1.orientation.y = final_quat[1] - pose1.orientation.z = final_quat[2] - pose1.orientation.w = final_quat[3] - req.target_poses.append(pose1) - - return req - -def create_complex_path_req(car_odom, ball_odom, goal_pos): - car_orient = car_odom.pose.pose.orientation - car_yaw = euler_from_quaternion( - np.array([car_orient.x, car_orient.y, car_orient.z, car_orient.w]))[2] - car_x = car_odom.pose.pose.position.x - ball_x = ball_odom.pose.pose.position.x - if (car_yaw < math.pi/2 or car_yaw > 3*math.pi/2) and (car_x > ball_x): - return create_backup_path_req(car_odom, ball_odom, goal_pos) - else: - return create_simple_path_req(car_odom, ball_odom, goal_pos) - -class PathPlanner(object): - def __init__(self): - rclpy.init() - global node - node = rclpy.create_node('path_planner') - - planner_type = node.declare_parameter('~planner_type', 'simple') - if planner_type == 'simple': - self.path_req = create_simple_path_req - elif planner_type == 'complex': - self.path_req = create_complex_path_req - else: - raise NotImplementedError(f'unrecognized planner type: {node.declare_parameter("~planner_type")}') - - # Subscribers - car_name = node.declare_parameter('~car_name') - node.create_subscription(Odometry, f'/cars/{car_name}/odom', self.car_odom_cb, rclpy.qos.QoSProfile()) - node.create_subscription(Odometry, '/ball/odom', self.ball_odom_cb) - - # Services - self.reset_server = node.create_service(Empty, 'reset_planner', self.reset) - self.path_client = node.create_client(CreateBezierPath, 'create_bezier_path') - - # Publishers - self.linear_path_pub = node.create_publisher(Path, 'linear_path', 1, latch=True) - self.bezier_path_pub = node.create_publisher(BezierPathList, 'bezier_path', 1, latch=True) - - self.car_odom = Odometry() - self.ball_odom = Odometry() - - self.goal_pos = (node.declare_paramete('/field/length', 1) / 2, 0.) - - rclpy.spin() - - def car_odom_cb(self, data: Odometry): - self.car_odom = data - - def ball_odom_cb(self, data: Odometry): - self.ball_odom = data - - def reset(self, _: EmptyRequest): - req = self.path_req(self.car_odom, self.ball_odom, self.goal_pos) - res = self.path_client(req) - if self.linear_path_pub: - self.linear_path_pub.publish(res.linear_path) - if self.bezier_path_pub: - bezier_path_list = BezierPathList() - bezier_path_list.paths = res.bezier_paths - self.bezier_path_pub.publish(bezier_path_list) - return EmptyResponse() - -if __name__ == '__main__': - PathPlanner() diff --git a/rktl_planner/rktl_planner/nodes/patrol_planner b/rktl_planner/rktl_planner/nodes/patrol_planner deleted file mode 100755 index e50df4b44..000000000 --- a/rktl_planner/rktl_planner/nodes/patrol_planner +++ /dev/null @@ -1,289 +0,0 @@ -#!/usr/bin/env python3 -"""Contains the PatrolPlanner node, a very simple planner. -License: - BSD 3-Clause License - Copyright (c) 2023, Autonomous Robotics Club of Purdue (Purdue ARC) - All rights reserved. -""" - -import rclpy -from rktl_dependencies.transformations import euler_from_quaternion -from nav_msgs.msg import Odometry -from rktl_msgs.msg import ControlCommand - -import math, time -from rktl_dependencies.rktl_dependencies.angles import shortest_angular_distance as sad - -class PatrolPlanner(object): - """A very simple strategy for rktl. Patrol around the field in a circle and - make a beeline at the ball if it seems like it would result in a goal.""" - - def __init__(self): - rclpy.init() - global node - node = rclpy.create_node('path_follower') - - # physical constants (global) - self.FIELD_WIDTH = node.declare_parameter('/field/width') - self.FIELD_HEIGHT = node.declare_parameter('/field/length') - self.MAX_CURVATURE = math.tan(node.declare_parameter('/cars/steering/max_throw')) / node.declare_parameter('/cars/length') - - # constants - # general stability parameters - self.SPEED = node.declare_parameter('~speed', 1.0) - self.CURVATURE_P_GAIN = node.declare_parameter('~curvature_gain/kp', 1.0) - self.CURVATURE_D_GAIN = node.declare_parameter('~curvature_gain/kd', 0.0) - self.CURVATURE_GAIN_FALLOFF_SQ = pow(node.declare_parameter('~curvature_gain/falloff', 1e-9), 2) - self.PATROL_DIST = node.declare_parameter('~patrol_wall_dist', 0.5) - - # wall avoidance parameters - self.WALL_REVERSE_TIME = node.declare_parameter('~wall_avoidance/reverse_time', 0.25) - self.WALL_DIST_MARGIN = node.declare_parameter('~wall_avoidance/distance_margin', 0.5) - self.WALL_HEADING_MARGIN = node.declare_parameter('~wall_avoidance/heading_margin', math.pi/4) - - # offense & defense parameters - self.ATTEMPT_TIMEOUT = node.declare_parameter('~attempt_timeout', 5.0) - self.DEFENSIVE_LINE = node.declare_parameter('~defensive_line', 0.0) - self.REVERSE_LINE = node.declare_parameter('~reverse_line', 0.0) - - # offensive related parameters - self.SCORING_MARGIN = node.declare_parameter('~scoring/heading_margin', math.pi/8.0) - self.LOOKAHEAD_DIST = node.declare_parameter('~scoring/car_lookahead_dist', 1.0) - self.LOOKAHEAD_TIME = node.declare_parameter('~scoring/ball_lookahead_time', 0.0) - self.GOAL_DEPTH_TARGET = node.declare_parameter('~scoring/goal_depth_target', 0.0) - - # defense related parameters - self.DEFENSE_TIME_GAIN = node.declare_parameter('~defense/reverse_time_gain', 0.5) - - # variables - self.ball_position = None - self.ball_velocity = None - self.attempt_start_time = None - self.prev_error = 0.0 - - # Publishers - self.cmd_pub = node.create_publisher(ControlCommand, 'command', 1) - - # Subscribers - node.create_subscription(Odometry, 'odom', self.car_odom_cb) - node.create_subscription(Odometry, '/ball/odom', self.ball_odom_cb) - - rclpy.spin() - - def ball_odom_cb(self, odom_msg): - """Callback for ball odometry.""" - self.ball_position = ( - odom_msg.pose.pose.position.x, - odom_msg.pose.pose.position.y - ) - self.ball_velocity = ( - odom_msg.twist.twist.linear.x, - odom_msg.twist.twist.linear.y, - ) - - def car_odom_cb(self, odom_msg): - """Callback for car odometry.""" - - # extract car's position & heading - x = odom_msg.pose.pose.position.x - y = odom_msg.pose.pose.position.y - __, __, yaw = euler_from_quaternion([ - odom_msg.pose.pose.orientation.x, - odom_msg.pose.pose.orientation.y, - odom_msg.pose.pose.orientation.z, - odom_msg.pose.pose.orientation.w - ]) - - # Prep command message for publish - cmd_msg = ControlCommand() - - # check that ball position is known - if self.ball_position is None or self.ball_velocity is None: - self.cmd_pub.publish(cmd_msg) - node.get_logger().warn("ball position unknown") - return - - # calculate cardinal distances / errors - north_wall_dist = abs(x - self.FIELD_HEIGHT/2.0) - south_wall_dist = abs(x + self.FIELD_HEIGHT/2.0) - east_wall_dist = abs(y + self.FIELD_WIDTH/2.0) - west_wall_dist = abs(y - self.FIELD_WIDTH/2.0) - - north_heading_error = sad(yaw, 0.0) - south_heading_error = sad(yaw, math.pi) - east_heading_error = sad(yaw, -math.pi/2.0) - west_heading_error = sad(yaw, +math.pi/2.0) - - # extract ball position and velocity, adding look-ahead time - bx, by = self.ball_position - vbx, vby = self.ball_velocity - bx += vbx * self.LOOKAHEAD_TIME - by += vby * self.LOOKAHEAD_TIME - - # known goal position - gx = self.FIELD_HEIGHT/2.0 + self.GOAL_DEPTH_TARGET - gy = 0.0 - - # add a look-ahead distance to the car - # (dependent on fwd or rev direction) - if by < self.REVERSE_LINE: - x_la = x + math.cos(yaw) * self.LOOKAHEAD_DIST - y_la = y + math.sin(yaw) * self.LOOKAHEAD_DIST - else: - x_la = x - math.cos(yaw) * self.LOOKAHEAD_DIST - y_la = y - math.sin(yaw) * self.LOOKAHEAD_DIST - - # calculate scoring / defensive heading errors - car_ball_heading = math.atan2(by - y, bx - x) - - car_ball_heading_la = math.atan2(by - y_la, bx - x_la) - ball_goal_heading = math.atan2(gy - by, gx - bx) - - # check to see if an existing attempt has timed out - if self.attempt_start_time is not None and (node.get_clock().now() - self.attempt_start_time).to_sec() > self.ATTEMPT_TIMEOUT: - self.attempt_start_time = None - node.get_logger().info("attempt timeout") - - # check if we should be offensive or defensive - if bx > self.DEFENSIVE_LINE: # offensive - # check if we are well aligned to score - if self.attempt_start_time is not None or abs(sad(ball_goal_heading, car_ball_heading_la)) < self.SCORING_MARGIN: - # check to see if we should score in forward or reverse - if by < self.REVERSE_LINE: # forward - cmd_msg.velocity = self.SPEED - cmd_msg.curvature = self.control_curvature(sad(yaw, car_ball_heading)) - else: # reverse - cmd_msg.velocity = -self.SPEED - cmd_msg.curvature = -self.control_curvature(sad(yaw, car_ball_heading + math.pi)) - - # apply falloff - car_ball_dist_sq = pow(by - y, 2) + pow(bx - x, 2) - if car_ball_dist_sq / self.CURVATURE_GAIN_FALLOFF_SQ < 1.0: - cmd_msg.curvature *= car_ball_dist_sq / self.CURVATURE_GAIN_FALLOFF_SQ - - self.cmd_pub.publish(cmd_msg) - - # mark attempt start - if self.attempt_start_time is None: - self.attempt_start_time = node.get_clock().now() - self.prev_error = 0.0 - node.get_logger().info("beginning scoring attempt") - return - - else: # defensive - if self.attempt_start_time is not None: - # determine if ball should be hit in fwd or rev - if by < self.REVERSE_LINE: # forward - cmd_msg.velocity = self.SPEED - cmd_msg.curvature = self.control_curvature(sad(yaw, car_ball_heading)) - else: # reverse - cmd_msg.velocity = -self.SPEED - cmd_msg.curvature = -self.control_curvature(sad(yaw, car_ball_heading + math.pi)) - - # apply falloff - car_ball_dist_sq = pow(by - y, 2.0) + pow(bx - x, 2) - if car_ball_dist_sq / self.CURVATURE_GAIN_FALLOFF_SQ < 1.0: - cmd_msg.curvature *= car_ball_dist_sq / self.CURVATURE_GAIN_FALLOFF_SQ - - self.cmd_pub.publish(cmd_msg) - return - - # if not already trying to hit the ball, wait until at bottom part of pattern - elif (abs(east_heading_error) < self.SCORING_MARGIN and - south_wall_dist < self.PATROL_DIST + 0.25 and - abs(east_wall_dist - west_wall_dist) < 0.5): - # run fwd or rev so easier to hit ball - if by < self.REVERSE_LINE: - cmd_msg.velocity = -self.SPEED - else: - cmd_msg.velocity = self.SPEED - - cmd_msg.curvature = 0.0 - self.cmd_pub.publish(cmd_msg) - - # mark attempt start - self.attempt_start_time = node.get_clock().now() - self.prev_error = 0.0 - node.get_logger().info("beginning defensive attempt") - - # reverse for longer, if the ball is nearer to the middle - time.sleep((self.FIELD_WIDTH/2.0 - abs(by)) * self.DEFENSE_TIME_GAIN) - return - - # If not trying to score, prioritize not running in to walls - # If running into a wall is detected, reverse for a set time limit - - # check if jammed in a corner, where a turn is required to get back on the proper path - if (north_wall_dist < self.WALL_DIST_MARGIN and west_wall_dist < self.WALL_DIST_MARGIN and - abs(north_heading_error) < self.WALL_HEADING_MARGIN or - south_wall_dist < self.WALL_DIST_MARGIN and east_wall_dist < self.WALL_DIST_MARGIN and - abs(south_heading_error) < self.WALL_HEADING_MARGIN or - east_wall_dist < self.WALL_DIST_MARGIN and north_wall_dist < self.WALL_DIST_MARGIN and - abs(east_heading_error) < self.WALL_HEADING_MARGIN or - west_wall_dist < self.WALL_DIST_MARGIN and south_wall_dist < self.WALL_DIST_MARGIN and - abs(west_heading_error) < self.WALL_HEADING_MARGIN): - cmd_msg.velocity = -self.SPEED - cmd_msg.curvature = -self.MAX_CURVATURE - self.cmd_pub.publish(cmd_msg) - time.sleep(self.WALL_REVERSE_TIME) # stay in reverse for a bit - return - - # check if jammed in a corner, anywhere in a 90 degree arc facing the corner - if (north_wall_dist < self.WALL_DIST_MARGIN and west_wall_dist < self.WALL_DIST_MARGIN and - north_heading_error < 0.0 and west_heading_error > 0.0 or - south_wall_dist < self.WALL_DIST_MARGIN and east_wall_dist < self.WALL_DIST_MARGIN and - south_heading_error < 0.0 and east_heading_error > 0.0 or - east_wall_dist < self.WALL_DIST_MARGIN and north_wall_dist < self.WALL_DIST_MARGIN and - east_heading_error < 0.0 and north_heading_error > 0.0 or - west_wall_dist < self.WALL_DIST_MARGIN and south_wall_dist < self.WALL_DIST_MARGIN and - west_heading_error < 0.0 and south_heading_error > 0.0): - cmd_msg.velocity = -self.SPEED - cmd_msg.curvature = 0.0 - self.cmd_pub.publish(cmd_msg) - time.sleep(self.WALL_REVERSE_TIME) # stay in reverse for a bit - return - - # check if near a wall and facing it - if (north_wall_dist < self.WALL_DIST_MARGIN and abs(north_heading_error) < self.WALL_HEADING_MARGIN or - south_wall_dist < self.WALL_DIST_MARGIN and abs(south_heading_error) < self.WALL_HEADING_MARGIN or - east_wall_dist < self.WALL_DIST_MARGIN and abs(east_heading_error) < self.WALL_HEADING_MARGIN or - west_wall_dist < self.WALL_DIST_MARGIN and abs(west_heading_error) < self.WALL_HEADING_MARGIN): - cmd_msg.velocity = -self.SPEED - cmd_msg.curvature = 0.0 - self.cmd_pub.publish(cmd_msg) - time.sleep(self.WALL_REVERSE_TIME) # stay in reverse for a bit - return - - # Follow simple rules to make it follow a "patrol" - # Check to see if it is at the top or bottom, where it should be curving - if (north_wall_dist < 1.0/self.MAX_CURVATURE + self.PATROL_DIST or - south_wall_dist < 1.0/self.MAX_CURVATURE + self.PATROL_DIST): - # check to see if should wait to turn - if (x > 0.0 and west_wall_dist > 1.0/self.MAX_CURVATURE + self.PATROL_DIST): - cmd_msg.curvature = self.control_curvature(west_heading_error) - elif (x < 0.0 and east_wall_dist > 1.0/self.MAX_CURVATURE + self.PATROL_DIST): - cmd_msg.curvature = self.control_curvature(east_heading_error) - else: - cmd_msg.curvature = self.MAX_CURVATURE - else: # send it straight north or south otherwise - if (y > 0.0): - # west wall (southern heading desired) - cmd_msg.curvature = self.control_curvature(south_heading_error) - else: - # east wall (northern heading desired) - cmd_msg.curvature = self.control_curvature(north_heading_error) - - cmd_msg.velocity = self.SPEED - self.cmd_pub.publish(cmd_msg) - - def control_curvature(self, error): - """PD controller for curvature.""" - delta_error = error - self.prev_error - curvature = ( - self.CURVATURE_P_GAIN * error + - self.CURVATURE_D_GAIN * delta_error) - self.prev_error = error - return curvature - -if __name__ == "__main__": - PatrolPlanner() diff --git a/rktl_planner/rktl_planner/pure_pursuit.py b/rktl_planner/rktl_planner/pure_pursuit.py deleted file mode 100755 index a8575db94..000000000 --- a/rktl_planner/rktl_planner/pure_pursuit.py +++ /dev/null @@ -1,101 +0,0 @@ -"""Contains pure pursuit helper functions. -License: - BSD 3-Clause License - Copyright (c) 2023, Autonomous Robotics Club of Purdue (Purdue ARC) - All rights reserved. -""" - -# 3rd party modules -import numpy as np -import math -from rktl_dependencies.rktl_dependencies.transformations import euler_from_quaternion - - -def find_intersection(path_seg, bot_path, lookahead_dist): - """ - Uses quadratic formula to find intersections between - the lookahead radius and the path. - """ - - a = np.dot(path_seg, path_seg) - b = 2 * np.dot(path_seg, bot_path) - c = np.dot(bot_path, bot_path) - (lookahead_dist * lookahead_dist) - discrim = (b*b) - (4*a*c) - - if discrim >= 0: - discrim = math.sqrt(discrim) - t1 = (-b - discrim)/(2*a) - t2 = (-b + discrim)/(2*a) - - if t1 >= 0 and t1 <= 1: - return path_seg * t1 - if t2 >= 0 and t2 <= 1: - return path_seg * t2 - return None - - -def calculate_lat_error(intersect_pos, bot_pos, bot_orient, lookahead_dist): - """Determines lateral error from intersection point.""" - - _, _, bot_yaw = euler_from_quaternion(bot_orient) - a = -math.tan(bot_yaw) - c = (math.tan(bot_yaw) * bot_pos[0]) - bot_pos[1] - dist = math.sqrt(math.pow(a, 2) + 1) - x = abs((a * intersect_pos[0]) + intersect_pos[1] + c) / dist - - bot_line_x = bot_pos[0] + (math.cos(bot_yaw) * lookahead_dist) - bot_line_y = bot_pos[1] + (math.sin(bot_yaw) * lookahead_dist) - bot_line = np.array([bot_line_x, bot_line_y, 0]) - tang_line = intersect_pos - bot_line - sign = np.sign(np.cross(tang_line, bot_line))[2] - return x * sign - - -def calculate_turn_rad( - intersect_pos, bot_pos, bot_orient, lookahead_dist, bkw): - """Determines turning radius to reach intersection point.""" - - _, _, bot_yaw = euler_from_quaternion(bot_orient) - a = -math.tan(bot_yaw) - c = (math.tan(bot_yaw) * bot_pos[0]) - bot_pos[1] - dist = math.sqrt(math.pow(a, 2) + 1) - x = abs((a * intersect_pos[0]) + intersect_pos[1] + c) / dist - radius = (lookahead_dist * lookahead_dist)/(2 * x) - - bot_line_x = math.cos(bot_yaw) * lookahead_dist - bot_line_y = math.sin(bot_yaw) * lookahead_dist - bot_line = np.array([bot_line_x, bot_line_y, 0]) - sign = np.sign(np.cross(intersect_pos - bot_pos, bot_line))[2] - if bkw: - sign *= -1 - - return radius * sign - - -def calculate_angle(intersect_pos, bot_pos, bot_orient, lookahead_dist, bkw): - """ - Determines angle from heading to the intersection point. - Angle is from -pi to pi, with 0 at car's heading. - """ - _, _, bot_yaw = euler_from_quaternion(bot_orient) - - bot_line_x = math.cos(bot_yaw) * lookahead_dist - bot_line_y = math.sin(bot_yaw) * lookahead_dist - bot_line = np.array([bot_line_x, bot_line_y, 0]) - - tang_line = intersect_pos - (bot_pos + bot_line) - dist = np.linalg.norm(tang_line) - angle = (2 * math.asin(round((dist / 2) / lookahead_dist, 6))) - sign = np.sign(np.cross(intersect_pos - bot_line, bot_line))[2] - - if bkw: - angle = math.pi - angle - return angle * sign - - -def get_angular_speed(linear_vel, turn_rad): - """Relates path to the angular velocity.""" - if turn_rad != 0: - return linear_vel / turn_rad - else: - return 0 diff --git a/rktl_planner/setup.cfg b/rktl_planner/setup.cfg deleted file mode 100644 index 2d1e4c1ef..000000000 --- a/rktl_planner/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/rktl_planner -[install] -install_scripts=$base/lib/rktl_planner diff --git a/rktl_planner/setup.py b/rktl_planner/setup.py deleted file mode 100644 index 76fec6eae..000000000 --- a/rktl_planner/setup.py +++ /dev/null @@ -1,25 +0,0 @@ -from setuptools import find_packages, setup - -package_name = 'rktl_planner' - -setup( - name=package_name, - version='1.0.0', - packages=find_packages(exclude=['test']), - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - ], - install_requires=['setuptools'], - zip_safe=True, - maintainer='Autonomous Robotics Club of Purdue', - maintainer_email='autonomy@purdue.edu', - description='Handles localized actions to plan and follow trajectories.', - license='BSD 3 Clause', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - ], - }, -) diff --git a/rktl_planner/srv/CreateBezierPath.srv b/rktl_planner/srv/CreateBezierPath.srv deleted file mode 100644 index 468e112fe..000000000 --- a/rktl_planner/srv/CreateBezierPath.srv +++ /dev/null @@ -1,8 +0,0 @@ -float64 velocity -geometry_msgs/Pose[] target_poses -std_msgs/Duration[] target_durations -std_msgs/Duration bezier_segment_duration -std_msgs/Duration linear_segment_duration ---- -rktl_msgs/BezierPath[] bezier_paths -rktl_msgs/Path linear_path diff --git a/rktl_planner/test/test_copyright.py b/rktl_planner/test/test_copyright.py deleted file mode 100644 index 97a39196e..000000000 --- a/rktl_planner/test/test_copyright.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_copyright.main import main -import pytest - - -# Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' diff --git a/rktl_planner/test/test_flake8.py b/rktl_planner/test/test_flake8.py deleted file mode 100644 index 27ee1078f..000000000 --- a/rktl_planner/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_flake8.main import main_with_errors -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ - '\n'.join(errors) diff --git a/rktl_planner/test/test_pep257.py b/rktl_planner/test/test_pep257.py deleted file mode 100644 index b234a3840..000000000 --- a/rktl_planner/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings'