From 77d3ce2b76616ed5843630f28fd650637a66342b Mon Sep 17 00:00:00 2001 From: hchengwang Date: Wed, 20 Sep 2023 07:50:11 +0800 Subject: [PATCH 01/52] add ln to docker run and join --- docker_join.bash | 1 + docker_run.bash | 1 + 2 files changed, 2 insertions(+) create mode 120000 docker_join.bash create mode 120000 docker_run.bash diff --git a/docker_join.bash b/docker_join.bash new file mode 120000 index 0000000..6fb77d8 --- /dev/null +++ b/docker_join.bash @@ -0,0 +1 @@ +docker/docker_join.bash \ No newline at end of file diff --git a/docker_run.bash b/docker_run.bash new file mode 120000 index 0000000..804015c --- /dev/null +++ b/docker_run.bash @@ -0,0 +1 @@ +docker/docker_run.bash \ No newline at end of file From 3ccd98fe3d039607e9c63efff69d2b579cc6dc81 Mon Sep 17 00:00:00 2001 From: Nick Wang Date: Wed, 20 Sep 2023 07:58:47 +0800 Subject: [PATCH 02/52] Create README.md --- README.md | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) create mode 100644 README.md diff --git a/README.md b/README.md new file mode 100644 index 0000000..d9673eb --- /dev/null +++ b/README.md @@ -0,0 +1,22 @@ +# LoCoBot-RSA + +This is the class repo for Design and Implementation of Robotic Systems and Applications. + +## Installation + +By default we use native Ubuntu 20.04 (or dual boot if you are using Windows). + +### Several tools are required + +Git: please follow +* SSH Key: https://github.com/ARG-NCTU/oop-python-nycu/blob/main/tutorials/00-ssh-key.md + +### Some tools are recommended +* NeoVim and GitHub Copilot: https://github.com/ARG-NCTU/oop-python-nycu/blob/main/tutorials/04-copilot-neovim-nodejs.md + +## To Get Started + +We will use a docker image with ROS noetic. +``` +source docker_run.sh +``` From 4f62da60e08461aa45e4641894ed271c59d90082 Mon Sep 17 00:00:00 2001 From: hchengwang Date: Wed, 20 Sep 2023 08:02:46 +0800 Subject: [PATCH 03/52] add README for locobot code --- locobot/README.md | 7 +++++++ 1 file changed, 7 insertions(+) create mode 100644 locobot/README.md diff --git a/locobot/README.md b/locobot/README.md new file mode 100644 index 0000000..1dc93d5 --- /dev/null +++ b/locobot/README.md @@ -0,0 +1,7 @@ +# Code to Run within a LoCoBot + +The code in this folder is supposed to run on NYCU LoCoBot + +## NYCU LoCoBot Settings + +The settings follows the orignial version distributed with the hardware. From 669baff50517ec75a251e2a7e452eef715cfafd5 Mon Sep 17 00:00:00 2001 From: hchengwang Date: Wed, 20 Sep 2023 08:03:24 +0800 Subject: [PATCH 04/52] update .gitignore --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index ae8578f..94d0a46 100644 --- a/.gitignore +++ b/.gitignore @@ -3,3 +3,4 @@ devel/ .catkin_tools logs __pycache__/ +tags From 8b300dcaecbff6d608889c4fabca14cb1a4aacf1 Mon Sep 17 00:00:00 2001 From: hchengwang Date: Wed, 20 Sep 2023 08:09:37 +0800 Subject: [PATCH 05/52] add host laptop/pc workspace --- low_cost_ws/README.md | 10 ++++++++++ 1 file changed, 10 insertions(+) create mode 100644 low_cost_ws/README.md diff --git a/low_cost_ws/README.md b/low_cost_ws/README.md new file mode 100644 index 0000000..b7ced75 --- /dev/null +++ b/low_cost_ws/README.md @@ -0,0 +1,10 @@ +# ROS to Run on Host Laptop/PC + +## Build the Project + +``` +cd ~/low_cost_ws +catkin_make +``` + +## Run From 7cea538c1368da71a3b8c12acb1cc08c8c35f0ea Mon Sep 17 00:00:00 2001 From: hchengwang Date: Wed, 20 Sep 2023 08:16:17 +0800 Subject: [PATCH 06/52] add mit ocw oop code --- mit-ocw-lecture-code/README.md | 4 + mit-ocw-lecture-code/lec8_classes.py | 123 ++++++++++++ mit-ocw-lecture-code/lec9_inheritance.py | 186 ++++++++++++++++++ mit-ocw-lecture-code/test_lec8_classes.py | 21 ++ mit-ocw-lecture-code/test_lec9_inheritance.py | 11 ++ 5 files changed, 345 insertions(+) create mode 100644 mit-ocw-lecture-code/README.md create mode 100644 mit-ocw-lecture-code/lec8_classes.py create mode 100644 mit-ocw-lecture-code/lec9_inheritance.py create mode 100644 mit-ocw-lecture-code/test_lec8_classes.py create mode 100644 mit-ocw-lecture-code/test_lec9_inheritance.py diff --git a/mit-ocw-lecture-code/README.md b/mit-ocw-lecture-code/README.md new file mode 100644 index 0000000..43b976e --- /dev/null +++ b/mit-ocw-lecture-code/README.md @@ -0,0 +1,4 @@ +# Lecture Code from MIT OCW + +* Introduction to Computer Science and Programming in Python +[link](https://ocw.mit.edu/courses/electrical-engineering-and-computer-science/6-0001-introduction-to-computer-science-and-programming-in-python-fall-2016/index.htm) diff --git a/mit-ocw-lecture-code/lec8_classes.py b/mit-ocw-lecture-code/lec8_classes.py new file mode 100644 index 0000000..746ebc7 --- /dev/null +++ b/mit-ocw-lecture-code/lec8_classes.py @@ -0,0 +1,123 @@ +################# +## EXAMPLE: simple Coordinate class +################# +class Coordinate(object): + """ A coordinate made up of an x and y value """ + def __init__(self, x, y): + """ Sets the x and y values """ + self.x = x + self.y = y + def __str__(self): + """ Returns a string representation of self """ + return "<" + str(self.x) + "," + str(self.y) + ">" + def distance(self, other): + """ Returns the euclidean distance between two points """ + x_diff_sq = (self.x-other.x)**2 + y_diff_sq = (self.y-other.y)**2 + return (x_diff_sq + y_diff_sq)**0.5 + + +# c = Coordinate(3,4) +# origin = Coordinate(0,0) +# print(c.x, origin.x) +# print(c.distance(origin)) +# print(Coordinate.distance(c, origin)) +# print(origin.distance(c)) +# print(c) + + +################# +## EXAMPLE: simple class to represent fractions +## Try adding more built-in operations like multiply, divide +### Try adding a reduce method to reduce the fraction (use gcd) +################# +class Fraction(object): + """ + A number represented as a fraction + """ + def __init__(self, num, denom): + """ num and denom are integers """ + assert type(num) == int and type(denom) == int, "ints not used" + self.num = num + self.denom = denom + def __str__(self): + """ Retunrs a string representation of self """ + return str(self.num) + "/" + str(self.denom) + def __add__(self, other): + """ Returns a new fraction representing the addition """ + top = self.num*other.denom + self.denom*other.num + bott = self.denom*other.denom + return Fraction(top, bott) + def __sub__(self, other): + """ Returns a new fraction representing the subtraction """ + top = self.num*other.denom - self.denom*other.num + bott = self.denom*other.denom + return Fraction(top, bott) + def __float__(self): + """ Returns a float value of the fraction """ + return self.num/self.denom + def inverse(self): + """ Returns a new fraction representing 1/self """ + return Fraction(self.denom, self.num) + +a = Fraction(1,4) +b = Fraction(3,4) +c = a + b # c is a Fraction object +print(c) +print(float(c)) +print(Fraction.__float__(c)) +print(float(b.inverse())) +##c = Fraction(3.14, 2.7) # assertion error +##print a*b # error, did not define how to multiply two Fraction objects + + +############## +## EXAMPLE: a set of integers as class +############## +class intSet(object): + """ + An intSet is a set of integers + The value is represented by a list of ints, self.vals + Each int in the set occurs in self.vals exactly once + """ + def __init__(self): + """ Create an empty set of integers """ + self.vals = [] + + def insert(self, e): + """ Assumes e is an integer and inserts e into self """ + if not e in self.vals: + self.vals.append(e) + + def member(self, e): + """ Assumes e is an integer + Returns True if e is in self, and False otherwise """ + return e in self.vals + + def remove(self, e): + """ Assumes e is an integer and removes e from self + Raises ValueError if e is not in self """ + try: + self.vals.remove(e) + except: + raise ValueError(str(e) + ' not found') + + def __str__(self): + """ Returns a string representation of self """ + self.vals.sort() + return '{' + ','.join([str(e) for e in self.vals]) + '}' + + +s = intSet() +print(s) +s.insert(3) +s.insert(4) +s.insert(3) +print(s) +s.member(3) +s.member(5) +s.insert(6) +print(s) +#s.remove(3) # leads to an error +print(s) +s.remove(3) diff --git a/mit-ocw-lecture-code/lec9_inheritance.py b/mit-ocw-lecture-code/lec9_inheritance.py new file mode 100644 index 0000000..376dfbc --- /dev/null +++ b/mit-ocw-lecture-code/lec9_inheritance.py @@ -0,0 +1,186 @@ +import random + +################################# +## Animal abstract data type +################################# +class Animal(object): + ```Animal Class + attributes: age + methods: get_age, set_age, get_name, set_name, __str__ + ``` + def __init__(self, age): + self.age = age + self.name = None + def get_age(self): + return self.age + def get_name(self): + return self.name + def set_age(self, newage): + self.age = newage + def set_name(self, newname=""): + self.name = newname + def __str__(self): + return "animal:"+str(self.name)+":"+str(self.age) + +# print("\n---- animal tests ----") +# a = Animal(4) +# print(a) +# print(a.get_age()) +# a.set_name("fluffy") +# print(a) +# a.set_name() +# print(a) + + + +################################# +## Inheritance example +################################# +class Cat(Animal): + def speak(self): + print("meow") + def __str__(self): + return "cat:"+str(self.name)+":"+str(self.age) + +print("\n---- cat tests ----") +c = Cat(5) +c.set_name("fluffy") +print(c) +c.speak() +print(c.get_age()) +#a.speak() # error because there is no speak method for Animal class + + +################################# +## Inheritance example +################################# +class Person(Animal): + def __init__(self, name, age): + Animal.__init__(self, age) + self.set_name(name) + self.friends = [] + def get_friends(self): + return self.friends + def speak(self): + print("hello") + def add_friend(self, fname): + if fname not in self.friends: + self.friends.append(fname) + def age_diff(self, other): + diff = self.age - other.age + print(abs(diff), "year difference") + def __str__(self): + return "person:"+str(self.name)+":"+str(self.age) + +print("\n---- person tests ----") +p1 = Person("jack", 30) +p2 = Person("jill", 25) +print(p1.get_name()) +print(p1.get_age()) +print(p2.get_name()) +print(p2.get_age()) +print(p1) +p1.speak() +p1.age_diff(p2) + + +################################# +## Inheritance example +################################# +class Student(Person): + def __init__(self, name, age, major=None): + Person.__init__(self, name, age) + self.major = major + def __str__(self): + return "student:"+str(self.name)+":"+str(self.age)+":"+str(self.major) + def change_major(self, major): + self.major = major + def speak(self): + r = random.random() + if r < 0.25: + print("i have homework") + elif 0.25 <= r < 0.5: + print("i need sleep") + elif 0.5 <= r < 0.75: + print("i should eat") + else: + print("i am watching tv") + +print("\n---- student tests ----") +s1 = Student('alice', 20, "CS") +s2 = Student('beth', 18) +print(s1) +print(s2) +print(s1.get_name(),"says:", end=" ") +s1.speak() +print(s2.get_name(),"says:", end=" ") +s2.speak() + + + +################################# +## Use of class variables +################################# +class Rabbit(Animal): + # a class variable, tag, shared across all instances + tag = 1 + def __init__(self, age, parent1=None, parent2=None): + Animal.__init__(self, age) + self.parent1 = parent1 + self.parent2 = parent2 + self.rid = Rabbit.tag + Rabbit.tag += 1 + def get_rid(self): + # zfill used to add leading zeroes 001 instead of 1 + return str(self.rid).zfill(3) + def get_parent1(self): + return self.parent1 + def get_parent2(self): + return self.parent2 + def __add__(self, other): + # returning object of same type as this class + return Rabbit(0, self, other) + def __eq__(self, other): + # compare the ids of self and other's parents + # don't care about the order of the parents + # the backslash tells python I want to break up my line + parents_same = self.parent1.rid == other.parent1.rid \ + and self.parent2.rid == other.parent2.rid + parents_opposite = self.parent2.rid == other.parent1.rid \ + and self.parent1.rid == other.parent2.rid + return parents_same or parents_opposite + def __str__(self): + return "rabbit:"+ self.get_rid() + +print("\n---- rabbit tests ----") +print("---- testing creating rabbits ----") +r1 = Rabbit(3) +r2 = Rabbit(4) +r3 = Rabbit(5) +print("r1:", r1) +print("r2:", r2) +print("r3:", r3) +print("r1 parent1:", r1.get_parent1()) +print("r1 parent2:", r1.get_parent2()) + +print("---- testing rabbit addition ----") +r4 = r1+r2 # r1.__add__(r2) +print("r1:", r1) +print("r2:", r2) +print("r4:", r4) +print("r4 parent1:", r4.get_parent1()) +print("r4 parent2:", r4.get_parent2()) + +print("---- testing rabbit equality ----") +r5 = r3+r4 +r6 = r4+r3 +print("r3:", r3) +print("r4:", r4) +print("r5:", r5) +print("r6:", r6) +print("r5 parent1:", r5.get_parent1()) +print("r5 parent2:", r5.get_parent2()) +print("r6 parent1:", r6.get_parent1()) +print("r6 parent2:", r6.get_parent2()) +print("r5 and r6 have same parents?", r5 == r6) +print("r4 and r6 have same parents?", r4 == r6) diff --git a/mit-ocw-lecture-code/test_lec8_classes.py b/mit-ocw-lecture-code/test_lec8_classes.py new file mode 100644 index 0000000..079669f --- /dev/null +++ b/mit-ocw-lecture-code/test_lec8_classes.py @@ -0,0 +1,21 @@ +import lec8_classes as lc + +def test_coordinate(): + c = lc.Coordinate(3, 4) + origin = lc.Coordinate(0,0) + assert c.x == 3 + assert c.y == 4 + assert c.distance(origin) == 5 + assert origin.distance(c) == 5 + +def test_intset(): + s = lc.intSet() + s.insert(3) + s.insert(4) + assert s.member(3) + assert s.member(4) + assert not s.member(5) + s.remove(3) + assert not s.member(3) + assert s.member(4) + diff --git a/mit-ocw-lecture-code/test_lec9_inheritance.py b/mit-ocw-lecture-code/test_lec9_inheritance.py new file mode 100644 index 0000000..b436304 --- /dev/null +++ b/mit-ocw-lecture-code/test_lec9_inheritance.py @@ -0,0 +1,11 @@ +import lec9_inheritance as inh +import pytest + +def test_animal(): + a = inh.Animal(4) + print(a) + print(a.get_age()) + a.set_name("fluffy") + print(a) + assert a.get_name() == "fluffy" + assert a.get_age() == 4 From 4dd6c4196490943a8e617ee35accec53493bdf32 Mon Sep 17 00:00:00 2001 From: sunfu-chou Date: Wed, 20 Sep 2023 21:16:09 +0800 Subject: [PATCH 07/52] Docker: Add pytest in dockerfile --- docker/dockerfile | 45 ++++++++++++++++++++++++--------------------- 1 file changed, 24 insertions(+), 21 deletions(-) diff --git a/docker/dockerfile b/docker/dockerfile index edbc743..f552b59 100644 --- a/docker/dockerfile +++ b/docker/dockerfile @@ -25,30 +25,33 @@ RUN echo "${USER}:rsa" | chpasswd RUN apt-get -o Acquire::ForceIPv4=true update && apt-get -yq dist-upgrade \ && apt-get -o Acquire::ForceIPv4=true install -yq --no-install-recommends \ - locales \ - curl \ - cmake \ - make \ - git \ - vim \ - wget \ - sudo \ - lsb-release \ - build-essential \ - net-tools \ - apt-utils \ - software-properties-common \ - dialog \ - libffi-dev \ - python3-dev \ - python3-pip \ - python3-setuptools \ - apt-transport-https \ - libglew-dev + locales \ + curl \ + cmake \ + make \ + git \ + vim \ + wget \ + sudo \ + lsb-release \ + build-essential \ + net-tools \ + apt-utils \ + software-properties-common \ + dialog \ + libffi-dev \ + python3-dev \ + python3-pip \ + python3-setuptools \ + apt-transport-https \ + libglew-dev RUN pip3 install --upgrade pip \ && pip3 install --upgrade setuptools \ - && pip3 install python_tsp pypozyx + && pip3 install \ + python_tsp \ + pypozyx \ + pytest RUN apt-get -o Acquire::ForceIPv4=true update && apt-get -yq dist-upgrade \ && apt-get -o Acquire::ForceIPv4=true install -yq --no-install-recommends \ From e5e7e5748188c8f3edcd9752fad3fd361268bcde Mon Sep 17 00:00:00 2001 From: sunfu-chou Date: Wed, 20 Sep 2023 21:18:18 +0800 Subject: [PATCH 08/52] Docker: Image is pushed to argnctu --- docker/build.bash | 2 +- docker/docker_join.bash | 2 +- docker/docker_run.bash | 2 +- docker/push.bash | 3 +++ 4 files changed, 6 insertions(+), 3 deletions(-) create mode 100644 docker/push.bash diff --git a/docker/build.bash b/docker/build.bash index c44728d..85ea147 100644 --- a/docker/build.bash +++ b/docker/build.bash @@ -1,6 +1,6 @@ #!/usr/bin/env bash -REPOSITORY="sunfuchou/rsa" +REPOSITORY="argnctu/rsa" TAG="amd64" IMG="${REPOSITORY}:${TAG}" diff --git a/docker/docker_join.bash b/docker/docker_join.bash index 341574c..71fc96a 100644 --- a/docker/docker_join.bash +++ b/docker/docker_join.bash @@ -1,7 +1,7 @@ #!/usr/bin/env bash NAME=locobot_rsa -REPOSITORY="sunfuchou/rsa" +REPOSITORY="argnctu/rsa" TAG="amd64" REPO_NAME=LoCoBot-RSA diff --git a/docker/docker_run.bash b/docker/docker_run.bash index 7779856..81401cd 100644 --- a/docker/docker_run.bash +++ b/docker/docker_run.bash @@ -3,7 +3,7 @@ ARGS=("$@") NAME=rsa -REPOSITORY="sunfuchou/rsa" +REPOSITORY="argnctu/rsa" TAG="amd64" REPO_NAME=LoCoBot-RSA diff --git a/docker/push.bash b/docker/push.bash new file mode 100644 index 0000000..d106ebe --- /dev/null +++ b/docker/push.bash @@ -0,0 +1,3 @@ +#!/usr/bin/env bash + +docker image push argnctu/rsa:amd64 From f16b2c239bbb65b0259c2b417f9dbc7227bf6ffd Mon Sep 17 00:00:00 2001 From: hchengwang Date: Fri, 22 Sep 2023 11:12:03 +0800 Subject: [PATCH 09/52] revise docstring --- mit-ocw-lecture-code/lec9_inheritance.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mit-ocw-lecture-code/lec9_inheritance.py b/mit-ocw-lecture-code/lec9_inheritance.py index 376dfbc..60c4ff7 100644 --- a/mit-ocw-lecture-code/lec9_inheritance.py +++ b/mit-ocw-lecture-code/lec9_inheritance.py @@ -4,10 +4,10 @@ ## Animal abstract data type ################################# class Animal(object): - ```Animal Class + '''Animal Class attributes: age methods: get_age, set_age, get_name, set_name, __str__ - ``` + ''' def __init__(self, age): self.age = age self.name = None From 86769585bca0835d48cd4f191274836148ce80ee Mon Sep 17 00:00:00 2001 From: hchengwang Date: Fri, 22 Sep 2023 11:19:04 +0800 Subject: [PATCH 10/52] add how to run pytest --- mit-ocw-lecture-code/README.md | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/mit-ocw-lecture-code/README.md b/mit-ocw-lecture-code/README.md index 43b976e..e2f4297 100644 --- a/mit-ocw-lecture-code/README.md +++ b/mit-ocw-lecture-code/README.md @@ -2,3 +2,12 @@ * Introduction to Computer Science and Programming in Python [link](https://ocw.mit.edu/courses/electrical-engineering-and-computer-science/6-0001-introduction-to-computer-science-and-programming-in-python-fall-2016/index.htm) + +## Run Pytest + +To run a test_xxx.py file, +``` +pytest -k test_lec8_classes.py +``` +Where -k is to show all print() + From e6585ed17b6cbb924921d0f0265b8db075ef1687 Mon Sep 17 00:00:00 2001 From: hchengwang Date: Fri, 22 Sep 2023 14:13:02 +0800 Subject: [PATCH 11/52] add test cat class via GitHub Copilot --- mit-ocw-lecture-code/test_lec9_inheritance.py | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/mit-ocw-lecture-code/test_lec9_inheritance.py b/mit-ocw-lecture-code/test_lec9_inheritance.py index b436304..3eb8a68 100644 --- a/mit-ocw-lecture-code/test_lec9_inheritance.py +++ b/mit-ocw-lecture-code/test_lec9_inheritance.py @@ -9,3 +9,14 @@ def test_animal(): print(a) assert a.get_name() == "fluffy" assert a.get_age() == 4 + +def test_cat(): + c = inh.Cat(5) + print(c) + print(c.get_age()) + c.set_name("fluffy") + print(c) + assert c.get_name() == "fluffy" + assert c.get_age() == 5 + print(c.speak()) + #assert c.speak() == 'meow' From 447eb8bede0c6702c939424670f2921a4e9fd2df Mon Sep 17 00:00:00 2001 From: hchengwang Date: Fri, 22 Sep 2023 14:43:59 +0800 Subject: [PATCH 12/52] add testing code for Person class via Copilot --- mit-ocw-lecture-code/lec9_inheritance.py | 19 +++++++-------- mit-ocw-lecture-code/test_lec9_inheritance.py | 23 +++++++++++++++++++ 2 files changed, 31 insertions(+), 11 deletions(-) diff --git a/mit-ocw-lecture-code/lec9_inheritance.py b/mit-ocw-lecture-code/lec9_inheritance.py index 60c4ff7..cafef38 100644 --- a/mit-ocw-lecture-code/lec9_inheritance.py +++ b/mit-ocw-lecture-code/lec9_inheritance.py @@ -37,6 +37,14 @@ def __str__(self): ## Inheritance example ################################# class Cat(Animal): + ''' + Cat class inherits from Animal class + + Attributes: + name + age + Methods: speak + ''' def speak(self): print("meow") def __str__(self): @@ -72,17 +80,6 @@ def age_diff(self, other): def __str__(self): return "person:"+str(self.name)+":"+str(self.age) -print("\n---- person tests ----") -p1 = Person("jack", 30) -p2 = Person("jill", 25) -print(p1.get_name()) -print(p1.get_age()) -print(p2.get_name()) -print(p2.get_age()) -print(p1) -p1.speak() -p1.age_diff(p2) - ################################# ## Inheritance example diff --git a/mit-ocw-lecture-code/test_lec9_inheritance.py b/mit-ocw-lecture-code/test_lec9_inheritance.py index 3eb8a68..f25d635 100644 --- a/mit-ocw-lecture-code/test_lec9_inheritance.py +++ b/mit-ocw-lecture-code/test_lec9_inheritance.py @@ -20,3 +20,26 @@ def test_cat(): assert c.get_age() == 5 print(c.speak()) #assert c.speak() == 'meow' + +def test_person(): + p = inh.Person("Captain", 30) + print(p) + print(p.get_age()) + p.set_name("Captain") + print(p) + assert p.get_name() == "Captain" + assert p.get_age() == 30 + + print("\n---- person tests ----") + p1 = inh.Person("jack", 30) + p2 = inh.Person("jill", 25) + print(p1.get_name()) + print(p1.get_age()) + print(p2.get_name()) + print(p2.get_age()) + print(p1) + p1.speak() + p1.age_diff(p2) + + + From 814b1d669337743bd886c236215d0db4b7087135 Mon Sep 17 00:00:00 2001 From: wellyowo Date: Tue, 3 Oct 2023 14:19:32 +0800 Subject: [PATCH 13/52] Feat: update environment directly set_ip 127.0.0.1 --- locobot/environment.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/locobot/environment.sh b/locobot/environment.sh index d868cd4..9f19e7c 100644 --- a/locobot/environment.sh +++ b/locobot/environment.sh @@ -2,4 +2,4 @@ source /opt/ros/melodic/setup.bash source ~/WFH_locobot/ROS/catkin_ws/devel/setup.bash -source ~/WFH_locobot/set_ip.sh $1 $2 \ No newline at end of file +source ~/WFH_locobot/set_ip.sh 127.0.0.1 127.0.0.1 From 6a17331eb7a84b3ba69b4bd3f3ac34590781d11a Mon Sep 17 00:00:00 2001 From: sunfu-chou Date: Tue, 3 Oct 2023 14:37:11 +0800 Subject: [PATCH 14/52] Docker: Add python pkg scipy opencv into docker --- docker/dockerfile | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/docker/dockerfile b/docker/dockerfile index f552b59..4ae1a59 100644 --- a/docker/dockerfile +++ b/docker/dockerfile @@ -51,10 +51,13 @@ RUN pip3 install --upgrade pip \ && pip3 install \ python_tsp \ pypozyx \ - pytest + pytest \ + scipy \ + opencv-python \ + dbg RUN apt-get -o Acquire::ForceIPv4=true update && apt-get -yq dist-upgrade \ - && apt-get -o Acquire::ForceIPv4=true install -yq --no-install-recommends \ - ros-${ROS_DISTRO}-robot-localization +&& apt-get -o Acquire::ForceIPv4=true install -yq --no-install-recommends \ +ros-${ROS_DISTRO}-robot-localization RUN echo 'source /opt/ros/noetic/setup.bash' >> ~/.bashrc From d374d90c2131ef1c31ccdd8b091f6bd916266763 Mon Sep 17 00:00:00 2001 From: wellyowo Date: Tue, 3 Oct 2023 16:09:18 +0800 Subject: [PATCH 15/52] Feat: update set ip with 127.0.0.1 --- locobot/set_wfh_workspace_env.sh | 2 +- locobot/top_camera.sh | 2 +- locobot/turn_on_locobot.sh | 2 +- locobot/vr_arm_control.sh | 3 ++- 4 files changed, 5 insertions(+), 4 deletions(-) diff --git a/locobot/set_wfh_workspace_env.sh b/locobot/set_wfh_workspace_env.sh index 93fb94f..cec0c99 100644 --- a/locobot/set_wfh_workspace_env.sh +++ b/locobot/set_wfh_workspace_env.sh @@ -3,7 +3,7 @@ # load pyrobot env load_pyrobot_env # source WFH workspace and set_rospkg_path -source ~/WFH_locobot/set_ip.sh $1 $2 +source ~/WFH_locobot/set_ip.sh 127.0.0.1 127.0.0.1 #source ROS/catkin_ws/devel/setup.bash #source ROS/catkin_ws/devel_isolated/setup.bash source ~/WFH_locobot/set_rospackage_path.sh diff --git a/locobot/top_camera.sh b/locobot/top_camera.sh index 8c8d834..1a9627d 100644 --- a/locobot/top_camera.sh +++ b/locobot/top_camera.sh @@ -1,7 +1,7 @@ #!/bin/bash source ~/WFH_locobot/environment.sh -source ~/WFH_locobot/set_ip.sh $1 $2 +source ~/WFH_locobot/set_ip.sh 127.0.0.1 127.0.0.1 rosservice call /calibration #rostopic pub /tilt/command std_msgs/Float64 "data: 0.8" #rostopic pub /pan/command std_msgs/Float64 "data: 0.0" diff --git a/locobot/turn_on_locobot.sh b/locobot/turn_on_locobot.sh index 8f95c3d..8ad764d 100644 --- a/locobot/turn_on_locobot.sh +++ b/locobot/turn_on_locobot.sh @@ -1,4 +1,4 @@ #! /bin/bash source ~/WFH_locobot/environment.sh -source ~/WFH_locobot/set_ip.sh $1 $2 +source ~/WFH_locobot/set_ip.sh 127.0.0.1 127.0.0.1 source ~/WFH_locobot/run_locobot.sh diff --git a/locobot/vr_arm_control.sh b/locobot/vr_arm_control.sh index 1719a57..d718c66 100644 --- a/locobot/vr_arm_control.sh +++ b/locobot/vr_arm_control.sh @@ -2,7 +2,8 @@ cd ~/WFH_locobot source ~/WFH_locobot/set_wfh_workspace_env.sh -source ~/WFH_locobot/set_ip.sh $1 $2 +source ~/WFH_locobot/set_ip.sh 127.0.0.1 127.0.0.1 + rosrun oculusVR vrarm.py From 07124c6c3ae6a20e062cfd25a84cc27d66cd3083 Mon Sep 17 00:00:00 2001 From: sunfu-chou Date: Tue, 3 Oct 2023 16:50:15 +0800 Subject: [PATCH 16/52] Docker: Fix Image tag from amd64 to latest --- docker/build.bash | 2 +- docker/docker_join.bash | 2 +- docker/docker_run.bash | 2 +- docker/push.bash | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/docker/build.bash b/docker/build.bash index 85ea147..ded1f8a 100644 --- a/docker/build.bash +++ b/docker/build.bash @@ -1,7 +1,7 @@ #!/usr/bin/env bash REPOSITORY="argnctu/rsa" -TAG="amd64" +TAG="latest" IMG="${REPOSITORY}:${TAG}" diff --git a/docker/docker_join.bash b/docker/docker_join.bash index 71fc96a..0f103da 100644 --- a/docker/docker_join.bash +++ b/docker/docker_join.bash @@ -2,7 +2,7 @@ NAME=locobot_rsa REPOSITORY="argnctu/rsa" -TAG="amd64" +TAG="latest" REPO_NAME=LoCoBot-RSA IMG="${REPOSITORY}:${TAG}" diff --git a/docker/docker_run.bash b/docker/docker_run.bash index 81401cd..f223f4f 100644 --- a/docker/docker_run.bash +++ b/docker/docker_run.bash @@ -4,7 +4,7 @@ ARGS=("$@") NAME=rsa REPOSITORY="argnctu/rsa" -TAG="amd64" +TAG="latest" REPO_NAME=LoCoBot-RSA IMG="${REPOSITORY}:${TAG}" diff --git a/docker/push.bash b/docker/push.bash index d106ebe..10bc484 100644 --- a/docker/push.bash +++ b/docker/push.bash @@ -1,3 +1,3 @@ #!/usr/bin/env bash -docker image push argnctu/rsa:amd64 +docker image push argnctu/rsa:latest From e449b667bc0b30a1a4921a12924ccedf8a814825 Mon Sep 17 00:00:00 2001 From: hchengwang Date: Tue, 3 Oct 2023 17:04:47 +0800 Subject: [PATCH 17/52] add rostest_example --- .../src/rostest_example/CMakeLists.txt | 195 ++++++++++++++++++ .../include/rostest_example/Quacker.py | 14 ++ .../include/rostest_example/__init__.py | 0 .../include/rostest_example/test_Quacker.py | 11 + .../launch/average_quacks_node.launch | 8 + .../launch/duckiecall_node.launch | 8 + low_cost_ws/src/rostest_example/package.xml | 55 +++++ low_cost_ws/src/rostest_example/setup.py | 10 + .../src/average_quacks_node.py | 31 +++ .../rostest_example/src/duckiecall_node.py | 33 +++ .../tests/duckiecall_tester_node.py | 61 ++++++ .../tests/duckiecall_tester_node.test | 9 + .../rostest_example/tests/quacker_tester.py | 24 +++ .../rostest_example/tests/quacker_tester.test | 3 + 14 files changed, 462 insertions(+) create mode 100644 low_cost_ws/src/rostest_example/CMakeLists.txt create mode 100644 low_cost_ws/src/rostest_example/include/rostest_example/Quacker.py create mode 100644 low_cost_ws/src/rostest_example/include/rostest_example/__init__.py create mode 100644 low_cost_ws/src/rostest_example/include/rostest_example/test_Quacker.py create mode 100644 low_cost_ws/src/rostest_example/launch/average_quacks_node.launch create mode 100644 low_cost_ws/src/rostest_example/launch/duckiecall_node.launch create mode 100644 low_cost_ws/src/rostest_example/package.xml create mode 100644 low_cost_ws/src/rostest_example/setup.py create mode 100755 low_cost_ws/src/rostest_example/src/average_quacks_node.py create mode 100755 low_cost_ws/src/rostest_example/src/duckiecall_node.py create mode 100755 low_cost_ws/src/rostest_example/tests/duckiecall_tester_node.py create mode 100644 low_cost_ws/src/rostest_example/tests/duckiecall_tester_node.test create mode 100755 low_cost_ws/src/rostest_example/tests/quacker_tester.py create mode 100644 low_cost_ws/src/rostest_example/tests/quacker_tester.test diff --git a/low_cost_ws/src/rostest_example/CMakeLists.txt b/low_cost_ws/src/rostest_example/CMakeLists.txt new file mode 100644 index 0000000..ae678db --- /dev/null +++ b/low_cost_ws/src/rostest_example/CMakeLists.txt @@ -0,0 +1,195 @@ +cmake_minimum_required(VERSION 2.8.3) +project(rostest_example) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a run_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a run_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES rostest_example +# CATKIN_DEPENDS roscpp rospy +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +# include_directories(include) +include_directories( + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(rostest_example +# src/${PROJECT_NAME}/rostest_example.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(rostest_example ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +# add_executable(rostest_example_node src/rostest_example_node.cpp) + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(rostest_example_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(rostest_example_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS rostest_example rostest_example_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_rostest_example.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) + +if (CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest(tests/quacker_tester.test) + add_rostest(tests/average_quacks_tester_node.test) + add_rostest(tests/duckiecall_tester_node.test) + add_rostest(tests/rostest_example_tester_node.test) +endif() \ No newline at end of file diff --git a/low_cost_ws/src/rostest_example/include/rostest_example/Quacker.py b/low_cost_ws/src/rostest_example/include/rostest_example/Quacker.py new file mode 100644 index 0000000..4394fd6 --- /dev/null +++ b/low_cost_ws/src/rostest_example/include/rostest_example/Quacker.py @@ -0,0 +1,14 @@ +#!/usr/bin/env python +import numpy as np + +class Quacker(object): + def __init__(self, quack="Quack!"): + self.quack = quack + + def rounded_mean(self, x): + # Returns the mean of x, rounded to the nearest integer + return np.round(np.mean(np.array(x))) + + def get_quack_string(self, n): + # Returns a string of n quacks based on the value in Quacker.quack + return ' '.join([self.quack]*n) \ No newline at end of file diff --git a/low_cost_ws/src/rostest_example/include/rostest_example/__init__.py b/low_cost_ws/src/rostest_example/include/rostest_example/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/low_cost_ws/src/rostest_example/include/rostest_example/test_Quacker.py b/low_cost_ws/src/rostest_example/include/rostest_example/test_Quacker.py new file mode 100644 index 0000000..bf64665 --- /dev/null +++ b/low_cost_ws/src/rostest_example/include/rostest_example/test_Quacker.py @@ -0,0 +1,11 @@ +from .Quacker import Quacker +import pytest + +# test get_quack_string function +def test_get_quack_string(): + # initialize a Quacker object + quacker = Quacker() + # test the get_quack_string function + assert quacker.get_quack_string(1) == "Quack!" + + diff --git a/low_cost_ws/src/rostest_example/launch/average_quacks_node.launch b/low_cost_ws/src/rostest_example/launch/average_quacks_node.launch new file mode 100644 index 0000000..a53a23d --- /dev/null +++ b/low_cost_ws/src/rostest_example/launch/average_quacks_node.launch @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/low_cost_ws/src/rostest_example/launch/duckiecall_node.launch b/low_cost_ws/src/rostest_example/launch/duckiecall_node.launch new file mode 100644 index 0000000..5b06f89 --- /dev/null +++ b/low_cost_ws/src/rostest_example/launch/duckiecall_node.launch @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/low_cost_ws/src/rostest_example/package.xml b/low_cost_ws/src/rostest_example/package.xml new file mode 100644 index 0000000..c485e46 --- /dev/null +++ b/low_cost_ws/src/rostest_example/package.xml @@ -0,0 +1,55 @@ + + + rostest_example + 0.0.0 + The rostest_example package + + + + + Teddy Ort + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + rospy + roscpp + rospy + + unittest + + + + + + + \ No newline at end of file diff --git a/low_cost_ws/src/rostest_example/setup.py b/low_cost_ws/src/rostest_example/setup.py new file mode 100644 index 0000000..950ba87 --- /dev/null +++ b/low_cost_ws/src/rostest_example/setup.py @@ -0,0 +1,10 @@ +## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + packages=['rostest_example'], + package_dir={'': 'include'}, +) +setup(**setup_args) \ No newline at end of file diff --git a/low_cost_ws/src/rostest_example/src/average_quacks_node.py b/low_cost_ws/src/rostest_example/src/average_quacks_node.py new file mode 100755 index 0000000..2578fcd --- /dev/null +++ b/low_cost_ws/src/rostest_example/src/average_quacks_node.py @@ -0,0 +1,31 @@ +#!/usr/bin/env python +import rospy +from std_msgs.msg import Float32MultiArray, Int32 +from rostest_example.Quacker import * + +# Average Quacks Node +# Author: Teddy Ort +# Inputs: ~list/Float32MultiArray - A list of quacks to average +# Outputs: ~number_of_quacks/Int32 - The rounded average of the list received + +class AverageQuacksNode(object): + def __init__(self): + self.node_name = 'average_quacks_node' + rospy.loginfo("[%s] has started", self.node_name) + + # Setup the publisher and subscriber + self.sub_list = rospy.Subscriber("~list", Float32MultiArray, self.listCallback) + self.pub_quacks = rospy.Publisher("~number_of_quacks", Int32, queue_size=1) + + # Setup the quacker + self.quacker = Quacker() + + def listCallback(self, msg): + msg_num_of_quacks = Int32() + msg_num_of_quacks.data = self.quacker.rounded_mean(msg.data) + self.pub_quacks.publish(msg_num_of_quacks) + +if __name__ == '__main__': + rospy.init_node('average_quacks_node', anonymous=False) + average_quacks_node = AverageQuacksNode() + rospy.spin() \ No newline at end of file diff --git a/low_cost_ws/src/rostest_example/src/duckiecall_node.py b/low_cost_ws/src/rostest_example/src/duckiecall_node.py new file mode 100755 index 0000000..e067fdc --- /dev/null +++ b/low_cost_ws/src/rostest_example/src/duckiecall_node.py @@ -0,0 +1,33 @@ +#!/usr/bin/env python +import rospy +from rostest_example.Quacker import * +from std_msgs.msg import String, Int32 + +# Duckiecall Node +# Author: Teddy Ort +# Inputs: ~number_of_quacks/Int32 - The number of quacks that should be in the message +# Outputs: ~duckiecall/String - The output duckiecall message containing a series of quacks + +class DuckiecallNode(object): + def __init__(self): + self.node_name = 'duckiecall_node' + + # Setup the publisher and subscriber + self.sub_num_of_quacks = rospy.Subscriber("~number_of_quacks", Int32, self.quacksCallback) + self.pub_duckiecall = rospy.Publisher("~duckiecall", String, queue_size=1) + + # Setup the quacker + self.quacker = Quacker() + + rospy.loginfo("[%s] has started", self.node_name) + + def quacksCallback(self, msg_quacks): + msg_duckiecall = String() + msg_duckiecall.data = self.quacker.get_quack_string(msg_quacks.data) + self.pub_duckiecall.publish(msg_duckiecall) + + +if __name__ == '__main__': + rospy.init_node('duckiecall_node', anonymous=False) + duckiecall_node = DuckiecallNode() + rospy.spin() \ No newline at end of file diff --git a/low_cost_ws/src/rostest_example/tests/duckiecall_tester_node.py b/low_cost_ws/src/rostest_example/tests/duckiecall_tester_node.py new file mode 100755 index 0000000..3951e64 --- /dev/null +++ b/low_cost_ws/src/rostest_example/tests/duckiecall_tester_node.py @@ -0,0 +1,61 @@ +#!/usr/bin/env python +import rospy +import unittest, rostest +from rostest_example.Quacker import * +from std_msgs.msg import String, Int32 + +class DuckiecallTesterNode(unittest.TestCase): + def __init__(self, *args): + super(DuckiecallTesterNode, self).__init__(*args) + self.msg_duckiecall = String() + self.msg_received = False + + def setup(self): + # Setup the node + rospy.init_node('duckiecall_tester_node', anonymous=False) + + # Setup the publisher and subscriber + self.pub_num_of_quacks = rospy.Publisher("~number_of_quacks", Int32, queue_size=1, latch=True) + self.sub_duckiecall = rospy.Subscriber("~duckiecall", String, self.duckiecallCallback) + + # Wait for the node to finish starting up + timeout = rospy.Time.now() + rospy.Duration(5) # Wait at most 5 seconds for the node to come up + while (self.pub_num_of_quacks.get_num_connections() < 1 or self.sub_duckiecall.get_num_connections() < 1) and \ + not rospy.is_shutdown() and rospy.Time.now() < timeout: + rospy.sleep(0.1) + + def duckiecallCallback(self, msg_duckiecall): + self.msg_duckiecall = msg_duckiecall + self.msg_received = True + + def test_publisher_and_subscriber(self): + self.setup() # Setup the node + self.assertGreaterEqual(self.pub_num_of_quacks.get_num_connections(), 1, "No connections found on num_of_quacks topic") + self.assertGreaterEqual(self.sub_duckiecall.get_num_connections(), 1, "No connections found on duckiecall topic") + + def test_duckiecall_output(self): + self.setup() # Setup the node + + # Send the message to the number_of_quacks topic + msg_num_of_quacks = Int32() + msg_num_of_quacks.data = 3 + self.pub_num_of_quacks.publish(msg_num_of_quacks) + rospy.loginfo("Published %d quacks to the number_of_quacks topic", msg_num_of_quacks.data) + + # Wait for the message to be received + timeout = rospy.Time.now() + rospy.Duration(5) # Wait at most 5 seconds for the node to reply + while not self.msg_received and not rospy.is_shutdown() and rospy.Time.now() < timeout: + rospy.sleep(0.1) + + rospy.loginfo("Received %s from the duckiecall topic", self.msg_duckiecall.data) + + # Send an error if the timeout was hit + self.assertLess(rospy.Time.now(), timeout, "The test timed out with no response from the duckiecall_node") + + # Test the response + response = self.msg_duckiecall.data + self.assertEqual(response, "Quack! Quack! Quack!") # Three Quacks! expected + +if __name__ == '__main__': + rospy.init_node('duckiecall_tester_node', anonymous=False) + rostest.rosrun('rostest_example', 'duckiecall_tester_node', DuckiecallTesterNode) diff --git a/low_cost_ws/src/rostest_example/tests/duckiecall_tester_node.test b/low_cost_ws/src/rostest_example/tests/duckiecall_tester_node.test new file mode 100644 index 0000000..8d67383 --- /dev/null +++ b/low_cost_ws/src/rostest_example/tests/duckiecall_tester_node.test @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/low_cost_ws/src/rostest_example/tests/quacker_tester.py b/low_cost_ws/src/rostest_example/tests/quacker_tester.py new file mode 100755 index 0000000..355ff79 --- /dev/null +++ b/low_cost_ws/src/rostest_example/tests/quacker_tester.py @@ -0,0 +1,24 @@ +#!/usr/bin/env python +import unittest, rosunit +from rostest_example.Quacker import * + +class QuackerTester(unittest.TestCase): + def test_quacker_default_quack(self): + quacker = Quacker() + self.assertEqual(quacker.quack, "Quack!") + + def test_get_quack_string(self): + quacker = Quacker("Quack!") + msg = quacker.get_quack_string(3) + self.assertEqual(msg, "Quack! Quack! Quack!") + # test with parameter 1 + msg = quacker.get_quack_string(1) + self.assertEqual(msg, "Quack!") + + def test_rounded_average(self): + quacker = Quacker() + x = quacker.rounded_mean([1, 1, 2, 3]) # Average is 7/4 ~ 2 + self.assertEqual(x,2) + +if __name__ == '__main__': + rosunit.unitrun('rostest_example', 'quacker_tester', QuackerTester) diff --git a/low_cost_ws/src/rostest_example/tests/quacker_tester.test b/low_cost_ws/src/rostest_example/tests/quacker_tester.test new file mode 100644 index 0000000..3b08ddd --- /dev/null +++ b/low_cost_ws/src/rostest_example/tests/quacker_tester.test @@ -0,0 +1,3 @@ + + + \ No newline at end of file From 5672d79f6ee83f0d74b6f63e5b335cc7851738cc Mon Sep 17 00:00:00 2001 From: hchengwang Date: Tue, 3 Oct 2023 17:09:36 +0800 Subject: [PATCH 18/52] update CMakeLists.txt --- low_cost_ws/src/rostest_example/CMakeLists.txt | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/low_cost_ws/src/rostest_example/CMakeLists.txt b/low_cost_ws/src/rostest_example/CMakeLists.txt index ae678db..372f550 100644 --- a/low_cost_ws/src/rostest_example/CMakeLists.txt +++ b/low_cost_ws/src/rostest_example/CMakeLists.txt @@ -189,7 +189,7 @@ include_directories( if (CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) add_rostest(tests/quacker_tester.test) - add_rostest(tests/average_quacks_tester_node.test) + #add_rostest(tests/average_quacks_tester_node.test) add_rostest(tests/duckiecall_tester_node.test) - add_rostest(tests/rostest_example_tester_node.test) -endif() \ No newline at end of file + #add_rostest(tests/rostest_example_tester_node.test) +endif() From 47ced0192d2d96d31df3598e2a1c83281e477187 Mon Sep 17 00:00:00 2001 From: hchengwang Date: Tue, 3 Oct 2023 17:34:21 +0800 Subject: [PATCH 19/52] rm unused average_quacks --- .../launch/average_quacks_node.launch | 8 ----- .../src/average_quacks_node.py | 31 ------------------- 2 files changed, 39 deletions(-) delete mode 100644 low_cost_ws/src/rostest_example/launch/average_quacks_node.launch delete mode 100755 low_cost_ws/src/rostest_example/src/average_quacks_node.py diff --git a/low_cost_ws/src/rostest_example/launch/average_quacks_node.launch b/low_cost_ws/src/rostest_example/launch/average_quacks_node.launch deleted file mode 100644 index a53a23d..0000000 --- a/low_cost_ws/src/rostest_example/launch/average_quacks_node.launch +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - diff --git a/low_cost_ws/src/rostest_example/src/average_quacks_node.py b/low_cost_ws/src/rostest_example/src/average_quacks_node.py deleted file mode 100755 index 2578fcd..0000000 --- a/low_cost_ws/src/rostest_example/src/average_quacks_node.py +++ /dev/null @@ -1,31 +0,0 @@ -#!/usr/bin/env python -import rospy -from std_msgs.msg import Float32MultiArray, Int32 -from rostest_example.Quacker import * - -# Average Quacks Node -# Author: Teddy Ort -# Inputs: ~list/Float32MultiArray - A list of quacks to average -# Outputs: ~number_of_quacks/Int32 - The rounded average of the list received - -class AverageQuacksNode(object): - def __init__(self): - self.node_name = 'average_quacks_node' - rospy.loginfo("[%s] has started", self.node_name) - - # Setup the publisher and subscriber - self.sub_list = rospy.Subscriber("~list", Float32MultiArray, self.listCallback) - self.pub_quacks = rospy.Publisher("~number_of_quacks", Int32, queue_size=1) - - # Setup the quacker - self.quacker = Quacker() - - def listCallback(self, msg): - msg_num_of_quacks = Int32() - msg_num_of_quacks.data = self.quacker.rounded_mean(msg.data) - self.pub_quacks.publish(msg_num_of_quacks) - -if __name__ == '__main__': - rospy.init_node('average_quacks_node', anonymous=False) - average_quacks_node = AverageQuacksNode() - rospy.spin() \ No newline at end of file From 1b933b007b9e1a2cae169537c6b4617fb4c48dce Mon Sep 17 00:00:00 2001 From: hchengwang Date: Tue, 3 Oct 2023 19:18:38 +0800 Subject: [PATCH 20/52] modify shebang to python3 --- low_cost_ws/src/rostest_example/src/duckiecall_node.py | 5 +++-- .../src/rostest_example/tests/duckiecall_tester_node.py | 2 +- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/low_cost_ws/src/rostest_example/src/duckiecall_node.py b/low_cost_ws/src/rostest_example/src/duckiecall_node.py index e067fdc..f9a744c 100755 --- a/low_cost_ws/src/rostest_example/src/duckiecall_node.py +++ b/low_cost_ws/src/rostest_example/src/duckiecall_node.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 import rospy from rostest_example.Quacker import * from std_msgs.msg import String, Int32 @@ -20,6 +20,7 @@ def __init__(self): self.quacker = Quacker() rospy.loginfo("[%s] has started", self.node_name) + breakpoint() def quacksCallback(self, msg_quacks): msg_duckiecall = String() @@ -30,4 +31,4 @@ def quacksCallback(self, msg_quacks): if __name__ == '__main__': rospy.init_node('duckiecall_node', anonymous=False) duckiecall_node = DuckiecallNode() - rospy.spin() \ No newline at end of file + rospy.spin() diff --git a/low_cost_ws/src/rostest_example/tests/duckiecall_tester_node.py b/low_cost_ws/src/rostest_example/tests/duckiecall_tester_node.py index 3951e64..1de2b73 100755 --- a/low_cost_ws/src/rostest_example/tests/duckiecall_tester_node.py +++ b/low_cost_ws/src/rostest_example/tests/duckiecall_tester_node.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 import rospy import unittest, rostest from rostest_example.Quacker import * From 135f89f62083b074a7427223428090416d5f8845 Mon Sep 17 00:00:00 2001 From: wellyowo Date: Tue, 3 Oct 2023 19:24:26 +0800 Subject: [PATCH 21/52] Feat: remove .pyc file add .pyc file type into .gitignore --- .gitignore | 1 + locobot/teleoperation.pyc | Bin 13040 -> 0 bytes 2 files changed, 1 insertion(+) delete mode 100644 locobot/teleoperation.pyc diff --git a/.gitignore b/.gitignore index 94d0a46..46b93f8 100644 --- a/.gitignore +++ b/.gitignore @@ -4,3 +4,4 @@ devel/ logs __pycache__/ tags +*.pyc diff --git a/locobot/teleoperation.pyc b/locobot/teleoperation.pyc deleted file mode 100644 index bad2280092694ee53db0c5e79ad50e229a6b5f3f..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 13040 zcmdU0TWlQHc|NndB*i5uQKCeOx{f5vUfLFATaq0+P84djqGVGfJzUk&y6JehGrJse zcV{&-D~gH*G?9}6eW?4`)_@z-ZPE1Z25kcrg%h-BQ4}bUm!d#XAiV^=qCnBhW8Lrj z&&+a3%SmIp4_#`{p38r3|M~CdDEsdt`KNx*@Tw~LWbykse52EbQm#@Jq=s@cs$r-K zGCH48l}wt?s!BG^=Ts$^=KEBoFU|L>N`IOkP?cS2eo$2g(|lf4@{-T0#*k_btIDu4 zIA2aRMpSdRs_fR~KGoQxnxm>Rs>}VVu~#+ssmeap98;As)!eTt`*quZY8+6_aa9>d zd6%k8C>L`cR4!(lRPKAHBf6%M`RGzn7BVV)A=AI-S9$%H36a=0?y7 z&AFf$Tnge7`7UdV?*)Nnej>WbM2s@#KJ4O(#+G`1pTS_9?t z*MmFQj_HI!r{%to&p$n7mRjx^uU7M_aS-Os&Cj}7c;fC9Y8+%XrqB?Xg)?`Naq+vs z7X7)0_IqrEG)FWmWl)31%&XoN|Y!;m+$U~qz7_03N z+Q)KrE!89Z(+$?K z6scQws%ugyQ#QO>Y!>29*fN>LETLatNvkA3WeQ}zShnWQ6wNp=al~;xJ_t$@+T5fT zklB(jK^1NC0X3;h_(oqx;(@2Y+#+7yglvI4$q1uCm}DgbNs^OHpYk9*`j~$J$N}Z{ zOFhKMfMf=h2hp)hahxH^4@!Pm@_EURklV?~yUFm(>>M!j`X-OsAnP^%gB zhU}5~EOunPp_H9duewUTa?=MqHr}V61%qRs&)&}|F!(AqJ;2=)c2`=nE3Fw!YX;Mr zJRw(i3H*?Xhbi$eg06%NWJYkt3}o<-)C>_fZ{#pYAL{K98FsfcVNoM$GTtMFQS~6J z46du&xEqaorEy=kaV+h*2aWrQQ(*H!MmBsv#_mGw`!K}#R*10=LriRi*bfMZ_TE`0 zw?Z5M1P-|u;*dZbPFFe(h$C1d%l1*J-KE054AC^AR*$LG<0>}Q>IvmSmtZz%5vY&} zRqrF}-NO{jI8&pAvqzsH6`n{tpH!=-s8!OoK@~b_+aqfAbh?HkY7#>cOPmrdkESh0 zbqmG@v2cY26;Da#@wDa%E`Uu>D)*RV&M5b|WTsVoR;`}p5(w~=a!tuR&2;q{SsHVC zpXbyMXQlbPy5GltJnjq1J;8k%n188o668J~6_B1?37X#7$Z4K^^~zN{k@06C<7xJ8XwlFVa-|l!F&yM_io-exn&_~1DFP)FSXY+-aD!jYA zOEXI)ySQ-u`po?0c)Yh|Y353)d@EhzmZp~$uFaRzUOqah`oFyLjbmT_=Wo4VxA^>Yt6n=cX%=`<98FJ87oG*Q%n7@wB^k<8$uvM8g3u+W<|V|E1$ebq z7u)BG)4)m7a)NQu-jD^?z1VI$q0_V|jji2Gon=4-ZK1(- zQm|LgZ!2H0(19`C{5)0jU$k)qy1<%xo$J4nKke{JgC@jo7*-WQTLqH?RG=D_Mm{0jwlemg;UcM+LW#DXp1qm zg$M>cGZywPYV@IJtK1~NL~N1uXOYmr#{n7&(iC?616>reX%(jma~=Os4`X@rh($_< zj;qi1vlzxA0a=%sl$bn&L~omnC6Yl)Z{HOP&;j9eqTr)0MZ9WM&p1;oVSi}6ykE-@#DfSWUPl_qD%SK zd4fEGWRtZ;w(F&9k-sx!km|a~_%#-K(7;u*$dw8jegYXBZ3xfGP%Sug8Txmc`g!W9 zEP#vW!tcuw9}o%?_=4}*4F3+kUAJL!{{%S2AlmxW8b1RKSn zb&8-@k;utagZ74X4JDaa%buHT}z04Yr6@N8s&=q-mQE+0gA^cdAS&I~8@C4G=! z?yLes^VD=6KAs#uYY>~rDV!dTP3YXvbWUpNIgNOIBc%=MBrW4(rvH49OMi5#1W5P+mhRYEu@ z)Y9{LG+HkZf+i5p7>1laQ{gLONS|VrITy*!PKvxDCxv-_ANhz>h{M653>w3*YDB?v z+W+SVW7V*SZO35MSts&K5HDhr7!lnCtNH&D^W;FmJk9ezM&aS8r^P|^7YXq$5|T6i z^%yY_YisbZUZmqhl_qQ+}x3{_O$1A4hShI<^bgUUxdoLzk zMjrzxEqf`YiVJgd*Om|$uq$@?s#RLLx-fTHq~pt_TlV6@l3j^T|1ajOHG&{43;}Y} z46dh{pH9=8)a<8m@}b;X0jtbNsQ zqZ~0Ftojc_3=d>Rja=>+JeUy!Ih4edBFA%~LV6&UP11V+G7(dX>JtalAxMi*y@+K(Vng-H43PU!mYO!TJ+5UnSCiNzWSv$AVR#>b z0G}8Vjar~xu2tAFa)RVq1NODo+B_7A$x*;~XN%1%l1=3~=VmUo_IY|YvieyTbV3O<7&d!EIiTLI! z{u6~nd=DCr({ne*v(*pzUW_B^qk;7T2EvKp2q-n7$P?$%Cw=J+d>%QC1=ij35JUTf z{3z>F81Mspx4X-v+#>7$22;X=09Wco8h8J!4-QoLSAAL|XezOrJ-dGvI8zNGSDobw^0Gq7aFL|YGs5RR8t-5tY3vcDImiPbqj@^+u9}3 zU+4n;m2~+70omQ{qOpxTBz-dj%RQEf?E!Url>aDbwD~xs*0L)6w5Yg;FDu!=)_HTX zRNW2NCg6nIpC&WF3|z&%l&DQyGO_P|6*Uc_^Z@U-dFZSi*PH9E1N+H~@(q94Akz&f z7+|8Q(K4hapUb|;ZWHutuylGOtpmAa695_3o&WyBopCetu+D#x-Rz9}Av!gWWH__Y zJ2Nha9@hEop3bAXGj2<^ci#K(&Og%A8COf3`5oyz);lvUj2<@giJs25F=FSx{BUR7 z6g{l--(@!U4ANug|CUQ0+!am8(X_tR0|J^9v}B2S0Rb5a=HKpt0$~tpHibH<2fEt> zLQ4xE%}tPWAd654V6*uv(wq>+^F7@sb@yMC_CuO*CVScs>GnTK+Br6;to2{?SGjbM z$6*cfj{*wot=AbodvLT*w?+s_NcNrCv<7KkBUwi0AqKu!tBpC%y5HR>Z8 z;y=hL@o9nh*X#F~devG+J*tuP9>oPlS$&M<@V!2$_Cyk49w#_wOfwXiu7Nzu(uw zrTHnf`gk(U?A6}oVGTAejxh)_eca2dqH-RAtk({(=;~Kzu9fZbwd*ChdPyzdmo9XR z^QBq4r%3x4_hVG-1eVtGK1xcgvWQnlGhx$QaUyfsgXbKrbYi}3c+!O99D1f7nGtR! zO&G>O_@pT|DxROz^>t0UfSVFg(_S^FyxBF-Ib(0DE>kzBeQ#~+duvPI-d6U#WkGl= zT<}AL)0p3!v$CaMrKgqsDx$U}Vk0f-?7WxbI4O2mrZJwj^QNv_!GWZL0TMziESBc& zS?k*3V#yNMRqkKdQk+}BHDl7$T^Zb$QrV{t_jx1{uR-z12XO}1^jmjeE4zH@^n&?? z`-LaarhO9{7g59W+Sy-rycfgmeON80m+j429_%fTe)|PKr^K+#uWvj>zydolIWhgdT*JiJl#ZVF((~n%g zj_d6#;BbYfFEwFH>iaXsxShBsz)#=6TIaw6NPu+Q8tu><;LKsCm4R6q{SNZV7=fk9 z`=&v|gfoyg#^AXnzu8f^2>rN0Vwrzv%kVyl-rh)N%$UH9)Avh91!fFBAz_Cx-&&8=3`-e zIv3WjFd^!!-$1hIYf0{7+`U@lBeHgM5=p-?d}A<+zrHr2PX%I2J@(0 z8_ZP!C4n>clS5iTM$=gmyAXQOA!AS)9GQDhQn(;k+zUn`Ov-R0M1RAgr~0IVmDgL$ z;JFssaEWvi_HXLTkqC5D;#WC2Pdthd@RE2CFRNRD$Ok<6Z2Ey#dT|vp1wo5Y2qjM5 zjzZXl&bmlVzSR(sA6HQ;kW_f(;?m4zYv$U#IFIslpyrBh2^#Rg@k{`Gqn{M)@S;)n zfkjqVI;1obpn}J{2jTp2aQTE0A4UzHH!kx@t$t{dT%zmOw{kY@MIx0meH0lG)%SgE zhzEY;!iPTaV?)<}rF7^D6?mP%#{<0ha&f7P0%9F-QY7y5)}*}o!)=WOt#H~jk@}OT zejGx-$dlKNlVA<_n__v=eX|ihrRPp{IUgTI3P~@!wzs?;u&eUwSWeAp1+5LlCZiwd zQI^FiEWQ)5c!S#$?>dd53@pzQT6mGA^*B|fWimOzbFU3l^<$Nvp(Z0Sks}cJ?s;#dw5%Kni%+d7e!6hvwvHSR#ChF8`{@tgy zm7m!ehACd(B8Eu~%IJe7!225g-H+Rw5shtPyo>EmP}5ozXrH*6eTO?Dfl$rrnCowH zp7)rDrs}GdP4szyZM#7g`f-^R#9$EVJ^wcH`W?XU zvWYSsv1nYgij_-l6s{9nv+lvUiNg)M8P%g{Bo>;LJono6#(o;l!SpLJQeAsP?iBTF zIQk3F8k=K~Qspt6e)dMoHLgu}-&)9jU5NCvldlr==a{_9 Date: Fri, 6 Oct 2023 09:44:44 +0800 Subject: [PATCH 22/52] Feat: add ros practice package to learn publisher subscriber --- .../src/practice_package/CMakeLists.txt | 206 ++++++++++++++++++ low_cost_ws/src/practice_package/package.xml | 68 ++++++ .../src/practice_package/src/publisher.py | 28 +++ .../src/practice_package/src/subscriber.py | 27 +++ 4 files changed, 329 insertions(+) create mode 100644 low_cost_ws/src/practice_package/CMakeLists.txt create mode 100644 low_cost_ws/src/practice_package/package.xml create mode 100755 low_cost_ws/src/practice_package/src/publisher.py create mode 100755 low_cost_ws/src/practice_package/src/subscriber.py diff --git a/low_cost_ws/src/practice_package/CMakeLists.txt b/low_cost_ws/src/practice_package/CMakeLists.txt new file mode 100644 index 0000000..38d4f94 --- /dev/null +++ b/low_cost_ws/src/practice_package/CMakeLists.txt @@ -0,0 +1,206 @@ +cmake_minimum_required(VERSION 3.0.2) +project(practice_package) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + std_msgs +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES practice_package +# CATKIN_DEPENDS roscpp rospy std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/practice_package.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/practice_package_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_practice_package.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/low_cost_ws/src/practice_package/package.xml b/low_cost_ws/src/practice_package/package.xml new file mode 100644 index 0000000..c16b584 --- /dev/null +++ b/low_cost_ws/src/practice_package/package.xml @@ -0,0 +1,68 @@ + + + practice_package + 0.0.0 + The practice_package package + + + + + rsa + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + + + + + + + + diff --git a/low_cost_ws/src/practice_package/src/publisher.py b/low_cost_ws/src/practice_package/src/publisher.py new file mode 100755 index 0000000..57cd268 --- /dev/null +++ b/low_cost_ws/src/practice_package/src/publisher.py @@ -0,0 +1,28 @@ +#!/usr/bin/python3 + +import rospy +from std_msgs.msg import String, Float32 +from geometry_msgs.msg import Twist, PoseStamped + +class Publisher(): + def __init__(self): + self.node_name = rospy.get_name() + rospy.loginfo("[%s] Initializing" % self.node_name) + + #publisher + self.pub_s = rospy.Publisher("/test/", String, queue_size=1) + + self.string = "Hello world!!" + self.rate = rospy.Duration(0.1) + + while not rospy.is_shutdown(): + self.run() + rospy.sleep(self.rate) + + def run(self): + self.pub_s.publish(self.string) + +if __name__=='__main__': + rospy.init_node("Test_Publisher") + publisher = Publisher() + rospy.spin() \ No newline at end of file diff --git a/low_cost_ws/src/practice_package/src/subscriber.py b/low_cost_ws/src/practice_package/src/subscriber.py new file mode 100755 index 0000000..bf83edd --- /dev/null +++ b/low_cost_ws/src/practice_package/src/subscriber.py @@ -0,0 +1,27 @@ +#!/usr/bin/python3 + +import rospy +from std_msgs.msg import String, Float32 +from geometry_msgs.msg import Twist, PoseStamped + +class Subscriber(): + def __init__(self): + self.node_name = rospy.get_name() + rospy.loginfo("[%s] Initializing" % self.node_name) + + #subscriber + self.sub_s = rospy.Subscriber("/test/", String, self.cb_test) + + self.string = "Publisher says : " + self.rate = rospy.Duration(1) + + while not rospy.is_shutdown(): + rospy.sleep(self.rate) + + def cb_test(self, msg): + print(self.string + msg.data + '\n') + +if __name__=='__main__': + rospy.init_node("Test_Subscriber") + subscriber = Subscriber() + rospy.spin() \ No newline at end of file From 2a7b5b45065e9dc6705fef10c06b4ad553ad7738 Mon Sep 17 00:00:00 2001 From: hchengwang Date: Fri, 6 Oct 2023 10:49:19 +0800 Subject: [PATCH 23/52] add scripts folder and sys.addpth for pytest --- low_cost_ws/src/rostest_example/scripts/add_path.py | 7 +++++++ low_cost_ws/src/rostest_example/scripts/test_Quacker1.py | 4 ++++ 2 files changed, 11 insertions(+) create mode 100644 low_cost_ws/src/rostest_example/scripts/add_path.py create mode 100644 low_cost_ws/src/rostest_example/scripts/test_Quacker1.py diff --git a/low_cost_ws/src/rostest_example/scripts/add_path.py b/low_cost_ws/src/rostest_example/scripts/add_path.py new file mode 100644 index 0000000..4a40333 --- /dev/null +++ b/low_cost_ws/src/rostest_example/scripts/add_path.py @@ -0,0 +1,7 @@ +#!/usr/bin/env python3 + +import os +import sys +sys.path.append( + os.path.join(os.path.dirname(os.path.abspath(__file__)), + '../include')) diff --git a/low_cost_ws/src/rostest_example/scripts/test_Quacker1.py b/low_cost_ws/src/rostest_example/scripts/test_Quacker1.py new file mode 100644 index 0000000..15370db --- /dev/null +++ b/low_cost_ws/src/rostest_example/scripts/test_Quacker1.py @@ -0,0 +1,4 @@ +import pytest + +import add_path +from rostest_example.Quacker import Quacker From 3a569820f4f6a1e08403d9795d8ebdad40190154 Mon Sep 17 00:00:00 2001 From: hchengwang Date: Fri, 6 Oct 2023 11:02:23 +0800 Subject: [PATCH 24/52] add test case within scripts --- low_cost_ws/src/rostest_example/scripts/test_Quacker1.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/low_cost_ws/src/rostest_example/scripts/test_Quacker1.py b/low_cost_ws/src/rostest_example/scripts/test_Quacker1.py index 15370db..acd6c00 100644 --- a/low_cost_ws/src/rostest_example/scripts/test_Quacker1.py +++ b/low_cost_ws/src/rostest_example/scripts/test_Quacker1.py @@ -2,3 +2,11 @@ import add_path from rostest_example.Quacker import Quacker + +# test Quacker rounded_mean function with a list of integers +def test_quacker_rounded_mean_int(): + q = Quacker() + assert q.rounded_mean([1,2,3,4,5]) == 3 + + + From 7be85c8c773870aa97304c7c4fac4c708accf2f8 Mon Sep 17 00:00:00 2001 From: hchengwang Date: Fri, 6 Oct 2023 21:06:48 +0800 Subject: [PATCH 25/52] update Quacker and test --- .../rostest_example/include/rostest_example/Quacker.py | 4 ++-- .../include/rostest_example/test_Quacker.py | 10 ++++++++-- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/low_cost_ws/src/rostest_example/include/rostest_example/Quacker.py b/low_cost_ws/src/rostest_example/include/rostest_example/Quacker.py index 4394fd6..34806cc 100644 --- a/low_cost_ws/src/rostest_example/include/rostest_example/Quacker.py +++ b/low_cost_ws/src/rostest_example/include/rostest_example/Quacker.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 import numpy as np class Quacker(object): @@ -11,4 +11,4 @@ def rounded_mean(self, x): def get_quack_string(self, n): # Returns a string of n quacks based on the value in Quacker.quack - return ' '.join([self.quack]*n) \ No newline at end of file + return ' '.join([self.quack]*n) diff --git a/low_cost_ws/src/rostest_example/include/rostest_example/test_Quacker.py b/low_cost_ws/src/rostest_example/include/rostest_example/test_Quacker.py index bf64665..6cac729 100644 --- a/low_cost_ws/src/rostest_example/include/rostest_example/test_Quacker.py +++ b/low_cost_ws/src/rostest_example/include/rostest_example/test_Quacker.py @@ -1,6 +1,7 @@ -from .Quacker import Quacker import pytest +from .Quacker import Quacker + # test get_quack_string function def test_get_quack_string(): # initialize a Quacker object @@ -8,4 +9,9 @@ def test_get_quack_string(): # test the get_quack_string function assert quacker.get_quack_string(1) == "Quack!" - +# test rounded mean funcition +def test_rounded_mean(): + # initialize a Quacker object + quacker = Quacker() + # test the rounded_mean function + assert quacker.rounded_mean([1, 2, 3]) == 2 From 1b6a5b3cfa7f633cbe60529338d111b5e79bc847 Mon Sep 17 00:00:00 2001 From: uwe Date: Mon, 9 Oct 2023 15:48:24 +0800 Subject: [PATCH 26/52] add arg_tools and add path from arg_utils to rostest --- low_cost_ws/src/arg_utils/CMakeLists.txt | 206 +++++++++++++ .../arg_utils/include/arg_utils/__init__.py | 0 .../include/arg_utils/anchor_logging.py | 80 +++++ .../include/arg_utils/camera_projection.py | 104 +++++++ .../src/arg_utils/include/arg_utils/get_ip.py | 143 +++++++++ .../include/arg_utils/import_me_if_u_can.py | 2 + .../src/arg_utils/include/arg_utils/mqtt.py | 60 ++++ .../arg_utils/include/arg_utils/random_map.py | 79 +++++ .../include/arg_utils/robot_model.py | 48 +++ .../src/arg_utils/include/arg_utils/tsp.py | 130 ++++++++ .../src/arg_utils/include/arg_utils/utils.py | 75 +++++ .../src/arg_utils/include/arg_utils/uwb.py | 283 ++++++++++++++++++ .../include/arg_utils/video2picture.py | 36 +++ .../include/arg_utils/websocket_rosbridge.py | 76 +++++ .../include/arg_utils/xbee_coding.py | 81 +++++ low_cost_ws/src/arg_utils/package.xml | 68 +++++ low_cost_ws/src/arg_utils/setup.py | 10 + low_cost_ws/src/arg_utils/src/add_path.py | 7 + .../src/arg_utils/src/testing_pypkg.py | 5 + .../src/rostest_example/scripts/add_path.py | 3 + .../scripts/testing_pypkg_from_arg_utils.py | 2 + .../rostest_example/tests/quacker_tester.py | 2 +- 22 files changed, 1499 insertions(+), 1 deletion(-) create mode 100644 low_cost_ws/src/arg_utils/CMakeLists.txt create mode 100644 low_cost_ws/src/arg_utils/include/arg_utils/__init__.py create mode 100644 low_cost_ws/src/arg_utils/include/arg_utils/anchor_logging.py create mode 100644 low_cost_ws/src/arg_utils/include/arg_utils/camera_projection.py create mode 100644 low_cost_ws/src/arg_utils/include/arg_utils/get_ip.py create mode 100644 low_cost_ws/src/arg_utils/include/arg_utils/import_me_if_u_can.py create mode 100644 low_cost_ws/src/arg_utils/include/arg_utils/mqtt.py create mode 100644 low_cost_ws/src/arg_utils/include/arg_utils/random_map.py create mode 100644 low_cost_ws/src/arg_utils/include/arg_utils/robot_model.py create mode 100644 low_cost_ws/src/arg_utils/include/arg_utils/tsp.py create mode 100644 low_cost_ws/src/arg_utils/include/arg_utils/utils.py create mode 100644 low_cost_ws/src/arg_utils/include/arg_utils/uwb.py create mode 100644 low_cost_ws/src/arg_utils/include/arg_utils/video2picture.py create mode 100644 low_cost_ws/src/arg_utils/include/arg_utils/websocket_rosbridge.py create mode 100644 low_cost_ws/src/arg_utils/include/arg_utils/xbee_coding.py create mode 100644 low_cost_ws/src/arg_utils/package.xml create mode 100644 low_cost_ws/src/arg_utils/setup.py create mode 100644 low_cost_ws/src/arg_utils/src/add_path.py create mode 100644 low_cost_ws/src/arg_utils/src/testing_pypkg.py create mode 100644 low_cost_ws/src/rostest_example/scripts/testing_pypkg_from_arg_utils.py diff --git a/low_cost_ws/src/arg_utils/CMakeLists.txt b/low_cost_ws/src/arg_utils/CMakeLists.txt new file mode 100644 index 0000000..229369a --- /dev/null +++ b/low_cost_ws/src/arg_utils/CMakeLists.txt @@ -0,0 +1,206 @@ +cmake_minimum_required(VERSION 3.0.2) +project(arg_utils) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + std_msgs +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES arg_utils +# CATKIN_DEPENDS roscpp rospy std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/arg_utils.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/arg_utils_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_arg_utils.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/low_cost_ws/src/arg_utils/include/arg_utils/__init__.py b/low_cost_ws/src/arg_utils/include/arg_utils/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/low_cost_ws/src/arg_utils/include/arg_utils/anchor_logging.py b/low_cost_ws/src/arg_utils/include/arg_utils/anchor_logging.py new file mode 100644 index 0000000..3a92ecd --- /dev/null +++ b/low_cost_ws/src/arg_utils/include/arg_utils/anchor_logging.py @@ -0,0 +1,80 @@ +# AUTOGENERATED! DO NOT EDIT! File to edit: ../09_anchor_logging.ipynb. + +# %% auto 0 +__all__ = ['examine_plot', 'examine_one_plot', 'examine_one_with_boat_plot'] + +# %% ../09_anchor_logging.ipynb 4 +from . import utils +import matplotlib.pyplot as plt +import numpy as np + +# %% ../09_anchor_logging.ipynb 5 +def examine_plot(lines): + plt.rcParams["figure.figsize"] = [8.00, 5.00] + plt.rcParams["figure.autolayout"] = True + fig = plt.figure() + ax = fig.add_subplot(111) + value = [] + for line in lines: + if (line.find("time") == -1):#is not time line + temp_list = line.split() + while 'boat_alive' in temp_list: + temp_list.remove('boat_alive') + value.append(float(len(temp_list))) + x_axis = np.array([i for i in range(int(count/2)+1)]) + ax.plot(x_axis, value) + hours = int(count/2/1800)+2 + hour_points=[] + for i in range(hours): + hour_points.append(float(1800*i)) + grid_points = hour_points + ax.xaxis.set_ticks(grid_points) + ax.grid(True) + plt.xlabel("Time") + plt.ylabel("Anchors") + plt.show() + +# %% ../09_anchor_logging.ipynb 6 +def examine_one_plot(anchor, lines): + plt.rcParams["figure.figsize"] = [5.00, 3.00] + value = [] + for line in lines: + if (line.find("time") == -1):#is not time line + temp_list = line.split() + if anchor in temp_list: + value.append(float(1)) + else: + value.append(float(0)) + x_axis = np.array([i for i in range(int(count/2)+1)]) + plt.grid(visible=True, axis='x') + plt.plot(x_axis, value) + plt.xlabel("Time") + plt.ylabel(anchor) + plt.show() + +# %% ../09_anchor_logging.ipynb 7 +def examine_one_with_boat_plot(anchor, lines): + plt.rcParams["figure.figsize"] = [5.00, 3.00] + value = [] + boat_value = [] + for line in lines: + if (line.find("time") == -1):#is not time line + temp_list = line.split() + if anchor in temp_list: + value.append(float(1)) + if 'boat_alive'in temp_list: + if (temp_list.index(anchor)+2) == temp_list.index('boat_alive'): + boat_value.append(float(1)) + else: + boat_value.append(float(0)) + else: + boat_value.append(float(0)) + else: + value.append(float(0)) + boat_value.append(float(0)) + x_axis = np.array([i for i in range(int(count/2)+1)]) + plt.grid(visible=True, axis='x') + plt.plot(x_axis, value, x_axis, boat_value) + plt.xlabel("Time") + plt.ylabel(anchor) + plt.show() diff --git a/low_cost_ws/src/arg_utils/include/arg_utils/camera_projection.py b/low_cost_ws/src/arg_utils/include/arg_utils/camera_projection.py new file mode 100644 index 0000000..ebac91a --- /dev/null +++ b/low_cost_ws/src/arg_utils/include/arg_utils/camera_projection.py @@ -0,0 +1,104 @@ +# AUTOGENERATED! DO NOT EDIT! File to edit: ../01_camera_projection.ipynb. + +# %% auto 0 +__all__ = ['camera_projection'] + +# %% ../01_camera_projection.ipynb 4 +import numpy as np +import scipy as sp +import cv2 +from cv2 import aruco +import apriltag +import time +import yaml + +import pytransform3d.rotations as pr +from mpl_toolkits.mplot3d import Axes3D +import matplotlib.pyplot as plt +import matplotlib as mpl + +import os +import sys +import gdown +from zipfile import ZipFile + +from scipy.spatial.transform import Rotation as R +from numpy.linalg import inv + +# %% ../01_camera_projection.ipynb 5 +class camera_projection: + def __init__(self): + """init a camera projection object with default arguments + """ + self.camera_info_path = 'ViperX_apriltags/camera_info.yaml' + self.img_path = 'ViperX_apriltags/rgb/' + self.depth_path = 'ViperX_apriltags/depth/' + self.tag_size = 0.0415 + self.s = 0.5 * self.tag_size + + def read_camera_info(self): + """read camera info from yaml file, path is given, camera info contains camera matrix and dist coefts + """ + with open(self.camera_info_path, "r") as stream: + try: + camera_data = yaml.safe_load(stream) + except yaml.YAMLError as exc: + print(exc) + self.camera_matrix = np.array(camera_data['camera_matrix']['data']) + self.camera_matrix = self.camera_matrix.reshape(3, 3) + self.dist_coeffs = np.array(camera_data['distortion_coefficients']['data']) + self.dist_coeffs = self.dist_coeffs.reshape(1, 5) + self.cameraParams_Intrinsic = [self.camera_matrix[0,0], self.camera_matrix[1,1], + self.camera_matrix[0,2], self.camera_matrix[1,2]] + + def read_images(self, idx): + """this function will load an image depend on the id number, often used in a for loop + """ + self.img_path = self.img_path + str(idx) + '.png' + self.depth_path = self.depth_path + str(idx) + '.png' + self.img = cv2.imread(self.img_path) + self.gray = cv2.cvtColor(self.img, cv2.COLOR_BGR2GRAY) + self.depth = cv2.imread(self.depth_path, -cv2.IMREAD_ANYDEPTH) + self.img_dst = cv2.cvtColor(self.img, cv2.COLOR_BGR2RGB) + + def apriltag_detection(self): + """detect if there is apriltag or not in self.gray, the self image + """ + print("[INFO] detecting AprilTags...") + options = apriltag.DetectorOptions(families="tag36h11") + detector = apriltag.Detector(options) + #results = detector.detect(gray) + self.detection_results, dimg = detector.detect(self.gray, return_image=True) + print("[INFO] {} total AprilTags detected".format(len(self.detection_results))) + + def solvePnP(self): + """this function will output the rotation matrix r_vec and translation matrix t_vex, this two matrixs is important for projection + """ + img_pts = self.detection_results[0].corners.reshape(1,4,2) + obj_pt1 = [-self.s, -self.s, 0.0] + obj_pt2 = [ self.s, -self.s, 0.0] + obj_pt3 = [ self.s, self.s, 0.0] + obj_pt4 = [-self.s, self.s, 0.0] + obj_pts = obj_pt1 + obj_pt2 + obj_pt3 + obj_pt4 + obj_pts = np.array(obj_pts).reshape(4,3) + + _, self.r_vec, self.t_vec = cv2.solvePnP(obj_pts, img_pts, self.camera_matrix, + self.dist_coeffs, flags=cv2.SOLVEPNP_ITERATIVE) + R_mat, _ = cv2.Rodrigues(self.r_vec) + T = np.hstack((R_mat, self.t_vec)).reshape(3,4) + tag_pose = np.vstack((T, [0,0,0,1])).reshape(4,4) + dist = np.linalg.norm(self.t_vec) + + def draw_point(self, tag_2_inv, base2joint): + """for visualization, draw the project points on the image is important + """ + # --------------- project a point --------------- + tag2joint = np.matmul(tag_2_inv, base2joint) + obj_pts = np.array([tag2joint[0,3], tag2joint[1,3], tag2joint[2,3]]).reshape(1,3) + proj_img_pts, jac = cv2.projectPoints(obj_pts, self.r_vec, self.t_vec, + self.camera_matrix, self.dist_coeffs) + proj_img_pts = np.array(proj_img_pts).reshape(2,1) + # --------------- draw a point --------------- + draw_image = cv2.circle(self.img_dst, (int(proj_img_pts[0]), int(proj_img_pts[1])), + radius=5, color=(255, 0, 0), thickness=-1) + return draw_image diff --git a/low_cost_ws/src/arg_utils/include/arg_utils/get_ip.py b/low_cost_ws/src/arg_utils/include/arg_utils/get_ip.py new file mode 100644 index 0000000..671c0ff --- /dev/null +++ b/low_cost_ws/src/arg_utils/include/arg_utils/get_ip.py @@ -0,0 +1,143 @@ +# AUTOGENERATED! DO NOT EDIT! File to edit: ../04_get_ip.ipynb. + +# %% auto 0 +__all__ = ['get_key', 'myip', 'whoami', 'get_xbee_address', 'get_xbee_address_boat', 'find_duckiepond_devices_yaml', + 'dp_load_config', 'dp_get_devices', 'device_get_sensors', 'sensor_get_topic', 'ssh_ping_nano', + 'ssh_ping_rpi', 'test_ssh_intranet', 'ssh_connection', 'test_ssh', 'ip_connection', 'test_ping', + 'ssh_rostopic', 'test_rostopic'] + +# %% ../04_get_ip.ipynb 3 +import subprocess +import os +import yaml +import re + +def get_key(dict,value): + + for k, v in dict.items(): + for v,v1 in v.items(): + for v1,v2 in v1.items(): + if v2 == value: + return k,v + +def myip(): + + ret_byte = subprocess.check_output(['ifconfig']) + ret_str = ret_byte.decode('utf-8') + # Cut string from 'equal symbol' to 'degree C symbol', then convert to float + en = ret_str[ret_str.find('eno1:'): ret_str.find('lo')] + ip = en[en.find('inet')+5: en.find('netmask')-2] + return ip + +def whoami(data): + ip = myip(data) + machine,device = get_key(data,ip) + return machine,device + +def get_xbee_address(dict,value): + for k, v in dict.items(): + for v,v1 in v.items(): + for v1,v2 in v1.items(): + if v2 == value: + address = dict[k]["rpi"]['xbee_rx'] + return address + +def get_xbee_address_boat(dict,value): + pair_device = dict[value]["xbee"]["xbee_pair"] + address = dict[pair_device]["rpi_2"]["xbee_rx"] + return address + +def find_duckiepond_devices_yaml(yaml_filename="duckiepond-devices.yaml"): + dp_yaml_path = "" + for root, dirs, files in os.walk(os.path.expanduser('~')): + for name in files: + if name == yaml_filename: + dp_yaml_path = os.path.abspath(os.path.join(root, name)) + break + return dp_yaml_path + +def dp_load_config(dp_yaml_path): + dp_dict = {} + with open(dp_yaml_path, 'r') as stream: + try: + dp_dict = yaml.safe_load(stream) + except yaml.YAMLError as exc: + print(exc) + return dp_dict + +def dp_get_devices(dp_yaml_path, pattern='boat*'): + dp_dict = dp_load_config(dp_yaml_path) + devices = [] + for key in dp_dict.keys(): + match = re.match(pattern, key) + if match: + devices.append(key) + return devices + +def device_get_sensors(dict, device='sensor1'): + sensors = [] + for key in dp_dict[device]['topics'].keys(): + sensors.append(key) + return sensors + +def sensor_get_topic(dp_dict, device='sensor1', find='zed'): + topic = dp_dict[device]['topics'][find] + return topic + +#ssh functions will not give testing example for now on, since hostname will depend on your running machine + +def ssh_ping_nano(hostname): + response = os.system("ssh $USER@" + hostname + " ping -c 1 192.168.0.100") + return response + +def ssh_ping_rpi(hostname): + response = os.system("ssh $USER@" + hostname + " ping -c 1 192.168.0.101") + return response + +def test_ssh_intranet(): + error = [] + for ip in hostnames: + num = ssh_ping_rpi(ip) + if num!=0: + error.append("ssh ping rpi error " + ip) + num = ssh_ping_nano(ip) + if num!=0: + error.append("ssh ping nano error " + ip) + assert not error, "errors occured:\n{}".format("\n".join(error)) + +def ssh_connection(hostname): + response = os.system("ssh $USER@" + hostname + " date") + return response + +def test_ssh(): + error = [] + for ip in hostnames: + num = ssh_connection(ip) + if num != 0: + error.append("ssh error " + ip) + assert not error, "errors occured:\n{}".format("\n".join(error)) + +def ip_connection(hostname): + response = os.system("ping -c 1 " + hostname) + return response + +def test_ping(): + error = [] + for ip in hostnames: + num = ip_connection(ip) + if num != 0: + error.append("Network Error " + ip) + assert not error, "errors occured:\n{}".format("\n".join(error)) + +def ssh_rostopic(hostname, rosversion="melodic"): + response = os.system('ssh $USER@' + hostname + ' "source /opt/ros/"' + rosversion + '"/setup.bash && rostopic list"') + return response + +def test_rostopic(): + error = [] + for ip in hostnames: + num = ssh_rostopic(ip) + if num != 0: + error.append("ssh rostopic list error " + ip) + assert not error, "errors occured:\n{}".format("\n".join(error)) + diff --git a/low_cost_ws/src/arg_utils/include/arg_utils/import_me_if_u_can.py b/low_cost_ws/src/arg_utils/include/arg_utils/import_me_if_u_can.py new file mode 100644 index 0000000..5cb5b20 --- /dev/null +++ b/low_cost_ws/src/arg_utils/include/arg_utils/import_me_if_u_can.py @@ -0,0 +1,2 @@ +def say_it_works(): + print("You have successed import me!\nfrom arg_utils pkg :D") diff --git a/low_cost_ws/src/arg_utils/include/arg_utils/mqtt.py b/low_cost_ws/src/arg_utils/include/arg_utils/mqtt.py new file mode 100644 index 0000000..48f5d9d --- /dev/null +++ b/low_cost_ws/src/arg_utils/include/arg_utils/mqtt.py @@ -0,0 +1,60 @@ +# AUTOGENERATED! DO NOT EDIT! File to edit: ../11_mqtt.ipynb. + +# %% auto 0 +__all__ = ['default_topic', 'MQTTpublisher', 'VehStateSender'] + +# %% ../11_mqtt.ipynb 4 +import paho.mqtt.client as mqtt +import socket + +default_topic = 'topic name' + +class MQTTpublisher(object): + def __init__(self): + mqtt_ip = '140.113.148.77' + mqtt_port = 1883 + self.mqtt_topic = 'VehStatsAnchor' + self.hostname = socket.gethostname() + + self.mqtt_client = mqtt.Client("arg_mqtt") + self.mqtt_client.on_publish = self.on_publish + self.mqtt_client.on_connect = self.on_connect + self.mqtt_client.connect(mqtt_ip, mqtt_port) + + def create_payload(self): + return str(self.hostname) + ': ' + + def on_connect(self, client, userdata, flags, rc): + print("Connected with broker, result: " + mqtt.connack_string(rc)) + + def on_publish(self, client, userdata, mid): + print("payload published " + str(mid)) + + def on_shutdown(self): + self.mqtt_client.disconnect() + print("Shutting down...") + + def loop(self, timeout = .1): + self.mqtt_client.loop(timeout) + +# %% ../11_mqtt.ipynb 6 +class VehStateSender(MQTTpublisher): + def __init__(self): + super(VehStateSender, self).__init__() + mqtt_ip = '140.113.148.77' + mqtt_port = 1883 + self.mqtt_topic = 'VehStatsAnchor' + self.tempcpu = float() + self.ip = str() + self.current_time = None + self.current = int() + self.tempenv = float() + + def create_payload(self): + data = VehStateType() + now = time.localtime() + r = requests.get(r'http://jsonip.com') + self.ip= r.json()['ip'] + self.current_time = time.strftime("%Y-%m-%dT%H:%M:%S", now) + data.setData(timestamp=self.current_time, mid=2, vid=2,globalx=0.0,globaly=0.0,ip=self.ip,powerlevel=self.current,tempcpu=self.tempcpu,tempenv=self.tempenv) + return data.toString() diff --git a/low_cost_ws/src/arg_utils/include/arg_utils/random_map.py b/low_cost_ws/src/arg_utils/include/arg_utils/random_map.py new file mode 100644 index 0000000..8043ae8 --- /dev/null +++ b/low_cost_ws/src/arg_utils/include/arg_utils/random_map.py @@ -0,0 +1,79 @@ +# AUTOGENERATED! DO NOT EDIT! File to edit: ../05_random_map.ipynb. + +# %% auto 0 +__all__ = ['random_generate', 'sub_random_generate', 'draw_line'] + +# %% ../05_random_map.ipynb 4 +from random import uniform +import matplotlib.pyplot as plt +import numpy as np + +def random_generate(offset_x, offset_y, point_num, area_length_x, area_length_y): + """ + generate random points with mutiple parameters. + Args: + offset_x : offset for x's range. + offset_y : offset for y's range. + point_num : number of points to generate. + area_length_x : x's range for generate random points. + area_length_y : y's range for generate random points. + Returns: + rand_point_x : list of random points' x coordinate. + rand_point_y : list of random points' y coordinate. + """ + rand_point_x = [] + rand_point_y = [] + rand_point_x = [(uniform(-area_length_x/2, area_length_x/2) + offset_x) for _ in range(point_num)] + rand_point_y = [(uniform(-area_length_y/2, area_length_y/2) + offset_y) for _ in range(point_num)] + return rand_point_x, rand_point_y + +def sub_random_generate(offset_x, offset_y, point_num, area_length_x, area_length_y, sub_area_num, sub_offset_x, sub_offset_y): + """ + generate random points with mutiple parameters, including sub_area_num and sub_offset. + Args : + offset_x : offset for x's range. + offset_y : offset for y's range. + point_num : number of points to generate. + area_length_x : x's range for generate random points. + area_length_y : y's range for generate random points. + sub_area_num : + sub_offset_x : + sub_offset_y : + Returns : + rand_point_x : list of random points' x coordinate. + rand_point_y : list of random points' y coordinate. + Outputs : + output_point.txt : random points' x and y coordinate. + """ + rand_point_x = [] + rand_point_y = [] + path = "output_point.txt" + f = open(path, "w") + for i in range(sub_area_num): + generated_x = [(uniform(-area_length_x/2 + i*(area_length_x/sub_area_num)+sub_offset_x, -area_length_x/2 + (i+1)*(area_length_x/sub_area_num)-sub_offset_x) + + offset_x) for _ in range(int(point_num/sub_area_num))] + generated_y = [(uniform(-area_length_y/2+sub_offset_y, area_length_y/2-sub_offset_y) + offset_y) for _ in range(int(point_num/sub_area_num))] + rand_point_x.extend(generated_x) + rand_point_y.extend(generated_y) + print("Area", i, ":", file=f) + for j in range(int(point_num/sub_area_num)): + print(generated_x[j], generated_y[j], file=f) + f.close() + return rand_point_x, rand_point_y + +def draw_line(sub_area_num, x_min, x_increment, y_min, y_max): + """ + draw straight line in a plot. + Args: + sub_area_num : number of area that separated by lines. + x_min : where the line starts. + x_increment : distance of two lines. + y_min : line's y lower coordinate. + y_max : line's y upper coordinate. + """ + + for i in range(sub_area_num): + plt.vlines(x_min, y_min, y_max, color='green') + x_min += x_increment + + diff --git a/low_cost_ws/src/arg_utils/include/arg_utils/robot_model.py b/low_cost_ws/src/arg_utils/include/arg_utils/robot_model.py new file mode 100644 index 0000000..5306666 --- /dev/null +++ b/low_cost_ws/src/arg_utils/include/arg_utils/robot_model.py @@ -0,0 +1,48 @@ +# AUTOGENERATED! DO NOT EDIT! File to edit: ../03_robot_model.ipynb. + +# %% auto 0 +__all__ = ['robot_model_loader'] + +# %% ../03_robot_model.ipynb 3 +#nbdev_comment from __future__ import print_function + +import gdown +from zipfile import ZipFile +import xml.etree.cElementTree as ET +from urdfpy import URDF +import os + +class robot_model_loader: + def __init__(self, url, name): + self.url = url + self.name = name + + def load(self): + """ + download a zipfile and unzip it under data directory + """ + dataset_url = 'https://drive.google.com/u/1/uc?id=' + self.url + dataset_name = self.name + + gdown.download(dataset_url, output=dataset_name + '.zip', quiet=False) + zip = ZipFile(dataset_name + '.zip') + zip.extractall(dataset_name) + zip.close() + + def list_all(self): + """ + list all urdf or xml file + """ + for file in os.listdir(self.name): + if file.find('.urdf') != -1 or file.find('.xml') != -1: + print(file,'\n') + + def show_link(self, path): + """ + show urdf file link + """ + + robot = URDF.load(path) + + for link in robot.links: + print(link.name) diff --git a/low_cost_ws/src/arg_utils/include/arg_utils/tsp.py b/low_cost_ws/src/arg_utils/include/arg_utils/tsp.py new file mode 100644 index 0000000..a89f777 --- /dev/null +++ b/low_cost_ws/src/arg_utils/include/arg_utils/tsp.py @@ -0,0 +1,130 @@ +# AUTOGENERATED! DO NOT EDIT! File to edit: ../07_tsp.ipynb. + +# %% auto 0 +__all__ = ['dist', 'distanceGenerate', 'sortWaypoint', 'solve_tsp_nearest_neighbor', 'solve_tsp_held_karp'] + +# %% ../07_tsp.ipynb 4 +import sys +import itertools +import random +import time +import matplotlib.pyplot as plt +import numpy as np +import math +from python_tsp.heuristics import solve_tsp_simulated_annealing + +def dist(p1, p2): + """ + calculate the distance between two waypoints. + Args: + p1 : point's (x,y) position. + p2 : point's (x,y) position. + Returns: + distance between two points. + """ + return math.sqrt(((p1-p2)**2).sum()) + +def distanceGenerate(point_set): + """ + generate a distance matrix based on the waypoints. + Args: + point_set : a set that contains all waypoint. + Returns: + a square matrix, which shows the distance between pairs of waypoint. + """ + return np.asarray([[dist(np.array(p1), np.array(p2)) for p2 in point_set] for p1 in point_set]) + +def sortWaypoint(permutation, point_set): + """ + accoriding to permutation calculated by tsp solver, return new set of waypoint which is sorted. + Args: + permutation : the order of waypoints calculated by tsp solver. + point_set : a set that contains all waypoint. + Returns: + new set of waypoint which is sorted. + """ + return [x for _, x in sorted(zip(permutation, point_set))] + +def solve_tsp_nearest_neighbor(distance_matrix): + """ + calculate tsp problem based on nearest neighbor, an algorithm that solves tsp using greedy assumption. + Args: + distance_matrix : a square matrix, which shows the distance between pairs of waypoint. + Returns: + A tuple, (path, cost) + cost : optimal cost of tsp + path : a orderd list of waypoint index based on distance matrix. + """ + path = [0] + cost = 0 + N = distance_matrix.shape[0] + mask = np.ones(N, dtype=bool) + mask[0] = False + + for i in range(N-1): + last = path[-1] + next_ind = np.argmin(distance_matrix[last][mask]) # find minimum of remaining locations + next_loc = np.arange(N)[mask][next_ind] # convert to original location + path.append(next_loc) + mask[next_loc] = False + cost += distance_matrix[last, next_loc] + if(i == N-2): + cost += distance_matrix[next_loc, 0] + + return path, cost + +def solve_tsp_held_karp(distance_matrix): + """ + calculate tsp problem based on Held-Karp, an algorithm that solves tsp using dynamic programming with memoization. + Args: + distance_matrix : a square matrix, which shows the distance between pairs of waypoint. + Returns: + A tuple, (path, cost) + cost : optimal cost of tsp + path : a orderd list of waypoint index based on distance matrix. + """ + n = len(distance_matrix) + C = {} + + # Set transition cost from initial state + for k in range(1, n): + C[(1 << k, k)] = (distance_matrix[0][k], 0) + + # Iterate subsets of increasing length and store intermediate results + # in classic dynamic programming manner + for subset_size in range(2, n): + for subset in itertools.combinations(range(1, n), subset_size): + bits = 0 + for bit in subset: + bits |= 1 << bit + # Find the lowest cost to get to this subset + for k in subset: + prev = bits & ~(1 << k) + res = [] + for m in subset: + if m == 0 or m == k: + continue + res.append((C[(prev, m)][0] + distance_matrix[m][k], m)) + C[(bits, k)] = min(res) + bits = (2**n - 1) - 1 + + # Calculate optimal cost + res = [] + for k in range(1, n): + res.append((C[(bits, k)][0] + distance_matrix[k][0], k)) + opt, parent = min(res) + + # Backtrack to find full path + path = [] + for i in range(n - 1): + path.append(parent) + new_bits = bits & ~(1 << parent) + _, parent = C[(bits, parent)] + bits = new_bits + + # Add implicit start state + path.append(0) + + return list(reversed(path)), opt + + diff --git a/low_cost_ws/src/arg_utils/include/arg_utils/utils.py b/low_cost_ws/src/arg_utils/include/arg_utils/utils.py new file mode 100644 index 0000000..6005fbf --- /dev/null +++ b/low_cost_ws/src/arg_utils/include/arg_utils/utils.py @@ -0,0 +1,75 @@ +# AUTOGENERATED! DO NOT EDIT! File to edit: ../00_utils.ipynb. + +# %% auto 0 +__all__ = ['gdown_unzip', 'gdown_download', 'pose_dis', 'waypoint'] + +# %% ../00_utils.ipynb 4 +import os +import sys +import gdown +import copy +import math +from zipfile import ZipFile + +def gdown_unzip(id, filename): + """download a zipfile and unzip it + """ + dataset_url = 'https://drive.google.com/u/1/uc?id=' + id + dataset_name = filename + + if not os.path.isdir(dataset_name): + gdown.download(dataset_url, output = dataset_name + '.zip', quiet=False) + zip_file = ZipFile( dataset_name + '.zip') + #zip_file.extractall() + zip_file.extractall() # depends on how to zip it + zip_file.close() + +def gdown_download(id, filename): + """download a file + """ + dataset_url = 'https://drive.google.com/u/1/uc?id=' + id + dataset_name = filename + + if not os.path.isdir(dataset_name): + gdown.download(dataset_url, output = dataset_name, quiet=False) + +def pose_dis(pose_1, pose_2): + """Compute distance between pose_1 and pose_2 + """ + x = pose_1[0] - pose_2[0] + y = pose_1[1] - pose_2[1] + z = pose_1[2] - pose_2[2] + + dis = math.sqrt(x**2+y**2+z**2) + + return dis + +def waypoint(current_pose, Target_pose): + """Generate a list of way points from current pose to target pose + + Input : current pose, target pose : list [x_pos, y_pos, z_pos, x_ori, y_ori, z_ori, w_ori] + Return : a list of way points + + """ + waypoint_list = [] + factor = 0.5 + sub_pose = copy.deepcopy(current_pose) + + # threshold : distance between sub_pose and target_pose = 0.05 meter + dis = pose_dis(sub_pose, Target_pose) + while dis > 0.05: + sub_pose[0] = (sub_pose[0] + Target_pose[0])*factor + sub_pose[1] = (sub_pose[1] + Target_pose[1])*factor + sub_pose[2] = (sub_pose[2] + Target_pose[2])*factor + sub_pose[3] = Target_pose[3] + sub_pose[4] = Target_pose[4] + sub_pose[5] = Target_pose[5] + sub_pose[6] = Target_pose[6] + + dis = pose_dis(sub_pose, Target_pose) + + waypoint_list.append(copy.deepcopy(sub_pose)) + + waypoint_list.append(Target_pose) + + return waypoint_list diff --git a/low_cost_ws/src/arg_utils/include/arg_utils/uwb.py b/low_cost_ws/src/arg_utils/include/arg_utils/uwb.py new file mode 100644 index 0000000..8f6ab45 --- /dev/null +++ b/low_cost_ws/src/arg_utils/include/arg_utils/uwb.py @@ -0,0 +1,283 @@ +# AUTOGENERATED! DO NOT EDIT! File to edit: ../06_uwb.ipynb. + +# %% auto 0 +__all__ = ['UWB'] + +# %% ../06_uwb.ipynb 4 +import yaml +import serial +from serial.tools.list_ports import comports +import pypozyx +from pypozyx import PozyxSerial +from pypozyx import NetworkID +from pypozyx import Coordinates, DeviceCoordinates +from pypozyx import DeviceRange +from pypozyx import PozyxConstants +from pypozyx.core import PozyxException +from typing import List + +# %% ../06_uwb.ipynb 5 +class UWB(): + def __init__(self, port = None): + self.port = port + self.network_id = None + self._pozyx_handler = None + self._pose = None + self._env_config = None + + #TODO: Make height parameterized + self._height = 500 + + @property + def network_id(self): + return self._network_id + + @property + def network_id_str(self) -> str: + """A getter method of network id string + + Convert network id to string to show in readable. + + Returns: + str: A string of id number in hexadecimal of The Pozyx + """ + return str(self._network_id) + + @network_id.setter + def network_id(self, value: int = None) -> None: + """A setter method of port string + + Args: + value (int, optional): A integer id number in hexadecimal or decimal of The Pozyx. Defaults to None. + """ + if value is None: + self._network_id = NetworkID() + else: + self._network_id = NetworkID(value) + + +# %% ../06_uwb.ipynb 6 +class UWB(UWB): + # pose + @property + def pose(self) -> List[float]: + """A getter method of UWB pose + + Returns: + list[float]: (pose.x, pose.y, pose.z) + """ + return (self._pose.x, self._pose.y, self._pose.z) + + @pose.setter + def pose(self, value: List[float] = None) -> None: + """A setter method of UWB pose + + Args: + value (List[float], optional): (pose.x, pose.y, pose.z) Defaults to None. + """ + if value is None: + self._pose = Coordinates() + else: + self._pose.x = value[0] + self._pose.y = value[1] + self._pose.z = value[2] + + +# %% ../06_uwb.ipynb 7 +class UWB(UWB): + # height + @property + def height(self) -> float: + """A getter method of UWB pose height + + Returns: + float: The default height for 2.5D localization. + """ + return self._height + + @height.setter + def height(self, value: float = 0) -> float: + """A setter method of UWB pose height + + Args: + value (int, optional): The default height for 2.5D localization.. Defaults to 0. + """ + self._height = value + + +# %% ../06_uwb.ipynb 8 +class UWB(UWB): + # env_config + @property + def env_config(self) -> dict: + """A getter method of environment config + + Returns: + dict: The environment config in dict format. + """ + return self._env_config + + +# %% ../06_uwb.ipynb 9 +class UWB(UWB): + # port_lost + def port_list(self) -> List[str]: + """A getter method of port list. + + Returns: + List[str]: The list contains UWB port device path like `/dev/ttyACM0`. + """ + return self._port_list + + +# %% ../06_uwb.ipynb 10 +class UWB(UWB): + # status + @property + def status(self) -> int: + """A getter method of UWB status. + + Returns: + int: The status got from Pozyx. 0 is success. + """ + return self._status + + +# %% ../06_uwb.ipynb 11 +class UWB(UWB): + def load_env_config(self, config_file_path: str) -> bool: + """Load UWB anchors' environment config. + + Args: + config_file_path (str): The environment config file path. + + Returns: + bool: True for success, False for failure. + """ + with open(config_file_path, "r") as config_file: + try: + self._env_config = yaml.safe_load(config_file) + except yaml.YAMLError as ex: + print(ex) + return False + return True + + +# %% ../06_uwb.ipynb 12 +class UWB(UWB): + def scan_port(self) -> None: + """Scan all port connecting to host. Store port device path in port list. + """ + self._port_list = [] + for port in comports(): + try: + if "Pozyx Labs" in port.manufacturer: + self._port_list.append(port.device) + break + except TypeError: + pass + try: + if "Pozyx" in port.product: + self._port_list.append(port.device) + break + except TypeError: + pass + +# %% ../06_uwb.ipynb 13 +class UWB(UWB): + def connect(self) -> bool: + """Try to connect pozyx device. + + Returns: + bool: Pozyx status + """ + self._status = PozyxConstants.STATUS_SUCCESS + if self.port is None: + self.scan_port() + if len(self._port_list) == 1: + self.port = self._port_list[0] + self._pozyx_handler = PozyxSerial(self.port) + self._status &= self._pozyx_handler.getNetworkId(self._network_id) + elif len(self._port_list) == 0: + return False + else: + return False + else: + try: + self._pozyx_handler = PozyxSerial(self.port) + self._status &= self._pozyx_handler.getNetworkId(self._network_id) + return True + except PozyxException as ex: + print(ex) + return False + + +# %% ../06_uwb.ipynb 14 +class UWB(UWB): + def write_env_config(self) -> bool: + """Write environment anchor location into Pozyx UWB device. + + Returns: + bool: Pozyx status + """ + self._status = PozyxConstants.STATUS_SUCCESS + ANCHOR_FLAG = 1 + self._status &= self._pozyx_handler.clearDevices() + for anchor_name, config in self.env_config.items(): + coordinate = Coordinates(config["x"], config["y"], config["z"]) + device_coordinate = DeviceCoordinates(config["id"], ANCHOR_FLAG, coordinate) + self._status &= self._pozyx_handler.addDevice(device_coordinate) + if len(self.env_config) > 4: + self._status &= self._pozyx_handler.setSelectionOfAnchorsAutomatic(len(self.env_config)) + return self._status + + +# %% ../06_uwb.ipynb 15 +class UWB(UWB): + def localize_2_5D(self) -> bool: + """Localize method in 2.5D. Need to know height. + + Returns: + bool: Pozyx status + """ + self._status &= self._pozyx_handler.doPositioning( + self._pose, + PozyxConstants.DIMENSION_2_5D, + self._height, + PozyxConstants.POSITIONING_ALGORITHM_UWB_ONLY, + ) + return self._status + + +# %% ../06_uwb.ipynb 16 +class UWB(UWB): + def localize_3D(self)->bool: + """Localize method in 3D. The height will be determined by Pozyx UWB device. + + Returns: + bool: Pozyx status + """ + self._status &= self._pozyx_handler.doPositioning( + self._pose, + PozyxConstants.DIMENSION_3D, + self._height, + PozyxConstants.POSITIONING_ALGORITHM_UWB_ONLY, + ) + return self._status + + +# %% ../06_uwb.ipynb 17 +class UWB(UWB): + def range_from(self, dest_id) -> float: + """Range method from this Pozyx UWB device to the destination Pozyx UWB device. + + Args: + dest_id (_type_): The target Pozyx UWB device id want to be ranged. + + Returns: + float: The range from this Pozyx UWB device to the destination Pozyx UWB device. + """ + ranges = DeviceRange() + self._pozyx_handler.doRanging(dest_id, ranges) + return ranges + diff --git a/low_cost_ws/src/arg_utils/include/arg_utils/video2picture.py b/low_cost_ws/src/arg_utils/include/arg_utils/video2picture.py new file mode 100644 index 0000000..770eb67 --- /dev/null +++ b/low_cost_ws/src/arg_utils/include/arg_utils/video2picture.py @@ -0,0 +1,36 @@ +# AUTOGENERATED! DO NOT EDIT! File to edit: ../02_video2picture.ipynb. + +# %% auto 0 +__all__ = ['get_images_from_video'] + +# %% ../02_video2picture.ipynb 4 +import gdown +from zipfile import ZipFile +from PIL import Image +import sys +import os +import cv2 + +# %% ../02_video2picture.ipynb 6 +def get_images_from_video(video_name, time_F): + ''' + open and read video,then save the images of video depending on the parameter(time_F) you setup. + ''' + video_images = [] + vc = cv2.VideoCapture(video_name) + c = 1 + + if vc.isOpened(): + rval, video_frame = vc.read() + else: + rval = False + + while rval: + rval, video_frame = vc.read() + + if(c % time_F == 0): + video_images.append(video_frame) + c = c + 1 + vc.release() + + return video_images diff --git a/low_cost_ws/src/arg_utils/include/arg_utils/websocket_rosbridge.py b/low_cost_ws/src/arg_utils/include/arg_utils/websocket_rosbridge.py new file mode 100644 index 0000000..000572c --- /dev/null +++ b/low_cost_ws/src/arg_utils/include/arg_utils/websocket_rosbridge.py @@ -0,0 +1,76 @@ +# AUTOGENERATED! DO NOT EDIT! File to edit: ../08_websocket_rosbridge.ipynb. + +# %% auto 0 +__all__ = ['ros_socket'] + +# %% ../08_websocket_rosbridge.ipynb 4 +import roslibpy +import time + +# %% ../08_websocket_rosbridge.ipynb 5 +class ros_socket(): + def __init__(self, ip, port=9090): + ''' + __init__ + + Input: + ip(type: string) ip address you want to connect + port(type: int) default is 9090 + ''' + self.ip = ip + self.port = port + self.topic = [] + self.node = [] + self.topic_name = '' + self.topic_type = '' + self.client = roslibpy.Ros(host = ip, port = port) + self.client.run() + + def get_topic(self): + self.topic = self.client.get_topics() + return self.topic + + def get_node(self): + self.node = self.client.get_nodes() + return self.node + + def check_connecting(self): + print('Is ROS connected?', self.client.is_connected) + + def subscriber(self, topic_name, subscribe_callback, rate_in_ms=1000): + ''' + subscriber + subscribe topic with rate (default = 1sec) + + Input: + topic_name(type:) + subscribe_callback(message) + (type: function) *only one argument message-> that will load with data you subscribing + ''' + self.topic_name = topic_name + self.topic_type = self.client.get_topic_type(self.topic_name) + listener = roslibpy.Topic(self.client, self.topic_name, self.topic_type,throttle_rate = rate_in_ms) + listener.subscribe(subscribe_callback) + + def publisher(self, topic_name, topic_type, message_data): + ''' + publisher + publish message_data to topic_name + + Input: + topic_name(type:string) + topic_type(type:) + message_data(type:topic_type) + ''' + talker = roslibpy.Topic(client, topic_name, topic_type) + talker.publish(roslibpy.Message({'': message_data})) + + + def println(self, ros_list): + if len(ros_list) == 0: + print('Empty') + else: + for i in ros_list: + print(i) + + diff --git a/low_cost_ws/src/arg_utils/include/arg_utils/xbee_coding.py b/low_cost_ws/src/arg_utils/include/arg_utils/xbee_coding.py new file mode 100644 index 0000000..ebdf0a9 --- /dev/null +++ b/low_cost_ws/src/arg_utils/include/arg_utils/xbee_coding.py @@ -0,0 +1,81 @@ +# AUTOGENERATED! DO NOT EDIT! File to edit: ../10_xbee_coding.ipynb. + +# %% auto 0 +__all__ = ['np_array_to_Odometry', 'xbee_encode', 'xbee_decode'] + +# %% ../10_xbee_coding.ipynb 4 +import pickle + +def np_array_to_Odometry(array): + msg = Odometry() + msg.header.frame_id = "odom" + msg.pose.pose.position.x = array[0] + msg.pose.pose.position.y = array[1] + msg.pose.pose.position.z = array[2] + msg.pose.pose.orientation.x = array[3] + msg.pose.pose.orientation.y = array[4] + msg.pose.pose.orientation.z = array[5] + msg.pose.pose.orientation.w = array[6] + msg.twist.twist.linear.x = array[7] + msg.twist.twist.linear.y = array[8] + msg.twist.twist.linear.z = array[9] + msg.twist.twist.angular.x = array[10] + msg.twist.twist.angular.y = array[11] + msg.twist.twist.angular.z = array[12] + return msg + +def xbee_encode(data_via_xbee, data_type): + data = data_via_xbee + + # send data + byte_arr = pickle.dumps( data ) + length, index, check= int(len(byte_arr)), 0, 0 + + for index in range(0,length,250) : + pack = bytearray(b'\xAB') #Header + pack.extend(bytearray(data_type)) #Type + pack.extend( length.to_bytes(4, byteorder='big') ) #bytes + index_end = index+250 if index+250 < length else length + pack.extend( byte_arr[index:(index_end)] ) #data + + if index_end == length : pack.extend(check.to_bytes(1, byteorder='big')) # checksum + else: check = 0xff & (check + pack[-1]) + + return pack + +def xbee_decode(xbee_message): + get_register = bytearray() + #print(xbee_message) + if not xbee_message[0:1] == b'\xAB' : # Header wrong + print('get xbee_message with wrong Header') + return + + if not ((xbee_message[1:2] == b'\x00') or (xbee_message[1:2] == b'\x01') or (xbee_message[1:2] == b'\x02') or (xbee_message[1:2] == b'\x03')): + rospy.loginfo('xbeejoy callback') + self.count += 1 + get_msg = pickle.loads(xbee_message) + axes = get_msg[0:8] + buttons = get_msg[8:] + rospy.loginfo(axes) + rospy.loginfo(buttons) + + msg = Joy() + msg.header.seq = self.count + msg.header.frame_id = "/dev/input/js0" + msg.header.stamp = rospy.Time.now() + msg.axes = axes + msg.buttons = buttons + return msg + + get_register.extend(xbee_message[6:]) + + if xbee_message[1:2] == b'\x00': + get_msg = pickle.loads(get_register[:-1]) + #print(get_msg) + return(get_msg) + + + if xbee_message[1:2] == b'\x02': # type: points + get_points = pickle.loads(get_register[:-1]) + pub_msg = np_array_to_Odometry(get_points) + return(pub_msg) diff --git a/low_cost_ws/src/arg_utils/package.xml b/low_cost_ws/src/arg_utils/package.xml new file mode 100644 index 0000000..530382a --- /dev/null +++ b/low_cost_ws/src/arg_utils/package.xml @@ -0,0 +1,68 @@ + + + arg_utils + 0.0.0 + The arg_utils package + + + + + uwe + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + + + + + + + + diff --git a/low_cost_ws/src/arg_utils/setup.py b/low_cost_ws/src/arg_utils/setup.py new file mode 100644 index 0000000..a6dffe3 --- /dev/null +++ b/low_cost_ws/src/arg_utils/setup.py @@ -0,0 +1,10 @@ +## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + packages=['arg_utils'], + package_dir={'': 'include'}, +) +setup(**setup_args) \ No newline at end of file diff --git a/low_cost_ws/src/arg_utils/src/add_path.py b/low_cost_ws/src/arg_utils/src/add_path.py new file mode 100644 index 0000000..df63445 --- /dev/null +++ b/low_cost_ws/src/arg_utils/src/add_path.py @@ -0,0 +1,7 @@ +#!/usr/bin/env python3 + +import os +import sys +sys.path.append( + os.path.join(os.path.dirname(os.path.abspath(__file__)), + '../include')) \ No newline at end of file diff --git a/low_cost_ws/src/arg_utils/src/testing_pypkg.py b/low_cost_ws/src/arg_utils/src/testing_pypkg.py new file mode 100644 index 0000000..6faf79e --- /dev/null +++ b/low_cost_ws/src/arg_utils/src/testing_pypkg.py @@ -0,0 +1,5 @@ +#!/usr/bin/env python3 + +import add_path +from arg_utils.import_me_if_u_can import * +say_it_works() diff --git a/low_cost_ws/src/rostest_example/scripts/add_path.py b/low_cost_ws/src/rostest_example/scripts/add_path.py index 4a40333..cd0bfd4 100644 --- a/low_cost_ws/src/rostest_example/scripts/add_path.py +++ b/low_cost_ws/src/rostest_example/scripts/add_path.py @@ -5,3 +5,6 @@ sys.path.append( os.path.join(os.path.dirname(os.path.abspath(__file__)), '../include')) +sys.path.append( + os.path.join(os.path.dirname(os.path.abspath(__file__)), + '../../arg_utils/include')) \ No newline at end of file diff --git a/low_cost_ws/src/rostest_example/scripts/testing_pypkg_from_arg_utils.py b/low_cost_ws/src/rostest_example/scripts/testing_pypkg_from_arg_utils.py new file mode 100644 index 0000000..2f8a164 --- /dev/null +++ b/low_cost_ws/src/rostest_example/scripts/testing_pypkg_from_arg_utils.py @@ -0,0 +1,2 @@ +from arg_utils.import_me_if_u_can import * +say_it_works() \ No newline at end of file diff --git a/low_cost_ws/src/rostest_example/tests/quacker_tester.py b/low_cost_ws/src/rostest_example/tests/quacker_tester.py index 355ff79..808da76 100755 --- a/low_cost_ws/src/rostest_example/tests/quacker_tester.py +++ b/low_cost_ws/src/rostest_example/tests/quacker_tester.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 import unittest, rosunit from rostest_example.Quacker import * From 101ad0f61708fe9f0af294b2b394ad3db83ea63a Mon Sep 17 00:00:00 2001 From: uwe Date: Mon, 9 Oct 2023 16:30:19 +0800 Subject: [PATCH 27/52] add diff dir --- .../src/arg_utils/include/for_example/__init__.py | 0 .../arg_utils/include/for_example/import_me_if_u_can.py | 2 ++ low_cost_ws/src/arg_utils/package.xml | 2 +- low_cost_ws/src/arg_utils/src/testing_pypkg.py | 3 +++ .../scripts/testing_pypkg_from_arg_utils.py | 9 +++++++-- 5 files changed, 13 insertions(+), 3 deletions(-) create mode 100644 low_cost_ws/src/arg_utils/include/for_example/__init__.py create mode 100644 low_cost_ws/src/arg_utils/include/for_example/import_me_if_u_can.py diff --git a/low_cost_ws/src/arg_utils/include/for_example/__init__.py b/low_cost_ws/src/arg_utils/include/for_example/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/low_cost_ws/src/arg_utils/include/for_example/import_me_if_u_can.py b/low_cost_ws/src/arg_utils/include/for_example/import_me_if_u_can.py new file mode 100644 index 0000000..a43803a --- /dev/null +++ b/low_cost_ws/src/arg_utils/include/for_example/import_me_if_u_can.py @@ -0,0 +1,2 @@ +def say_it_works(): + print("You have successed import me!\nfrom for_example pkg :D") diff --git a/low_cost_ws/src/arg_utils/package.xml b/low_cost_ws/src/arg_utils/package.xml index 530382a..31e03f7 100644 --- a/low_cost_ws/src/arg_utils/package.xml +++ b/low_cost_ws/src/arg_utils/package.xml @@ -58,7 +58,7 @@ roscpp rospy std_msgs - + module diff --git a/low_cost_ws/src/arg_utils/src/testing_pypkg.py b/low_cost_ws/src/arg_utils/src/testing_pypkg.py index 6faf79e..d45063f 100644 --- a/low_cost_ws/src/arg_utils/src/testing_pypkg.py +++ b/low_cost_ws/src/arg_utils/src/testing_pypkg.py @@ -2,4 +2,7 @@ import add_path from arg_utils.import_me_if_u_can import * +from for_example.import_me_if_u_can import say_it_works as sat_it_works_2 + say_it_works() +say_it_works_2() \ No newline at end of file diff --git a/low_cost_ws/src/rostest_example/scripts/testing_pypkg_from_arg_utils.py b/low_cost_ws/src/rostest_example/scripts/testing_pypkg_from_arg_utils.py index 2f8a164..5930a28 100644 --- a/low_cost_ws/src/rostest_example/scripts/testing_pypkg_from_arg_utils.py +++ b/low_cost_ws/src/rostest_example/scripts/testing_pypkg_from_arg_utils.py @@ -1,2 +1,7 @@ -from arg_utils.import_me_if_u_can import * -say_it_works() \ No newline at end of file +import add_path + +from arg_utils.import_me_if_u_can import say_it_works as say_it_works_1 +from for_example.import_me_if_u_can import say_it_works as sat_it_works_2 + +say_it_works_1() +sat_it_works_2() \ No newline at end of file From ead68b37656a6c0b307ad0e3c520aab38b074889 Mon Sep 17 00:00:00 2001 From: uwe_home Date: Mon, 9 Oct 2023 21:08:38 +0800 Subject: [PATCH 28/52] add pytest --- .../include/arg_utils/import_me_if_u_can.py | 3 +++ .../include/for_example/import_me_if_u_can.py | 3 +++ .../src/rostest_example/scripts/test_import_me.py | 12 ++++++++++++ 3 files changed, 18 insertions(+) create mode 100644 low_cost_ws/src/rostest_example/scripts/test_import_me.py diff --git a/low_cost_ws/src/arg_utils/include/arg_utils/import_me_if_u_can.py b/low_cost_ws/src/arg_utils/include/arg_utils/import_me_if_u_can.py index 5cb5b20..d19d345 100644 --- a/low_cost_ws/src/arg_utils/include/arg_utils/import_me_if_u_can.py +++ b/low_cost_ws/src/arg_utils/include/arg_utils/import_me_if_u_can.py @@ -1,2 +1,5 @@ def say_it_works(): print("You have successed import me!\nfrom arg_utils pkg :D") + +def say_it_pytest(): + return "You have successed import me! from arg_utils pkg" \ No newline at end of file diff --git a/low_cost_ws/src/arg_utils/include/for_example/import_me_if_u_can.py b/low_cost_ws/src/arg_utils/include/for_example/import_me_if_u_can.py index a43803a..a418b76 100644 --- a/low_cost_ws/src/arg_utils/include/for_example/import_me_if_u_can.py +++ b/low_cost_ws/src/arg_utils/include/for_example/import_me_if_u_can.py @@ -1,2 +1,5 @@ def say_it_works(): print("You have successed import me!\nfrom for_example pkg :D") + +def say_it_pytest(): + return "You have successed import me! from for_example pkg" \ No newline at end of file diff --git a/low_cost_ws/src/rostest_example/scripts/test_import_me.py b/low_cost_ws/src/rostest_example/scripts/test_import_me.py new file mode 100644 index 0000000..0f63968 --- /dev/null +++ b/low_cost_ws/src/rostest_example/scripts/test_import_me.py @@ -0,0 +1,12 @@ +import pytest +import add_path + +from arg_utils.import_me_if_u_can import * +from for_example.import_me_if_u_can import say_it_pytest as say_it_pytest_1 + +def test_say_it_from_arg_utils(): + assert say_it_pytest() == "You have successed import me! from arg_utils pkg" + +def test_say_it_from_for_example(): + assert say_it_pytest_1() == "You have successed import me! from for_example pkg" + From 149c22f46b99c07a5dc57156b755389cd38e3447 Mon Sep 17 00:00:00 2001 From: "Zhang, Yu-Wei" Date: Wed, 11 Oct 2023 16:01:18 +0800 Subject: [PATCH 29/52] Create README.md --- low_cost_ws/src/arg_utils/README.md | 5 +++++ 1 file changed, 5 insertions(+) create mode 100644 low_cost_ws/src/arg_utils/README.md diff --git a/low_cost_ws/src/arg_utils/README.md b/low_cost_ws/src/arg_utils/README.md new file mode 100644 index 0000000..f0fc3cf --- /dev/null +++ b/low_cost_ws/src/arg_utils/README.md @@ -0,0 +1,5 @@ +# arg_utils +## Python package mangement in ros package example +Put your python moudle into /include/for_example/ or new a folder inside include. +Add a add_path.py where your main code want to be. +Then code like this... From fbfd35aba824ea84e81d2341cfc50d2e4140d6a1 Mon Sep 17 00:00:00 2001 From: uwe Date: Wed, 11 Oct 2023 16:03:18 +0800 Subject: [PATCH 30/52] add image --- .../src/arg_utils/image/add_path_example.png | Bin 0 -> 34617 bytes 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 low_cost_ws/src/arg_utils/image/add_path_example.png diff --git a/low_cost_ws/src/arg_utils/image/add_path_example.png b/low_cost_ws/src/arg_utils/image/add_path_example.png new file mode 100644 index 0000000000000000000000000000000000000000..b8139bc0d67f73ac09970bd7259b46e3af87fa16 GIT binary patch literal 34617 zcma%iRa6{Z5GGFW1c%@*!QI{69fG^NySux4aCZngxJ%H%-Q8J||LmUKv-_~~Fw=ee z_Py2BRrQrkxV)?w0xUKx2nYy*gt)LG2nZO}=ke$lsL$`n6HCm`9|%W53FR-JAFnS) zVV~z%P9o|~O136WuKEtfAf`69*2XlBh7QKYHjZYtP8Z-kyq}YJ%$3!hgdB|Zoy=`* z2$apOjXwuLKp5#6nfBTm_oDsxv%(k{_EU=qD31Kubelzdb=!~I7gbhPsMZ%PovTvoBPzc{5)eH|35tsgCg?LtkU92b zx9vWNfwjf)2gE^mZPcXFrH`lIcul1<9e7RMa38qOMnpz(eS;AA3L$_Qh+2s2NAM+p z;7i|6;R2$62Wh0pXn!w$9r_Vp7DWkrZJP!tsE`&?R~>6z-LL!Uf8HqOtg4ijcAw~; z#V7NM`gdvp)A6a5E^}nC8YW1%JutVw@*M*sIT-6<>Yxv<2&Gzz&67&)-HqF1;V9hM zOnFzAD|B#4Qy%H10VV)?)y$bxRsfT%H>A10A4qD=fA7^8#Grn&y9g9QnUGboH zUSAI>L_&IH))(ARSw_rc|AgiA$L9HM4#=gaM3XuNU9!2$Uzv}!d|7$U-UwBt1n?6a zPrIdU%gtiEkd`P=s0iDC+%SDZr4>gBJ_-~~bdbLLYQ>t)`zU?xZ3whSMkBQ_hL@YO z(F~kRGr()hgCaQ^sTV#pll5O0Hu*Ev`+KBHQb&P5f7b*Mb2!suPRpJuHFZ;ejt3A^T^=6N{{}XXnda8dE*a6Z`7m?I$D)kSo**oqrM9qRSL$)UG*16gK@u^^l9!0=WL06jTB^HEG2C_UHCY_Oa?#Gw?ivAT zxD0VRRy{55_1(S*e^o@^uqt}1z#O84azcivK&m?-#^{A8E0r{xg!w*AW_*U0(38^5U=#KJ>8?u#m7K z5p564W_k_TG+ZG{D0zjuU`%zmT;{l{kO2W%!6;a{TLvfi58ILD(`ac71t8Ds$Q4mZ zdTDvrZ)aAnfz^M)IG1gm(^W@H{C2Wg4c<0prRxn}PeHzaXp!x_I!tOY`v1AOpWOtH zGk=^rV9qmMvp$ID6R%uOWcu^GqUsXbnDiraaz3(({bSZedLr6G5tQ@@@BaKS`IJE6 z*7bm1{JnbOG@!T~MrbpSg5nWQhK92Z8f5(v!Q*AWZmoMh}*O@Y&>`Xfl zTx~y&xGk3K+deGQiV5^&J_)558!33l;7ACXN)RcE(no%P1_q?YgL&BFyQ*f7ifk_l z8cjz>fFkPtPq_!B20B>EGVLplvzmzqy|G9oSvsy^o7^-CzRbSrU?p$tkDCv~t$S4tw!yf+pR;=uP6^E=Ct zM%TUW@)Z|NB~jSXb~qKxVYBG+aGYKq_q?48du!Nd^9Tl6F0s&K{SA;;wXE z-Vs&MsX02T3@uK-G)P~^ad|c3H}2CCkB*+Dcf~lzYBXyh6whwhueZVG>+)jRT4FP8 zU0gh3_Erwxl0bXDiz&et>HrU*OVZfm2azTZRv#vR{+a;9EFGKUPn3uWd<|myUFJfH zo1h$!CKo(YUEtl2`Ex#C8MVk*FU;;EQsGx(Q5F#`lFNN6MTQ}1n<>)dCP5^;bmi(tBLxSq z{GC1ywT@p7ELFOQznHzGzuG{XmsFL{`2+3v#i%hJNIRsJ)$ zcwk}1a};S7aqo@S%9)8XB2(EH(A;)Jf-FoL(YQf}=6wQ+Tg47`kie%ANgO96J30}Q z!JsHJbMYvU0@H3}7kt@@nsz~1(m6g7173)n{rMzV`s3tOo@zFwCVg(SS$IZczHQSJ zxkCe!fmZLpuOn_f9XHBhqp8h#n3tTzeY{H{4!+K=da*wE$sOkfvAW=B$QiewDL0{J z%iju&L%|QWHO6U}3Rl14)tVUbCxqKGH8tAWAwEMSb83Ym$}MrKa56R!X!ER<{6ZOS zV5%k%?6H7bx2+<^&dzXnvFg@G)A)WTTxlM8^ZJi@seAqTiMNDu|{d=f11Ibicua+=h{?$S4p?t0ZXJ5U2O! zn=KMjn*bEQqbw+7w)H&jg~Fybml7R)9@Ft~S5X>Ra%{;KYdTJ9#2N=HbJGNKE4157 zZDc!Cc!DLFBBN|5tyHLcKy$F}RPf4kycBj0oS77P2NbtobQJjzw`o#y7nyzi(si+Q^DE+~ zLL6U7XR`$dN1_gN`QZ4jPQ8PUVVAk3Q+P{si?%?|qTcXvDuxjdnz97O#*Mvc&ECF@RF z78>D8vGcAc5rAP{|D9_Po3X`$f*k zE?FP@;^_t3bXZ^h(|>5LA%;-E%{3Riua z>d%Er*QZ~~C`jfd!$gURB>vl67@k96COJo{Zc)P8`s64|>@SrNTV;f#RL5D%S-#cr z{d(1D<;1>V87Fq%49;X|ddiYJy7 zb;K83k7U4-b)F1EVn>fF^kRF{@I6OTNd-(B+f6D$O+BSsBdR3X#sJO1WRY+w8D6FF z&b^v@)#NfYLqo&v@!q&&wNbParCKt&zD(rKpAVu3+r+lLl~DDhWxi*2J@0$soC6u zlHLVDw{W|naCGUUi7Tvolap*$J+J%eR%}sftuCQMPb0kj5K&dT2=6Bh%l@tno}SVe zjhgP5ac`>nyFG)ejt?bOMT2{`y7&w=v;5o^6ZM&crS_0*k#X>`&|`{usrN&pS~#T^(uSyk@)Y6 z`#Oh2dyv0Asc&0x{){ir2oxKOC_S>nV(_`}fW6QYeJ!=!L;@W}cDU$<)miwC<5m*V z$_e-8UQF{2@lV`drypkHUn?4;#pESsbLs7&zM>S4WXlchj*`KU(O3p5$wApZ z+tCPaz!!MmlX95@GCn1Qoazrxt?b(({WE5_{xo=(DCN6nPaIoM8tYO+;zkYe zbo+wnt`)mZnIIkuco(?M05{UJlh>$@HvzJV8bak&;mybiH%qZ*j(=(?+6QBy+&ORi zPJOcIHJ-vF+i2{VmOvLRZB#1UV^Xl8m02R1?c!jWoLJhxWxow^A$Iw3uW--v3RTtd zN6FsEem zK`Q|;tgb-@mNgnOE7z9@%`ef9j$O&lUJr8{Lt)OxwK_bJv^sCyTyb(PpoAoSG(t zmUQy*p|cCdw}Oky>AR%@Rrrgm-OCEPxxDU}SNxAGZkmB(*SXu88rRQN;$VdqQLla4 zzmCKqf?~Wc_!gK)`=;5${XRP|65YD@6)Ug1U6ZfR}6BMG&Oyi8A*5RS1<@5TOQV&TrdOXGie#Fpst=nl75xLeQv!7y#Z=hiEP|$#P(4+yOXE5ZU%)%=x+xniMrNJ@Fhd;h!PbU4&qX`y(A}p!sE|D5%3Ao z%Au^ai5*Lew!KlywjYnkV|=vx)njyfeNhOV80VTLoxg`8D||*#;;r`Iazk5pkm*8s zPczcwvwczca=D^IXy~rx3o$I0W!Abj#RlHS+^cvtN-MLji^r?Iq5;5PL?mbo>o_4v ziG1CT-+hb1jHU1mo_qJW8&^3_tyO$ZI3@Q5$8O|yTu9^(QWMi1cP%Bqs&_;__BYTk zvrR8{d7K4km}Re;v46=Ux_JST18URTLw&_aQev3%!DuY_|9M({>3gl-%BUqbX&Tu2 z7crbe_sTx@IFswltrccp2JP8fw#+KFYpn(>4mC#YH-%V({gk;fU2OAMENO4XK`csA zr-UzF<5H>#pF*Pf_yA5X_K zYdy%-s>Ik?XbMav^Qv(_kp;MTr>Viexb}l6lNCh<>~LCLnFJU53v+8YS0+r=2gF15 zFlD5|Uk5^TbTd4HF*F<_in#LuE^Fpn)FH{p2z$}?K;l<9uz~nQ0?OF=-vj11Ee~D< z)1u#(cTIogzkKNCq5x+gSIjR%7_d$;Uy@J(tGZIF<|{FWLv&xTX!Bo#a~aQbNOx&- z{Mubf732!iux}!0WmG+Rg-r)0P~ny)m1;4C9vi+CH#$7;U$+JLxIhWdb>b{}(27S<2N@G8|n( zt$Hrh!;9c>3cNYvy&|^3Q~~+NY<@`k?#GjST(gEXr@(CPUyJzQBy%>rs9U5v zGD|1bP!L(`_ihf{s;pa6ALzfg^o-p`cJ~V{=|yTXbOI8U6wsbYm-M?90usK|t>`fG zJh1h>{gO`5vl()gf(*gcg1I%%hD%iZQ@i#mbWL+a@DhHiF0zhKjWxHk3-j07g8Z$p zggU)3O1EVM@2nMaZLW?S|4-k6r8P7mR(^HOyUyS!fgyk`9@WT$EO7VGt9pomyL#H z`2L~J7vi(13?7Yt-nwlH1MoI6hgXt`5Vr6*Mazcc`~`(91^jXo94VTg`7fzvSU2Gj zmlJo_CpGZT8$G<&yI(~AqDoW>6PZG|@Z|2NZY6$;&T&hiwAcO|&srD8Pi7G5p9kFA zfAG=ao$-r`9jR2lKJwc9z~1DM=r8wCP-xhqBkFC9>gsq|H0mrIDo+AWpC#Lj!S~M1y zTcJ*S0;T5(nvBZxjYea_cjJiwzqLoVGJyUEZ1{z4yzIm=`hB)}ArYKuD?6ST3SQfb zu_>%ZR2@YW%M4=HXZ*H3fNH@qDx73TB4B|jv2bOntXO6Bc;@Ie@3re3W^yqC?LLdD zk#?>wxUr_#=po*&1EJa@uDSenuR{B}a?h^4mX z%(RCBFdX&EGvrdK;Ty@2qmszWf8C;M)z>$9fK7G$SlI)C__&YdemQN=saM`fxBr0F zc2Iy^5hwLjeN94I!o#^fAI5c{JEcS{m zXk_ut1rQ#Y0#4@trk+U*@?&|vGs7SinS=k*-VntaiLTZNenxtk*U>A@q~{49L%v6tx<_j zi9kVU++jZzz}x0TwIEjYWHsr65-|-h%ggScL@X$;p2CJBHz>t*Ed_N9fik8FX(`4k z?J0#&DT(&dLibQ1ujBvz-iY&~+yoIvR-c+$a9vZTutrZby#%1E8}@4tMCKc~OGj{& z6M_XK$+kXNm8`Z;8}%eJ&;2zKNe!HaZEu-^z*=m@a`?faup#X2BA5e>2MKZzXD3PY za@k4+$1X?mvQ>oekI}E~QV4>0)44y6giyc#*RAwn{gDI1OU zR_IUW;+0{=MPb~vIM&cfW|l3}JGtu-G?T)==*ppHv*7w#wDCmTxgmA+QpiNWUt*dHL-gqjwJGx2`!K?7e2h6m7QZY*F(=Fm)ZGwB9rO2|O6|Fj%WlR-#yOBZ z0)DUP4B+Pkk97MtpI0h&{6yD0V8uoRD;s`&e;8q~DmmW*ehV)qmMP=)znNO>aDYv$k6dEKtKtZ@GJdSCZC*uaor_D-z%Y)$yD z7eHq+;}<4NY&M`UHoNsmjLQ|vxaKWl$#)mvvxDMw0B~*ixbZB<+E$2>z3n1s^OAsk5PfH#_ zd3c9=XwqoC&Cj(1_VZf!G=>b3h|ksRj6jn)9?IEX?o8>(d*ShQKCt55^7ecCo>e<2 zq1SF^O}w7QK%0%Hja*|?ALORk96;A}_&cG$9=iCbt4`hU`3B=AIn2HJip9@5kKgY~ zOOONb3$OpQOj4O!5brx-R0~<>O_2gk0LL}G3vbC3w=R-!QNuRm(G%a0C1CJ;GT`Rtc_W zl77+0hlLgF`po}?5;X#s`smG(-3C0 zCd+`B&aW#I=S2K!W{2X+ilX@ebFS0xYfxl75qWAGE^tlY348{zJu;Z2Jo9m!g5`bg zub4kAA2=y1dJp8Q1;h(&MFr?5<@v9)JRROGG|v0>#w_s`k$S_nfjiUV3hzK)+^o8~ zG!(ALNEw3TSFQ0|i*hhYdeER&Zg;U|!hrSuT6i@MO@T~RA-L0>%8+|(gcNUox~-aM zOq$%*Dh4^y_IQIds{EkKajQts49#4Tv3xuO_>^dKMz0lG+BIxdO_@i5Sa&=+$psWh zJn0x0r8*vg(Qs|}WIjB!nyodY9{_)0(F@T{Ddb;~nQ^aXdUFc3X5$Gmq{jhd2=WCp zeMkt^em17bL+Y?=dI#+=IxvT=E{oPWnw`W&EgQMCbF6I>l8g<{5W^Sdu>o^{Rb7x9o&l~ z0`xnzasS+h&dNi9 z(9CZAlvjH!4(z-MTq2oLp`~oIVP1TNCYYW0?}{T~3aP&j;GCEEDdUsAB0+|Y!WeN# z=k?3jvA>xq3an&VKME=sX^0{=Zy}P*9M5&oBE#~S6eh@bB*uhA;aqK5Al+re>-cXN z31hkJoPHr3W9(%mKTm@yl;Dfv=eL~f{o!ogNwh8)WACOzX&eiuCgx|SEE*Gf)#Uz# z+WD}f0b<06i07z(0bMv|3}RK=%>!6Nb(_YNtOIaS7e#a>UFbh~&*xVxj*kld0zf7) zN{<7|h+V9pW@AOt$^!JEar3Kzf`67m#+ZI<&>wHvX?@{HCPG8nfkP}!q4KXOEUbVc z#VV*Gjz}^W=9?E*tr8T;vZIf}ks}ZbaBB4ys+Pe*{^UaD@6}_XuV*U<)r$m1X~W96 zHKL>&gs{6 zdyp$)z5yd9+)VA=@yy`+broUqf1$SXTl^%mPv;5sNY@){$^OzN*B@!KB~{LDCxn~L z$SwO@ivV-0J|WP3FogM`-|VT&t{Y=(a8&eIo(=~t*(QF@2)d&4?zI&#Pz0Ey{z0l$ z{)8sCipw}YZcC&;(#(iY%&fKigt{g%96}?7#9|vECi{KTazv_;u|dOjoNkY!<`0J? z&_y7r%32h3i^xu1xgHO%eS*0dM-&C&_!<2=3QOTNWcfp?PmHS(I(AL?_~H;q}hXJRP6l*gH&L>3o|({BN@8^a5*V_vCQt6x1(> zrw7fAy(~YiD2|VdySiP$C>KY?V#2<=LPq;=;dSdcFm_g9+I`P3j@-}txSpGnAN0V9 z{$BPp!+P65MK4`Va55xSt8y^nT( zPSwk93HDrmknqqTRonf7^8&QOs~zqwL+wI_GJfy{F;et7X!wzF3yJ@FBe$%#E-k3h z8vyK_;D9Grc*;m3UArIaF%)=v;kJJ_;oZT2 zPA18!Y0XJx9fX*R-K7f_9idJjcr4!3PWyXHNCu(+Mp0ZH5Pk3m)w%Mh{j$WI#ADBy zd|D#+#O8xe+?tRNDEO-?yglZt11xI#ll1t>+m6)cUC3~qae?EY#MJ!P8%FUTS0fx1)$pNC;o+L7r!)G#NMzmk;y3l4 z$oH4ph9=MpU*CY8^3w7be7+T|J%Dc3oXqYYM|uQyhaRYtR!SbavC-Pus8SB7(JRy> z)7kf<$kQsD^HHB-#>zIp`|J)S{~gN|h$Ii0Cn87s8}?2;Y?{Gc!`9hUM*^zfthU38 zw`szHsQpm98q_EpQ@j?Z8AtovJZkjGf{}_UKP@gZD>=bYLCH6V(&R+v`m2^!yV3Mr z^!we>1fIxB(3q+W53G99QMseDYpfC`NoFYQ?DryQzJx-CX^WZcTXi@}1BWQ)=w{Q( z6lNHSxs&nKh_5jchf(Q8V*+dbUykA7JY)jwm_f_L<v6mK&7F z$O4OIZcpKo(W@NtoGIkXRLYV{4g6qWqD2Kv1@TtatzEM$%9m`F2?_5al;W#}RCg*ziVg_E`L1h_1v}$X7V_)u zh2zhfy;Bc-xMHMg^6!iDLrFyt66+fFj+m|L{U~B(@7SJm>*V>aW|ailU#mh?cx0RA z{R`S+iRAudLcYu?%FL2PGLb%JLg8Kr8N9&EqM1DLVebb^Q}Vb)rNSjWQ&^7O{`KQs z`RvHJS^oX@~yEN7Orc=V! zbc81XV6!VaZywt(SJgEF&NIeF)Kuoc*$&=`fTJN&^@C&r4Mv6336jsOr9^1t`$TJr z)WrcC9rx5nHpivDxuz3c>Uhm><@Piwo34zgm4(xC=!A(z4#Tv`Hq1l_3OI^4fUl9F z=m*QdC1+;76K$eY)hE+yG}HAdxUtLKP?^AU3?y>vIVhiP;4GdV`kC&^03>PjBHQ zQL9CGlBS@Iguu2bIT12si99CkBX~&hpumAUGDcK|K7~UIuv6h#V**M)X0&G<>h%6m zV&umzf#LDqlyZ>-C!^(h%uBBtMg?_N|Le{j_-%YPsW18nEbiX)u|<;sJW{bF5=QE0>dXvY-QRF{IMIY;e@!1Xt$SNKsXIp9P6`>>s^#Ka znyCFM7;XG{>OAQt}onAz)e!5uheKqS-Yu4~O*fevvFT+AT_O3dBnmoLQe= zl`-wLVJTP=FH=VLP=dlX5DuMdc@#Fn?=FK?vT@Bi61Cz;|9Z^12yOjL*?LV|Jy~h$ z>CBXqloO7j1E^P|%lxpu8D@O}DG!5vQ~5tq-!0XE2M-CKHY^|w8m5aC>GVO%H?rbd z{kEsjU9DMxdz9t6h;}|z#Ew|Wi43~o_uh)Nm@?9>rtf>w@ZNQ&HVxlSU0*$ zcTYMlis_wBrO$WEen+?6;**iE*2il(x|`R7E?3PgT+7drdth!Yqnb$VwN#FK~Em^4(D?R!R zET=iY#TT&7j#yE<3QO7G#FKLhb6hv=e!Go}Al6$+hAGWYB}@+sWt=7sOAtJ@$Df)z z)(W#+lps;ygS1hGuugw3(sz(2p~P>q!9G3!XhfGcgN{I-&k$W;@0N(K8gU^wfE}fabR@D~u(<% zU^|W#%3*5#01gpGG~z#7WJOF5KOAo{X$=)^BHYHs=-0yQOi3ZMu+7KD3d`2ODAVN$ z#HG2iIFnJc&FJ?&6AF~#C(>UL8fVvLJzGK3V@jYnty$|~hY~CNgntA9_OJ}FkQZke zE4wUMVaZiMzFJqD-LZQ@*z)@~xpA*Fk|^{fmMq!1d6>2i8Tb_*($l0SfUso_>3lVx~?NTo;{*7Vcc4fl^TWCXL14Oy|>YP|dBb!=dW5OLwup32-o!?;3 zwR#L{TL9?Au*H-ZJJG3mw(C)b7g(>+U%(?4JfBke`scHl&G=q&L9jcN4`rajAi@Te z$`~ut=8Rvfh_i|)oG@iz4SMMv1cPb0zI!wBE9Ei}pdQ|GB6pK@nmQyD$mb~@)oL<{ zKVf5u22Yk3GsG23Aj6ADP&B4BAW#Ta&qRtn4zY9XKc}}xMd*zS%xI2m9jM{Yk&t8| za4z5T;WbCQV1h+BF|{>o&%?I=ddQY(74t$qVQv)VYQ|yt)hLUsz*jXv`^>wWKy>&= zZbCfNB|>7kI7x#9c?J=&1@Vmms3e;|7-u+CqUy|I9K_9xi`Qqk5^%|auyV_!ievu# zty{yUZ4X`7=Pg+P_^~9G1F_i)jfY(x_gMiAQm}KGFZ*IzyxQZsY8Lvpfgq}$L9ejh z=}7VYd8sGH1@m8BEh<2X0XE^kZVq$8|LqS_Uf{-F57v0EH`j(C3w#_@wBEPo8@{dE zMdU$=`@r#+3LC@TC7igKtOu!$jGG5|!-6BGu#ocDvz>QeU9WACP(sV2b+6lJ=kY_| zA4cO_>9HTdo+Y`n+oBR=+6^gsids&)Z4bEJ;nQD9`b~G+Cz1Qq5 zMSy&AUVa^m^VgA0pI0ZKe_&3aWEO4312JyaKA|>{=S{9?-Q~cXQkR6vlyaeu?{X8G zF6Q?uJ4{wbOPzl1M^Wip%IfiQ?K{YM&5e4S0XAC<{!~o6%6bD|Za8z&#tS48uk+mR z{@z%U<%t+)m3gNuw;2Yy1D9Dc=DDZA{h63uOH3ia5GDalXLKxY>yssLjec@qiZ<@) zrlW3-b=>49;zHKxTjeN0JBZ(L08o^C{Lta)L6{^jMp?O8(g2i4BZs@iQvpmowP z!~13TI4N5?yKOCm-2?3Oy7WrYsgmyNRqnHb76tETeh=1Rx|{&rNwLcv;ci0-FVqgI z!s|eWs%H}R7vDj8X}2ysR&((W>6ZwxRWXl%(1s-Ke#;)tIWYpz>EDoJd0Rk2x9#F! z+XQA>Jp&R`*Y6ii+0~59n3bw#YqDL2wdGVRWl#NGhCZFSB~!0f?aQMNrk=~)?tZ3r zikU}iJH%%J#D8qXsexU8X14-3OSLMlH zgIp_%j~;Trab|p}n5NYgy-IMDXelGuC9mH>Io4Z?;x2kKF&RXK7@(>4-D zTLY+l*wf`&AH9=GFr|541diu0;3=t+Jt8Xh>}&A8_FDu<$a}v`al2i7CJ;Nfc2=r= z=X=H@xN=T&QoryAzO`etJwHc|B)`trXilzDt1|6gi6pbycpib;oaPKuwBD1Z@GQD2 z_D3D1+5vUo+VbW>crmzmeR54zRcB{#1D)RQI`1nubeQPHTpGNNsI3mW0>YPSub1{t z)tC97T1*v9ARi{1xUYt>sb1jeOH6&vnGf9(Mh!)Ck6*H#niuEZ?=#sKA2Je$6$Li3 zrD~;LuZf0Y%tD^6f={i&b8yJ9uvz>$tc4aL@aH7CSYw{FGTkUwmB9 z;6@uEPF!y`a(r%5#hKg<7ACawAnb15yFOwMT<`TFgTMRUOnF&OWp<|4`5E8b)Jef$ z^SK3Usm(hM8p>Vw&vU-_8Kp?K#1nodX9p#7G*-K&iovSe9rcvE8id)#!)TEE!;-w4SLg zIZoV4ADV3zqWNyOBO(lSWl+FUQc5 zy<-t5v&T%Smzx;{d)lLv@+!*jSKP&P`9=aQb0J<&-P~yLZ6ZJ+$G!D&Biq?Z_omct zW=FR+uFZ$jeCgEIJXrQbR4;LhOcoNo8XeZlr^bZz7gZ?T?F2Y8CIZLXRJ-ude?ZOI zZlbE1!Ef9eX7>eI(W&It?C~ciBDSEY387X0&dX&V)2MyFWwqJ|XXbgO@Wj^>X?>&` zJ~}-IiSKVV!$2DKxJzYOe;Xn|xHppm3AsTSr`Yn-Q77{N`K6cXRU| ztX0&+a(cn4i%o_JIrD~8hWC4i?FwD}4?Z55^@BL)1Ek?hJ14ogr?-nl!?NA!{@D71 zQo89&U7jm_x#Q}5r<7#e4=)Be8l54{he7R^%XK4HwM2^sW;eb!*U#$b0{5pwTz)a+!d*-;n+4uKvYMkHfobmBN3tx2kv{+$m<*wW#NC8#0(v>%zrso} z<2pi@0CYchckirtb?#bIc^1LipdKdgCh$)RI!*tUkhmC2=X z!qQtGL#iFsjO5H(i?!ReX@rSj5+6SyE0mI=yqc+z`DZ=hr7(hiF5j>;ko~;{5XX%& z*tb$YLkof9yf!{raRa9*%}N4#dnsWA82FkZrz>sp0LlHKxL{D=ZX^@|Y#^q1aiPua zY4$@sAqeS0U4~$b#}xU_B%$X$*lB*juotbKz|*JkD!L~K1_F($pI;{D+h19pTgy2U z9uG&xP;a=WuRva~j|6y_bsf)qMOS)%C(Vq-v``6k<(~oIkENgq$M{5Q)X_W4z`=LC zd0DEnO7nS5#$9s2dwwvZ&)K8%@q`j?Ibt$4nXEI0d*X+@=={9Mbn6ntpiAKZxea%8fY5r?Ur!tiNrJjHcIui*5k^k8XpjJmt6{;9T+O*DV zu$JQn5$gCd^11~!xXghn0*K4p3uk;?TrERTEXK54gp8p5PBF1)|F($-Uz(AFqKn^? za`TK_tGhei%zphAGFvgborKYn_L#=*WR5X1CBFOFCT%nta5P^|N$90GE!<#vVspD9 zoY3MGyFvL?#W3(RKoF~7w$5_hC!q<7`K8;?v5#lBdrJK0;E?id38VD$*eOqv<4fG$ zZT_f=16H}GZ2Z=4@JE$ zNA#Exz5o4-n3gMX(L^&n#RQeFLDS8}32{XR*KfG17FPo|eo3&raR@;`_0xs%=}Ljx z9%)_cPYJm-Op4+_88PX8^z?gA3A(o?FT5$?22pWQL%LEX@96?R@}6k)>NI=-<=GVt z)4HQ=uYh7OP01IHI?t6OE^63*pOI&W70X*?!~wRp*oRD;D=d2W1$Sg>8Et?wjO%)Q z#oY!ZYQ;u{+HljWbwaaQK$nC{dvfNk<6j#j4kwbES-(Ji#Fh8m6`Vf+s0eh1-QpqNEpROi>eUuBu|7AfEYQvDRWS&X?pjGfeeq)2G%s#t zfY>`h+XJA2&`kAbaP*x5=YEHUq|Qa8DIxb%v2DC;MwWJw z642-t#eM{ol@<%nHW;vjqB?2vI6?2nVWlTS)CK;)grSVit#L?|t<{7k?^z``z^36$x^qTHrSC0y^2*OG!jwkKUbyRu>( zG0XdOsW+KAv9qTru?W8k$U^>F&^;k&zVtrBXPven#kvcKnGj8XoX;C_yZ$STi~AKz3nK$`CE{cBf!@o9^E z4|A+r&wG>E@{&tO@99CT5`Gk6B)RP#qU75KXYJ01c~)1xk#-dK+icN4|L)!^F3N)( z;~KHA!zNh1(F%C}xVV7D&rlYr&W5=NkpY5o+u-jMz}w?3>C3IHHtu{*FQ|l?SEac< zf=8^Z77DgC&+e5NbGmq55<&&=r6F$w>Y^o$_>2#o|8xs8xAdj9ZPIH0qdcS~Q?1M3 z6#O41n}ryuWf?Ouh{W_(IqLSiHz!2VB39Os%S{ zMM=9mJ9`!{sW*Ss@JwLaOXUjMIEK3?9M{&RTqyd-@nU&*7*-G0ojl9AIT(Phsb3V=7{&(QQ;;K*_#JJ) z_2cxjn&3&5&24o6fj5=UTqF>6zXU{2+ri90Z z+3@UJNmr@cH2uLKwzi&U#3Xb~XAx1CLvT>d;qD1+X_ypi+F zd4#(LryW#bSRR)Fgi|fEbmn8XH#Cn&Y;^7^Ty(r!2;%yl`YrA&Qk817-sK$cD*$~( znJ5DKQgT=t1`mU1*#!@xYw2r&pLfZ@`GQ90i4NN5ShO@}ACc;Uax71htWY!R_nSel zP!c@4|KhHXwR+^qOpObYleXg=1!>)D?Cz9dC7)KU@pnsNjb0 z&{6boWY0|!y|?8MmWdOVB7=Is1vcyOA)k$jyQG`q5+us63Jaf(XiRU_bo)DC4{>9n zaW_1fXEc-P{H5}ZBvDv*URB%s5vvE4N2;Zpi(I^oKP-XP&zzXWu}9AnZIgQmtGar{ z_vKK|wTGuw-{bC-%amKD+}$*74DJ!SKMRU@;bbl>TS3HuiW}8FO zV>E-_iy&+qC04VIgYA3WW#zCXa#x3henZoj{L;glBX)G>si|FTUC>kWSO3w4@h-vH zYPM(2sMKCSi!+h@G%c{FKkHa{USAe~TOd_JmjNBtswTKK)yrqsvXWMuXuv4cpWtY$7D7c+)c z3%rv%-KC}k{|$#=rNjYkynA!*7F6=MrR#GK*^!L?c#AHbalEah3ZXEu?vlnw!`f8Z zPErLLge`sWNLvU7HsK81n@!IVLr>Vmi#<8R1k0ad;<|HRKU9lxTLGR!Rp^=?8{?ru zIV$W+{{i_Q-_I}bWUV?kA~qBHGx+b<0fxv=yhT+;MA~{cP|vC;9drUqW&Hj&04Wny zlr$o|hQqy4dJx63O_Gd>bV`fZanI;yP{Bemn3@_`Jk9#`8joNL+rk~JfO_IjX{lR)>Df=m2>K_Mc@qo zXenb2U1YiK5Fkcq6zFMmASh9?b<&gkDiN%#CeS{3`y|-gnQ>X5v-?3^5oE;jt|D~* zQCT#Bg8FQ}fH~mgJUC$JMEj~tNw&nFwQZ6^2qipMrtac#XJ|n4&R&FUd;RYeB%HzQp zC&ime`Zb{AHavsns(gMVW|*e8jM?@Tyvf_M`tU(KY^vMthnC4`7bV4gX{d4#T-UYt zVGB~L#Q<2`vzZvrA@8k*R!Y%oY~Qq(hFfDOVjTVgkc(t9&m?~yf< zUr20yz((XdkY?A;mOUBN$7(Io{VeHn>cSKtdGdR*@HcGC0ANd{MQ6i&a6HhC^503V zaSYLiBi@?qtTyKp0cR#YF+YyWH+Z;OKli^_OEJ$N6={)ys$X>t$c8{v+`^GhQ+ZQNVB@W9)rlY(Bq=<{1<~PmnQ3 z$6h?9=?wj^@NnOM3vY#x?I$Aw4-Ze{8u+=2>2C+PBYfK)x7WiIOwMj=t$Sr-WH>m} z&3{ketuVL@mO20YN-9mG=0pSp^aGcghkS?p_i29pYQ*!;C%~KWXLY=qE>8P|uzI36 zAOQO?;^a7zG#t(hv&2}c6HCLj(!ZXXDBE{yQDHE-ZoHmq9PZhfbZ#DJaOGL_GZ;Sv zfs`Q!^b5h*g~z+GooPm`(23Gj?Q(KY$Akb5jxn0I#Wn8 zI##wi+=qg7@8AQy%l*|Xbt2RSx;;LPm=)J=pp_$FwSUb3i}o_xMBVTP74y&K(KF~M z!ei;6Z(Pbea&Mr)oxg?q`cs8H{K?FN<%1u5@9UkI zPwdC~VWF|H)~+Ak@hj$s;aHyisyymia>(oXMz3!IL`p24ttL$7^fehuvta~ zk?7==Z{$gh=GV5JQ?KWctmc)XVf0KVx3cq@buZ%In(JR#tleta4!zJ^Tq1kggO460*gF}WNT=x z_GQkqw19p*04&r8NNIGNHpJ^flOxv32nkJ0^kOO6VnpVPBtt`H=jv+Kde_paDu5O( zLw}xl!b~?k7^p>^e^9Gy11g-YsAlwGH=p~Ki7mTC!3yXYcaD`AOVJapF? zg3**6@MrQ?eR~J}72lY`&-seUHm}L6-&|v-&0VkG!3+CG6m$s+hGw}hD^BwFPORer zH+8p|;`HHc26lxqxtm?ztUt|^hbU?zn<`)Hn1&To+O7!MP#hJ`l>XK1W1q+D*(=pL z#<8G-F3Lx%ErS&vF|4O3uK^&L%KLS5%mN1z>yKP`AgTW%8GUs9&T^zU@rdBY7D>`k zH?TA(d@j%>{#TN5A<_)CsF7|7cZ-IrGb~;sRTRs!#Z0owgx)zmGO)-CJjv+JvAJ>F zxmif6SBOG2EuH@YB5vt+lY;WlA_2$O@I0V;^FpDoFE3@DU3ho^ZI&0&L3S${k`)46 zZBPO(Auuu4c8yskWij)iarI{FV_*27KIpNyMQ|6UHsnm!(t7o)Q_afwjU%$%V|d*d zq&|HFUR%0CvKArlfh zTSuP0qr@6VP-X-IYQ8x`=graoZDVou#a(uvLfySP2-)OJw5Xk#sLd_7;PSh*Hr=-& z07Se-uYUC}&N8!Y(+CAsJO~JmJjN|r;{N(zfcz$?y(LEmD9G1NsI7C{?rO2$V%GqK zj0*bQn|-b*I|4e<{zn+&+?5EHq#GbRp{tW^Ngjs2Zqb6d5J9y~yt1USIhH~ztJ0wc zIZGa8U~{#S9i6C=t9Zqlg#%QtmXJe{(yh}|@Hwhji_?@Q@k>W?oOhX%W!ujynYvw7 zmXdwmI(6|K6rAs6cHFvUMt}OyL z`-wLrI`X8t^eM#{L7~C+1flm&+ppM}3K^Dmm4=R^OsPXwZ%L1K)VwQt&UK1k5GR8c z0t=7F5@xYe56Y|waW0V-%<8RRZ2La@?BG)@Z4UpmSO^!}&(Ir1mprLmHu^|v2K=9G zSWj1xznn!}*y9r2QNQSiI{&2KW-Dz^(m0Fz>r{(9;9(sW^Y05H&Hp8JtjU? zzO8>O2y#20Pj3?p|G|N3r&P=Az8bOu4rdF9F@T|S)3qrUdJ?v_`JqKc$5s=VlYVEk zLg)9)yY1C8STWX`y!bLB5gFIjw3cN-{rdHxW~wR|=iFCQ0Aesk{I4m@#~wZ3YNQr) zAZz?T5^gzN>aU)sz5qG#M|bX#G~!T5^t-nNB?b16vkjlp({-{|F#A{3C+%sn3}1`f zN#w9A;#zU{Lo1GDPnmv?vn+4uAU<0>%AiOSUF{zZoNZ%QEsB7BztIHvFmXn3>`3}- z^NH3g-y~2I4~!>?ZBBleXxG8z5a8Xw&DZA3+xrz_qpF)y00H?33c9XfhH;e=^SNd) z<9NsrEw|Ls-#1Q1@o!y@tv=UicDJwPGa&vnFC#|!n4ck(_fvH?TY~RBx8zo&-~-{k zF$Kdi-mN|snVJ#%$BIi4vX>IP|CZcE0#!v25D<0j8o^1=83nkXix988zP&Mf^tl24 zNAWx2wf&Ew{bwjq2N4qbuT#k=svya^wt2=sSVFi@!Z^`Bvi0_5Y8d)TvJDW(uf31P z%ZD%3J=KB!DX=hna%OggFOyP+yLYH|KR0^?&ci7-Oe~7^!G`BA|9J`!91@I;$N8am zIe&NOK`jzM3;5UehD>R05kJ{X*<50HG1o6OQbwL5Tjf$IQan7o8(BRKIXjQQ;kUNX_l9;Sv;^8?_8<38H zXA?nx4{G2Ssdi*=jiWknSI`re$q@A$OAqvDpdQfwG0NK6SSlL07At}#|B66x!{2DC z?;o7lZb1W>ho!BvKlCfF8~DlN;12Kj0kWnmh`l~Ab#J@W&6Y^aexqyW!1?#YP(sR~ zubP#FelZ5GG=^`VG2A*W@faDZr27xQk=fOu#AAIjQ8sH0f#5;t>rDu*;j#*f$?AL{ z>xVZEueNdqcd04um%j1zUy$d4ytnf5!HjYksMgV86Y{piS$+)U97Ltu`ZU9uyZp)A z7zV!{a_F$msYHys*V!2y=8QwqG`^O3bN7i^*bXY;idc0PoRR<6I?R^s-WQvkwsjri zLu}YWEE5|eTmj7M{)*L>b}2z8qjZjt!24&gVs1|4r#Ymy{hkuT6a}W+fgLLn#LlFS_nxs;H`-zh%Ba&n9)-Y&%N{SlGd%Xzb_Li5k4p_kJJROPNp#xm4 zXDJ&yl(Lp4u^HD_ADxMQJe)f#OZ+c~W%||-Br^QZPq^ej@cPpb&7YNgjNMw(iFeP^ z#UXdlP{Pz(lB;T@4S$epf0Hi%p28baEBB+y*2k{!&ka}T!WKi#YN${ zI`A*G?LP(&KqQ?`=}!s8^c_A;V61R6%Sl=`-Gd{ulq;sLfy<~uI|{EF#9hO+j(oq8 zxpWvT?mQ;c-JnMq227R^Nxig!6^$-naUbqIgZE#)mdn**u@}~6qF(uNz zybTL@ftZ?KmY=P$Wp+0pk(d!<^u)a6)j44jrGZsLeQ-2;YSy)mSpOX~M zd<7+6&v^s$Wd8Tp|2fCv1z5~H5*l&XkUCFPf<9xSX}mlH!btDq&!J>t$Uf|{eewse z*r8)J`(nhm1gNwc)@a~buLh?UW2J){qDx07`vKTif27rW1m-IRwz(*Ols=aYYx+O) zCySRj&_tFTg2Ct`T!UMQ^q%c5m1zB=IrzX>V$sng%)}v)iA2ga5;ZMX5o-#O59vvg zeYphxbOAGZ>yHYK!37~%u2hbUJ`jKxRY+zqcjsyXwepLz*Bk>N(Pyy{UaitD`JU8>V6A~ zVrYnH7-Z$NAw0PddV?cfFvojAyLD0d0p#OyW)uS$6>AGnBg1d)UtP$hg0X5bZRmyJsSvgTzLn&q5 zkq*3TG3jUWyR$3}$m~C14`<|xvf|py977@0k_5O|tNsAReUXT$&);yV5BsH=dOC`f z=Y+2_v&?s}ojF2)!N5M(r-7V#KN?;3^Q}kTOI}y8<`{mts+W^ zHA>k9LXGqY6jM?&`ON5l0^{Bs;EW$fTu|JqClr~N%KY|#GFm`nR?if`YOP~$rW-Nb zTYKg3P*zj3O9}rKd>$UwGu-*9>;St8$0z@PsjqICQRC9*I9Vv)w{Sm#uZQnln4I_0M&fv1g%Z!1E2oj{ zu*AFYs148EZd!Qj=pKZRCiQk}Ba5eMkmeUZ#%O&@$-QLFqi-8fdMCD036UOgMmy%;;dTlJN$L(vJcx4wTkSw|M=KHUFp5<{G&o_mQRek8QycXQfCySt^#V#2sC@`M~H_ z?#344;Q+d$BD-G^oWq?o%_m3GC>_P=ZCGPQcUl?+dGP|Pb7xO7CnYb9$rAOD#}`oX zU?+bXdCG-A9g#)*j!FgU{8SSWNRPlkyWkf=nTf|;m#&ym{~a(@<|E`R;TRdki~~J$ z*nzc|1@kHy#tI$_gnm8NO?}9eEj}&el~YV6a`)J0%4^qPj=*(gEbpU z|M_*s&-4Z)36T!GI;6a1Z9M$;Iyy-sEVX;$jN_i(Ijdb=dfTTHw)&1mg97qDb)TW} z-plAEGn1%K$)vbqfK{!Q8o`#E@)!BS(E>6rpM<$bBBGW9cyy4L?>0d#M> zjk#l7!WtbkzTRO)ny06ZDmK8mF#qu={gU(jCK^#78GGV$Uz-k%9a=&MLJuf5NT7#a z%@Nd0)Q88Fa$m{X5~tTO4<8;H12px(WB+OV{rmys58r~)qEPsez$u6h#ACX)7@A?1X<{sh;GBc+KWuPpJRrt7sqSqe?=%a!zP>aln5`B^)bTEViKBp4& zScSN(puBNPkuiRP^NUYEPt$)-=DAp}fgly==q)eNfh;G3s*zE8yGN{cWjM8h2U13E zglGw#1Z0nV^3f;uczPtGW>;&>G`zz7Xpb=H8oDfDHRgon12h>$i5$uCcJp zVTmr^hyVECmMy2B$j)-K+bO%CLWeTptRGM)LseFOZ>&a!Io#5JSN~$1K4heqOJ$8s`v}R(%&7_!@w1J02v=hD-wOwmFm?H8@!nCUo1{O{_P%l zw(+v&-QrWPMDa}2nrv#hiwPL_zssKI_}1{1sd`%2MmR+ft)DR^?y9#nM1YW0=T_^f zKKZ8e(PR00L|Z@TQgI$MLDW&5Il0Du$IQ2Rx@^~LN!Z!BghV2mHrSW(Ae+&g>P!?h zZCjm zOK$}@TOlM1n+;rn(DL-|+_6rv1iKk_d5=-v5{uWx$?Av|3FyF6Y31(| zYP_X<{fgL3t;F+oMua*J-n_aE%3(zQp7kCrnNL<;A_j_`^PW%_j>HTUmk)J5*3akJ zl!{`H`Hg=^baoFaS-O#EwD!yvcfCwyPIk_fx@;0WKRpc!X)pRko{WsCV6z+^&&eE3 zJTOEa{9ksIj$BA(KJBs%Z; zaiFvRt`7sJl?Hw^0uy<+r*6+d0{%xp^^tt)O?UrHUCmn4bj};r0msP`-3JV8%UBYB zcWU9;bFIIB1R=VR*U5d24pxdyXC`y6C0@0c5zp;e>p<`+2mq#k+&z4j(BvWalQzoW zOuPHNJqiZ5+Cz9rY-BiKr8`!l9^BQFh<*sZA`L72_#|jNs&`$2sa98WmnqSS#^yp) z;NK-no|(%lV*e3S`9*--)g(`YQra4yM>KN{2CDwfg zi#Sm?NBngSy+1tabm0$B7`XoOB3;Q`x9%_d5`;V08l&N{ z$pfV^_lD8ncUc1Nw9$f1PO@=!-jBI9A`1))QdnV!7{0U@Q3itYO&f`@bkOH~X3e(OOr!W7!w!8<}MyE8tX z9A;5Tb^Y&GCd_E%Cp+-fST2sJl{O`s_(7h>vX>a<>{BLjf7Tryl5m!x$6gV6$+M{% zx8wPR^m=@|y;Bh(tZmccWamS^!Y;g#Q-!BXgcJuw_ennYg5^3GPAGav4+kC<4jr24x6q=jP4=G8^mfq(*h z-7t3f(=wodI(%Lc*vZRPOuU!O0jdkIqYe*gus9Lvtr$ z(19xfw}oqvTbs_2;+m;F?l&p02UGu)rS$&bG6yt-FLqq4a;*Ri*$2H{j{e)O#soKo zZG)wfGBOdF-Oak}mJHQsZvn#>tl%!MjVCGMJAGbSE?jAi-r6ua(~s=E>w=^omgyO< z_ctRb>UnF<1x{4qxT24^i}lXO_K(ksFWbqP1*2t?1M$BIs7K{#25V|KMr~m9Ur$7z z(7|pu@>NV#%t)LRMMt;a>DG^%8nYu?~-Gs&xja_*kQq&0fS zHV0R&Qq=OGRIWhwXah*dR zq5=a>hAKnLk5SlA{MH|HMuWr-tnIGT^&aT7K}i}K71q-FFN@rcNKNq|}r0URwufDI#+fMWlOM)?G`$*4OJBq0ocUpfO1DmRS6Oqmc{%a**7{4DhX>jCu z$i?J>Q&jhI_(}(NF!lS~tRdP=NtSVcv?Gz<7ZbAHB&}yN zQ&uDuw#V}Kh_(Z1Fl?nKKzJed)li5AiIjrg9=*r3OVtl_iXfTS0CqeJg{vgS|9v{m&`dy>6+(nW(W+iy<#t8`uG zz_A$}Tu5dCJByb{XpML0Iw$#wqVN#RzDY@EbK#7v?zz8?OBaBKNPomL3Fg}d-`b>6 zb6FGbd&kZi&C*F7mpJvIodPO=IH6PX=3X9{y%(w{3@Q{2MVP}yWuOJ*s1Qj6<2so3 z=YKFdK0B#%SD;~}CsCiP@8o^C!ye2l?j9tBLvIFD1V5EB&imB^cbmJKp(Bc*4Xwrxas2hOM>H)35w$-7cVtukVmLNI$W;G3QW2S6*4;(h)gFD{U|J4=@AN6BS+)tzBvgp&*S z7X%<-z(bOq=L+gg;d7lYu^1IvIMjkL8NsxW4aOE1JaL?$pcpjmm^+}O42W7|5@?>V z(8ZVPeZ91D;I~ot>l-SL)iyy#AXB0A$_Inis*3aN`3fHVH=zmmAE8MO+0#_T&HBvM zTwRvAlKrk)Q42SLJAs~s_JBUu*Ek`{;*H{!vO^mH!EEq8FYY%A8^?U&>vR-lG{7X$&vdUE?6f^~90mcad0UVT|UOo z_#wYjuDPOvM=I_8`$x+kFwmX32FkloR9P;>ks>#pxNQ)htk&c~aZRiJ%!}0`!Dt7< z*M8L3Gh2#56mo3`m_MhmiM=gl@X{`k*W?WPe*ssul2@prwtLeQvTV=Sk&m-uX7sjn zH(VqDW14Wrng&{aADc?$bD+F(Y#Rn zOjN&`!YFWk0!4{-FuD$@NOfTaPXP5l?i>V+Lv2sh4f3`g_L{1o-Y2Y0q*1me>m3kI(3 zs|mq6Z&+76^WwyXv$wYBcm&y0_0p7iG3zU9TShmD7ax_-6kw}!#bU|`x|;_qr{?*+ zlZ4&FZ$a<#0tLLQa)jzrMqi|70H(c$burdh@jB36n;ROJWB+$)Q~Q!NaDc*A7nsVoQsw>VXp43PS&&@& zp0T!*Nn~0A03If*^ETF^L*Ex16+-4Cvo|jA1i`SesxuB9+LVqdABUC~a92V$kI>)X z6xxD2{xaapL(~1~0`6d?{vQ>CGAnq;R69{w^-pY8+N-v)9y_zz+CC5ABN0z$6ggcR z)0o3R*smn&QI?_$TQ92eVVm-KKMR?BAT@cH0MlQ>FCKvv;+!^s5Kz8n3(Ze7>h>lYRi*Vp;;jw|PWyjNk-of`e=U zqoQKT`*BCsxs9t?$MyPv!p>p&`vJ8r-TO0;?-EA#u3~o%?MLRFW-ovn>o+!$zo@JgxSMI=@^k1LeCB5@52pNOF6n z@mGt38u;jqcP75{HiE)rq{n@WY2!Ce%yTIj5J^~@*^K^&C09>Qn^AZ-Ut1UHSQx`e zsUwgC{?S2}=9`Oof`Q`PKB5_m7XyQmykwc(g{zO%;A`_0iTA6q@bp`J8=*KE%}g8) zW>oWfD0^3`b2Tyk3pT+L@g1rdmxk;$=(rJc=erKlJ!##cMlBkH7;A9_d;!9rbLuZO zt7FNVF{MZlR&;@b4S}d=c++2>EjssU+k(cPZHeedm}v9yD^9(=5lu#|c{%n}!OPn$ z3qSC?M0(w67QH0ban8}Lqh?;KtYFHE{{14zL4PIXQ zK_x?T;)aA)u(;NWcS9LAh~vM*A5y|L-O_vO7BQ3bB_;XiO|ZT_#UR>F2WZ~IS2Xx2 zmoR5&V#y=^%C)m8S)#|R4}{}}pFPCQ+F+gQ$m`FRAaq?==u_s{L?uTx>a4&WPd~8P zz;}eYH5yfoU00W0FD& zaG7iJCo{`r{rBkx7`p9bs*VZ%3Y>FycQ^AD8(X)Y%uGQSR#{gPrZD!JLYOHub%wy; z^fYXhvd3|-59AL-dZJ_24L;)~GEr+J5k_xKWdF)l%l^vI72WUA@PvVi5Oo~fh0aB;$bt<{!YO-g)NnT+W1CnES^A{igU~S(@c!Qv2U-vvrvg;^4-4oKZGkW3E^R zheO>|BW-}B2o3rJvi*1|iJukmak);!8l9<7nytD5veZ3FJSr0lV@T~-kXtZpKO13i zXqIFCO^F5cBlpxnoovlILxp4zh+ks-GHvtr-|h~zZgb_;coC#`l@tYLlYu0sa2zb^ z2Ds5MZEm2XY4Zc44)||+KhBWhi%U?>cEEJ$62=k0U(SD%2kJk}%73N`Oq>2Yohth; zG()`!7Hzz4%EYw)DG2>C?&>^|HWJN-xXM^*kWjD5jE!!fjW#XvxpB48SoRM{c@e%{ zXYj-jX+~CC2Qd;ru!6D}qCNQ$XtTbH-OTmG$qeNzQP}xTtE0^I^+&lHxmD-qp1u8i z>)MsxO#XV0OQ&YmM9^s5$x{7ZrtV3FL;I^7{#J4^v*%lo>|B4`gJSg7$(#a?$#in~ z;->~4F}BskUc4!%tJcPbQPb@#fH=dn9xlex*yA5Gk5G$DpUenE)AVXa`{io?CCY#B9m?lE;^X}B1hPkkSQ@|ma4j^J z!|+I%dhlcfp$&q{M*!~0`A#};?C($G{{dI){tH|cu0pkBAh0(Pv~i&!CXK(#ZQ>k6uXW;-i8mORtF%hKTGsq}nJK+EOzPP`^yRD(bKeKm!?E@V9FM!h`ad*&g0Ihhq# z8BY?-?_OMyb~TImX+GbExY-Z(0zXLGw>k|EF6W4{7FT*!mwy~SMRoSpXxKW>d>DM3 zL8@k&f{ex)<8jd9Z^TAUN#R{yfxT5;jEF8~4ylZnR@^SZ!)# zVJ7R?Flkg&xyv5<2V+IRLuc11k=Y>t#k!Ff!L(7En100l3rB4?)zep6_G zl9N+b9hw>zu_dTkWN%V|Q`OJ{Yvpo9cqRh1`6g6rsB81}{dC3LTy_q<^MZ@Hy>k%H z3Py@gR}7=J83aaVT`w3=Ov^_pG`41DMVhMETqcxWZl=`atX|QS(eA{bo?2W*8?jL3 z5f)sk4v%~yxJE;v!QVc^OambdcpooIDxHx;ibKUlgHLNn_ zFzq(w0E=T)bg9qsR8w`VM%wuC&h@o|ITWiSn59Gv{(Z4Q$PSgkyd(>MvJ=#kMPy2T ze9I0MEVr$SC=ixlaz&Zf(v+0r4y0S(VB|$D6XY{p$r^2!k}k|)3ho?HUJZGd8C-Z492Yhhz>LR9fPR!DpBD7u`c)ig2Y9 zSZT}mwu}@Q*HiG&w-;TIUeU>L1lIPP;43JZ_bzH{^NXoQujB_gGSyPjhqjdHzCUz- zp>rS#f3+$ySJuIEKEPOw8vmjpbY7F%4}&6Y-}HJ=UL%_fY3AeA`TJRMe_~~`S5zIb z-Is=gVSG<)KIm+I|G~)Ow2J{0%)VmQa=W1Fq!grXf?nPcbcyRm%V`MH@iJcG?7d$r zx3W5@iiP<+-|Fye#E$hB)34l;p-^>$^N}5tHV>9D$Pg?Y7dsKA6A>{iDZz$5$kF6aPR>Dz~sdQ`4rWo9U4P&o(pM1#%1?(0|uwJ zd|pXD6&4k=JXkVP9%gTjywyGR^bhLH?uE+}G)aq#uIX45>=E8NG*;Fu`rH)YktM7w z%WHD4dO0}V@a#y&L^m-wS~X0ke4ov=;jtCGBMffCwoMG}VJf8i8+jg_}C95yRKO^*8>jN zo4XdQLid2!GEZt+pPuaD_=Qxm&Z5k-dz`J4bed5<87D0+S{pS9E$48Oh_TnhWN_Cy z5;3q-eelO-yh-r)ctfy2^k1e(iHW+4!0dz;=F0mM>Bg*L)wAxcu*o$?av^DdH5o9M z*vPU*iDzzHfyoA$%Nqc*#5XZC=86jZc+pUh4^1yj1#GA$fu!Y4IZ|agg0iCJCZarS!Et zJ3By6dh+FrUy_Mux---gSoe<}bI@!fJP@JW-j=m=AhW#lIz=!K8=vPbg|#-q;e$`i zcr-3P!YS3lwsBqyLl%?NgCYm^LC%G44DISI*(7z*cY1Z$-~W@6VLqKU(0R?&v#`_lJ6bt zD@~QbQW>~Mv#${A2@PW}gG=dw<>gGBPx}~nBU@QEvok5Szg0NC$+X7trS+_B*VzyDO5nYnJ+VhB;qdP_v@xqVOX8m-iD+|%DTk6`Ykae*!O9@}MSjc!w>r?kOA#R03qYW)( zWa_D3e6&`$Q;Z(0A?3sszubMKJj%hB4k=uyxyPfu?sE7iqTP7h&tAiz*nq7%@4Z>z zd0TFbt*bP{oM!nS3UnmZ8Fu9`1X+5XE~augcA*yjsMs~Fv2`9_JFX9^Wwops&Zq}qcb@e7C3*IXDs|MrYn zPQap1Qoh-dn!ivDe=&WFA&p`(41{ALfKUvCQ!#u=jR-c-c_91QlI}L1Or20p#{UaH za70u^u8sHS1Dw9KXf8n@9Dise@y}I%_R%MUQ2cv{DnjW$2M`#D{a-!c(T6#Zhsb83 RmKETqhzUsxR`cun{}0>SU7-K~ literal 0 HcmV?d00001 From 7a1f98304a457ca9fa0fa864c3da1b4778d592cc Mon Sep 17 00:00:00 2001 From: "Zhang, Yu-Wei" Date: Wed, 11 Oct 2023 16:14:28 +0800 Subject: [PATCH 31/52] Update README.md --- low_cost_ws/src/arg_utils/README.md | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/low_cost_ws/src/arg_utils/README.md b/low_cost_ws/src/arg_utils/README.md index f0fc3cf..65ae23b 100644 --- a/low_cost_ws/src/arg_utils/README.md +++ b/low_cost_ws/src/arg_utils/README.md @@ -1,5 +1,17 @@ # arg_utils ## Python package mangement in ros package example -Put your python moudle into /include/for_example/ or new a folder inside include. -Add a add_path.py where your main code want to be. +Put your python moudle into /include/for_example/ or new a folder inside include. + +Add a add_path.py where your main code want to be. + Then code like this... + + + +If you want to new a python module, just add the following code to your add_path.py +``` +sys.path.append( + os.path.join(os.path.dirname(os.path.abspath(__file__)), + '')) +``` +and make sure your < path > is pointing correctly to your python package. From 4f1d0a4b1daad1ff30479968285699d58bdf7483 Mon Sep 17 00:00:00 2001 From: "Zhang, Yu-Wei" Date: Wed, 11 Oct 2023 16:20:43 +0800 Subject: [PATCH 32/52] Update README.md --- low_cost_ws/src/arg_utils/README.md | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/low_cost_ws/src/arg_utils/README.md b/low_cost_ws/src/arg_utils/README.md index 65ae23b..dcc03bb 100644 --- a/low_cost_ws/src/arg_utils/README.md +++ b/low_cost_ws/src/arg_utils/README.md @@ -15,3 +15,10 @@ sys.path.append( '')) ``` and make sure your < path > is pointing correctly to your python package. +### testing python package +after runing the docker +``` +$ cd ~/LoCoBot-RSA/low_cost_ws/src/rostest_example/scripts/ +$ pytest test_import_me.py +$ python3 testing_pypkg_from_arg_utils.py +``` From 927f93bdd527b49f824d5db5739862307da3cd8b Mon Sep 17 00:00:00 2001 From: sunfu-chou Date: Thu, 12 Oct 2023 20:32:35 +0800 Subject: [PATCH 33/52] Feat: Update uwb node --- low_cost_ws/src/localization/src/imu_cov.py | 3 +- .../src/localization/src/localization.py | 35 ++-- .../localization/src/multilateration_ros.py | 134 +++++++++++++ low_cost_ws/src/localization/src/odom_cov.py | 11 +- low_cost_ws/src/localization/src/ranging.py | 51 +++++ low_cost_ws/src/localization/src/uwb.py | 184 ++++++++---------- 6 files changed, 297 insertions(+), 121 deletions(-) create mode 100755 low_cost_ws/src/localization/src/multilateration_ros.py create mode 100755 low_cost_ws/src/localization/src/ranging.py diff --git a/low_cost_ws/src/localization/src/imu_cov.py b/low_cost_ws/src/localization/src/imu_cov.py index e4c00fa..01e46e7 100755 --- a/low_cost_ws/src/localization/src/imu_cov.py +++ b/low_cost_ws/src/localization/src/imu_cov.py @@ -3,13 +3,14 @@ import rospy from sensor_msgs.msg import Imu + def cb(msg): msg.linear_acceleration_covariance = [0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] msg.angular_velocity_covariance = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.1] + 0.0, 0.0, 0.1] odom_pub.publish(msg) if __name__ == '__main__': diff --git a/low_cost_ws/src/localization/src/localization.py b/low_cost_ws/src/localization/src/localization.py index 18c0791..e294dba 100755 --- a/low_cost_ws/src/localization/src/localization.py +++ b/low_cost_ws/src/localization/src/localization.py @@ -1,11 +1,11 @@ #!/usr/bin/env python3 +import time + import rospy from geometry_msgs.msg import PoseWithCovarianceStamped from std_msgs.msg import Float64MultiArray - from uwb import UWB -import time uwb = UWB() @@ -28,6 +28,13 @@ def uwb_error_handler(): def timer_callback(e): now = rospy.Time.now() + validate = uwb.validate() + if not validate: + rospy.logwarn_throttle(1, uwb.network_id_str + " UWB not validated") + uwb.connect() + if uwb.validate(): + rospy.logwarn(uwb.network_id_str + "UWB has reconnected") + uwb.localize_2_5D() uwb.range_all() @@ -35,13 +42,13 @@ def timer_callback(e): distances_pub.publish(distances) - if uwb.pose[0] == 0 and uwb.pose[1] == 0 and uwb.pose[2] == 0: - uwb_error_handler() - return + # if uwb.pose[0] == 0 and uwb.pose[1] == 0 and uwb.pose[2] == 0: + # uwb_error_handler() + # return - if uwb.pose[0] == last_pose[0] and uwb.pose[1] == last_pose[1] and uwb.pose[2] == last_pose[2]: - uwb_error_handler() - return + # if uwb.pose[0] == last_pose[0] and uwb.pose[1] == last_pose[1] and uwb.pose[2] == last_pose[2]: + # uwb_error_handler() + # return pose.header.stamp = now pose.header.frame_id = "map" @@ -61,12 +68,14 @@ def timer_callback(e): if __name__ == "__main__": rospy.init_node("uwb_localization", anonymous=False) if uwb.connect(): - uwb.reset() rospy.loginfo("Pozyx UWB connected") - if not uwb.connect(): - uwb.reset() - rospy.loginfo('Connect error') - exit() + while not uwb.connect() and not rospy.is_shutdown(): + validate = uwb.validate() + if not validate: + rospy.logwarn_throttle(1, uwb.network_id_str + " UWB not validated") + uwb.connect() + if uwb.validate(): + rospy.logwarn(uwb.network_id_str + "UWB has reconnected") uwb.load_env_config(rospy.get_param("~config_file_path")) diff --git a/low_cost_ws/src/localization/src/multilateration_ros.py b/low_cost_ws/src/localization/src/multilateration_ros.py new file mode 100755 index 0000000..b4f6522 --- /dev/null +++ b/low_cost_ws/src/localization/src/multilateration_ros.py @@ -0,0 +1,134 @@ +#! /usr/bin/env python3 + +import time + +import numpy as np +import rospy +import yaml +from geometry_msgs.msg import PoseStamped +from scipy.optimize import minimize +from std_msgs.msg import Float64MultiArray + +method = "Nelder-Mead" + +minimize_options = {} +minimize_options["Nelder-Mead"] = { + "maxiter": int(1e4), + "maxfev": int(1e4), + "xatol": 1e-6, + "fatol": 1e-6, +} +minimize_options["TNC"] = {"maxfun": int(1e3)} +minimize_options["BFGS"] = {"maxiter": int(1e4)} + + +class Args: + def __init__(self, dictionary): + for key, value in dictionary.items(): + setattr(self, key, value) + + +class AnchorLoader: + @staticmethod + def load_yaml(yaml_file): + with open(yaml_file, "r") as file: + try: + data = yaml.safe_load(file) + return data + except yaml.YAMLError as e: + print(f"Error loading YAML file: {e}") + return None + + @staticmethod + def convert_to_numpy(data): + anchor_list = [] + for key, value in data.items(): + anchor_list.append([value["x"], value["y"], value["z"]]) + return np.array(anchor_list) + + @staticmethod + def load_and_convert(yaml_file): + data = AnchorLoader.load_yaml(yaml_file) + if data: + return AnchorLoader.convert_to_numpy(data) + else: + return None + + +class Multilateration: + def __init__(self, args): + self.distances_msg = Float64MultiArray() + self.distances_sub = rospy.Subscriber("uwb1/distances", Float64MultiArray, self.distances_callback) + self.pose_pub = rospy.Publisher("uwb1/pose", PoseStamped, queue_size=100) + self.distances = np.zeros((10, 6)) + self.anchor_pos = AnchorLoader.load_and_convert(args.config_path) + self.robot_pos = np.zeros((1, 3)) + self.optim_pos = np.zeros((1, 3)) + print("Anchor poses:\n", self.anchor_pos) + + def distances_callback(self, msg): + msg.data = np.array(msg.data) + self.distances_msg = msg + self.distances = np.array([msg.data]) + self.optimize() + self.pub_pose() + + def residual(self, robot_pos): + estimated_distances = np.linalg.norm(self.anchor_pos - robot_pos, axis=1) + error = estimated_distances - self.distances + squared_error = np.square(error) + return np.sum(squared_error) + + def optimize(self): + # time this function + start_time = time.time() + result = minimize(self.residual, self.optim_pos, method=method, options=minimize_options[method]) + end_time = time.time() + time_elapsed = end_time - start_time + + if 0 and time_elapsed * 1000.0 > 10.0 or not result.success: + print("Time elapsed: {:.2f} ms".format(time_elapsed * 1000.0)) + print(f"nfev: {result.nfev}") + print(f"nit: {result.nit}") + print(f"succes: {result.success}") + self.optim_pos = np.array([result.x]) + if result.success: + self.robot_pos = np.array([result.x]) + else: + print("Optimization failed to converge.") + print( + "Robot Coordinates (x, y, z): ({:.2f}, {:.2f}, {:.2f})".format( + self.robot_pos[0][0] / 1000.0, + self.robot_pos[0][1] / 1000.0, + self.robot_pos[0][2] / 1000.0, + ) + ) + + def pub_pose(self): + pose = PoseStamped() + pose.header.stamp = rospy.Time.now() + pose.header.frame_id = "wamv/uwb_origin" + pose.pose.position.x = self.robot_pos[0][0] / 1000.0 + pose.pose.position.y = self.robot_pos[0][1] / 1000.0 + pose.pose.position.z = self.robot_pos[0][2] / 1000.0 + self.pose_pub.publish(pose) + + def update_robot_position(self, new_position): + # Calculate the difference between the new and old positions + diff = new_position - self.robot_pos + norm_diff = np.linalg.norm(diff) + + if norm_diff > 0: + # Scale the difference to stay within the max_position_update_distance + scaling_factor = min(norm_diff, self.max_position_update_distance * 1000) / norm_diff + print("Scaling factor: {:.2f}".format(scaling_factor)) + self.robot_pos += scaling_factor * diff * self.position_update_rate + + +if __name__ == "__main__": + rospy.init_node("multilateration", anonymous=False) + config_path = rospy.get_param("~config_path") + args = {"config_path": config_path} + args = Args(args) + multilateration = Multilateration(args) + rospy.spin() diff --git a/low_cost_ws/src/localization/src/odom_cov.py b/low_cost_ws/src/localization/src/odom_cov.py index fcd72c4..bf61f20 100755 --- a/low_cost_ws/src/localization/src/odom_cov.py +++ b/low_cost_ws/src/localization/src/odom_cov.py @@ -3,13 +3,14 @@ import rospy from nav_msgs.msg import Odometry + def cb(msg): msg.twist.covariance = [0.01, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 5.0] + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 5.0] odom_pub.publish(msg) if __name__ == '__main__': diff --git a/low_cost_ws/src/localization/src/ranging.py b/low_cost_ws/src/localization/src/ranging.py new file mode 100755 index 0000000..72368e3 --- /dev/null +++ b/low_cost_ws/src/localization/src/ranging.py @@ -0,0 +1,51 @@ +#!/usr/bin/env python3 + +import rospy +from std_msgs.msg import Float64MultiArray +from uwb import UWB + +uwb = UWB() + +distances = Float64MultiArray() + + +def timer_callback(e): + # return + now = rospy.Time.now() + validate = uwb.validate() + if not validate: + rospy.logwarn_throttle(1, uwb.network_id_str + " UWB not validated") + uwb.connect() + if uwb.validate(): + rospy.logwarn(uwb.network_id_str + "UWB has reconnected") + + uwb.range_all() + + distances.data = [0.0 for _ in uwb.ranges_all] + + for i in range(len(uwb.ranges_all)): + if uwb.ranges_all[i].distance != 0 and uwb.ranges_all[i].distance < 200000: + distances.data[i] = uwb.ranges_all[i].distance + + distances_pub.publish(distances) + + # print(uwb.network_id, distances.data, validate, end='\r') + + +if __name__ == "__main__": + rospy.init_node("uwb_ranging", anonymous=False) + + config_path = rospy.get_param("~config_path") + port = rospy.get_param("~port", None) + rate = rospy.get_param("~rate", 10) + network_id = rospy.get_param("~network_id", "0x6A1B") + # hex to int + network_id = int(network_id, 16) + uwb.connect(port, network_id) + print(f"uwb.network_id: {uwb.network_id_str}") + uwb.load_env_config(config_path) + # uwb.write_env_config() + + distances_pub = rospy.Publisher("distances", Float64MultiArray, queue_size=10) + localization_timer = rospy.Timer(rospy.Duration(1 / rate), timer_callback) + rospy.spin() diff --git a/low_cost_ws/src/localization/src/uwb.py b/low_cost_ws/src/localization/src/uwb.py index a3bf515..16b3735 100644 --- a/low_cost_ws/src/localization/src/uwb.py +++ b/low_cost_ws/src/localization/src/uwb.py @@ -1,20 +1,21 @@ -# AUTOGENERATED! DO NOT EDIT! File to edit: ../06_uwb.ipynb. - -# %% auto 0 -__all__ = ['UWB'] +from typing import List -# %% ../06_uwb.ipynb 4 -import yaml -import serial -from serial.tools.list_ports import comports import pypozyx -from pypozyx import PozyxSerial -from pypozyx import NetworkID -from pypozyx import Coordinates, DeviceCoordinates -from pypozyx import DeviceRange -from pypozyx import PozyxConstants +import serial +import yaml +from pypozyx import ( + Coordinates, + DeviceCoordinates, + DeviceRange, + NetworkID, + PozyxConstants, + PozyxSerial, +) from pypozyx.core import PozyxException -from typing import List +from pypozyx.definitions.constants import POZYX_SUCCESS +from pypozyx.structures.generic import SingleRegister +from serial.tools.list_ports import comports + # %% ../06_uwb.ipynb 5 class UWB(): @@ -28,10 +29,12 @@ def __init__(self, port = None): self.anchor_ids = [] self._height = 500 + + self._status = 0 @property def network_id(self): - return self._network_id + return self._network_id.id @property def network_id_str(self) -> str: @@ -56,10 +59,6 @@ def network_id(self, value: int = None) -> None: else: self._network_id = NetworkID(value) - -# %% ../06_uwb.ipynb 6 -class UWB(UWB): - # pose @property def pose(self) -> List[float]: """A getter method of UWB pose @@ -83,10 +82,6 @@ def pose(self, value: List[float] = None) -> None: self._pose.y = value[1] self._pose.z = value[2] - -# %% ../06_uwb.ipynb 7 -class UWB(UWB): - # height @property def height(self) -> float: """A getter method of UWB pose height @@ -106,9 +101,6 @@ def height(self, value: float = 0) -> float: self._height = value -# %% ../06_uwb.ipynb 8 -class UWB(UWB): - # env_config @property def env_config(self) -> dict: """A getter method of environment config @@ -119,9 +111,6 @@ def env_config(self) -> dict: return self._env_config -# %% ../06_uwb.ipynb 9 -class UWB(UWB): - # port_lost def port_list(self) -> List[str]: """A getter method of port list. @@ -131,9 +120,6 @@ def port_list(self) -> List[str]: return self._port_list -# %% ../06_uwb.ipynb 10 -class UWB(UWB): - # status @property def status(self) -> int: """A getter method of UWB status. @@ -143,9 +129,6 @@ def status(self) -> int: """ return self._status - -# %% ../06_uwb.ipynb 11 -class UWB(UWB): def load_env_config(self, config_file_path: str) -> bool: """Load UWB anchors' environment config. @@ -160,79 +143,86 @@ def load_env_config(self, config_file_path: str) -> bool: self._env_config = yaml.safe_load(config_file) self.ranges_all = [DeviceRange() for _ in range(len(self._env_config))] self.anchor_ids = [config[1]['id'] for config in self._env_config.items()] - print(self.anchor_ids) + print(hex(self.network_id), [hex(anchor_id) for anchor_id in self.anchor_ids]) except yaml.YAMLError as ex: - print(ex) + # print(ex) return False return True - -# %% ../06_uwb.ipynb 12 -class UWB(UWB): - def scan_port(self) -> None: + def scan_port(self, network_id=None) -> None: """Scan all port connecting to host. Store port device path in port list. """ - self._port_list = [] + if network_id is None: + network_id = self.network_id + + port_list = [] for port in comports(): try: if "Pozyx Labs" in port.manufacturer: - self._port_list.append(port.device) - break + port_list.append(port.device) + continue except TypeError: pass try: if "Pozyx" in port.product: - self._port_list.append(port.device) - break + port_list.append(port.device) + continue except TypeError: pass + print(f'port_list: {port_list}') + for port in port_list: + try: + pozyx_handler = PozyxSerial(port) + network_id_ = NetworkID() + pozyx_handler.getNetworkId(network_id_) + print(f'read net: {network_id_}') + print(f'network_id: {hex(network_id)}') + print(f'self network_id: {hex(self.network_id)}') + if network_id == network_id_: + return port + except Exception as e: + # print(e) + pass -# %% ../06_uwb.ipynb 13 -class UWB(UWB): - def reset(self) -> bool: - s = 'F,b0,,1' - if self.port != None: - ser = serial.Serial(self.port) - ser.write(s.encode()) - - def connect(self) -> bool: + def connect(self, port=None, network_id=None) -> bool: """Try to connect pozyx device. Returns: bool: Pozyx status """ self._status = PozyxConstants.STATUS_SUCCESS - if self.port is None: - self.scan_port() - if len(self._port_list) == 1: - self.port = self._port_list[0] - try: - self._pozyx_handler = PozyxSerial(self.port) - self._status &= self._pozyx_handler.getNetworkId(self._network_id) - return self._status - except PozyxException as ex: - print(ex) - self.reset() - return False - elif len(self._port_list) == 0: - print('No Pozyx devices found') - self._status = PozyxConstants.STATUS_FAILURE - return False - else: - return False + + if network_id is not None: + self.network_id = network_id + + if port is not None and network_id is None: + self.port = port else: - try: - self._pozyx_handler = PozyxSerial(self.port) - self._status &= self._pozyx_handler.getNetworkId(self._network_id) - return True - except PozyxException as ex: - print(ex) - self.reset() - return False - + self.port = self.scan_port(network_id) + + print(f'port: {self.port}') + + try: + self._pozyx_handler = PozyxSerial(self.port) + self._status &= self._pozyx_handler.getNetworkId(self._network_id) + return True + except Exception as e: + # print(e) + return False + + + def validate(self, clear_port=False) -> bool: + whoami = SingleRegister() + try: + ret_validate = self._pozyx_handler.getWhoAmI(whoami) == POZYX_SUCCESS + except: + return False + + if clear_port and not ret_validate: + self.port = None + + return ret_validate -# %% ../06_uwb.ipynb 14 -class UWB(UWB): def write_env_config(self) -> bool: """Write environment anchor location into Pozyx UWB device. @@ -250,9 +240,6 @@ def write_env_config(self) -> bool: self._status &= self._pozyx_handler.setSelectionOfAnchorsAutomatic(len(self.env_config)) return self._status - -# %% ../06_uwb.ipynb 15 -class UWB(UWB): def localize_2_5D(self) -> bool: """Localize method in 2.5D. Need to know height. @@ -267,10 +254,7 @@ def localize_2_5D(self) -> bool: ) return self._status - -# %% ../06_uwb.ipynb 16 -class UWB(UWB): - def localize_3D(self)->bool: + def localize_3D(self) -> bool: """Localize method in 3D. The height will be determined by Pozyx UWB device. Returns: @@ -284,9 +268,6 @@ def localize_3D(self)->bool: ) return self._status - -# %% ../06_uwb.ipynb 17 -class UWB(UWB): def range_from(self, dest_id) -> float: """Range method from this Pozyx UWB device to the destination Pozyx UWB device. @@ -300,10 +281,7 @@ def range_from(self, dest_id) -> float: self._status &= self._pozyx_handler.doRanging(dest_id, ranges) return ranges - -# %% ../06_uwb.ipynb 18 -class UWB(UWB): - def range_all(self) -> float: + def range_all(self) -> bool: """Range method from this Pozyx UWB device to the destination Pozyx UWB device. Args: @@ -312,7 +290,9 @@ def range_all(self) -> float: Returns: float: The range from this Pozyx UWB device to the destination Pozyx UWB device. """ - for i, anchor_id in enumerate(self.anchor_ids): - self._status &= self._pozyx_handler.doRanging(anchor_id, self.ranges_all[i]) - - + try: + for i, anchor_id in enumerate(self.anchor_ids): + self._status &= self._pozyx_handler.doRanging(anchor_id, self.ranges_all[i]) + return True + except: + return False From b447fd1d4d0d4a8242f3ebc3c93a155274371fbd Mon Sep 17 00:00:00 2001 From: sunfu-chou Date: Thu, 12 Oct 2023 20:46:29 +0800 Subject: [PATCH 34/52] Feat: Update uwb node --- low_cost_ws/src/localization/src/localization.py | 7 ++++--- low_cost_ws/src/localization/src/uwb.py | 2 ++ 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/low_cost_ws/src/localization/src/localization.py b/low_cost_ws/src/localization/src/localization.py index e294dba..93d7aa8 100755 --- a/low_cost_ws/src/localization/src/localization.py +++ b/low_cost_ws/src/localization/src/localization.py @@ -67,8 +67,9 @@ def timer_callback(e): if __name__ == "__main__": rospy.init_node("uwb_localization", anonymous=False) - if uwb.connect(): - rospy.loginfo("Pozyx UWB connected") + + uwb.connect() + while not uwb.connect() and not rospy.is_shutdown(): validate = uwb.validate() if not validate: @@ -83,5 +84,5 @@ def timer_callback(e): uwb.write_env_config() pose_pub = rospy.Publisher("uwb_pose", PoseWithCovarianceStamped, queue_size=10) distances_pub = rospy.Publisher("uwb_distances", Float64MultiArray, queue_size=10) - localization_timer = rospy.Timer(rospy.Duration(0.05), timer_callback) + localization_timer = rospy.Timer(rospy.Duration(0.2), timer_callback) rospy.spin() diff --git a/low_cost_ws/src/localization/src/uwb.py b/low_cost_ws/src/localization/src/uwb.py index 16b3735..39f149b 100644 --- a/low_cost_ws/src/localization/src/uwb.py +++ b/low_cost_ws/src/localization/src/uwb.py @@ -170,6 +170,8 @@ def scan_port(self, network_id=None) -> None: except TypeError: pass print(f'port_list: {port_list}') + if len(port_list) == 1: + return port_list[0] for port in port_list: try: pozyx_handler = PozyxSerial(port) From cb50fa5277f51950637071b998256dfa9412b71d Mon Sep 17 00:00:00 2001 From: sunfu-chou Date: Thu, 12 Oct 2023 21:18:59 +0800 Subject: [PATCH 35/52] Config: Update uwb config for ee632 --- .../src/localization/config/ee632.yaml | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/low_cost_ws/src/localization/config/ee632.yaml b/low_cost_ws/src/localization/config/ee632.yaml index b2182e1..352629f 100644 --- a/low_cost_ws/src/localization/config/ee632.yaml +++ b/low_cost_ws/src/localization/config/ee632.yaml @@ -1,36 +1,36 @@ anchor12: id: 0x6a45 - x: 0 - y: 3050 + x: 6500 + y: 6750 z: 950 anchor8: id: 0x6a27 x: 0 - y: 5170 + y: 5020 z: 950 anchor9: id: 0x6a4a - x: 0 - y: 6970 + x: 6500 + y: 4800 z: 950 anchor15: id: 0x6a21 - x: 6500 - y: 2750 + x: 0 + y: 6900 z: 950 anchor14: id: 0x6a42 - x: 6500 - y: 4720 + x: 0 + y: 2920 z: 950 anchor11: id: 0x6a7a x: 6500 - y: 6570 + y: 2700 z: 950 From adbd11b1b79bb6d613a7afab99c82392a2250114 Mon Sep 17 00:00:00 2001 From: wellyowo Date: Thu, 12 Oct 2023 21:50:17 +0800 Subject: [PATCH 36/52] Update: change set ros master ros ip method for locobot --- locobot/environment.sh | 30 +++++++++++++++++++++++++++++- locobot/set_wfh_workspace_env.sh | 31 ++++++++++++++++++++++++++++++- locobot/top_camera.sh | 31 ++++++++++++++++++++++++++++++- locobot/vr_arm_control.sh | 31 ++++++++++++++++++++++++++++++- 4 files changed, 119 insertions(+), 4 deletions(-) diff --git a/locobot/environment.sh b/locobot/environment.sh index 9f19e7c..d45a2c0 100644 --- a/locobot/environment.sh +++ b/locobot/environment.sh @@ -1,5 +1,33 @@ #! /bin/bash +if [ "$1" ]; then + + echo "ROS MASRER $1" + + export ROS_MASTER_URI=http://$1:11311 + +else + + echo "ROS MASRER 127.0.0.1" + + export ROS_MASTER_URI=http://127.0.0.1:11311 + +fi + + + +if [ "$2" ]; then + + echo "ROS IP $2" + + export ROS_IP=$2 + +else + + echo "ROS IP 127.0.0.1" + + export ROS_IP=127.0.0.1 + +fi source /opt/ros/melodic/setup.bash source ~/WFH_locobot/ROS/catkin_ws/devel/setup.bash -source ~/WFH_locobot/set_ip.sh 127.0.0.1 127.0.0.1 diff --git a/locobot/set_wfh_workspace_env.sh b/locobot/set_wfh_workspace_env.sh index cec0c99..7076186 100644 --- a/locobot/set_wfh_workspace_env.sh +++ b/locobot/set_wfh_workspace_env.sh @@ -3,7 +3,36 @@ # load pyrobot env load_pyrobot_env # source WFH workspace and set_rospkg_path -source ~/WFH_locobot/set_ip.sh 127.0.0.1 127.0.0.1 +if [ "$1" ]; then + + echo "ROS MASRER $1" + + export ROS_MASTER_URI=http://$1:11311 + +else + + echo "ROS MASRER 127.0.0.1" + + export ROS_MASTER_URI=http://127.0.0.1:11311 + +fi + + + +if [ "$2" ]; then + + echo "ROS IP $2" + + export ROS_IP=$2 + +else + + echo "ROS IP 127.0.0.1" + + export ROS_IP=127.0.0.1 + +fi + #source ROS/catkin_ws/devel/setup.bash #source ROS/catkin_ws/devel_isolated/setup.bash source ~/WFH_locobot/set_rospackage_path.sh diff --git a/locobot/top_camera.sh b/locobot/top_camera.sh index 1a9627d..ac1988f 100644 --- a/locobot/top_camera.sh +++ b/locobot/top_camera.sh @@ -1,7 +1,36 @@ #!/bin/bash +if [ "$1" ]; then + + echo "ROS MASRER $1" + + export ROS_MASTER_URI=http://$1:11311 + +else + + echo "ROS MASRER 127.0.0.1" + + export ROS_MASTER_URI=http://127.0.0.1:11311 + +fi + + + +if [ "$2" ]; then + + echo "ROS IP $2" + + export ROS_IP=$2 + +else + + echo "ROS IP 127.0.0.1" + + export ROS_IP=127.0.0.1 + +fi + source ~/WFH_locobot/environment.sh -source ~/WFH_locobot/set_ip.sh 127.0.0.1 127.0.0.1 rosservice call /calibration #rostopic pub /tilt/command std_msgs/Float64 "data: 0.8" #rostopic pub /pan/command std_msgs/Float64 "data: 0.0" diff --git a/locobot/vr_arm_control.sh b/locobot/vr_arm_control.sh index d718c66..b6335ae 100644 --- a/locobot/vr_arm_control.sh +++ b/locobot/vr_arm_control.sh @@ -2,7 +2,36 @@ cd ~/WFH_locobot source ~/WFH_locobot/set_wfh_workspace_env.sh -source ~/WFH_locobot/set_ip.sh 127.0.0.1 127.0.0.1 +if [ "$1" ]; then + + echo "ROS MASRER $1" + + export ROS_MASTER_URI=http://$1:11311 + +else + + echo "ROS MASRER 127.0.0.1" + + export ROS_MASTER_URI=http://127.0.0.1:11311 + +fi + + + +if [ "$2" ]; then + + echo "ROS IP $2" + + export ROS_IP=$2 + +else + + echo "ROS IP 127.0.0.1" + + export ROS_IP=127.0.0.1 + +fi + rosrun oculusVR vrarm.py From eefb01cdaa648d9c90060c521f6dd7890983762d Mon Sep 17 00:00:00 2001 From: wellyowo Date: Thu, 12 Oct 2023 22:04:50 +0800 Subject: [PATCH 37/52] Update: set_ip method of turn on locobot --- locobot/turn_on_locobot.sh | 31 ++++++++++++++++++++++++++++++- 1 file changed, 30 insertions(+), 1 deletion(-) diff --git a/locobot/turn_on_locobot.sh b/locobot/turn_on_locobot.sh index 8ad764d..051fe7d 100644 --- a/locobot/turn_on_locobot.sh +++ b/locobot/turn_on_locobot.sh @@ -1,4 +1,33 @@ #! /bin/bash +if [ "$1" ]; then + + echo "ROS MASRER $1" + + export ROS_MASTER_URI=http://$1:11311 + +else + + echo "ROS MASRER 127.0.0.1" + + export ROS_MASTER_URI=http://127.0.0.1:11311 + +fi + + + +if [ "$2" ]; then + + echo "ROS IP $2" + + export ROS_IP=$2 + +else + + echo "ROS IP 127.0.0.1" + + export ROS_IP=127.0.0.1 + +fi + source ~/WFH_locobot/environment.sh -source ~/WFH_locobot/set_ip.sh 127.0.0.1 127.0.0.1 source ~/WFH_locobot/run_locobot.sh From 7889cf6503fbd722f1219ae24ce0bff66f71b39b Mon Sep 17 00:00:00 2001 From: hchengwang Date: Thu, 12 Oct 2023 22:43:26 +0800 Subject: [PATCH 38/52] mv arg_utils ros package src to scripts --- low_cost_ws/src/arg_utils/{src => scripts}/add_path.py | 0 low_cost_ws/src/arg_utils/{src => scripts}/testing_pypkg.py | 0 2 files changed, 0 insertions(+), 0 deletions(-) rename low_cost_ws/src/arg_utils/{src => scripts}/add_path.py (100%) rename low_cost_ws/src/arg_utils/{src => scripts}/testing_pypkg.py (100%) diff --git a/low_cost_ws/src/arg_utils/src/add_path.py b/low_cost_ws/src/arg_utils/scripts/add_path.py similarity index 100% rename from low_cost_ws/src/arg_utils/src/add_path.py rename to low_cost_ws/src/arg_utils/scripts/add_path.py diff --git a/low_cost_ws/src/arg_utils/src/testing_pypkg.py b/low_cost_ws/src/arg_utils/scripts/testing_pypkg.py similarity index 100% rename from low_cost_ws/src/arg_utils/src/testing_pypkg.py rename to low_cost_ws/src/arg_utils/scripts/testing_pypkg.py From c21f63e41ca1499246b686eb55be69e904379c6f Mon Sep 17 00:00:00 2001 From: hchengwang Date: Thu, 12 Oct 2023 23:08:21 +0800 Subject: [PATCH 39/52] rename testing_XXX -> test_XXX --- .../src/arg_utils/scripts/{testing_pypkg.py => test_pypkg.py} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename low_cost_ws/src/arg_utils/scripts/{testing_pypkg.py => test_pypkg.py} (100%) diff --git a/low_cost_ws/src/arg_utils/scripts/testing_pypkg.py b/low_cost_ws/src/arg_utils/scripts/test_pypkg.py similarity index 100% rename from low_cost_ws/src/arg_utils/scripts/testing_pypkg.py rename to low_cost_ws/src/arg_utils/scripts/test_pypkg.py From 4e2fc956e4e7b9b54ba1973030c633787ce0cb25 Mon Sep 17 00:00:00 2001 From: hchengwang Date: Thu, 12 Oct 2023 23:27:08 +0800 Subject: [PATCH 40/52] update test_pypkg.py as pytest --- .../include/arg_utils/import_me_if_u_can.py | 1 + .../include/for_example/import_me_if_u_can.py | 1 + low_cost_ws/src/arg_utils/scripts/test_pypkg.py | 16 ++++++++++++---- 3 files changed, 14 insertions(+), 4 deletions(-) diff --git a/low_cost_ws/src/arg_utils/include/arg_utils/import_me_if_u_can.py b/low_cost_ws/src/arg_utils/include/arg_utils/import_me_if_u_can.py index 5cb5b20..4dbc1ce 100644 --- a/low_cost_ws/src/arg_utils/include/arg_utils/import_me_if_u_can.py +++ b/low_cost_ws/src/arg_utils/include/arg_utils/import_me_if_u_can.py @@ -1,2 +1,3 @@ def say_it_works(): print("You have successed import me!\nfrom arg_utils pkg :D") + return "It works!" diff --git a/low_cost_ws/src/arg_utils/include/for_example/import_me_if_u_can.py b/low_cost_ws/src/arg_utils/include/for_example/import_me_if_u_can.py index a43803a..ab135bc 100644 --- a/low_cost_ws/src/arg_utils/include/for_example/import_me_if_u_can.py +++ b/low_cost_ws/src/arg_utils/include/for_example/import_me_if_u_can.py @@ -1,2 +1,3 @@ def say_it_works(): print("You have successed import me!\nfrom for_example pkg :D") + return "It works!" diff --git a/low_cost_ws/src/arg_utils/scripts/test_pypkg.py b/low_cost_ws/src/arg_utils/scripts/test_pypkg.py index d45063f..6c5a938 100644 --- a/low_cost_ws/src/arg_utils/scripts/test_pypkg.py +++ b/low_cost_ws/src/arg_utils/scripts/test_pypkg.py @@ -1,8 +1,16 @@ #!/usr/bin/env python3 import add_path -from arg_utils.import_me_if_u_can import * -from for_example.import_me_if_u_can import say_it_works as sat_it_works_2 -say_it_works() -say_it_works_2() \ No newline at end of file +from arg_utils.import_me_if_u_can import say_it_works as say_it_works +from for_example.import_me_if_u_can import say_it_works as say_it_works_2 + +# write a test function for say_it_works +def test_say_it_works(): + assert say_it_works() == "It works!" + +def test_say_it_works_2(): + assert say_it_works_2() == "It works!" + +#say_it_works() +#say_it_works_2() From ed4159736821c676748fcd7e1ae7f81119eafdbd Mon Sep 17 00:00:00 2001 From: sunfu-chou Date: Fri, 13 Oct 2023 02:08:18 +0800 Subject: [PATCH 41/52] Docker: Add python3-tk --- docker/dockerfile | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/docker/dockerfile b/docker/dockerfile index 4ae1a59..0c99fc9 100644 --- a/docker/dockerfile +++ b/docker/dockerfile @@ -44,7 +44,8 @@ RUN apt-get -o Acquire::ForceIPv4=true update && apt-get -yq dist-upgrade \ python3-pip \ python3-setuptools \ apt-transport-https \ - libglew-dev + libglew-dev \ + python3-tk RUN pip3 install --upgrade pip \ && pip3 install --upgrade setuptools \ From 68626b14d55091c33246cd44aa5e9bf0d4708b33 Mon Sep 17 00:00:00 2001 From: hchengwang Date: Fri, 13 Oct 2023 02:59:03 +0800 Subject: [PATCH 42/52] add transformation and plotting libs --- .../arg_utils/include/arg_utils/plotting.py | 182 ++ .../include/arg_utils/transformations.py | 1973 +++++++++++++++++ .../src/arg_utils/scripts/plot_lines.py | 22 + .../src/arg_utils/scripts/plot_poses.py | 60 + .../arg_utils/scripts/test_transformations.py | 8 + 5 files changed, 2245 insertions(+) create mode 100644 low_cost_ws/src/arg_utils/include/arg_utils/plotting.py create mode 100644 low_cost_ws/src/arg_utils/include/arg_utils/transformations.py create mode 100644 low_cost_ws/src/arg_utils/scripts/plot_lines.py create mode 100644 low_cost_ws/src/arg_utils/scripts/plot_poses.py create mode 100644 low_cost_ws/src/arg_utils/scripts/test_transformations.py diff --git a/low_cost_ws/src/arg_utils/include/arg_utils/plotting.py b/low_cost_ws/src/arg_utils/include/arg_utils/plotting.py new file mode 100644 index 0000000..962a8b9 --- /dev/null +++ b/low_cost_ws/src/arg_utils/include/arg_utils/plotting.py @@ -0,0 +1,182 @@ +""" +This file is part of Learned Inertial Model Odometry. +Copyright (C) 2023 Giovanni Cioffi +(Robotics and Perception Group, University of Zurich, Switzerland). +This file is subject to the terms and conditions defined in the file +'LICENSE', which is part of this source code package. +""" + +import matplotlib.gridspec as gridspec +import matplotlib.pyplot as plt + + +def xy_plot(title, labelx, labely, + vec1, label1, + vec2 = None, label2 = None, + vec3 = None, label3 = None, + vec4 = None, label4 = None): + plt.plot(vec1[:, 0], vec1[:, 1], label=label1) + if vec2 is not None: + plt.plot(vec2[:, 0], vec2[:, 1], label=label2) + if vec3 is not None: + plt.plot(vec3[:, 0], vec3[:, 1], label=label3) + if vec4 is not None: + plt.plot(vec4[:, 0], vec4[:, 1], label=label4) + plt.grid() + plt.legend() + plt.xlabel(labelx) + plt.ylabel(labely) + plt.title(title) + + +def xyzt_plot(title, + vec1, label1, + vec2 = None, label2 = None, + vec3 = None, label3 = None, + vec4 = None, label4 = None): + plt.subplot(311) + plt.plot(vec1[:,0], vec1[:,1], label=label1) + if vec2 is not None: + plt.plot(vec2[:,0], vec2[:,1], label=label2) + if vec3 is not None: + plt.plot(vec3[:,0], vec3[:,1], label=label3) + if vec4 is not None: + plt.plot(vec4[:,0], vec4[:,1], label=label4) + plt.grid() + plt.legend() + plt.xlabel('t') + plt.ylabel('x') + plt.title(title) + + plt.subplot(312) + plt.plot(vec1[:,0], vec1[:,2], label=label1) + if vec2 is not None: + plt.plot(vec2[:,0], vec2[:,2], label=label2) + if vec3 is not None: + plt.plot(vec3[:,0], vec3[:,2], label=label3) + if vec4 is not None: + plt.plot(vec4[:,0], vec4[:,2], label=label4) + plt.grid() + plt.legend() + plt.xlabel('t') + plt.ylabel('y') + + plt.subplot(313) + plt.plot(vec1[:,0], vec1[:,3], label=label1) + if vec2 is not None: + plt.plot(vec2[:,0], vec2[:,3], label=label2) + if vec3 is not None: + plt.plot(vec3[:,0], vec3[:,3], label=label3) + if vec4 is not None: + plt.plot(vec4[:,0], vec4[:,3], label=label4) + plt.grid() + plt.legend() + plt.xlabel('t') + plt.ylabel('z') + + +def plot_biases(ts, bg, ba): + fig = plt.figure('IMU biases') + plt.subplot(211) + plt.plot(ts, bg[:,0], label="x") + plt.plot(ts, bg[:,1], label="y") + plt.plot(ts, bg[:,2], label="z") + plt.grid() + plt.legend() + plt.title('Gyro bias') + plt.xlabel('t') + plt.ylabel('bias [rad/s]') + + plt.subplot(212) + plt.plot(ts, ba[:,0], label="x") + plt.plot(ts, ba[:,1], label="y") + plt.plot(ts, ba[:,2], label="z") + plt.grid() + plt.legend() + plt.title('Accel bias') + plt.xlabel('t') + plt.ylabel('bias [m/s2]') + + +def make_position_plots(traj, gt): + # 2d positions + fig = plt.figure('2D views') + gs = gridspec.GridSpec(2, 2) + + fig.add_subplot(gs[:, 0]) + xyPlot('XY plot', 'x', 'y', + traj[:, 1:3], 'estim. traj', + gt[:, 1:3], 'gt') + + fig.add_subplot(gs[0, 1]) + xyPlot('XZ plot', 'x', 'z', + traj[:, [1,3]], 'estim. traj', + gt[:, [1,3]], 'gt') + + fig.add_subplot(gs[1, 1]) + xyPlot('YZ plot', 'y', 'z', + traj[:, [2,3]], 'estim. traj', + gt[:, [2,3]], 'gt') + + # xyz time plots + plt.figure('XYZt view') + xyztPlot('XYZt', traj[:,:4], 'estim. traj', gt[:,:4], 'gt') + + +def make_velocity_plots(est_vel, gt_vel): + plt.figure("Velocity") + + plt.subplot(311) + plt.plot(gt_vel[:,0], gt_vel[:,1], label='gt') + plt.plot(est_vel[:,0], est_vel[:,1], label='est') + plt.title('x') + plt.xlabel('t') + plt.legend() + plt.grid() + + plt.subplot(312) + plt.plot(gt_vel[:,0], gt_vel[:,2], label='gt') + plt.plot(est_vel[:,0], est_vel[:,2], label='est') + plt.title('y') + plt.xlabel('t') + plt.legend() + plt.grid() + + plt.subplot(313) + plt.plot(gt_vel[:,0], gt_vel[:,3], label='gt') + plt.plot(est_vel[:,0], est_vel[:,3], label='est') + plt.title('z') + plt.xlabel('t') + plt.legend() + plt.grid() + + +def make_ori_euler_plots(est_xyz, gt_xyz): + plt.figure("Orientation [Euler angles]") + + plt.subplot(311) + plt.plot(gt_xyz[:, 0], gt_xyz[:, 1], label='gt') + plt.plot(est_xyz[:, 0], est_xyz[:, 1], label='est') + plt.title('Roll') + plt.ylabel('x') + plt.xlabel('t') + plt.legend() + plt.grid() + + plt.subplot(312) + plt.plot(gt_xyz[:, 0], gt_xyz[:, 2], label='gt') + plt.plot(est_xyz[:, 0], est_xyz[:, 2], label='est') + plt.title('Pitch') + plt.ylabel('y') + plt.xlabel('t') + plt.legend() + plt.grid() + + plt.subplot(313) + plt.plot(gt_xyz[:, 0], gt_xyz[:, 3], label='gt') + plt.plot(est_xyz[:, 0], est_xyz[:, 3], label='est') + plt.title('Yaw') + plt.ylabel('z') + plt.xlabel('t') + plt.legend() + plt.grid() diff --git a/low_cost_ws/src/arg_utils/include/arg_utils/transformations.py b/low_cost_ws/src/arg_utils/include/arg_utils/transformations.py new file mode 100644 index 0000000..edf4302 --- /dev/null +++ b/low_cost_ws/src/arg_utils/include/arg_utils/transformations.py @@ -0,0 +1,1973 @@ +# -*- 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 skew(v): + """Returns the skew-symmetric matrix of a vector + cfo, 2015/08/13 + + """ + return numpy.array([[0, -v[2], v[1]], + [v[2], 0, -v[0]], + [-v[1], v[0], 0]], dtype=numpy.float64) + + +def unskew(R): + """Returns the coordinates of a skew-symmetric matrix + cfo, 2015/08/13 + + """ + return numpy.array([R[2, 1], R[0, 2], R[1, 0]], dtype=numpy.float64) + + +def first_order_rotation(rotvec): + """First order approximation of a rotation: I + skew(rotvec) + cfo, 2015/08/13 + + """ + R = numpy.zeros((3, 3), dtype=numpy.float64) + R[0, 0] = 1.0 + R[1, 0] = rotvec[2] + R[2, 0] = -rotvec[1] + R[0, 1] = -rotvec[2] + R[1, 1] = 1.0 + R[2, 1] = rotvec[0] + R[0, 2] = rotvec[1] + R[1, 2] = -rotvec[0] + R[2, 2] = 1.0 + return R + + +def axis_angle(axis, theta): + """Compute a rotation matrix from an axis and an angle. + Returns 3x3 Matrix. + Is the same as transformations.rotation_matrix(theta, axis). + cfo, 2015/08/13 + + """ + if theta*theta > _EPS: + wx = axis[0] + wy = axis[1] + wz = axis[2] + costheta = numpy.cos(theta) + sintheta = numpy.sin(theta) + c_1 = 1.0 - costheta + wx_sintheta = wx * sintheta + wy_sintheta = wy * sintheta + wz_sintheta = wz * sintheta + C00 = c_1 * wx * wx + C01 = c_1 * wx * wy + C02 = c_1 * wx * wz + C11 = c_1 * wy * wy + C12 = c_1 * wy * wz + C22 = c_1 * wz * wz + R = numpy.zeros((3, 3), dtype=numpy.float64) + R[0, 0] = costheta + C00 + R[1, 0] = wz_sintheta + C01 + R[2, 0] = -wy_sintheta + C02 + R[0, 1] = -wz_sintheta + C01 + R[1, 1] = costheta + C11 + R[2, 1] = wx_sintheta + C12 + R[0, 2] = wy_sintheta + C02 + R[1, 2] = -wx_sintheta + C12 + R[2, 2] = costheta + C22 + return R + else: + return first_order_rotation(axis*theta) + + +def expmap_so3(rotvec): + """Exponential map at identity. + Create a rotation from canonical coordinates using Rodrigues' formula. + cfo, 2015/08/13 + + """ + theta = numpy.linalg.norm(rotvec) + axis = rotvec/theta + return axis_angle(axis, theta) + + +def logmap_so3(R): + """Logmap at the identity. + Returns canonical coordinates of rotation. + cfo, 2015/08/13 + + """ + R11 = R[0, 0] + R12 = R[0, 1] + R13 = R[0, 2] + R21 = R[1, 0] + R22 = R[1, 1] + R23 = R[1, 2] + R31 = R[2, 0] + R32 = R[2, 1] + R33 = R[2, 2] + tr = numpy.trace(R) + omega = numpy.empty((3,), dtype=numpy.float64) + + # when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, we do something + # special + if(numpy.abs(tr + 1.0) < 1e-10): + if(numpy.abs(R33 + 1.0) > 1e-10): + omega = (numpy.pi / numpy.sqrt(2.0 + 2.0 * R33)) * \ + numpy.array([R13, R23, 1.0+R33]) + elif(numpy.abs(R22 + 1.0) > 1e-10): + omega = (numpy.pi / numpy.sqrt(2.0 + 2.0 * R22)) * \ + numpy.array([R12, 1.0+R22, R32]) + else: + omega = (numpy.pi / numpy.sqrt(2.0 + 2.0 * R11)) * \ + numpy.array([1.0+R11, R21, R31]) + else: + magnitude = 1.0 + tr_3 = tr - 3.0 + if tr_3 < -1e-7: + theta = numpy.arccos((tr - 1.0) / 2.0) + magnitude = theta / (2.0 * numpy.sin(theta)) + else: + # when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0) + # use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2) + magnitude = 0.5 - tr_3 * tr_3 / 12.0 + + omega = magnitude * numpy.array([R32 - R23, R13 - R31, R21 - R12]) + + return omega + + +def right_jacobian_so3(rotvec): + """Right Jacobian for Exponential map in SO(3) + Equation (10.86) and following equations in G.S. Chirikjian, "Stochastic + Models, Information Theory, and Lie Groups", Volume 2, 2008. + + > expmap_so3(thetahat + omega) \approx expmap_so3(thetahat) * expmap_so3(Jr * omega) + where Jr = right_jacobian_so3(thetahat); + This maps a perturbation in the tangent space (omega) to a perturbation + on the manifold (expmap_so3(Jr * omega)) + cfo, 2015/08/13 + + """ + + theta2 = numpy.dot(rotvec, rotvec) + if theta2 <= _EPS: + return numpy.identity(3, dtype=numpy.float64) + else: + theta = numpy.sqrt(theta2) + Y = skew(rotvec) / theta + I_3x3 = numpy.identity(3, dtype=numpy.float64) + J_r = I_3x3 - ((1.0 - numpy.cos(theta)) / theta) * Y + \ + (1.0 - numpy.sin(theta) / theta) * numpy.dot(Y, Y) + return J_r + + +def S_inv_eulerZYX_body(euler_coordinates): + """ Relates angular rates w to changes in eulerZYX coordinates. + dot(euler) = S^-1(euler_coordinates) * omega + Also called: rotation-rate matrix. (E in Lupton paper) + cfo, 2015/08/13 + + """ + y = euler_coordinates[1] + z = euler_coordinates[2] + E = numpy.zeros((3, 3)) + E[0, 1] = numpy.sin(z)/numpy.cos(y) + E[0, 2] = numpy.cos(z)/numpy.cos(y) + E[1, 1] = numpy.cos(z) + E[1, 2] = -numpy.sin(z) + E[2, 0] = 1.0 + E[2, 1] = numpy.sin(z)*numpy.sin(y)/numpy.cos(y) + E[2, 2] = numpy.cos(z)*numpy.sin(y)/numpy.cos(y) + return E + + +def S_inv_eulerZYX_body_deriv(euler_coordinates, omega): + """ Compute dE(euler_coordinates)*omega/deuler_coordinates + cfo, 2015/08/13 + + """ + + y = euler_coordinates[1] + z = euler_coordinates[2] + + """ + w1 = omega[0]; w2 = omega[1]; w3 = omega[2] + J = numpy.zeros((3,3)) + J[0,0] = 0 + J[0,1] = math.tan(y) / math.cos(y) * (math.sin(z) * w2 + math.cos(z) * w3) + J[0,2] = w2/math.cos(y)*math.cos(z) - w3/math.cos(y)*math.sin(z) + J[1,0] = 0 + J[1,1] = 0 + J[1,2] = -w2*math.sin(z) - w3*math.cos(z) + J[2,0] = w1 + J[2,1] = 1.0/math.cos(y)**2 * (w2 * math.sin(z) + w3 * math.cos(z)) + J[2,2] = w2*math.tan(y)*math.cos(z) - w3*math.tan(y)*math.sin(z) + + """ + + # second version, x = psi, y = theta, z = phi + # J_x = numpy.zeros((3,3)) + J_y = numpy.zeros((3, 3)) + J_z = numpy.zeros((3, 3)) + + # dE^-1/dtheta + J_y[0, 1] = math.tan(y)/math.cos(y)*math.sin(z) + J_y[0, 2] = math.tan(y)/math.cos(y)*math.cos(z) + J_y[2, 1] = math.sin(z)/(math.cos(y))**2 + J_y[2, 2] = math.cos(z)/(math.cos(y))**2 + + # dE^-1/dphi + J_z[0, 1] = math.cos(z)/math.cos(y) + J_z[0, 2] = -math.sin(z)/math.cos(y) + J_z[1, 1] = -math.sin(z) + J_z[1, 2] = -math.cos(z) + J_z[2, 1] = math.cos(z)*math.tan(y) + J_z[2, 2] = -math.sin(z)*math.tan(y) + + J = numpy.zeros((3, 3)) + J[:, 1] = numpy.dot(J_y, omega) + J[:, 2] = numpy.dot(J_z, omega) + + return J + + +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 convert_3x3_to_4x4(matrix_3x3): + M = numpy.identity(4) + M[:3, :3] = matrix_3x3 + return M + + +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 %s" % 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 /= 2.0 + aj /= 2.0 + 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 matrix_from_quaternion(quaternion): + return quaternion_matrix(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 quaternionJPL_matrix(quaternion): + """Return homogeneous rotation matrix from quaternion in JPL notation. + quaternion = [x y z w] + """ + q0 = quaternion[0] + q1 = quaternion[1] + q2 = quaternion[2] + q3 = quaternion[3] + return numpy.array([ + [q0**2 - q1**2 - q2**2 + q3**2, 2.0*q0*q1 + + 2.0*q2*q3, 2.0*q0*q2 - 2.0*q1*q3, 0], + [2.0*q0*q1 - 2.0*q2*q3, - q0**2 + q1**2 - + q2**2 + q3**2, 2.0*q0*q3 + 2.0*q1*q2, 0], + [2.0*q0*q2 + 2.0*q1*q3, 2.0*q1*q2 - 2.0*q0 * + q3, - q0**2 - q1**2 + q2**2 + q3**2, 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)) + + +def random_direction_3d(): + """ equal-area projection according to: + https://math.stackexchange.com/questions/44689/how-to-find-a-random-axis-or-unit-vector-in-3d + cfo, 2015/10/16 + """ + z = numpy.random.rand() * 2.0 - 1.0 + t = numpy.random.rand() * 2.0 * numpy.pi + r = numpy.sqrt(1.0 - z*z) + x = r * numpy.cos(t) + y = r * numpy.sin(t) + return numpy.array([x, y, z], dtype=numpy.float64) + + +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 diff --git a/low_cost_ws/src/arg_utils/scripts/plot_lines.py b/low_cost_ws/src/arg_utils/scripts/plot_lines.py new file mode 100644 index 0000000..4260ada --- /dev/null +++ b/low_cost_ws/src/arg_utils/scripts/plot_lines.py @@ -0,0 +1,22 @@ +import numpy as np +import matplotlib.pyplot as plt + +import add_path +from arg_utils.plotting import * + +# try xy_plot first +vec1 = np.array([[1, 2], [3, 4], [5, 6]]) +vec2 = np.array([[1, 3], [2, 4], [3, 5]]) +vec3 = np.array([[1, 4], [2, 5], [3, 6]]) +vec4 = np.array([[1, 5], [2, 6], [3, 7]]) +xy_plot('xy_plot', 'x', 'y', vec1, 'vec1', vec2, 'vec2', vec3, 'vec3', vec4, 'vec4') +plt.show() + +# try xyzt_plot next +vec1 = np.array([[1, 2, 3, 4], [2, 3, 4, 5], [3, 4, 5, 6]]) +vec2 = np.array([[1, 3, 4, 5], [2, 4, 5, 6], [3, 5, 6, 7]]) +vec3 = np.array([[1, 4, 5, 6], [2, 5, 6, 7], [3, 6, 7, 8]]) +vec4 = np.array([[1, 5, 6, 7], [2, 6, 7, 8], [3, 7, 8, 9]]) +xyzt_plot('xyzt_plot', vec1, 'vec1', vec2, 'vec2', vec3, 'vec3', vec4, 'vec4') +plt.show() + diff --git a/low_cost_ws/src/arg_utils/scripts/plot_poses.py b/low_cost_ws/src/arg_utils/scripts/plot_poses.py new file mode 100644 index 0000000..8d8e053 --- /dev/null +++ b/low_cost_ws/src/arg_utils/scripts/plot_poses.py @@ -0,0 +1,60 @@ +# Examples in pytransform3d https://dfki-ric.github.io/pytransform3d/_auto_examples/index.html + +import numpy as np +import matplotlib.pyplot as plt + +from pytransform3d.transformations import plot_transform +from pytransform3d.plot_utils import make_3d_axis +import pytransform3d.camera as pc +import pytransform3d.transformations as pt +from pytransform3d import rotations as pr +from pytransform3d.plot_utils import remove_frame + +ax = make_3d_axis(ax_s=1, unit="m", n_ticks=6) +plot_transform(ax=ax) +plt.tight_layout() +plt.show() + + +cam2world = pt.transform_from_pq([0, 0, 0, np.sqrt(0.5), -np.sqrt(0.5), 0, 0]) +# default parameters of a camera in Blender +sensor_size = np.array([0.036, 0.024]) +intrinsic_matrix = np.array([ + [0.05, 0, sensor_size[0] / 2.0], + [0, 0.05, sensor_size[1] / 2.0], + [0, 0, 1] +]) +virtual_image_distance = 1 + +ax = pt.plot_transform(A2B=cam2world, s=0.2) +pc.plot_camera( + ax, cam2world=cam2world, M=intrinsic_matrix, sensor_size=sensor_size, + virtual_image_distance=virtual_image_distance) +plt.show() + + + +alpha, beta, gamma = 0.5 * np.pi, 0.5 * np.pi, 0.5 * np.pi +p = np.array([1, 1, 1]) + +plt.figure(figsize=(5, 5)) + +ax = pr.plot_basis(R=np.eye(3), p=-1.5 * p, ax_s=2) +pr.plot_axis_angle(ax, [1, 0, 0, alpha], -1.5 * p) + +pr.plot_basis( + ax, pr.active_matrix_from_extrinsic_euler_xyz([alpha, 0, 0]), -0.5 * p) +pr.plot_axis_angle(ax, [0, 1, 0, beta], p=-0.5 * p) + +pr.plot_basis( + ax, pr.active_matrix_from_extrinsic_euler_xyz([alpha, beta, 0]), 0.5 * p) +pr.plot_axis_angle(ax, [0, 0, 1, gamma], 0.5 * p) + +pr.plot_basis( + ax, + pr.active_matrix_from_extrinsic_euler_xyz([alpha, beta, gamma]), 1.5 * p, + lw=5) + +remove_frame(ax) + +plt.show() diff --git a/low_cost_ws/src/arg_utils/scripts/test_transformations.py b/low_cost_ws/src/arg_utils/scripts/test_transformations.py new file mode 100644 index 0000000..c4965e5 --- /dev/null +++ b/low_cost_ws/src/arg_utils/scripts/test_transformations.py @@ -0,0 +1,8 @@ + +import pytest + +import add_path +from arg_utils import transformations + +# test cases for transformations.py + From f4fa54ad32478d6b934793f15117d8d9602fffe8 Mon Sep 17 00:00:00 2001 From: wellyowo Date: Fri, 13 Oct 2023 09:09:23 +0800 Subject: [PATCH 43/52] Docker: add tkinter and pytransform3d into docker_file --- docker/dockerfile | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/docker/dockerfile b/docker/dockerfile index 4ae1a59..e0571b4 100644 --- a/docker/dockerfile +++ b/docker/dockerfile @@ -44,7 +44,8 @@ RUN apt-get -o Acquire::ForceIPv4=true update && apt-get -yq dist-upgrade \ python3-pip \ python3-setuptools \ apt-transport-https \ - libglew-dev + libglew-dev \ + python3-tk RUN pip3 install --upgrade pip \ && pip3 install --upgrade setuptools \ @@ -54,7 +55,9 @@ RUN pip3 install --upgrade pip \ pytest \ scipy \ opencv-python \ - dbg + dbg \ + pytransform3d + RUN apt-get -o Acquire::ForceIPv4=true update && apt-get -yq dist-upgrade \ && apt-get -o Acquire::ForceIPv4=true install -yq --no-install-recommends \ From 41814a79ea7f66300e152a64acaba0771d70b512 Mon Sep 17 00:00:00 2001 From: hchengwang Date: Fri, 13 Oct 2023 11:16:58 +0800 Subject: [PATCH 44/52] add apriltag ros test submodules --- .gitmodules | 6 ++++++ low_cost_ws/src/apriltags_ros | 1 + low_cost_ws/src/apriltags_ros_test | 1 + 3 files changed, 8 insertions(+) create mode 100644 .gitmodules create mode 160000 low_cost_ws/src/apriltags_ros create mode 160000 low_cost_ws/src/apriltags_ros_test diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..2b32ed6 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,6 @@ +[submodule "low_cost_ws/src/apriltags_ros_test"] + path = low_cost_ws/src/apriltags_ros_test + url = git@github.com:Sensing-Intelligent-System/apriltags_ros_test.git +[submodule "low_cost_ws/src/apriltags_ros"] + path = low_cost_ws/src/apriltags_ros + url = git@github.com:Sensing-Intelligent-System/apriltags_ros.git diff --git a/low_cost_ws/src/apriltags_ros b/low_cost_ws/src/apriltags_ros new file mode 160000 index 0000000..62fbdc5 --- /dev/null +++ b/low_cost_ws/src/apriltags_ros @@ -0,0 +1 @@ +Subproject commit 62fbdc5797e67058a88a2495b19664a455903b30 diff --git a/low_cost_ws/src/apriltags_ros_test b/low_cost_ws/src/apriltags_ros_test new file mode 160000 index 0000000..b70a275 --- /dev/null +++ b/low_cost_ws/src/apriltags_ros_test @@ -0,0 +1 @@ +Subproject commit b70a2759008c2b753106f1ee2a6492b8a192c446 From 249a49085389bb96f37c867a327c9977faae4610 Mon Sep 17 00:00:00 2001 From: hchengwang Date: Fri, 13 Oct 2023 11:33:31 +0800 Subject: [PATCH 45/52] update submodule --- low_cost_ws/src/apriltags_ros_test | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/low_cost_ws/src/apriltags_ros_test b/low_cost_ws/src/apriltags_ros_test index b70a275..71025b3 160000 --- a/low_cost_ws/src/apriltags_ros_test +++ b/low_cost_ws/src/apriltags_ros_test @@ -1 +1 @@ -Subproject commit b70a2759008c2b753106f1ee2a6492b8a192c446 +Subproject commit 71025b3c13b7aba1628ff1fbc484505c60415926 From 91cbdee40beb0150e32ce14750a824b557d64e73 Mon Sep 17 00:00:00 2001 From: hchengwang Date: Fri, 13 Oct 2023 12:49:54 +0800 Subject: [PATCH 46/52] sync to submodule --- low_cost_ws/src/apriltags_ros | 2 +- low_cost_ws/src/apriltags_ros_test | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/low_cost_ws/src/apriltags_ros b/low_cost_ws/src/apriltags_ros index 62fbdc5..ff305d0 160000 --- a/low_cost_ws/src/apriltags_ros +++ b/low_cost_ws/src/apriltags_ros @@ -1 +1 @@ -Subproject commit 62fbdc5797e67058a88a2495b19664a455903b30 +Subproject commit ff305d0c81fba51f2fe5a08a76483a0a7897c1f6 diff --git a/low_cost_ws/src/apriltags_ros_test b/low_cost_ws/src/apriltags_ros_test index 71025b3..629f635 160000 --- a/low_cost_ws/src/apriltags_ros_test +++ b/low_cost_ws/src/apriltags_ros_test @@ -1 +1 @@ -Subproject commit 71025b3c13b7aba1628ff1fbc484505c60415926 +Subproject commit 629f63553463a28748b2d907c892af3c29d8172e From 01dd8c0bed7362d237177458bedc4a75e8a1bd6a Mon Sep 17 00:00:00 2001 From: uwe Date: Tue, 17 Oct 2023 12:28:45 +0800 Subject: [PATCH 47/52] delete arg_utils to add a submodule arg_utils --- low_cost_ws/src/arg_utils/CMakeLists.txt | 206 -- low_cost_ws/src/arg_utils/README.md | 24 - .../src/arg_utils/image/add_path_example.png | Bin 34617 -> 0 bytes .../arg_utils/include/arg_utils/__init__.py | 0 .../include/arg_utils/anchor_logging.py | 80 - .../include/arg_utils/camera_projection.py | 104 - .../src/arg_utils/include/arg_utils/get_ip.py | 143 -- .../include/arg_utils/import_me_if_u_can.py | 5 - .../src/arg_utils/include/arg_utils/mqtt.py | 60 - .../arg_utils/include/arg_utils/plotting.py | 182 -- .../arg_utils/include/arg_utils/random_map.py | 79 - .../include/arg_utils/robot_model.py | 48 - .../include/arg_utils/transformations.py | 1973 ----------------- .../src/arg_utils/include/arg_utils/tsp.py | 130 -- .../src/arg_utils/include/arg_utils/utils.py | 75 - .../src/arg_utils/include/arg_utils/uwb.py | 283 --- .../include/arg_utils/video2picture.py | 36 - .../include/arg_utils/websocket_rosbridge.py | 76 - .../include/arg_utils/xbee_coding.py | 81 - .../arg_utils/include/for_example/__init__.py | 0 .../include/for_example/import_me_if_u_can.py | 5 - low_cost_ws/src/arg_utils/package.xml | 68 - low_cost_ws/src/arg_utils/scripts/add_path.py | 7 - .../src/arg_utils/scripts/plot_lines.py | 22 - .../src/arg_utils/scripts/plot_poses.py | 60 - .../src/arg_utils/scripts/test_pypkg.py | 16 - .../arg_utils/scripts/test_transformations.py | 8 - low_cost_ws/src/arg_utils/setup.py | 10 - 28 files changed, 3781 deletions(-) delete mode 100644 low_cost_ws/src/arg_utils/CMakeLists.txt delete mode 100644 low_cost_ws/src/arg_utils/README.md delete mode 100644 low_cost_ws/src/arg_utils/image/add_path_example.png delete mode 100644 low_cost_ws/src/arg_utils/include/arg_utils/__init__.py delete mode 100644 low_cost_ws/src/arg_utils/include/arg_utils/anchor_logging.py delete mode 100644 low_cost_ws/src/arg_utils/include/arg_utils/camera_projection.py delete mode 100644 low_cost_ws/src/arg_utils/include/arg_utils/get_ip.py delete mode 100644 low_cost_ws/src/arg_utils/include/arg_utils/import_me_if_u_can.py delete mode 100644 low_cost_ws/src/arg_utils/include/arg_utils/mqtt.py delete mode 100644 low_cost_ws/src/arg_utils/include/arg_utils/plotting.py delete mode 100644 low_cost_ws/src/arg_utils/include/arg_utils/random_map.py delete mode 100644 low_cost_ws/src/arg_utils/include/arg_utils/robot_model.py delete mode 100644 low_cost_ws/src/arg_utils/include/arg_utils/transformations.py delete mode 100644 low_cost_ws/src/arg_utils/include/arg_utils/tsp.py delete mode 100644 low_cost_ws/src/arg_utils/include/arg_utils/utils.py delete mode 100644 low_cost_ws/src/arg_utils/include/arg_utils/uwb.py delete mode 100644 low_cost_ws/src/arg_utils/include/arg_utils/video2picture.py delete mode 100644 low_cost_ws/src/arg_utils/include/arg_utils/websocket_rosbridge.py delete mode 100644 low_cost_ws/src/arg_utils/include/arg_utils/xbee_coding.py delete mode 100644 low_cost_ws/src/arg_utils/include/for_example/__init__.py delete mode 100644 low_cost_ws/src/arg_utils/include/for_example/import_me_if_u_can.py delete mode 100644 low_cost_ws/src/arg_utils/package.xml delete mode 100644 low_cost_ws/src/arg_utils/scripts/add_path.py delete mode 100644 low_cost_ws/src/arg_utils/scripts/plot_lines.py delete mode 100644 low_cost_ws/src/arg_utils/scripts/plot_poses.py delete mode 100644 low_cost_ws/src/arg_utils/scripts/test_pypkg.py delete mode 100644 low_cost_ws/src/arg_utils/scripts/test_transformations.py delete mode 100644 low_cost_ws/src/arg_utils/setup.py diff --git a/low_cost_ws/src/arg_utils/CMakeLists.txt b/low_cost_ws/src/arg_utils/CMakeLists.txt deleted file mode 100644 index 229369a..0000000 --- a/low_cost_ws/src/arg_utils/CMakeLists.txt +++ /dev/null @@ -1,206 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(arg_utils) - -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - roscpp - rospy - std_msgs -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES arg_utils -# CATKIN_DEPENDS roscpp rospy std_msgs -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( -# include - ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/arg_utils.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/arg_utils_node.cpp) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_arg_utils.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/low_cost_ws/src/arg_utils/README.md b/low_cost_ws/src/arg_utils/README.md deleted file mode 100644 index dcc03bb..0000000 --- a/low_cost_ws/src/arg_utils/README.md +++ /dev/null @@ -1,24 +0,0 @@ -# arg_utils -## Python package mangement in ros package example -Put your python moudle into /include/for_example/ or new a folder inside include. - -Add a add_path.py where your main code want to be. - -Then code like this... - - - -If you want to new a python module, just add the following code to your add_path.py -``` -sys.path.append( - os.path.join(os.path.dirname(os.path.abspath(__file__)), - '')) -``` -and make sure your < path > is pointing correctly to your python package. -### testing python package -after runing the docker -``` -$ cd ~/LoCoBot-RSA/low_cost_ws/src/rostest_example/scripts/ -$ pytest test_import_me.py -$ python3 testing_pypkg_from_arg_utils.py -``` diff --git a/low_cost_ws/src/arg_utils/image/add_path_example.png b/low_cost_ws/src/arg_utils/image/add_path_example.png deleted file mode 100644 index b8139bc0d67f73ac09970bd7259b46e3af87fa16..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 34617 zcma%iRa6{Z5GGFW1c%@*!QI{69fG^NySux4aCZngxJ%H%-Q8J||LmUKv-_~~Fw=ee z_Py2BRrQrkxV)?w0xUKx2nYy*gt)LG2nZO}=ke$lsL$`n6HCm`9|%W53FR-JAFnS) zVV~z%P9o|~O136WuKEtfAf`69*2XlBh7QKYHjZYtP8Z-kyq}YJ%$3!hgdB|Zoy=`* z2$apOjXwuLKp5#6nfBTm_oDsxv%(k{_EU=qD31Kubelzdb=!~I7gbhPsMZ%PovTvoBPzc{5)eH|35tsgCg?LtkU92b zx9vWNfwjf)2gE^mZPcXFrH`lIcul1<9e7RMa38qOMnpz(eS;AA3L$_Qh+2s2NAM+p z;7i|6;R2$62Wh0pXn!w$9r_Vp7DWkrZJP!tsE`&?R~>6z-LL!Uf8HqOtg4ijcAw~; z#V7NM`gdvp)A6a5E^}nC8YW1%JutVw@*M*sIT-6<>Yxv<2&Gzz&67&)-HqF1;V9hM zOnFzAD|B#4Qy%H10VV)?)y$bxRsfT%H>A10A4qD=fA7^8#Grn&y9g9QnUGboH zUSAI>L_&IH))(ARSw_rc|AgiA$L9HM4#=gaM3XuNU9!2$Uzv}!d|7$U-UwBt1n?6a zPrIdU%gtiEkd`P=s0iDC+%SDZr4>gBJ_-~~bdbLLYQ>t)`zU?xZ3whSMkBQ_hL@YO z(F~kRGr()hgCaQ^sTV#pll5O0Hu*Ev`+KBHQb&P5f7b*Mb2!suPRpJuHFZ;ejt3A^T^=6N{{}XXnda8dE*a6Z`7m?I$D)kSo**oqrM9qRSL$)UG*16gK@u^^l9!0=WL06jTB^HEG2C_UHCY_Oa?#Gw?ivAT zxD0VRRy{55_1(S*e^o@^uqt}1z#O84azcivK&m?-#^{A8E0r{xg!w*AW_*U0(38^5U=#KJ>8?u#m7K z5p564W_k_TG+ZG{D0zjuU`%zmT;{l{kO2W%!6;a{TLvfi58ILD(`ac71t8Ds$Q4mZ zdTDvrZ)aAnfz^M)IG1gm(^W@H{C2Wg4c<0prRxn}PeHzaXp!x_I!tOY`v1AOpWOtH zGk=^rV9qmMvp$ID6R%uOWcu^GqUsXbnDiraaz3(({bSZedLr6G5tQ@@@BaKS`IJE6 z*7bm1{JnbOG@!T~MrbpSg5nWQhK92Z8f5(v!Q*AWZmoMh}*O@Y&>`Xfl zTx~y&xGk3K+deGQiV5^&J_)558!33l;7ACXN)RcE(no%P1_q?YgL&BFyQ*f7ifk_l z8cjz>fFkPtPq_!B20B>EGVLplvzmzqy|G9oSvsy^o7^-CzRbSrU?p$tkDCv~t$S4tw!yf+pR;=uP6^E=Ct zM%TUW@)Z|NB~jSXb~qKxVYBG+aGYKq_q?48du!Nd^9Tl6F0s&K{SA;;wXE z-Vs&MsX02T3@uK-G)P~^ad|c3H}2CCkB*+Dcf~lzYBXyh6whwhueZVG>+)jRT4FP8 zU0gh3_Erwxl0bXDiz&et>HrU*OVZfm2azTZRv#vR{+a;9EFGKUPn3uWd<|myUFJfH zo1h$!CKo(YUEtl2`Ex#C8MVk*FU;;EQsGx(Q5F#`lFNN6MTQ}1n<>)dCP5^;bmi(tBLxSq z{GC1ywT@p7ELFOQznHzGzuG{XmsFL{`2+3v#i%hJNIRsJ)$ zcwk}1a};S7aqo@S%9)8XB2(EH(A;)Jf-FoL(YQf}=6wQ+Tg47`kie%ANgO96J30}Q z!JsHJbMYvU0@H3}7kt@@nsz~1(m6g7173)n{rMzV`s3tOo@zFwCVg(SS$IZczHQSJ zxkCe!fmZLpuOn_f9XHBhqp8h#n3tTzeY{H{4!+K=da*wE$sOkfvAW=B$QiewDL0{J z%iju&L%|QWHO6U}3Rl14)tVUbCxqKGH8tAWAwEMSb83Ym$}MrKa56R!X!ER<{6ZOS zV5%k%?6H7bx2+<^&dzXnvFg@G)A)WTTxlM8^ZJi@seAqTiMNDu|{d=f11Ibicua+=h{?$S4p?t0ZXJ5U2O! zn=KMjn*bEQqbw+7w)H&jg~Fybml7R)9@Ft~S5X>Ra%{;KYdTJ9#2N=HbJGNKE4157 zZDc!Cc!DLFBBN|5tyHLcKy$F}RPf4kycBj0oS77P2NbtobQJjzw`o#y7nyzi(si+Q^DE+~ zLL6U7XR`$dN1_gN`QZ4jPQ8PUVVAk3Q+P{si?%?|qTcXvDuxjdnz97O#*Mvc&ECF@RF z78>D8vGcAc5rAP{|D9_Po3X`$f*k zE?FP@;^_t3bXZ^h(|>5LA%;-E%{3Riua z>d%Er*QZ~~C`jfd!$gURB>vl67@k96COJo{Zc)P8`s64|>@SrNTV;f#RL5D%S-#cr z{d(1D<;1>V87Fq%49;X|ddiYJy7 zb;K83k7U4-b)F1EVn>fF^kRF{@I6OTNd-(B+f6D$O+BSsBdR3X#sJO1WRY+w8D6FF z&b^v@)#NfYLqo&v@!q&&wNbParCKt&zD(rKpAVu3+r+lLl~DDhWxi*2J@0$soC6u zlHLVDw{W|naCGUUi7Tvolap*$J+J%eR%}sftuCQMPb0kj5K&dT2=6Bh%l@tno}SVe zjhgP5ac`>nyFG)ejt?bOMT2{`y7&w=v;5o^6ZM&crS_0*k#X>`&|`{usrN&pS~#T^(uSyk@)Y6 z`#Oh2dyv0Asc&0x{){ir2oxKOC_S>nV(_`}fW6QYeJ!=!L;@W}cDU$<)miwC<5m*V z$_e-8UQF{2@lV`drypkHUn?4;#pESsbLs7&zM>S4WXlchj*`KU(O3p5$wApZ z+tCPaz!!MmlX95@GCn1Qoazrxt?b(({WE5_{xo=(DCN6nPaIoM8tYO+;zkYe zbo+wnt`)mZnIIkuco(?M05{UJlh>$@HvzJV8bak&;mybiH%qZ*j(=(?+6QBy+&ORi zPJOcIHJ-vF+i2{VmOvLRZB#1UV^Xl8m02R1?c!jWoLJhxWxow^A$Iw3uW--v3RTtd zN6FsEem zK`Q|;tgb-@mNgnOE7z9@%`ef9j$O&lUJr8{Lt)OxwK_bJv^sCyTyb(PpoAoSG(t zmUQy*p|cCdw}Oky>AR%@Rrrgm-OCEPxxDU}SNxAGZkmB(*SXu88rRQN;$VdqQLla4 zzmCKqf?~Wc_!gK)`=;5${XRP|65YD@6)Ug1U6ZfR}6BMG&Oyi8A*5RS1<@5TOQV&TrdOXGie#Fpst=nl75xLeQv!7y#Z=hiEP|$#P(4+yOXE5ZU%)%=x+xniMrNJ@Fhd;h!PbU4&qX`y(A}p!sE|D5%3Ao z%Au^ai5*Lew!KlywjYnkV|=vx)njyfeNhOV80VTLoxg`8D||*#;;r`Iazk5pkm*8s zPczcwvwczca=D^IXy~rx3o$I0W!Abj#RlHS+^cvtN-MLji^r?Iq5;5PL?mbo>o_4v ziG1CT-+hb1jHU1mo_qJW8&^3_tyO$ZI3@Q5$8O|yTu9^(QWMi1cP%Bqs&_;__BYTk zvrR8{d7K4km}Re;v46=Ux_JST18URTLw&_aQev3%!DuY_|9M({>3gl-%BUqbX&Tu2 z7crbe_sTx@IFswltrccp2JP8fw#+KFYpn(>4mC#YH-%V({gk;fU2OAMENO4XK`csA zr-UzF<5H>#pF*Pf_yA5X_K zYdy%-s>Ik?XbMav^Qv(_kp;MTr>Viexb}l6lNCh<>~LCLnFJU53v+8YS0+r=2gF15 zFlD5|Uk5^TbTd4HF*F<_in#LuE^Fpn)FH{p2z$}?K;l<9uz~nQ0?OF=-vj11Ee~D< z)1u#(cTIogzkKNCq5x+gSIjR%7_d$;Uy@J(tGZIF<|{FWLv&xTX!Bo#a~aQbNOx&- z{Mubf732!iux}!0WmG+Rg-r)0P~ny)m1;4C9vi+CH#$7;U$+JLxIhWdb>b{}(27S<2N@G8|n( zt$Hrh!;9c>3cNYvy&|^3Q~~+NY<@`k?#GjST(gEXr@(CPUyJzQBy%>rs9U5v zGD|1bP!L(`_ihf{s;pa6ALzfg^o-p`cJ~V{=|yTXbOI8U6wsbYm-M?90usK|t>`fG zJh1h>{gO`5vl()gf(*gcg1I%%hD%iZQ@i#mbWL+a@DhHiF0zhKjWxHk3-j07g8Z$p zggU)3O1EVM@2nMaZLW?S|4-k6r8P7mR(^HOyUyS!fgyk`9@WT$EO7VGt9pomyL#H z`2L~J7vi(13?7Yt-nwlH1MoI6hgXt`5Vr6*Mazcc`~`(91^jXo94VTg`7fzvSU2Gj zmlJo_CpGZT8$G<&yI(~AqDoW>6PZG|@Z|2NZY6$;&T&hiwAcO|&srD8Pi7G5p9kFA zfAG=ao$-r`9jR2lKJwc9z~1DM=r8wCP-xhqBkFC9>gsq|H0mrIDo+AWpC#Lj!S~M1y zTcJ*S0;T5(nvBZxjYea_cjJiwzqLoVGJyUEZ1{z4yzIm=`hB)}ArYKuD?6ST3SQfb zu_>%ZR2@YW%M4=HXZ*H3fNH@qDx73TB4B|jv2bOntXO6Bc;@Ie@3re3W^yqC?LLdD zk#?>wxUr_#=po*&1EJa@uDSenuR{B}a?h^4mX z%(RCBFdX&EGvrdK;Ty@2qmszWf8C;M)z>$9fK7G$SlI)C__&YdemQN=saM`fxBr0F zc2Iy^5hwLjeN94I!o#^fAI5c{JEcS{m zXk_ut1rQ#Y0#4@trk+U*@?&|vGs7SinS=k*-VntaiLTZNenxtk*U>A@q~{49L%v6tx<_j zi9kVU++jZzz}x0TwIEjYWHsr65-|-h%ggScL@X$;p2CJBHz>t*Ed_N9fik8FX(`4k z?J0#&DT(&dLibQ1ujBvz-iY&~+yoIvR-c+$a9vZTutrZby#%1E8}@4tMCKc~OGj{& z6M_XK$+kXNm8`Z;8}%eJ&;2zKNe!HaZEu-^z*=m@a`?faup#X2BA5e>2MKZzXD3PY za@k4+$1X?mvQ>oekI}E~QV4>0)44y6giyc#*RAwn{gDI1OU zR_IUW;+0{=MPb~vIM&cfW|l3}JGtu-G?T)==*ppHv*7w#wDCmTxgmA+QpiNWUt*dHL-gqjwJGx2`!K?7e2h6m7QZY*F(=Fm)ZGwB9rO2|O6|Fj%WlR-#yOBZ z0)DUP4B+Pkk97MtpI0h&{6yD0V8uoRD;s`&e;8q~DmmW*ehV)qmMP=)znNO>aDYv$k6dEKtKtZ@GJdSCZC*uaor_D-z%Y)$yD z7eHq+;}<4NY&M`UHoNsmjLQ|vxaKWl$#)mvvxDMw0B~*ixbZB<+E$2>z3n1s^OAsk5PfH#_ zd3c9=XwqoC&Cj(1_VZf!G=>b3h|ksRj6jn)9?IEX?o8>(d*ShQKCt55^7ecCo>e<2 zq1SF^O}w7QK%0%Hja*|?ALORk96;A}_&cG$9=iCbt4`hU`3B=AIn2HJip9@5kKgY~ zOOONb3$OpQOj4O!5brx-R0~<>O_2gk0LL}G3vbC3w=R-!QNuRm(G%a0C1CJ;GT`Rtc_W zl77+0hlLgF`po}?5;X#s`smG(-3C0 zCd+`B&aW#I=S2K!W{2X+ilX@ebFS0xYfxl75qWAGE^tlY348{zJu;Z2Jo9m!g5`bg zub4kAA2=y1dJp8Q1;h(&MFr?5<@v9)JRROGG|v0>#w_s`k$S_nfjiUV3hzK)+^o8~ zG!(ALNEw3TSFQ0|i*hhYdeER&Zg;U|!hrSuT6i@MO@T~RA-L0>%8+|(gcNUox~-aM zOq$%*Dh4^y_IQIds{EkKajQts49#4Tv3xuO_>^dKMz0lG+BIxdO_@i5Sa&=+$psWh zJn0x0r8*vg(Qs|}WIjB!nyodY9{_)0(F@T{Ddb;~nQ^aXdUFc3X5$Gmq{jhd2=WCp zeMkt^em17bL+Y?=dI#+=IxvT=E{oPWnw`W&EgQMCbF6I>l8g<{5W^Sdu>o^{Rb7x9o&l~ z0`xnzasS+h&dNi9 z(9CZAlvjH!4(z-MTq2oLp`~oIVP1TNCYYW0?}{T~3aP&j;GCEEDdUsAB0+|Y!WeN# z=k?3jvA>xq3an&VKME=sX^0{=Zy}P*9M5&oBE#~S6eh@bB*uhA;aqK5Al+re>-cXN z31hkJoPHr3W9(%mKTm@yl;Dfv=eL~f{o!ogNwh8)WACOzX&eiuCgx|SEE*Gf)#Uz# z+WD}f0b<06i07z(0bMv|3}RK=%>!6Nb(_YNtOIaS7e#a>UFbh~&*xVxj*kld0zf7) zN{<7|h+V9pW@AOt$^!JEar3Kzf`67m#+ZI<&>wHvX?@{HCPG8nfkP}!q4KXOEUbVc z#VV*Gjz}^W=9?E*tr8T;vZIf}ks}ZbaBB4ys+Pe*{^UaD@6}_XuV*U<)r$m1X~W96 zHKL>&gs{6 zdyp$)z5yd9+)VA=@yy`+broUqf1$SXTl^%mPv;5sNY@){$^OzN*B@!KB~{LDCxn~L z$SwO@ivV-0J|WP3FogM`-|VT&t{Y=(a8&eIo(=~t*(QF@2)d&4?zI&#Pz0Ey{z0l$ z{)8sCipw}YZcC&;(#(iY%&fKigt{g%96}?7#9|vECi{KTazv_;u|dOjoNkY!<`0J? z&_y7r%32h3i^xu1xgHO%eS*0dM-&C&_!<2=3QOTNWcfp?PmHS(I(AL?_~H;q}hXJRP6l*gH&L>3o|({BN@8^a5*V_vCQt6x1(> zrw7fAy(~YiD2|VdySiP$C>KY?V#2<=LPq;=;dSdcFm_g9+I`P3j@-}txSpGnAN0V9 z{$BPp!+P65MK4`Va55xSt8y^nT( zPSwk93HDrmknqqTRonf7^8&QOs~zqwL+wI_GJfy{F;et7X!wzF3yJ@FBe$%#E-k3h z8vyK_;D9Grc*;m3UArIaF%)=v;kJJ_;oZT2 zPA18!Y0XJx9fX*R-K7f_9idJjcr4!3PWyXHNCu(+Mp0ZH5Pk3m)w%Mh{j$WI#ADBy zd|D#+#O8xe+?tRNDEO-?yglZt11xI#ll1t>+m6)cUC3~qae?EY#MJ!P8%FUTS0fx1)$pNC;o+L7r!)G#NMzmk;y3l4 z$oH4ph9=MpU*CY8^3w7be7+T|J%Dc3oXqYYM|uQyhaRYtR!SbavC-Pus8SB7(JRy> z)7kf<$kQsD^HHB-#>zIp`|J)S{~gN|h$Ii0Cn87s8}?2;Y?{Gc!`9hUM*^zfthU38 zw`szHsQpm98q_EpQ@j?Z8AtovJZkjGf{}_UKP@gZD>=bYLCH6V(&R+v`m2^!yV3Mr z^!we>1fIxB(3q+W53G99QMseDYpfC`NoFYQ?DryQzJx-CX^WZcTXi@}1BWQ)=w{Q( z6lNHSxs&nKh_5jchf(Q8V*+dbUykA7JY)jwm_f_L<v6mK&7F z$O4OIZcpKo(W@NtoGIkXRLYV{4g6qWqD2Kv1@TtatzEM$%9m`F2?_5al;W#}RCg*ziVg_E`L1h_1v}$X7V_)u zh2zhfy;Bc-xMHMg^6!iDLrFyt66+fFj+m|L{U~B(@7SJm>*V>aW|ailU#mh?cx0RA z{R`S+iRAudLcYu?%FL2PGLb%JLg8Kr8N9&EqM1DLVebb^Q}Vb)rNSjWQ&^7O{`KQs z`RvHJS^oX@~yEN7Orc=V! zbc81XV6!VaZywt(SJgEF&NIeF)Kuoc*$&=`fTJN&^@C&r4Mv6336jsOr9^1t`$TJr z)WrcC9rx5nHpivDxuz3c>Uhm><@Piwo34zgm4(xC=!A(z4#Tv`Hq1l_3OI^4fUl9F z=m*QdC1+;76K$eY)hE+yG}HAdxUtLKP?^AU3?y>vIVhiP;4GdV`kC&^03>PjBHQ zQL9CGlBS@Iguu2bIT12si99CkBX~&hpumAUGDcK|K7~UIuv6h#V**M)X0&G<>h%6m zV&umzf#LDqlyZ>-C!^(h%uBBtMg?_N|Le{j_-%YPsW18nEbiX)u|<;sJW{bF5=QE0>dXvY-QRF{IMIY;e@!1Xt$SNKsXIp9P6`>>s^#Ka znyCFM7;XG{>OAQt}onAz)e!5uheKqS-Yu4~O*fevvFT+AT_O3dBnmoLQe= zl`-wLVJTP=FH=VLP=dlX5DuMdc@#Fn?=FK?vT@Bi61Cz;|9Z^12yOjL*?LV|Jy~h$ z>CBXqloO7j1E^P|%lxpu8D@O}DG!5vQ~5tq-!0XE2M-CKHY^|w8m5aC>GVO%H?rbd z{kEsjU9DMxdz9t6h;}|z#Ew|Wi43~o_uh)Nm@?9>rtf>w@ZNQ&HVxlSU0*$ zcTYMlis_wBrO$WEen+?6;**iE*2il(x|`R7E?3PgT+7drdth!Yqnb$VwN#FK~Em^4(D?R!R zET=iY#TT&7j#yE<3QO7G#FKLhb6hv=e!Go}Al6$+hAGWYB}@+sWt=7sOAtJ@$Df)z z)(W#+lps;ygS1hGuugw3(sz(2p~P>q!9G3!XhfGcgN{I-&k$W;@0N(K8gU^wfE}fabR@D~u(<% zU^|W#%3*5#01gpGG~z#7WJOF5KOAo{X$=)^BHYHs=-0yQOi3ZMu+7KD3d`2ODAVN$ z#HG2iIFnJc&FJ?&6AF~#C(>UL8fVvLJzGK3V@jYnty$|~hY~CNgntA9_OJ}FkQZke zE4wUMVaZiMzFJqD-LZQ@*z)@~xpA*Fk|^{fmMq!1d6>2i8Tb_*($l0SfUso_>3lVx~?NTo;{*7Vcc4fl^TWCXL14Oy|>YP|dBb!=dW5OLwup32-o!?;3 zwR#L{TL9?Au*H-ZJJG3mw(C)b7g(>+U%(?4JfBke`scHl&G=q&L9jcN4`rajAi@Te z$`~ut=8Rvfh_i|)oG@iz4SMMv1cPb0zI!wBE9Ei}pdQ|GB6pK@nmQyD$mb~@)oL<{ zKVf5u22Yk3GsG23Aj6ADP&B4BAW#Ta&qRtn4zY9XKc}}xMd*zS%xI2m9jM{Yk&t8| za4z5T;WbCQV1h+BF|{>o&%?I=ddQY(74t$qVQv)VYQ|yt)hLUsz*jXv`^>wWKy>&= zZbCfNB|>7kI7x#9c?J=&1@Vmms3e;|7-u+CqUy|I9K_9xi`Qqk5^%|auyV_!ievu# zty{yUZ4X`7=Pg+P_^~9G1F_i)jfY(x_gMiAQm}KGFZ*IzyxQZsY8Lvpfgq}$L9ejh z=}7VYd8sGH1@m8BEh<2X0XE^kZVq$8|LqS_Uf{-F57v0EH`j(C3w#_@wBEPo8@{dE zMdU$=`@r#+3LC@TC7igKtOu!$jGG5|!-6BGu#ocDvz>QeU9WACP(sV2b+6lJ=kY_| zA4cO_>9HTdo+Y`n+oBR=+6^gsids&)Z4bEJ;nQD9`b~G+Cz1Qq5 zMSy&AUVa^m^VgA0pI0ZKe_&3aWEO4312JyaKA|>{=S{9?-Q~cXQkR6vlyaeu?{X8G zF6Q?uJ4{wbOPzl1M^Wip%IfiQ?K{YM&5e4S0XAC<{!~o6%6bD|Za8z&#tS48uk+mR z{@z%U<%t+)m3gNuw;2Yy1D9Dc=DDZA{h63uOH3ia5GDalXLKxY>yssLjec@qiZ<@) zrlW3-b=>49;zHKxTjeN0JBZ(L08o^C{Lta)L6{^jMp?O8(g2i4BZs@iQvpmowP z!~13TI4N5?yKOCm-2?3Oy7WrYsgmyNRqnHb76tETeh=1Rx|{&rNwLcv;ci0-FVqgI z!s|eWs%H}R7vDj8X}2ysR&((W>6ZwxRWXl%(1s-Ke#;)tIWYpz>EDoJd0Rk2x9#F! z+XQA>Jp&R`*Y6ii+0~59n3bw#YqDL2wdGVRWl#NGhCZFSB~!0f?aQMNrk=~)?tZ3r zikU}iJH%%J#D8qXsexU8X14-3OSLMlH zgIp_%j~;Trab|p}n5NYgy-IMDXelGuC9mH>Io4Z?;x2kKF&RXK7@(>4-D zTLY+l*wf`&AH9=GFr|541diu0;3=t+Jt8Xh>}&A8_FDu<$a}v`al2i7CJ;Nfc2=r= z=X=H@xN=T&QoryAzO`etJwHc|B)`trXilzDt1|6gi6pbycpib;oaPKuwBD1Z@GQD2 z_D3D1+5vUo+VbW>crmzmeR54zRcB{#1D)RQI`1nubeQPHTpGNNsI3mW0>YPSub1{t z)tC97T1*v9ARi{1xUYt>sb1jeOH6&vnGf9(Mh!)Ck6*H#niuEZ?=#sKA2Je$6$Li3 zrD~;LuZf0Y%tD^6f={i&b8yJ9uvz>$tc4aL@aH7CSYw{FGTkUwmB9 z;6@uEPF!y`a(r%5#hKg<7ACawAnb15yFOwMT<`TFgTMRUOnF&OWp<|4`5E8b)Jef$ z^SK3Usm(hM8p>Vw&vU-_8Kp?K#1nodX9p#7G*-K&iovSe9rcvE8id)#!)TEE!;-w4SLg zIZoV4ADV3zqWNyOBO(lSWl+FUQc5 zy<-t5v&T%Smzx;{d)lLv@+!*jSKP&P`9=aQb0J<&-P~yLZ6ZJ+$G!D&Biq?Z_omct zW=FR+uFZ$jeCgEIJXrQbR4;LhOcoNo8XeZlr^bZz7gZ?T?F2Y8CIZLXRJ-ude?ZOI zZlbE1!Ef9eX7>eI(W&It?C~ciBDSEY387X0&dX&V)2MyFWwqJ|XXbgO@Wj^>X?>&` zJ~}-IiSKVV!$2DKxJzYOe;Xn|xHppm3AsTSr`Yn-Q77{N`K6cXRU| ztX0&+a(cn4i%o_JIrD~8hWC4i?FwD}4?Z55^@BL)1Ek?hJ14ogr?-nl!?NA!{@D71 zQo89&U7jm_x#Q}5r<7#e4=)Be8l54{he7R^%XK4HwM2^sW;eb!*U#$b0{5pwTz)a+!d*-;n+4uKvYMkHfobmBN3tx2kv{+$m<*wW#NC8#0(v>%zrso} z<2pi@0CYchckirtb?#bIc^1LipdKdgCh$)RI!*tUkhmC2=X z!qQtGL#iFsjO5H(i?!ReX@rSj5+6SyE0mI=yqc+z`DZ=hr7(hiF5j>;ko~;{5XX%& z*tb$YLkof9yf!{raRa9*%}N4#dnsWA82FkZrz>sp0LlHKxL{D=ZX^@|Y#^q1aiPua zY4$@sAqeS0U4~$b#}xU_B%$X$*lB*juotbKz|*JkD!L~K1_F($pI;{D+h19pTgy2U z9uG&xP;a=WuRva~j|6y_bsf)qMOS)%C(Vq-v``6k<(~oIkENgq$M{5Q)X_W4z`=LC zd0DEnO7nS5#$9s2dwwvZ&)K8%@q`j?Ibt$4nXEI0d*X+@=={9Mbn6ntpiAKZxea%8fY5r?Ur!tiNrJjHcIui*5k^k8XpjJmt6{;9T+O*DV zu$JQn5$gCd^11~!xXghn0*K4p3uk;?TrERTEXK54gp8p5PBF1)|F($-Uz(AFqKn^? za`TK_tGhei%zphAGFvgborKYn_L#=*WR5X1CBFOFCT%nta5P^|N$90GE!<#vVspD9 zoY3MGyFvL?#W3(RKoF~7w$5_hC!q<7`K8;?v5#lBdrJK0;E?id38VD$*eOqv<4fG$ zZT_f=16H}GZ2Z=4@JE$ zNA#Exz5o4-n3gMX(L^&n#RQeFLDS8}32{XR*KfG17FPo|eo3&raR@;`_0xs%=}Ljx z9%)_cPYJm-Op4+_88PX8^z?gA3A(o?FT5$?22pWQL%LEX@96?R@}6k)>NI=-<=GVt z)4HQ=uYh7OP01IHI?t6OE^63*pOI&W70X*?!~wRp*oRD;D=d2W1$Sg>8Et?wjO%)Q z#oY!ZYQ;u{+HljWbwaaQK$nC{dvfNk<6j#j4kwbES-(Ji#Fh8m6`Vf+s0eh1-QpqNEpROi>eUuBu|7AfEYQvDRWS&X?pjGfeeq)2G%s#t zfY>`h+XJA2&`kAbaP*x5=YEHUq|Qa8DIxb%v2DC;MwWJw z642-t#eM{ol@<%nHW;vjqB?2vI6?2nVWlTS)CK;)grSVit#L?|t<{7k?^z``z^36$x^qTHrSC0y^2*OG!jwkKUbyRu>( zG0XdOsW+KAv9qTru?W8k$U^>F&^;k&zVtrBXPven#kvcKnGj8XoX;C_yZ$STi~AKz3nK$`CE{cBf!@o9^E z4|A+r&wG>E@{&tO@99CT5`Gk6B)RP#qU75KXYJ01c~)1xk#-dK+icN4|L)!^F3N)( z;~KHA!zNh1(F%C}xVV7D&rlYr&W5=NkpY5o+u-jMz}w?3>C3IHHtu{*FQ|l?SEac< zf=8^Z77DgC&+e5NbGmq55<&&=r6F$w>Y^o$_>2#o|8xs8xAdj9ZPIH0qdcS~Q?1M3 z6#O41n}ryuWf?Ouh{W_(IqLSiHz!2VB39Os%S{ zMM=9mJ9`!{sW*Ss@JwLaOXUjMIEK3?9M{&RTqyd-@nU&*7*-G0ojl9AIT(Phsb3V=7{&(QQ;;K*_#JJ) z_2cxjn&3&5&24o6fj5=UTqF>6zXU{2+ri90Z z+3@UJNmr@cH2uLKwzi&U#3Xb~XAx1CLvT>d;qD1+X_ypi+F zd4#(LryW#bSRR)Fgi|fEbmn8XH#Cn&Y;^7^Ty(r!2;%yl`YrA&Qk817-sK$cD*$~( znJ5DKQgT=t1`mU1*#!@xYw2r&pLfZ@`GQ90i4NN5ShO@}ACc;Uax71htWY!R_nSel zP!c@4|KhHXwR+^qOpObYleXg=1!>)D?Cz9dC7)KU@pnsNjb0 z&{6boWY0|!y|?8MmWdOVB7=Is1vcyOA)k$jyQG`q5+us63Jaf(XiRU_bo)DC4{>9n zaW_1fXEc-P{H5}ZBvDv*URB%s5vvE4N2;Zpi(I^oKP-XP&zzXWu}9AnZIgQmtGar{ z_vKK|wTGuw-{bC-%amKD+}$*74DJ!SKMRU@;bbl>TS3HuiW}8FO zV>E-_iy&+qC04VIgYA3WW#zCXa#x3henZoj{L;glBX)G>si|FTUC>kWSO3w4@h-vH zYPM(2sMKCSi!+h@G%c{FKkHa{USAe~TOd_JmjNBtswTKK)yrqsvXWMuXuv4cpWtY$7D7c+)c z3%rv%-KC}k{|$#=rNjYkynA!*7F6=MrR#GK*^!L?c#AHbalEah3ZXEu?vlnw!`f8Z zPErLLge`sWNLvU7HsK81n@!IVLr>Vmi#<8R1k0ad;<|HRKU9lxTLGR!Rp^=?8{?ru zIV$W+{{i_Q-_I}bWUV?kA~qBHGx+b<0fxv=yhT+;MA~{cP|vC;9drUqW&Hj&04Wny zlr$o|hQqy4dJx63O_Gd>bV`fZanI;yP{Bemn3@_`Jk9#`8joNL+rk~JfO_IjX{lR)>Df=m2>K_Mc@qo zXenb2U1YiK5Fkcq6zFMmASh9?b<&gkDiN%#CeS{3`y|-gnQ>X5v-?3^5oE;jt|D~* zQCT#Bg8FQ}fH~mgJUC$JMEj~tNw&nFwQZ6^2qipMrtac#XJ|n4&R&FUd;RYeB%HzQp zC&ime`Zb{AHavsns(gMVW|*e8jM?@Tyvf_M`tU(KY^vMthnC4`7bV4gX{d4#T-UYt zVGB~L#Q<2`vzZvrA@8k*R!Y%oY~Qq(hFfDOVjTVgkc(t9&m?~yf< zUr20yz((XdkY?A;mOUBN$7(Io{VeHn>cSKtdGdR*@HcGC0ANd{MQ6i&a6HhC^503V zaSYLiBi@?qtTyKp0cR#YF+YyWH+Z;OKli^_OEJ$N6={)ys$X>t$c8{v+`^GhQ+ZQNVB@W9)rlY(Bq=<{1<~PmnQ3 z$6h?9=?wj^@NnOM3vY#x?I$Aw4-Ze{8u+=2>2C+PBYfK)x7WiIOwMj=t$Sr-WH>m} z&3{ketuVL@mO20YN-9mG=0pSp^aGcghkS?p_i29pYQ*!;C%~KWXLY=qE>8P|uzI36 zAOQO?;^a7zG#t(hv&2}c6HCLj(!ZXXDBE{yQDHE-ZoHmq9PZhfbZ#DJaOGL_GZ;Sv zfs`Q!^b5h*g~z+GooPm`(23Gj?Q(KY$Akb5jxn0I#Wn8 zI##wi+=qg7@8AQy%l*|Xbt2RSx;;LPm=)J=pp_$FwSUb3i}o_xMBVTP74y&K(KF~M z!ei;6Z(Pbea&Mr)oxg?q`cs8H{K?FN<%1u5@9UkI zPwdC~VWF|H)~+Ak@hj$s;aHyisyymia>(oXMz3!IL`p24ttL$7^fehuvta~ zk?7==Z{$gh=GV5JQ?KWctmc)XVf0KVx3cq@buZ%In(JR#tleta4!zJ^Tq1kggO460*gF}WNT=x z_GQkqw19p*04&r8NNIGNHpJ^flOxv32nkJ0^kOO6VnpVPBtt`H=jv+Kde_paDu5O( zLw}xl!b~?k7^p>^e^9Gy11g-YsAlwGH=p~Ki7mTC!3yXYcaD`AOVJapF? zg3**6@MrQ?eR~J}72lY`&-seUHm}L6-&|v-&0VkG!3+CG6m$s+hGw}hD^BwFPORer zH+8p|;`HHc26lxqxtm?ztUt|^hbU?zn<`)Hn1&To+O7!MP#hJ`l>XK1W1q+D*(=pL z#<8G-F3Lx%ErS&vF|4O3uK^&L%KLS5%mN1z>yKP`AgTW%8GUs9&T^zU@rdBY7D>`k zH?TA(d@j%>{#TN5A<_)CsF7|7cZ-IrGb~;sRTRs!#Z0owgx)zmGO)-CJjv+JvAJ>F zxmif6SBOG2EuH@YB5vt+lY;WlA_2$O@I0V;^FpDoFE3@DU3ho^ZI&0&L3S${k`)46 zZBPO(Auuu4c8yskWij)iarI{FV_*27KIpNyMQ|6UHsnm!(t7o)Q_afwjU%$%V|d*d zq&|HFUR%0CvKArlfh zTSuP0qr@6VP-X-IYQ8x`=graoZDVou#a(uvLfySP2-)OJw5Xk#sLd_7;PSh*Hr=-& z07Se-uYUC}&N8!Y(+CAsJO~JmJjN|r;{N(zfcz$?y(LEmD9G1NsI7C{?rO2$V%GqK zj0*bQn|-b*I|4e<{zn+&+?5EHq#GbRp{tW^Ngjs2Zqb6d5J9y~yt1USIhH~ztJ0wc zIZGa8U~{#S9i6C=t9Zqlg#%QtmXJe{(yh}|@Hwhji_?@Q@k>W?oOhX%W!ujynYvw7 zmXdwmI(6|K6rAs6cHFvUMt}OyL z`-wLrI`X8t^eM#{L7~C+1flm&+ppM}3K^Dmm4=R^OsPXwZ%L1K)VwQt&UK1k5GR8c z0t=7F5@xYe56Y|waW0V-%<8RRZ2La@?BG)@Z4UpmSO^!}&(Ir1mprLmHu^|v2K=9G zSWj1xznn!}*y9r2QNQSiI{&2KW-Dz^(m0Fz>r{(9;9(sW^Y05H&Hp8JtjU? zzO8>O2y#20Pj3?p|G|N3r&P=Az8bOu4rdF9F@T|S)3qrUdJ?v_`JqKc$5s=VlYVEk zLg)9)yY1C8STWX`y!bLB5gFIjw3cN-{rdHxW~wR|=iFCQ0Aesk{I4m@#~wZ3YNQr) zAZz?T5^gzN>aU)sz5qG#M|bX#G~!T5^t-nNB?b16vkjlp({-{|F#A{3C+%sn3}1`f zN#w9A;#zU{Lo1GDPnmv?vn+4uAU<0>%AiOSUF{zZoNZ%QEsB7BztIHvFmXn3>`3}- z^NH3g-y~2I4~!>?ZBBleXxG8z5a8Xw&DZA3+xrz_qpF)y00H?33c9XfhH;e=^SNd) z<9NsrEw|Ls-#1Q1@o!y@tv=UicDJwPGa&vnFC#|!n4ck(_fvH?TY~RBx8zo&-~-{k zF$Kdi-mN|snVJ#%$BIi4vX>IP|CZcE0#!v25D<0j8o^1=83nkXix988zP&Mf^tl24 zNAWx2wf&Ew{bwjq2N4qbuT#k=svya^wt2=sSVFi@!Z^`Bvi0_5Y8d)TvJDW(uf31P z%ZD%3J=KB!DX=hna%OggFOyP+yLYH|KR0^?&ci7-Oe~7^!G`BA|9J`!91@I;$N8am zIe&NOK`jzM3;5UehD>R05kJ{X*<50HG1o6OQbwL5Tjf$IQan7o8(BRKIXjQQ;kUNX_l9;Sv;^8?_8<38H zXA?nx4{G2Ssdi*=jiWknSI`re$q@A$OAqvDpdQfwG0NK6SSlL07At}#|B66x!{2DC z?;o7lZb1W>ho!BvKlCfF8~DlN;12Kj0kWnmh`l~Ab#J@W&6Y^aexqyW!1?#YP(sR~ zubP#FelZ5GG=^`VG2A*W@faDZr27xQk=fOu#AAIjQ8sH0f#5;t>rDu*;j#*f$?AL{ z>xVZEueNdqcd04um%j1zUy$d4ytnf5!HjYksMgV86Y{piS$+)U97Ltu`ZU9uyZp)A z7zV!{a_F$msYHys*V!2y=8QwqG`^O3bN7i^*bXY;idc0PoRR<6I?R^s-WQvkwsjri zLu}YWEE5|eTmj7M{)*L>b}2z8qjZjt!24&gVs1|4r#Ymy{hkuT6a}W+fgLLn#LlFS_nxs;H`-zh%Ba&n9)-Y&%N{SlGd%Xzb_Li5k4p_kJJROPNp#xm4 zXDJ&yl(Lp4u^HD_ADxMQJe)f#OZ+c~W%||-Br^QZPq^ej@cPpb&7YNgjNMw(iFeP^ z#UXdlP{Pz(lB;T@4S$epf0Hi%p28baEBB+y*2k{!&ka}T!WKi#YN${ zI`A*G?LP(&KqQ?`=}!s8^c_A;V61R6%Sl=`-Gd{ulq;sLfy<~uI|{EF#9hO+j(oq8 zxpWvT?mQ;c-JnMq227R^Nxig!6^$-naUbqIgZE#)mdn**u@}~6qF(uNz zybTL@ftZ?KmY=P$Wp+0pk(d!<^u)a6)j44jrGZsLeQ-2;YSy)mSpOX~M zd<7+6&v^s$Wd8Tp|2fCv1z5~H5*l&XkUCFPf<9xSX}mlH!btDq&!J>t$Uf|{eewse z*r8)J`(nhm1gNwc)@a~buLh?UW2J){qDx07`vKTif27rW1m-IRwz(*Ols=aYYx+O) zCySRj&_tFTg2Ct`T!UMQ^q%c5m1zB=IrzX>V$sng%)}v)iA2ga5;ZMX5o-#O59vvg zeYphxbOAGZ>yHYK!37~%u2hbUJ`jKxRY+zqcjsyXwepLz*Bk>N(Pyy{UaitD`JU8>V6A~ zVrYnH7-Z$NAw0PddV?cfFvojAyLD0d0p#OyW)uS$6>AGnBg1d)UtP$hg0X5bZRmyJsSvgTzLn&q5 zkq*3TG3jUWyR$3}$m~C14`<|xvf|py977@0k_5O|tNsAReUXT$&);yV5BsH=dOC`f z=Y+2_v&?s}ojF2)!N5M(r-7V#KN?;3^Q}kTOI}y8<`{mts+W^ zHA>k9LXGqY6jM?&`ON5l0^{Bs;EW$fTu|JqClr~N%KY|#GFm`nR?if`YOP~$rW-Nb zTYKg3P*zj3O9}rKd>$UwGu-*9>;St8$0z@PsjqICQRC9*I9Vv)w{Sm#uZQnln4I_0M&fv1g%Z!1E2oj{ zu*AFYs148EZd!Qj=pKZRCiQk}Ba5eMkmeUZ#%O&@$-QLFqi-8fdMCD036UOgMmy%;;dTlJN$L(vJcx4wTkSw|M=KHUFp5<{G&o_mQRek8QycXQfCySt^#V#2sC@`M~H_ z?#344;Q+d$BD-G^oWq?o%_m3GC>_P=ZCGPQcUl?+dGP|Pb7xO7CnYb9$rAOD#}`oX zU?+bXdCG-A9g#)*j!FgU{8SSWNRPlkyWkf=nTf|;m#&ym{~a(@<|E`R;TRdki~~J$ z*nzc|1@kHy#tI$_gnm8NO?}9eEj}&el~YV6a`)J0%4^qPj=*(gEbpU z|M_*s&-4Z)36T!GI;6a1Z9M$;Iyy-sEVX;$jN_i(Ijdb=dfTTHw)&1mg97qDb)TW} z-plAEGn1%K$)vbqfK{!Q8o`#E@)!BS(E>6rpM<$bBBGW9cyy4L?>0d#M> zjk#l7!WtbkzTRO)ny06ZDmK8mF#qu={gU(jCK^#78GGV$Uz-k%9a=&MLJuf5NT7#a z%@Nd0)Q88Fa$m{X5~tTO4<8;H12px(WB+OV{rmys58r~)qEPsez$u6h#ACX)7@A?1X<{sh;GBc+KWuPpJRrt7sqSqe?=%a!zP>aln5`B^)bTEViKBp4& zScSN(puBNPkuiRP^NUYEPt$)-=DAp}fgly==q)eNfh;G3s*zE8yGN{cWjM8h2U13E zglGw#1Z0nV^3f;uczPtGW>;&>G`zz7Xpb=H8oDfDHRgon12h>$i5$uCcJp zVTmr^hyVECmMy2B$j)-K+bO%CLWeTptRGM)LseFOZ>&a!Io#5JSN~$1K4heqOJ$8s`v}R(%&7_!@w1J02v=hD-wOwmFm?H8@!nCUo1{O{_P%l zw(+v&-QrWPMDa}2nrv#hiwPL_zssKI_}1{1sd`%2MmR+ft)DR^?y9#nM1YW0=T_^f zKKZ8e(PR00L|Z@TQgI$MLDW&5Il0Du$IQ2Rx@^~LN!Z!BghV2mHrSW(Ae+&g>P!?h zZCjm zOK$}@TOlM1n+;rn(DL-|+_6rv1iKk_d5=-v5{uWx$?Av|3FyF6Y31(| zYP_X<{fgL3t;F+oMua*J-n_aE%3(zQp7kCrnNL<;A_j_`^PW%_j>HTUmk)J5*3akJ zl!{`H`Hg=^baoFaS-O#EwD!yvcfCwyPIk_fx@;0WKRpc!X)pRko{WsCV6z+^&&eE3 zJTOEa{9ksIj$BA(KJBs%Z; zaiFvRt`7sJl?Hw^0uy<+r*6+d0{%xp^^tt)O?UrHUCmn4bj};r0msP`-3JV8%UBYB zcWU9;bFIIB1R=VR*U5d24pxdyXC`y6C0@0c5zp;e>p<`+2mq#k+&z4j(BvWalQzoW zOuPHNJqiZ5+Cz9rY-BiKr8`!l9^BQFh<*sZA`L72_#|jNs&`$2sa98WmnqSS#^yp) z;NK-no|(%lV*e3S`9*--)g(`YQra4yM>KN{2CDwfg zi#Sm?NBngSy+1tabm0$B7`XoOB3;Q`x9%_d5`;V08l&N{ z$pfV^_lD8ncUc1Nw9$f1PO@=!-jBI9A`1))QdnV!7{0U@Q3itYO&f`@bkOH~X3e(OOr!W7!w!8<}MyE8tX z9A;5Tb^Y&GCd_E%Cp+-fST2sJl{O`s_(7h>vX>a<>{BLjf7Tryl5m!x$6gV6$+M{% zx8wPR^m=@|y;Bh(tZmccWamS^!Y;g#Q-!BXgcJuw_ennYg5^3GPAGav4+kC<4jr24x6q=jP4=G8^mfq(*h z-7t3f(=wodI(%Lc*vZRPOuU!O0jdkIqYe*gus9Lvtr$ z(19xfw}oqvTbs_2;+m;F?l&p02UGu)rS$&bG6yt-FLqq4a;*Ri*$2H{j{e)O#soKo zZG)wfGBOdF-Oak}mJHQsZvn#>tl%!MjVCGMJAGbSE?jAi-r6ua(~s=E>w=^omgyO< z_ctRb>UnF<1x{4qxT24^i}lXO_K(ksFWbqP1*2t?1M$BIs7K{#25V|KMr~m9Ur$7z z(7|pu@>NV#%t)LRMMt;a>DG^%8nYu?~-Gs&xja_*kQq&0fS zHV0R&Qq=OGRIWhwXah*dR zq5=a>hAKnLk5SlA{MH|HMuWr-tnIGT^&aT7K}i}K71q-FFN@rcNKNq|}r0URwufDI#+fMWlOM)?G`$*4OJBq0ocUpfO1DmRS6Oqmc{%a**7{4DhX>jCu z$i?J>Q&jhI_(}(NF!lS~tRdP=NtSVcv?Gz<7ZbAHB&}yN zQ&uDuw#V}Kh_(Z1Fl?nKKzJed)li5AiIjrg9=*r3OVtl_iXfTS0CqeJg{vgS|9v{m&`dy>6+(nW(W+iy<#t8`uG zz_A$}Tu5dCJByb{XpML0Iw$#wqVN#RzDY@EbK#7v?zz8?OBaBKNPomL3Fg}d-`b>6 zb6FGbd&kZi&C*F7mpJvIodPO=IH6PX=3X9{y%(w{3@Q{2MVP}yWuOJ*s1Qj6<2so3 z=YKFdK0B#%SD;~}CsCiP@8o^C!ye2l?j9tBLvIFD1V5EB&imB^cbmJKp(Bc*4Xwrxas2hOM>H)35w$-7cVtukVmLNI$W;G3QW2S6*4;(h)gFD{U|J4=@AN6BS+)tzBvgp&*S z7X%<-z(bOq=L+gg;d7lYu^1IvIMjkL8NsxW4aOE1JaL?$pcpjmm^+}O42W7|5@?>V z(8ZVPeZ91D;I~ot>l-SL)iyy#AXB0A$_Inis*3aN`3fHVH=zmmAE8MO+0#_T&HBvM zTwRvAlKrk)Q42SLJAs~s_JBUu*Ek`{;*H{!vO^mH!EEq8FYY%A8^?U&>vR-lG{7X$&vdUE?6f^~90mcad0UVT|UOo z_#wYjuDPOvM=I_8`$x+kFwmX32FkloR9P;>ks>#pxNQ)htk&c~aZRiJ%!}0`!Dt7< z*M8L3Gh2#56mo3`m_MhmiM=gl@X{`k*W?WPe*ssul2@prwtLeQvTV=Sk&m-uX7sjn zH(VqDW14Wrng&{aADc?$bD+F(Y#Rn zOjN&`!YFWk0!4{-FuD$@NOfTaPXP5l?i>V+Lv2sh4f3`g_L{1o-Y2Y0q*1me>m3kI(3 zs|mq6Z&+76^WwyXv$wYBcm&y0_0p7iG3zU9TShmD7ax_-6kw}!#bU|`x|;_qr{?*+ zlZ4&FZ$a<#0tLLQa)jzrMqi|70H(c$burdh@jB36n;ROJWB+$)Q~Q!NaDc*A7nsVoQsw>VXp43PS&&@& zp0T!*Nn~0A03If*^ETF^L*Ex16+-4Cvo|jA1i`SesxuB9+LVqdABUC~a92V$kI>)X z6xxD2{xaapL(~1~0`6d?{vQ>CGAnq;R69{w^-pY8+N-v)9y_zz+CC5ABN0z$6ggcR z)0o3R*smn&QI?_$TQ92eVVm-KKMR?BAT@cH0MlQ>FCKvv;+!^s5Kz8n3(Ze7>h>lYRi*Vp;;jw|PWyjNk-of`e=U zqoQKT`*BCsxs9t?$MyPv!p>p&`vJ8r-TO0;?-EA#u3~o%?MLRFW-ovn>o+!$zo@JgxSMI=@^k1LeCB5@52pNOF6n z@mGt38u;jqcP75{HiE)rq{n@WY2!Ce%yTIj5J^~@*^K^&C09>Qn^AZ-Ut1UHSQx`e zsUwgC{?S2}=9`Oof`Q`PKB5_m7XyQmykwc(g{zO%;A`_0iTA6q@bp`J8=*KE%}g8) zW>oWfD0^3`b2Tyk3pT+L@g1rdmxk;$=(rJc=erKlJ!##cMlBkH7;A9_d;!9rbLuZO zt7FNVF{MZlR&;@b4S}d=c++2>EjssU+k(cPZHeedm}v9yD^9(=5lu#|c{%n}!OPn$ z3qSC?M0(w67QH0ban8}Lqh?;KtYFHE{{14zL4PIXQ zK_x?T;)aA)u(;NWcS9LAh~vM*A5y|L-O_vO7BQ3bB_;XiO|ZT_#UR>F2WZ~IS2Xx2 zmoR5&V#y=^%C)m8S)#|R4}{}}pFPCQ+F+gQ$m`FRAaq?==u_s{L?uTx>a4&WPd~8P zz;}eYH5yfoU00W0FD& zaG7iJCo{`r{rBkx7`p9bs*VZ%3Y>FycQ^AD8(X)Y%uGQSR#{gPrZD!JLYOHub%wy; z^fYXhvd3|-59AL-dZJ_24L;)~GEr+J5k_xKWdF)l%l^vI72WUA@PvVi5Oo~fh0aB;$bt<{!YO-g)NnT+W1CnES^A{igU~S(@c!Qv2U-vvrvg;^4-4oKZGkW3E^R zheO>|BW-}B2o3rJvi*1|iJukmak);!8l9<7nytD5veZ3FJSr0lV@T~-kXtZpKO13i zXqIFCO^F5cBlpxnoovlILxp4zh+ks-GHvtr-|h~zZgb_;coC#`l@tYLlYu0sa2zb^ z2Ds5MZEm2XY4Zc44)||+KhBWhi%U?>cEEJ$62=k0U(SD%2kJk}%73N`Oq>2Yohth; zG()`!7Hzz4%EYw)DG2>C?&>^|HWJN-xXM^*kWjD5jE!!fjW#XvxpB48SoRM{c@e%{ zXYj-jX+~CC2Qd;ru!6D}qCNQ$XtTbH-OTmG$qeNzQP}xTtE0^I^+&lHxmD-qp1u8i z>)MsxO#XV0OQ&YmM9^s5$x{7ZrtV3FL;I^7{#J4^v*%lo>|B4`gJSg7$(#a?$#in~ z;->~4F}BskUc4!%tJcPbQPb@#fH=dn9xlex*yA5Gk5G$DpUenE)AVXa`{io?CCY#B9m?lE;^X}B1hPkkSQ@|ma4j^J z!|+I%dhlcfp$&q{M*!~0`A#};?C($G{{dI){tH|cu0pkBAh0(Pv~i&!CXK(#ZQ>k6uXW;-i8mORtF%hKTGsq}nJK+EOzPP`^yRD(bKeKm!?E@V9FM!h`ad*&g0Ihhq# z8BY?-?_OMyb~TImX+GbExY-Z(0zXLGw>k|EF6W4{7FT*!mwy~SMRoSpXxKW>d>DM3 zL8@k&f{ex)<8jd9Z^TAUN#R{yfxT5;jEF8~4ylZnR@^SZ!)# zVJ7R?Flkg&xyv5<2V+IRLuc11k=Y>t#k!Ff!L(7En100l3rB4?)zep6_G zl9N+b9hw>zu_dTkWN%V|Q`OJ{Yvpo9cqRh1`6g6rsB81}{dC3LTy_q<^MZ@Hy>k%H z3Py@gR}7=J83aaVT`w3=Ov^_pG`41DMVhMETqcxWZl=`atX|QS(eA{bo?2W*8?jL3 z5f)sk4v%~yxJE;v!QVc^OambdcpooIDxHx;ibKUlgHLNn_ zFzq(w0E=T)bg9qsR8w`VM%wuC&h@o|ITWiSn59Gv{(Z4Q$PSgkyd(>MvJ=#kMPy2T ze9I0MEVr$SC=ixlaz&Zf(v+0r4y0S(VB|$D6XY{p$r^2!k}k|)3ho?HUJZGd8C-Z492Yhhz>LR9fPR!DpBD7u`c)ig2Y9 zSZT}mwu}@Q*HiG&w-;TIUeU>L1lIPP;43JZ_bzH{^NXoQujB_gGSyPjhqjdHzCUz- zp>rS#f3+$ySJuIEKEPOw8vmjpbY7F%4}&6Y-}HJ=UL%_fY3AeA`TJRMe_~~`S5zIb z-Is=gVSG<)KIm+I|G~)Ow2J{0%)VmQa=W1Fq!grXf?nPcbcyRm%V`MH@iJcG?7d$r zx3W5@iiP<+-|Fye#E$hB)34l;p-^>$^N}5tHV>9D$Pg?Y7dsKA6A>{iDZz$5$kF6aPR>Dz~sdQ`4rWo9U4P&o(pM1#%1?(0|uwJ zd|pXD6&4k=JXkVP9%gTjywyGR^bhLH?uE+}G)aq#uIX45>=E8NG*;Fu`rH)YktM7w z%WHD4dO0}V@a#y&L^m-wS~X0ke4ov=;jtCGBMffCwoMG}VJf8i8+jg_}C95yRKO^*8>jN zo4XdQLid2!GEZt+pPuaD_=Qxm&Z5k-dz`J4bed5<87D0+S{pS9E$48Oh_TnhWN_Cy z5;3q-eelO-yh-r)ctfy2^k1e(iHW+4!0dz;=F0mM>Bg*L)wAxcu*o$?av^DdH5o9M z*vPU*iDzzHfyoA$%Nqc*#5XZC=86jZc+pUh4^1yj1#GA$fu!Y4IZ|agg0iCJCZarS!Et zJ3By6dh+FrUy_Mux---gSoe<}bI@!fJP@JW-j=m=AhW#lIz=!K8=vPbg|#-q;e$`i zcr-3P!YS3lwsBqyLl%?NgCYm^LC%G44DISI*(7z*cY1Z$-~W@6VLqKU(0R?&v#`_lJ6bt zD@~QbQW>~Mv#${A2@PW}gG=dw<>gGBPx}~nBU@QEvok5Szg0NC$+X7trS+_B*VzyDO5nYnJ+VhB;qdP_v@xqVOX8m-iD+|%DTk6`Ykae*!O9@}MSjc!w>r?kOA#R03qYW)( zWa_D3e6&`$Q;Z(0A?3sszubMKJj%hB4k=uyxyPfu?sE7iqTP7h&tAiz*nq7%@4Z>z zd0TFbt*bP{oM!nS3UnmZ8Fu9`1X+5XE~augcA*yjsMs~Fv2`9_JFX9^Wwops&Zq}qcb@e7C3*IXDs|MrYn zPQap1Qoh-dn!ivDe=&WFA&p`(41{ALfKUvCQ!#u=jR-c-c_91QlI}L1Or20p#{UaH za70u^u8sHS1Dw9KXf8n@9Dise@y}I%_R%MUQ2cv{DnjW$2M`#D{a-!c(T6#Zhsb83 RmKETqhzUsxR`cun{}0>SU7-K~ diff --git a/low_cost_ws/src/arg_utils/include/arg_utils/__init__.py b/low_cost_ws/src/arg_utils/include/arg_utils/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/low_cost_ws/src/arg_utils/include/arg_utils/anchor_logging.py b/low_cost_ws/src/arg_utils/include/arg_utils/anchor_logging.py deleted file mode 100644 index 3a92ecd..0000000 --- a/low_cost_ws/src/arg_utils/include/arg_utils/anchor_logging.py +++ /dev/null @@ -1,80 +0,0 @@ -# AUTOGENERATED! DO NOT EDIT! File to edit: ../09_anchor_logging.ipynb. - -# %% auto 0 -__all__ = ['examine_plot', 'examine_one_plot', 'examine_one_with_boat_plot'] - -# %% ../09_anchor_logging.ipynb 4 -from . import utils -import matplotlib.pyplot as plt -import numpy as np - -# %% ../09_anchor_logging.ipynb 5 -def examine_plot(lines): - plt.rcParams["figure.figsize"] = [8.00, 5.00] - plt.rcParams["figure.autolayout"] = True - fig = plt.figure() - ax = fig.add_subplot(111) - value = [] - for line in lines: - if (line.find("time") == -1):#is not time line - temp_list = line.split() - while 'boat_alive' in temp_list: - temp_list.remove('boat_alive') - value.append(float(len(temp_list))) - x_axis = np.array([i for i in range(int(count/2)+1)]) - ax.plot(x_axis, value) - hours = int(count/2/1800)+2 - hour_points=[] - for i in range(hours): - hour_points.append(float(1800*i)) - grid_points = hour_points - ax.xaxis.set_ticks(grid_points) - ax.grid(True) - plt.xlabel("Time") - plt.ylabel("Anchors") - plt.show() - -# %% ../09_anchor_logging.ipynb 6 -def examine_one_plot(anchor, lines): - plt.rcParams["figure.figsize"] = [5.00, 3.00] - value = [] - for line in lines: - if (line.find("time") == -1):#is not time line - temp_list = line.split() - if anchor in temp_list: - value.append(float(1)) - else: - value.append(float(0)) - x_axis = np.array([i for i in range(int(count/2)+1)]) - plt.grid(visible=True, axis='x') - plt.plot(x_axis, value) - plt.xlabel("Time") - plt.ylabel(anchor) - plt.show() - -# %% ../09_anchor_logging.ipynb 7 -def examine_one_with_boat_plot(anchor, lines): - plt.rcParams["figure.figsize"] = [5.00, 3.00] - value = [] - boat_value = [] - for line in lines: - if (line.find("time") == -1):#is not time line - temp_list = line.split() - if anchor in temp_list: - value.append(float(1)) - if 'boat_alive'in temp_list: - if (temp_list.index(anchor)+2) == temp_list.index('boat_alive'): - boat_value.append(float(1)) - else: - boat_value.append(float(0)) - else: - boat_value.append(float(0)) - else: - value.append(float(0)) - boat_value.append(float(0)) - x_axis = np.array([i for i in range(int(count/2)+1)]) - plt.grid(visible=True, axis='x') - plt.plot(x_axis, value, x_axis, boat_value) - plt.xlabel("Time") - plt.ylabel(anchor) - plt.show() diff --git a/low_cost_ws/src/arg_utils/include/arg_utils/camera_projection.py b/low_cost_ws/src/arg_utils/include/arg_utils/camera_projection.py deleted file mode 100644 index ebac91a..0000000 --- a/low_cost_ws/src/arg_utils/include/arg_utils/camera_projection.py +++ /dev/null @@ -1,104 +0,0 @@ -# AUTOGENERATED! DO NOT EDIT! File to edit: ../01_camera_projection.ipynb. - -# %% auto 0 -__all__ = ['camera_projection'] - -# %% ../01_camera_projection.ipynb 4 -import numpy as np -import scipy as sp -import cv2 -from cv2 import aruco -import apriltag -import time -import yaml - -import pytransform3d.rotations as pr -from mpl_toolkits.mplot3d import Axes3D -import matplotlib.pyplot as plt -import matplotlib as mpl - -import os -import sys -import gdown -from zipfile import ZipFile - -from scipy.spatial.transform import Rotation as R -from numpy.linalg import inv - -# %% ../01_camera_projection.ipynb 5 -class camera_projection: - def __init__(self): - """init a camera projection object with default arguments - """ - self.camera_info_path = 'ViperX_apriltags/camera_info.yaml' - self.img_path = 'ViperX_apriltags/rgb/' - self.depth_path = 'ViperX_apriltags/depth/' - self.tag_size = 0.0415 - self.s = 0.5 * self.tag_size - - def read_camera_info(self): - """read camera info from yaml file, path is given, camera info contains camera matrix and dist coefts - """ - with open(self.camera_info_path, "r") as stream: - try: - camera_data = yaml.safe_load(stream) - except yaml.YAMLError as exc: - print(exc) - self.camera_matrix = np.array(camera_data['camera_matrix']['data']) - self.camera_matrix = self.camera_matrix.reshape(3, 3) - self.dist_coeffs = np.array(camera_data['distortion_coefficients']['data']) - self.dist_coeffs = self.dist_coeffs.reshape(1, 5) - self.cameraParams_Intrinsic = [self.camera_matrix[0,0], self.camera_matrix[1,1], - self.camera_matrix[0,2], self.camera_matrix[1,2]] - - def read_images(self, idx): - """this function will load an image depend on the id number, often used in a for loop - """ - self.img_path = self.img_path + str(idx) + '.png' - self.depth_path = self.depth_path + str(idx) + '.png' - self.img = cv2.imread(self.img_path) - self.gray = cv2.cvtColor(self.img, cv2.COLOR_BGR2GRAY) - self.depth = cv2.imread(self.depth_path, -cv2.IMREAD_ANYDEPTH) - self.img_dst = cv2.cvtColor(self.img, cv2.COLOR_BGR2RGB) - - def apriltag_detection(self): - """detect if there is apriltag or not in self.gray, the self image - """ - print("[INFO] detecting AprilTags...") - options = apriltag.DetectorOptions(families="tag36h11") - detector = apriltag.Detector(options) - #results = detector.detect(gray) - self.detection_results, dimg = detector.detect(self.gray, return_image=True) - print("[INFO] {} total AprilTags detected".format(len(self.detection_results))) - - def solvePnP(self): - """this function will output the rotation matrix r_vec and translation matrix t_vex, this two matrixs is important for projection - """ - img_pts = self.detection_results[0].corners.reshape(1,4,2) - obj_pt1 = [-self.s, -self.s, 0.0] - obj_pt2 = [ self.s, -self.s, 0.0] - obj_pt3 = [ self.s, self.s, 0.0] - obj_pt4 = [-self.s, self.s, 0.0] - obj_pts = obj_pt1 + obj_pt2 + obj_pt3 + obj_pt4 - obj_pts = np.array(obj_pts).reshape(4,3) - - _, self.r_vec, self.t_vec = cv2.solvePnP(obj_pts, img_pts, self.camera_matrix, - self.dist_coeffs, flags=cv2.SOLVEPNP_ITERATIVE) - R_mat, _ = cv2.Rodrigues(self.r_vec) - T = np.hstack((R_mat, self.t_vec)).reshape(3,4) - tag_pose = np.vstack((T, [0,0,0,1])).reshape(4,4) - dist = np.linalg.norm(self.t_vec) - - def draw_point(self, tag_2_inv, base2joint): - """for visualization, draw the project points on the image is important - """ - # --------------- project a point --------------- - tag2joint = np.matmul(tag_2_inv, base2joint) - obj_pts = np.array([tag2joint[0,3], tag2joint[1,3], tag2joint[2,3]]).reshape(1,3) - proj_img_pts, jac = cv2.projectPoints(obj_pts, self.r_vec, self.t_vec, - self.camera_matrix, self.dist_coeffs) - proj_img_pts = np.array(proj_img_pts).reshape(2,1) - # --------------- draw a point --------------- - draw_image = cv2.circle(self.img_dst, (int(proj_img_pts[0]), int(proj_img_pts[1])), - radius=5, color=(255, 0, 0), thickness=-1) - return draw_image diff --git a/low_cost_ws/src/arg_utils/include/arg_utils/get_ip.py b/low_cost_ws/src/arg_utils/include/arg_utils/get_ip.py deleted file mode 100644 index 671c0ff..0000000 --- a/low_cost_ws/src/arg_utils/include/arg_utils/get_ip.py +++ /dev/null @@ -1,143 +0,0 @@ -# AUTOGENERATED! DO NOT EDIT! File to edit: ../04_get_ip.ipynb. - -# %% auto 0 -__all__ = ['get_key', 'myip', 'whoami', 'get_xbee_address', 'get_xbee_address_boat', 'find_duckiepond_devices_yaml', - 'dp_load_config', 'dp_get_devices', 'device_get_sensors', 'sensor_get_topic', 'ssh_ping_nano', - 'ssh_ping_rpi', 'test_ssh_intranet', 'ssh_connection', 'test_ssh', 'ip_connection', 'test_ping', - 'ssh_rostopic', 'test_rostopic'] - -# %% ../04_get_ip.ipynb 3 -import subprocess -import os -import yaml -import re - -def get_key(dict,value): - - for k, v in dict.items(): - for v,v1 in v.items(): - for v1,v2 in v1.items(): - if v2 == value: - return k,v - -def myip(): - - ret_byte = subprocess.check_output(['ifconfig']) - ret_str = ret_byte.decode('utf-8') - # Cut string from 'equal symbol' to 'degree C symbol', then convert to float - en = ret_str[ret_str.find('eno1:'): ret_str.find('lo')] - ip = en[en.find('inet')+5: en.find('netmask')-2] - return ip - -def whoami(data): - ip = myip(data) - machine,device = get_key(data,ip) - return machine,device - -def get_xbee_address(dict,value): - for k, v in dict.items(): - for v,v1 in v.items(): - for v1,v2 in v1.items(): - if v2 == value: - address = dict[k]["rpi"]['xbee_rx'] - return address - -def get_xbee_address_boat(dict,value): - pair_device = dict[value]["xbee"]["xbee_pair"] - address = dict[pair_device]["rpi_2"]["xbee_rx"] - return address - -def find_duckiepond_devices_yaml(yaml_filename="duckiepond-devices.yaml"): - dp_yaml_path = "" - for root, dirs, files in os.walk(os.path.expanduser('~')): - for name in files: - if name == yaml_filename: - dp_yaml_path = os.path.abspath(os.path.join(root, name)) - break - return dp_yaml_path - -def dp_load_config(dp_yaml_path): - dp_dict = {} - with open(dp_yaml_path, 'r') as stream: - try: - dp_dict = yaml.safe_load(stream) - except yaml.YAMLError as exc: - print(exc) - return dp_dict - -def dp_get_devices(dp_yaml_path, pattern='boat*'): - dp_dict = dp_load_config(dp_yaml_path) - devices = [] - for key in dp_dict.keys(): - match = re.match(pattern, key) - if match: - devices.append(key) - return devices - -def device_get_sensors(dict, device='sensor1'): - sensors = [] - for key in dp_dict[device]['topics'].keys(): - sensors.append(key) - return sensors - -def sensor_get_topic(dp_dict, device='sensor1', find='zed'): - topic = dp_dict[device]['topics'][find] - return topic - -#ssh functions will not give testing example for now on, since hostname will depend on your running machine - -def ssh_ping_nano(hostname): - response = os.system("ssh $USER@" + hostname + " ping -c 1 192.168.0.100") - return response - -def ssh_ping_rpi(hostname): - response = os.system("ssh $USER@" + hostname + " ping -c 1 192.168.0.101") - return response - -def test_ssh_intranet(): - error = [] - for ip in hostnames: - num = ssh_ping_rpi(ip) - if num!=0: - error.append("ssh ping rpi error " + ip) - num = ssh_ping_nano(ip) - if num!=0: - error.append("ssh ping nano error " + ip) - assert not error, "errors occured:\n{}".format("\n".join(error)) - -def ssh_connection(hostname): - response = os.system("ssh $USER@" + hostname + " date") - return response - -def test_ssh(): - error = [] - for ip in hostnames: - num = ssh_connection(ip) - if num != 0: - error.append("ssh error " + ip) - assert not error, "errors occured:\n{}".format("\n".join(error)) - -def ip_connection(hostname): - response = os.system("ping -c 1 " + hostname) - return response - -def test_ping(): - error = [] - for ip in hostnames: - num = ip_connection(ip) - if num != 0: - error.append("Network Error " + ip) - assert not error, "errors occured:\n{}".format("\n".join(error)) - -def ssh_rostopic(hostname, rosversion="melodic"): - response = os.system('ssh $USER@' + hostname + ' "source /opt/ros/"' + rosversion + '"/setup.bash && rostopic list"') - return response - -def test_rostopic(): - error = [] - for ip in hostnames: - num = ssh_rostopic(ip) - if num != 0: - error.append("ssh rostopic list error " + ip) - assert not error, "errors occured:\n{}".format("\n".join(error)) - diff --git a/low_cost_ws/src/arg_utils/include/arg_utils/import_me_if_u_can.py b/low_cost_ws/src/arg_utils/include/arg_utils/import_me_if_u_can.py deleted file mode 100644 index 333e260..0000000 --- a/low_cost_ws/src/arg_utils/include/arg_utils/import_me_if_u_can.py +++ /dev/null @@ -1,5 +0,0 @@ -def say_it_works(): - print("You have successed import me!\nfrom arg_utils pkg :D") - -def say_it_pytest(): - return "It works!" diff --git a/low_cost_ws/src/arg_utils/include/arg_utils/mqtt.py b/low_cost_ws/src/arg_utils/include/arg_utils/mqtt.py deleted file mode 100644 index 48f5d9d..0000000 --- a/low_cost_ws/src/arg_utils/include/arg_utils/mqtt.py +++ /dev/null @@ -1,60 +0,0 @@ -# AUTOGENERATED! DO NOT EDIT! File to edit: ../11_mqtt.ipynb. - -# %% auto 0 -__all__ = ['default_topic', 'MQTTpublisher', 'VehStateSender'] - -# %% ../11_mqtt.ipynb 4 -import paho.mqtt.client as mqtt -import socket - -default_topic = 'topic name' - -class MQTTpublisher(object): - def __init__(self): - mqtt_ip = '140.113.148.77' - mqtt_port = 1883 - self.mqtt_topic = 'VehStatsAnchor' - self.hostname = socket.gethostname() - - self.mqtt_client = mqtt.Client("arg_mqtt") - self.mqtt_client.on_publish = self.on_publish - self.mqtt_client.on_connect = self.on_connect - self.mqtt_client.connect(mqtt_ip, mqtt_port) - - def create_payload(self): - return str(self.hostname) + ': ' - - def on_connect(self, client, userdata, flags, rc): - print("Connected with broker, result: " + mqtt.connack_string(rc)) - - def on_publish(self, client, userdata, mid): - print("payload published " + str(mid)) - - def on_shutdown(self): - self.mqtt_client.disconnect() - print("Shutting down...") - - def loop(self, timeout = .1): - self.mqtt_client.loop(timeout) - -# %% ../11_mqtt.ipynb 6 -class VehStateSender(MQTTpublisher): - def __init__(self): - super(VehStateSender, self).__init__() - mqtt_ip = '140.113.148.77' - mqtt_port = 1883 - self.mqtt_topic = 'VehStatsAnchor' - self.tempcpu = float() - self.ip = str() - self.current_time = None - self.current = int() - self.tempenv = float() - - def create_payload(self): - data = VehStateType() - now = time.localtime() - r = requests.get(r'http://jsonip.com') - self.ip= r.json()['ip'] - self.current_time = time.strftime("%Y-%m-%dT%H:%M:%S", now) - data.setData(timestamp=self.current_time, mid=2, vid=2,globalx=0.0,globaly=0.0,ip=self.ip,powerlevel=self.current,tempcpu=self.tempcpu,tempenv=self.tempenv) - return data.toString() diff --git a/low_cost_ws/src/arg_utils/include/arg_utils/plotting.py b/low_cost_ws/src/arg_utils/include/arg_utils/plotting.py deleted file mode 100644 index 962a8b9..0000000 --- a/low_cost_ws/src/arg_utils/include/arg_utils/plotting.py +++ /dev/null @@ -1,182 +0,0 @@ -""" -This file is part of Learned Inertial Model Odometry. -Copyright (C) 2023 Giovanni Cioffi -(Robotics and Perception Group, University of Zurich, Switzerland). -This file is subject to the terms and conditions defined in the file -'LICENSE', which is part of this source code package. -""" - -import matplotlib.gridspec as gridspec -import matplotlib.pyplot as plt - - -def xy_plot(title, labelx, labely, - vec1, label1, - vec2 = None, label2 = None, - vec3 = None, label3 = None, - vec4 = None, label4 = None): - plt.plot(vec1[:, 0], vec1[:, 1], label=label1) - if vec2 is not None: - plt.plot(vec2[:, 0], vec2[:, 1], label=label2) - if vec3 is not None: - plt.plot(vec3[:, 0], vec3[:, 1], label=label3) - if vec4 is not None: - plt.plot(vec4[:, 0], vec4[:, 1], label=label4) - plt.grid() - plt.legend() - plt.xlabel(labelx) - plt.ylabel(labely) - plt.title(title) - - -def xyzt_plot(title, - vec1, label1, - vec2 = None, label2 = None, - vec3 = None, label3 = None, - vec4 = None, label4 = None): - plt.subplot(311) - plt.plot(vec1[:,0], vec1[:,1], label=label1) - if vec2 is not None: - plt.plot(vec2[:,0], vec2[:,1], label=label2) - if vec3 is not None: - plt.plot(vec3[:,0], vec3[:,1], label=label3) - if vec4 is not None: - plt.plot(vec4[:,0], vec4[:,1], label=label4) - plt.grid() - plt.legend() - plt.xlabel('t') - plt.ylabel('x') - plt.title(title) - - plt.subplot(312) - plt.plot(vec1[:,0], vec1[:,2], label=label1) - if vec2 is not None: - plt.plot(vec2[:,0], vec2[:,2], label=label2) - if vec3 is not None: - plt.plot(vec3[:,0], vec3[:,2], label=label3) - if vec4 is not None: - plt.plot(vec4[:,0], vec4[:,2], label=label4) - plt.grid() - plt.legend() - plt.xlabel('t') - plt.ylabel('y') - - plt.subplot(313) - plt.plot(vec1[:,0], vec1[:,3], label=label1) - if vec2 is not None: - plt.plot(vec2[:,0], vec2[:,3], label=label2) - if vec3 is not None: - plt.plot(vec3[:,0], vec3[:,3], label=label3) - if vec4 is not None: - plt.plot(vec4[:,0], vec4[:,3], label=label4) - plt.grid() - plt.legend() - plt.xlabel('t') - plt.ylabel('z') - - -def plot_biases(ts, bg, ba): - fig = plt.figure('IMU biases') - plt.subplot(211) - plt.plot(ts, bg[:,0], label="x") - plt.plot(ts, bg[:,1], label="y") - plt.plot(ts, bg[:,2], label="z") - plt.grid() - plt.legend() - plt.title('Gyro bias') - plt.xlabel('t') - plt.ylabel('bias [rad/s]') - - plt.subplot(212) - plt.plot(ts, ba[:,0], label="x") - plt.plot(ts, ba[:,1], label="y") - plt.plot(ts, ba[:,2], label="z") - plt.grid() - plt.legend() - plt.title('Accel bias') - plt.xlabel('t') - plt.ylabel('bias [m/s2]') - - -def make_position_plots(traj, gt): - # 2d positions - fig = plt.figure('2D views') - gs = gridspec.GridSpec(2, 2) - - fig.add_subplot(gs[:, 0]) - xyPlot('XY plot', 'x', 'y', - traj[:, 1:3], 'estim. traj', - gt[:, 1:3], 'gt') - - fig.add_subplot(gs[0, 1]) - xyPlot('XZ plot', 'x', 'z', - traj[:, [1,3]], 'estim. traj', - gt[:, [1,3]], 'gt') - - fig.add_subplot(gs[1, 1]) - xyPlot('YZ plot', 'y', 'z', - traj[:, [2,3]], 'estim. traj', - gt[:, [2,3]], 'gt') - - # xyz time plots - plt.figure('XYZt view') - xyztPlot('XYZt', traj[:,:4], 'estim. traj', gt[:,:4], 'gt') - - -def make_velocity_plots(est_vel, gt_vel): - plt.figure("Velocity") - - plt.subplot(311) - plt.plot(gt_vel[:,0], gt_vel[:,1], label='gt') - plt.plot(est_vel[:,0], est_vel[:,1], label='est') - plt.title('x') - plt.xlabel('t') - plt.legend() - plt.grid() - - plt.subplot(312) - plt.plot(gt_vel[:,0], gt_vel[:,2], label='gt') - plt.plot(est_vel[:,0], est_vel[:,2], label='est') - plt.title('y') - plt.xlabel('t') - plt.legend() - plt.grid() - - plt.subplot(313) - plt.plot(gt_vel[:,0], gt_vel[:,3], label='gt') - plt.plot(est_vel[:,0], est_vel[:,3], label='est') - plt.title('z') - plt.xlabel('t') - plt.legend() - plt.grid() - - -def make_ori_euler_plots(est_xyz, gt_xyz): - plt.figure("Orientation [Euler angles]") - - plt.subplot(311) - plt.plot(gt_xyz[:, 0], gt_xyz[:, 1], label='gt') - plt.plot(est_xyz[:, 0], est_xyz[:, 1], label='est') - plt.title('Roll') - plt.ylabel('x') - plt.xlabel('t') - plt.legend() - plt.grid() - - plt.subplot(312) - plt.plot(gt_xyz[:, 0], gt_xyz[:, 2], label='gt') - plt.plot(est_xyz[:, 0], est_xyz[:, 2], label='est') - plt.title('Pitch') - plt.ylabel('y') - plt.xlabel('t') - plt.legend() - plt.grid() - - plt.subplot(313) - plt.plot(gt_xyz[:, 0], gt_xyz[:, 3], label='gt') - plt.plot(est_xyz[:, 0], est_xyz[:, 3], label='est') - plt.title('Yaw') - plt.ylabel('z') - plt.xlabel('t') - plt.legend() - plt.grid() diff --git a/low_cost_ws/src/arg_utils/include/arg_utils/random_map.py b/low_cost_ws/src/arg_utils/include/arg_utils/random_map.py deleted file mode 100644 index 8043ae8..0000000 --- a/low_cost_ws/src/arg_utils/include/arg_utils/random_map.py +++ /dev/null @@ -1,79 +0,0 @@ -# AUTOGENERATED! DO NOT EDIT! File to edit: ../05_random_map.ipynb. - -# %% auto 0 -__all__ = ['random_generate', 'sub_random_generate', 'draw_line'] - -# %% ../05_random_map.ipynb 4 -from random import uniform -import matplotlib.pyplot as plt -import numpy as np - -def random_generate(offset_x, offset_y, point_num, area_length_x, area_length_y): - """ - generate random points with mutiple parameters. - Args: - offset_x : offset for x's range. - offset_y : offset for y's range. - point_num : number of points to generate. - area_length_x : x's range for generate random points. - area_length_y : y's range for generate random points. - Returns: - rand_point_x : list of random points' x coordinate. - rand_point_y : list of random points' y coordinate. - """ - rand_point_x = [] - rand_point_y = [] - rand_point_x = [(uniform(-area_length_x/2, area_length_x/2) + offset_x) for _ in range(point_num)] - rand_point_y = [(uniform(-area_length_y/2, area_length_y/2) + offset_y) for _ in range(point_num)] - return rand_point_x, rand_point_y - -def sub_random_generate(offset_x, offset_y, point_num, area_length_x, area_length_y, sub_area_num, sub_offset_x, sub_offset_y): - """ - generate random points with mutiple parameters, including sub_area_num and sub_offset. - Args : - offset_x : offset for x's range. - offset_y : offset for y's range. - point_num : number of points to generate. - area_length_x : x's range for generate random points. - area_length_y : y's range for generate random points. - sub_area_num : - sub_offset_x : - sub_offset_y : - Returns : - rand_point_x : list of random points' x coordinate. - rand_point_y : list of random points' y coordinate. - Outputs : - output_point.txt : random points' x and y coordinate. - """ - rand_point_x = [] - rand_point_y = [] - path = "output_point.txt" - f = open(path, "w") - for i in range(sub_area_num): - generated_x = [(uniform(-area_length_x/2 + i*(area_length_x/sub_area_num)+sub_offset_x, -area_length_x/2 + (i+1)*(area_length_x/sub_area_num)-sub_offset_x) - + offset_x) for _ in range(int(point_num/sub_area_num))] - generated_y = [(uniform(-area_length_y/2+sub_offset_y, area_length_y/2-sub_offset_y) + offset_y) for _ in range(int(point_num/sub_area_num))] - rand_point_x.extend(generated_x) - rand_point_y.extend(generated_y) - print("Area", i, ":", file=f) - for j in range(int(point_num/sub_area_num)): - print(generated_x[j], generated_y[j], file=f) - f.close() - return rand_point_x, rand_point_y - -def draw_line(sub_area_num, x_min, x_increment, y_min, y_max): - """ - draw straight line in a plot. - Args: - sub_area_num : number of area that separated by lines. - x_min : where the line starts. - x_increment : distance of two lines. - y_min : line's y lower coordinate. - y_max : line's y upper coordinate. - """ - - for i in range(sub_area_num): - plt.vlines(x_min, y_min, y_max, color='green') - x_min += x_increment - - diff --git a/low_cost_ws/src/arg_utils/include/arg_utils/robot_model.py b/low_cost_ws/src/arg_utils/include/arg_utils/robot_model.py deleted file mode 100644 index 5306666..0000000 --- a/low_cost_ws/src/arg_utils/include/arg_utils/robot_model.py +++ /dev/null @@ -1,48 +0,0 @@ -# AUTOGENERATED! DO NOT EDIT! File to edit: ../03_robot_model.ipynb. - -# %% auto 0 -__all__ = ['robot_model_loader'] - -# %% ../03_robot_model.ipynb 3 -#nbdev_comment from __future__ import print_function - -import gdown -from zipfile import ZipFile -import xml.etree.cElementTree as ET -from urdfpy import URDF -import os - -class robot_model_loader: - def __init__(self, url, name): - self.url = url - self.name = name - - def load(self): - """ - download a zipfile and unzip it under data directory - """ - dataset_url = 'https://drive.google.com/u/1/uc?id=' + self.url - dataset_name = self.name - - gdown.download(dataset_url, output=dataset_name + '.zip', quiet=False) - zip = ZipFile(dataset_name + '.zip') - zip.extractall(dataset_name) - zip.close() - - def list_all(self): - """ - list all urdf or xml file - """ - for file in os.listdir(self.name): - if file.find('.urdf') != -1 or file.find('.xml') != -1: - print(file,'\n') - - def show_link(self, path): - """ - show urdf file link - """ - - robot = URDF.load(path) - - for link in robot.links: - print(link.name) diff --git a/low_cost_ws/src/arg_utils/include/arg_utils/transformations.py b/low_cost_ws/src/arg_utils/include/arg_utils/transformations.py deleted file mode 100644 index edf4302..0000000 --- a/low_cost_ws/src/arg_utils/include/arg_utils/transformations.py +++ /dev/null @@ -1,1973 +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 skew(v): - """Returns the skew-symmetric matrix of a vector - cfo, 2015/08/13 - - """ - return numpy.array([[0, -v[2], v[1]], - [v[2], 0, -v[0]], - [-v[1], v[0], 0]], dtype=numpy.float64) - - -def unskew(R): - """Returns the coordinates of a skew-symmetric matrix - cfo, 2015/08/13 - - """ - return numpy.array([R[2, 1], R[0, 2], R[1, 0]], dtype=numpy.float64) - - -def first_order_rotation(rotvec): - """First order approximation of a rotation: I + skew(rotvec) - cfo, 2015/08/13 - - """ - R = numpy.zeros((3, 3), dtype=numpy.float64) - R[0, 0] = 1.0 - R[1, 0] = rotvec[2] - R[2, 0] = -rotvec[1] - R[0, 1] = -rotvec[2] - R[1, 1] = 1.0 - R[2, 1] = rotvec[0] - R[0, 2] = rotvec[1] - R[1, 2] = -rotvec[0] - R[2, 2] = 1.0 - return R - - -def axis_angle(axis, theta): - """Compute a rotation matrix from an axis and an angle. - Returns 3x3 Matrix. - Is the same as transformations.rotation_matrix(theta, axis). - cfo, 2015/08/13 - - """ - if theta*theta > _EPS: - wx = axis[0] - wy = axis[1] - wz = axis[2] - costheta = numpy.cos(theta) - sintheta = numpy.sin(theta) - c_1 = 1.0 - costheta - wx_sintheta = wx * sintheta - wy_sintheta = wy * sintheta - wz_sintheta = wz * sintheta - C00 = c_1 * wx * wx - C01 = c_1 * wx * wy - C02 = c_1 * wx * wz - C11 = c_1 * wy * wy - C12 = c_1 * wy * wz - C22 = c_1 * wz * wz - R = numpy.zeros((3, 3), dtype=numpy.float64) - R[0, 0] = costheta + C00 - R[1, 0] = wz_sintheta + C01 - R[2, 0] = -wy_sintheta + C02 - R[0, 1] = -wz_sintheta + C01 - R[1, 1] = costheta + C11 - R[2, 1] = wx_sintheta + C12 - R[0, 2] = wy_sintheta + C02 - R[1, 2] = -wx_sintheta + C12 - R[2, 2] = costheta + C22 - return R - else: - return first_order_rotation(axis*theta) - - -def expmap_so3(rotvec): - """Exponential map at identity. - Create a rotation from canonical coordinates using Rodrigues' formula. - cfo, 2015/08/13 - - """ - theta = numpy.linalg.norm(rotvec) - axis = rotvec/theta - return axis_angle(axis, theta) - - -def logmap_so3(R): - """Logmap at the identity. - Returns canonical coordinates of rotation. - cfo, 2015/08/13 - - """ - R11 = R[0, 0] - R12 = R[0, 1] - R13 = R[0, 2] - R21 = R[1, 0] - R22 = R[1, 1] - R23 = R[1, 2] - R31 = R[2, 0] - R32 = R[2, 1] - R33 = R[2, 2] - tr = numpy.trace(R) - omega = numpy.empty((3,), dtype=numpy.float64) - - # when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, we do something - # special - if(numpy.abs(tr + 1.0) < 1e-10): - if(numpy.abs(R33 + 1.0) > 1e-10): - omega = (numpy.pi / numpy.sqrt(2.0 + 2.0 * R33)) * \ - numpy.array([R13, R23, 1.0+R33]) - elif(numpy.abs(R22 + 1.0) > 1e-10): - omega = (numpy.pi / numpy.sqrt(2.0 + 2.0 * R22)) * \ - numpy.array([R12, 1.0+R22, R32]) - else: - omega = (numpy.pi / numpy.sqrt(2.0 + 2.0 * R11)) * \ - numpy.array([1.0+R11, R21, R31]) - else: - magnitude = 1.0 - tr_3 = tr - 3.0 - if tr_3 < -1e-7: - theta = numpy.arccos((tr - 1.0) / 2.0) - magnitude = theta / (2.0 * numpy.sin(theta)) - else: - # when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0) - # use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2) - magnitude = 0.5 - tr_3 * tr_3 / 12.0 - - omega = magnitude * numpy.array([R32 - R23, R13 - R31, R21 - R12]) - - return omega - - -def right_jacobian_so3(rotvec): - """Right Jacobian for Exponential map in SO(3) - Equation (10.86) and following equations in G.S. Chirikjian, "Stochastic - Models, Information Theory, and Lie Groups", Volume 2, 2008. - - > expmap_so3(thetahat + omega) \approx expmap_so3(thetahat) * expmap_so3(Jr * omega) - where Jr = right_jacobian_so3(thetahat); - This maps a perturbation in the tangent space (omega) to a perturbation - on the manifold (expmap_so3(Jr * omega)) - cfo, 2015/08/13 - - """ - - theta2 = numpy.dot(rotvec, rotvec) - if theta2 <= _EPS: - return numpy.identity(3, dtype=numpy.float64) - else: - theta = numpy.sqrt(theta2) - Y = skew(rotvec) / theta - I_3x3 = numpy.identity(3, dtype=numpy.float64) - J_r = I_3x3 - ((1.0 - numpy.cos(theta)) / theta) * Y + \ - (1.0 - numpy.sin(theta) / theta) * numpy.dot(Y, Y) - return J_r - - -def S_inv_eulerZYX_body(euler_coordinates): - """ Relates angular rates w to changes in eulerZYX coordinates. - dot(euler) = S^-1(euler_coordinates) * omega - Also called: rotation-rate matrix. (E in Lupton paper) - cfo, 2015/08/13 - - """ - y = euler_coordinates[1] - z = euler_coordinates[2] - E = numpy.zeros((3, 3)) - E[0, 1] = numpy.sin(z)/numpy.cos(y) - E[0, 2] = numpy.cos(z)/numpy.cos(y) - E[1, 1] = numpy.cos(z) - E[1, 2] = -numpy.sin(z) - E[2, 0] = 1.0 - E[2, 1] = numpy.sin(z)*numpy.sin(y)/numpy.cos(y) - E[2, 2] = numpy.cos(z)*numpy.sin(y)/numpy.cos(y) - return E - - -def S_inv_eulerZYX_body_deriv(euler_coordinates, omega): - """ Compute dE(euler_coordinates)*omega/deuler_coordinates - cfo, 2015/08/13 - - """ - - y = euler_coordinates[1] - z = euler_coordinates[2] - - """ - w1 = omega[0]; w2 = omega[1]; w3 = omega[2] - J = numpy.zeros((3,3)) - J[0,0] = 0 - J[0,1] = math.tan(y) / math.cos(y) * (math.sin(z) * w2 + math.cos(z) * w3) - J[0,2] = w2/math.cos(y)*math.cos(z) - w3/math.cos(y)*math.sin(z) - J[1,0] = 0 - J[1,1] = 0 - J[1,2] = -w2*math.sin(z) - w3*math.cos(z) - J[2,0] = w1 - J[2,1] = 1.0/math.cos(y)**2 * (w2 * math.sin(z) + w3 * math.cos(z)) - J[2,2] = w2*math.tan(y)*math.cos(z) - w3*math.tan(y)*math.sin(z) - - """ - - # second version, x = psi, y = theta, z = phi - # J_x = numpy.zeros((3,3)) - J_y = numpy.zeros((3, 3)) - J_z = numpy.zeros((3, 3)) - - # dE^-1/dtheta - J_y[0, 1] = math.tan(y)/math.cos(y)*math.sin(z) - J_y[0, 2] = math.tan(y)/math.cos(y)*math.cos(z) - J_y[2, 1] = math.sin(z)/(math.cos(y))**2 - J_y[2, 2] = math.cos(z)/(math.cos(y))**2 - - # dE^-1/dphi - J_z[0, 1] = math.cos(z)/math.cos(y) - J_z[0, 2] = -math.sin(z)/math.cos(y) - J_z[1, 1] = -math.sin(z) - J_z[1, 2] = -math.cos(z) - J_z[2, 1] = math.cos(z)*math.tan(y) - J_z[2, 2] = -math.sin(z)*math.tan(y) - - J = numpy.zeros((3, 3)) - J[:, 1] = numpy.dot(J_y, omega) - J[:, 2] = numpy.dot(J_z, omega) - - return J - - -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 convert_3x3_to_4x4(matrix_3x3): - M = numpy.identity(4) - M[:3, :3] = matrix_3x3 - return M - - -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 %s" % 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 /= 2.0 - aj /= 2.0 - 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 matrix_from_quaternion(quaternion): - return quaternion_matrix(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 quaternionJPL_matrix(quaternion): - """Return homogeneous rotation matrix from quaternion in JPL notation. - quaternion = [x y z w] - """ - q0 = quaternion[0] - q1 = quaternion[1] - q2 = quaternion[2] - q3 = quaternion[3] - return numpy.array([ - [q0**2 - q1**2 - q2**2 + q3**2, 2.0*q0*q1 + - 2.0*q2*q3, 2.0*q0*q2 - 2.0*q1*q3, 0], - [2.0*q0*q1 - 2.0*q2*q3, - q0**2 + q1**2 - - q2**2 + q3**2, 2.0*q0*q3 + 2.0*q1*q2, 0], - [2.0*q0*q2 + 2.0*q1*q3, 2.0*q1*q2 - 2.0*q0 * - q3, - q0**2 - q1**2 + q2**2 + q3**2, 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)) - - -def random_direction_3d(): - """ equal-area projection according to: - https://math.stackexchange.com/questions/44689/how-to-find-a-random-axis-or-unit-vector-in-3d - cfo, 2015/10/16 - """ - z = numpy.random.rand() * 2.0 - 1.0 - t = numpy.random.rand() * 2.0 * numpy.pi - r = numpy.sqrt(1.0 - z*z) - x = r * numpy.cos(t) - y = r * numpy.sin(t) - return numpy.array([x, y, z], dtype=numpy.float64) - - -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 diff --git a/low_cost_ws/src/arg_utils/include/arg_utils/tsp.py b/low_cost_ws/src/arg_utils/include/arg_utils/tsp.py deleted file mode 100644 index a89f777..0000000 --- a/low_cost_ws/src/arg_utils/include/arg_utils/tsp.py +++ /dev/null @@ -1,130 +0,0 @@ -# AUTOGENERATED! DO NOT EDIT! File to edit: ../07_tsp.ipynb. - -# %% auto 0 -__all__ = ['dist', 'distanceGenerate', 'sortWaypoint', 'solve_tsp_nearest_neighbor', 'solve_tsp_held_karp'] - -# %% ../07_tsp.ipynb 4 -import sys -import itertools -import random -import time -import matplotlib.pyplot as plt -import numpy as np -import math -from python_tsp.heuristics import solve_tsp_simulated_annealing - -def dist(p1, p2): - """ - calculate the distance between two waypoints. - Args: - p1 : point's (x,y) position. - p2 : point's (x,y) position. - Returns: - distance between two points. - """ - return math.sqrt(((p1-p2)**2).sum()) - -def distanceGenerate(point_set): - """ - generate a distance matrix based on the waypoints. - Args: - point_set : a set that contains all waypoint. - Returns: - a square matrix, which shows the distance between pairs of waypoint. - """ - return np.asarray([[dist(np.array(p1), np.array(p2)) for p2 in point_set] for p1 in point_set]) - -def sortWaypoint(permutation, point_set): - """ - accoriding to permutation calculated by tsp solver, return new set of waypoint which is sorted. - Args: - permutation : the order of waypoints calculated by tsp solver. - point_set : a set that contains all waypoint. - Returns: - new set of waypoint which is sorted. - """ - return [x for _, x in sorted(zip(permutation, point_set))] - -def solve_tsp_nearest_neighbor(distance_matrix): - """ - calculate tsp problem based on nearest neighbor, an algorithm that solves tsp using greedy assumption. - Args: - distance_matrix : a square matrix, which shows the distance between pairs of waypoint. - Returns: - A tuple, (path, cost) - cost : optimal cost of tsp - path : a orderd list of waypoint index based on distance matrix. - """ - path = [0] - cost = 0 - N = distance_matrix.shape[0] - mask = np.ones(N, dtype=bool) - mask[0] = False - - for i in range(N-1): - last = path[-1] - next_ind = np.argmin(distance_matrix[last][mask]) # find minimum of remaining locations - next_loc = np.arange(N)[mask][next_ind] # convert to original location - path.append(next_loc) - mask[next_loc] = False - cost += distance_matrix[last, next_loc] - if(i == N-2): - cost += distance_matrix[next_loc, 0] - - return path, cost - -def solve_tsp_held_karp(distance_matrix): - """ - calculate tsp problem based on Held-Karp, an algorithm that solves tsp using dynamic programming with memoization. - Args: - distance_matrix : a square matrix, which shows the distance between pairs of waypoint. - Returns: - A tuple, (path, cost) - cost : optimal cost of tsp - path : a orderd list of waypoint index based on distance matrix. - """ - n = len(distance_matrix) - C = {} - - # Set transition cost from initial state - for k in range(1, n): - C[(1 << k, k)] = (distance_matrix[0][k], 0) - - # Iterate subsets of increasing length and store intermediate results - # in classic dynamic programming manner - for subset_size in range(2, n): - for subset in itertools.combinations(range(1, n), subset_size): - bits = 0 - for bit in subset: - bits |= 1 << bit - # Find the lowest cost to get to this subset - for k in subset: - prev = bits & ~(1 << k) - res = [] - for m in subset: - if m == 0 or m == k: - continue - res.append((C[(prev, m)][0] + distance_matrix[m][k], m)) - C[(bits, k)] = min(res) - bits = (2**n - 1) - 1 - - # Calculate optimal cost - res = [] - for k in range(1, n): - res.append((C[(bits, k)][0] + distance_matrix[k][0], k)) - opt, parent = min(res) - - # Backtrack to find full path - path = [] - for i in range(n - 1): - path.append(parent) - new_bits = bits & ~(1 << parent) - _, parent = C[(bits, parent)] - bits = new_bits - - # Add implicit start state - path.append(0) - - return list(reversed(path)), opt - - diff --git a/low_cost_ws/src/arg_utils/include/arg_utils/utils.py b/low_cost_ws/src/arg_utils/include/arg_utils/utils.py deleted file mode 100644 index 6005fbf..0000000 --- a/low_cost_ws/src/arg_utils/include/arg_utils/utils.py +++ /dev/null @@ -1,75 +0,0 @@ -# AUTOGENERATED! DO NOT EDIT! File to edit: ../00_utils.ipynb. - -# %% auto 0 -__all__ = ['gdown_unzip', 'gdown_download', 'pose_dis', 'waypoint'] - -# %% ../00_utils.ipynb 4 -import os -import sys -import gdown -import copy -import math -from zipfile import ZipFile - -def gdown_unzip(id, filename): - """download a zipfile and unzip it - """ - dataset_url = 'https://drive.google.com/u/1/uc?id=' + id - dataset_name = filename - - if not os.path.isdir(dataset_name): - gdown.download(dataset_url, output = dataset_name + '.zip', quiet=False) - zip_file = ZipFile( dataset_name + '.zip') - #zip_file.extractall() - zip_file.extractall() # depends on how to zip it - zip_file.close() - -def gdown_download(id, filename): - """download a file - """ - dataset_url = 'https://drive.google.com/u/1/uc?id=' + id - dataset_name = filename - - if not os.path.isdir(dataset_name): - gdown.download(dataset_url, output = dataset_name, quiet=False) - -def pose_dis(pose_1, pose_2): - """Compute distance between pose_1 and pose_2 - """ - x = pose_1[0] - pose_2[0] - y = pose_1[1] - pose_2[1] - z = pose_1[2] - pose_2[2] - - dis = math.sqrt(x**2+y**2+z**2) - - return dis - -def waypoint(current_pose, Target_pose): - """Generate a list of way points from current pose to target pose - - Input : current pose, target pose : list [x_pos, y_pos, z_pos, x_ori, y_ori, z_ori, w_ori] - Return : a list of way points - - """ - waypoint_list = [] - factor = 0.5 - sub_pose = copy.deepcopy(current_pose) - - # threshold : distance between sub_pose and target_pose = 0.05 meter - dis = pose_dis(sub_pose, Target_pose) - while dis > 0.05: - sub_pose[0] = (sub_pose[0] + Target_pose[0])*factor - sub_pose[1] = (sub_pose[1] + Target_pose[1])*factor - sub_pose[2] = (sub_pose[2] + Target_pose[2])*factor - sub_pose[3] = Target_pose[3] - sub_pose[4] = Target_pose[4] - sub_pose[5] = Target_pose[5] - sub_pose[6] = Target_pose[6] - - dis = pose_dis(sub_pose, Target_pose) - - waypoint_list.append(copy.deepcopy(sub_pose)) - - waypoint_list.append(Target_pose) - - return waypoint_list diff --git a/low_cost_ws/src/arg_utils/include/arg_utils/uwb.py b/low_cost_ws/src/arg_utils/include/arg_utils/uwb.py deleted file mode 100644 index 8f6ab45..0000000 --- a/low_cost_ws/src/arg_utils/include/arg_utils/uwb.py +++ /dev/null @@ -1,283 +0,0 @@ -# AUTOGENERATED! DO NOT EDIT! File to edit: ../06_uwb.ipynb. - -# %% auto 0 -__all__ = ['UWB'] - -# %% ../06_uwb.ipynb 4 -import yaml -import serial -from serial.tools.list_ports import comports -import pypozyx -from pypozyx import PozyxSerial -from pypozyx import NetworkID -from pypozyx import Coordinates, DeviceCoordinates -from pypozyx import DeviceRange -from pypozyx import PozyxConstants -from pypozyx.core import PozyxException -from typing import List - -# %% ../06_uwb.ipynb 5 -class UWB(): - def __init__(self, port = None): - self.port = port - self.network_id = None - self._pozyx_handler = None - self._pose = None - self._env_config = None - - #TODO: Make height parameterized - self._height = 500 - - @property - def network_id(self): - return self._network_id - - @property - def network_id_str(self) -> str: - """A getter method of network id string - - Convert network id to string to show in readable. - - Returns: - str: A string of id number in hexadecimal of The Pozyx - """ - return str(self._network_id) - - @network_id.setter - def network_id(self, value: int = None) -> None: - """A setter method of port string - - Args: - value (int, optional): A integer id number in hexadecimal or decimal of The Pozyx. Defaults to None. - """ - if value is None: - self._network_id = NetworkID() - else: - self._network_id = NetworkID(value) - - -# %% ../06_uwb.ipynb 6 -class UWB(UWB): - # pose - @property - def pose(self) -> List[float]: - """A getter method of UWB pose - - Returns: - list[float]: (pose.x, pose.y, pose.z) - """ - return (self._pose.x, self._pose.y, self._pose.z) - - @pose.setter - def pose(self, value: List[float] = None) -> None: - """A setter method of UWB pose - - Args: - value (List[float], optional): (pose.x, pose.y, pose.z) Defaults to None. - """ - if value is None: - self._pose = Coordinates() - else: - self._pose.x = value[0] - self._pose.y = value[1] - self._pose.z = value[2] - - -# %% ../06_uwb.ipynb 7 -class UWB(UWB): - # height - @property - def height(self) -> float: - """A getter method of UWB pose height - - Returns: - float: The default height for 2.5D localization. - """ - return self._height - - @height.setter - def height(self, value: float = 0) -> float: - """A setter method of UWB pose height - - Args: - value (int, optional): The default height for 2.5D localization.. Defaults to 0. - """ - self._height = value - - -# %% ../06_uwb.ipynb 8 -class UWB(UWB): - # env_config - @property - def env_config(self) -> dict: - """A getter method of environment config - - Returns: - dict: The environment config in dict format. - """ - return self._env_config - - -# %% ../06_uwb.ipynb 9 -class UWB(UWB): - # port_lost - def port_list(self) -> List[str]: - """A getter method of port list. - - Returns: - List[str]: The list contains UWB port device path like `/dev/ttyACM0`. - """ - return self._port_list - - -# %% ../06_uwb.ipynb 10 -class UWB(UWB): - # status - @property - def status(self) -> int: - """A getter method of UWB status. - - Returns: - int: The status got from Pozyx. 0 is success. - """ - return self._status - - -# %% ../06_uwb.ipynb 11 -class UWB(UWB): - def load_env_config(self, config_file_path: str) -> bool: - """Load UWB anchors' environment config. - - Args: - config_file_path (str): The environment config file path. - - Returns: - bool: True for success, False for failure. - """ - with open(config_file_path, "r") as config_file: - try: - self._env_config = yaml.safe_load(config_file) - except yaml.YAMLError as ex: - print(ex) - return False - return True - - -# %% ../06_uwb.ipynb 12 -class UWB(UWB): - def scan_port(self) -> None: - """Scan all port connecting to host. Store port device path in port list. - """ - self._port_list = [] - for port in comports(): - try: - if "Pozyx Labs" in port.manufacturer: - self._port_list.append(port.device) - break - except TypeError: - pass - try: - if "Pozyx" in port.product: - self._port_list.append(port.device) - break - except TypeError: - pass - -# %% ../06_uwb.ipynb 13 -class UWB(UWB): - def connect(self) -> bool: - """Try to connect pozyx device. - - Returns: - bool: Pozyx status - """ - self._status = PozyxConstants.STATUS_SUCCESS - if self.port is None: - self.scan_port() - if len(self._port_list) == 1: - self.port = self._port_list[0] - self._pozyx_handler = PozyxSerial(self.port) - self._status &= self._pozyx_handler.getNetworkId(self._network_id) - elif len(self._port_list) == 0: - return False - else: - return False - else: - try: - self._pozyx_handler = PozyxSerial(self.port) - self._status &= self._pozyx_handler.getNetworkId(self._network_id) - return True - except PozyxException as ex: - print(ex) - return False - - -# %% ../06_uwb.ipynb 14 -class UWB(UWB): - def write_env_config(self) -> bool: - """Write environment anchor location into Pozyx UWB device. - - Returns: - bool: Pozyx status - """ - self._status = PozyxConstants.STATUS_SUCCESS - ANCHOR_FLAG = 1 - self._status &= self._pozyx_handler.clearDevices() - for anchor_name, config in self.env_config.items(): - coordinate = Coordinates(config["x"], config["y"], config["z"]) - device_coordinate = DeviceCoordinates(config["id"], ANCHOR_FLAG, coordinate) - self._status &= self._pozyx_handler.addDevice(device_coordinate) - if len(self.env_config) > 4: - self._status &= self._pozyx_handler.setSelectionOfAnchorsAutomatic(len(self.env_config)) - return self._status - - -# %% ../06_uwb.ipynb 15 -class UWB(UWB): - def localize_2_5D(self) -> bool: - """Localize method in 2.5D. Need to know height. - - Returns: - bool: Pozyx status - """ - self._status &= self._pozyx_handler.doPositioning( - self._pose, - PozyxConstants.DIMENSION_2_5D, - self._height, - PozyxConstants.POSITIONING_ALGORITHM_UWB_ONLY, - ) - return self._status - - -# %% ../06_uwb.ipynb 16 -class UWB(UWB): - def localize_3D(self)->bool: - """Localize method in 3D. The height will be determined by Pozyx UWB device. - - Returns: - bool: Pozyx status - """ - self._status &= self._pozyx_handler.doPositioning( - self._pose, - PozyxConstants.DIMENSION_3D, - self._height, - PozyxConstants.POSITIONING_ALGORITHM_UWB_ONLY, - ) - return self._status - - -# %% ../06_uwb.ipynb 17 -class UWB(UWB): - def range_from(self, dest_id) -> float: - """Range method from this Pozyx UWB device to the destination Pozyx UWB device. - - Args: - dest_id (_type_): The target Pozyx UWB device id want to be ranged. - - Returns: - float: The range from this Pozyx UWB device to the destination Pozyx UWB device. - """ - ranges = DeviceRange() - self._pozyx_handler.doRanging(dest_id, ranges) - return ranges - diff --git a/low_cost_ws/src/arg_utils/include/arg_utils/video2picture.py b/low_cost_ws/src/arg_utils/include/arg_utils/video2picture.py deleted file mode 100644 index 770eb67..0000000 --- a/low_cost_ws/src/arg_utils/include/arg_utils/video2picture.py +++ /dev/null @@ -1,36 +0,0 @@ -# AUTOGENERATED! DO NOT EDIT! File to edit: ../02_video2picture.ipynb. - -# %% auto 0 -__all__ = ['get_images_from_video'] - -# %% ../02_video2picture.ipynb 4 -import gdown -from zipfile import ZipFile -from PIL import Image -import sys -import os -import cv2 - -# %% ../02_video2picture.ipynb 6 -def get_images_from_video(video_name, time_F): - ''' - open and read video,then save the images of video depending on the parameter(time_F) you setup. - ''' - video_images = [] - vc = cv2.VideoCapture(video_name) - c = 1 - - if vc.isOpened(): - rval, video_frame = vc.read() - else: - rval = False - - while rval: - rval, video_frame = vc.read() - - if(c % time_F == 0): - video_images.append(video_frame) - c = c + 1 - vc.release() - - return video_images diff --git a/low_cost_ws/src/arg_utils/include/arg_utils/websocket_rosbridge.py b/low_cost_ws/src/arg_utils/include/arg_utils/websocket_rosbridge.py deleted file mode 100644 index 000572c..0000000 --- a/low_cost_ws/src/arg_utils/include/arg_utils/websocket_rosbridge.py +++ /dev/null @@ -1,76 +0,0 @@ -# AUTOGENERATED! DO NOT EDIT! File to edit: ../08_websocket_rosbridge.ipynb. - -# %% auto 0 -__all__ = ['ros_socket'] - -# %% ../08_websocket_rosbridge.ipynb 4 -import roslibpy -import time - -# %% ../08_websocket_rosbridge.ipynb 5 -class ros_socket(): - def __init__(self, ip, port=9090): - ''' - __init__ - - Input: - ip(type: string) ip address you want to connect - port(type: int) default is 9090 - ''' - self.ip = ip - self.port = port - self.topic = [] - self.node = [] - self.topic_name = '' - self.topic_type = '' - self.client = roslibpy.Ros(host = ip, port = port) - self.client.run() - - def get_topic(self): - self.topic = self.client.get_topics() - return self.topic - - def get_node(self): - self.node = self.client.get_nodes() - return self.node - - def check_connecting(self): - print('Is ROS connected?', self.client.is_connected) - - def subscriber(self, topic_name, subscribe_callback, rate_in_ms=1000): - ''' - subscriber - subscribe topic with rate (default = 1sec) - - Input: - topic_name(type:) - subscribe_callback(message) - (type: function) *only one argument message-> that will load with data you subscribing - ''' - self.topic_name = topic_name - self.topic_type = self.client.get_topic_type(self.topic_name) - listener = roslibpy.Topic(self.client, self.topic_name, self.topic_type,throttle_rate = rate_in_ms) - listener.subscribe(subscribe_callback) - - def publisher(self, topic_name, topic_type, message_data): - ''' - publisher - publish message_data to topic_name - - Input: - topic_name(type:string) - topic_type(type:) - message_data(type:topic_type) - ''' - talker = roslibpy.Topic(client, topic_name, topic_type) - talker.publish(roslibpy.Message({'': message_data})) - - - def println(self, ros_list): - if len(ros_list) == 0: - print('Empty') - else: - for i in ros_list: - print(i) - - diff --git a/low_cost_ws/src/arg_utils/include/arg_utils/xbee_coding.py b/low_cost_ws/src/arg_utils/include/arg_utils/xbee_coding.py deleted file mode 100644 index ebdf0a9..0000000 --- a/low_cost_ws/src/arg_utils/include/arg_utils/xbee_coding.py +++ /dev/null @@ -1,81 +0,0 @@ -# AUTOGENERATED! DO NOT EDIT! File to edit: ../10_xbee_coding.ipynb. - -# %% auto 0 -__all__ = ['np_array_to_Odometry', 'xbee_encode', 'xbee_decode'] - -# %% ../10_xbee_coding.ipynb 4 -import pickle - -def np_array_to_Odometry(array): - msg = Odometry() - msg.header.frame_id = "odom" - msg.pose.pose.position.x = array[0] - msg.pose.pose.position.y = array[1] - msg.pose.pose.position.z = array[2] - msg.pose.pose.orientation.x = array[3] - msg.pose.pose.orientation.y = array[4] - msg.pose.pose.orientation.z = array[5] - msg.pose.pose.orientation.w = array[6] - msg.twist.twist.linear.x = array[7] - msg.twist.twist.linear.y = array[8] - msg.twist.twist.linear.z = array[9] - msg.twist.twist.angular.x = array[10] - msg.twist.twist.angular.y = array[11] - msg.twist.twist.angular.z = array[12] - return msg - -def xbee_encode(data_via_xbee, data_type): - data = data_via_xbee - - # send data - byte_arr = pickle.dumps( data ) - length, index, check= int(len(byte_arr)), 0, 0 - - for index in range(0,length,250) : - pack = bytearray(b'\xAB') #Header - pack.extend(bytearray(data_type)) #Type - pack.extend( length.to_bytes(4, byteorder='big') ) #bytes - index_end = index+250 if index+250 < length else length - pack.extend( byte_arr[index:(index_end)] ) #data - - if index_end == length : pack.extend(check.to_bytes(1, byteorder='big')) # checksum - else: check = 0xff & (check + pack[-1]) - - return pack - -def xbee_decode(xbee_message): - get_register = bytearray() - #print(xbee_message) - if not xbee_message[0:1] == b'\xAB' : # Header wrong - print('get xbee_message with wrong Header') - return - - if not ((xbee_message[1:2] == b'\x00') or (xbee_message[1:2] == b'\x01') or (xbee_message[1:2] == b'\x02') or (xbee_message[1:2] == b'\x03')): - rospy.loginfo('xbeejoy callback') - self.count += 1 - get_msg = pickle.loads(xbee_message) - axes = get_msg[0:8] - buttons = get_msg[8:] - rospy.loginfo(axes) - rospy.loginfo(buttons) - - msg = Joy() - msg.header.seq = self.count - msg.header.frame_id = "/dev/input/js0" - msg.header.stamp = rospy.Time.now() - msg.axes = axes - msg.buttons = buttons - return msg - - get_register.extend(xbee_message[6:]) - - if xbee_message[1:2] == b'\x00': - get_msg = pickle.loads(get_register[:-1]) - #print(get_msg) - return(get_msg) - - - if xbee_message[1:2] == b'\x02': # type: points - get_points = pickle.loads(get_register[:-1]) - pub_msg = np_array_to_Odometry(get_points) - return(pub_msg) diff --git a/low_cost_ws/src/arg_utils/include/for_example/__init__.py b/low_cost_ws/src/arg_utils/include/for_example/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/low_cost_ws/src/arg_utils/include/for_example/import_me_if_u_can.py b/low_cost_ws/src/arg_utils/include/for_example/import_me_if_u_can.py deleted file mode 100644 index b23efac..0000000 --- a/low_cost_ws/src/arg_utils/include/for_example/import_me_if_u_can.py +++ /dev/null @@ -1,5 +0,0 @@ -def say_it_works(): - print("You have successed import me!\nfrom for_example pkg :D") - -def say_it_pytest(): - return "It works!" diff --git a/low_cost_ws/src/arg_utils/package.xml b/low_cost_ws/src/arg_utils/package.xml deleted file mode 100644 index 31e03f7..0000000 --- a/low_cost_ws/src/arg_utils/package.xml +++ /dev/null @@ -1,68 +0,0 @@ - - - arg_utils - 0.0.0 - The arg_utils package - - - - - uwe - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - roscpp - rospy - std_msgs - roscpp - rospy - std_msgs - roscpp - rospy - std_msgs - module - - - - - - - diff --git a/low_cost_ws/src/arg_utils/scripts/add_path.py b/low_cost_ws/src/arg_utils/scripts/add_path.py deleted file mode 100644 index df63445..0000000 --- a/low_cost_ws/src/arg_utils/scripts/add_path.py +++ /dev/null @@ -1,7 +0,0 @@ -#!/usr/bin/env python3 - -import os -import sys -sys.path.append( - os.path.join(os.path.dirname(os.path.abspath(__file__)), - '../include')) \ No newline at end of file diff --git a/low_cost_ws/src/arg_utils/scripts/plot_lines.py b/low_cost_ws/src/arg_utils/scripts/plot_lines.py deleted file mode 100644 index 4260ada..0000000 --- a/low_cost_ws/src/arg_utils/scripts/plot_lines.py +++ /dev/null @@ -1,22 +0,0 @@ -import numpy as np -import matplotlib.pyplot as plt - -import add_path -from arg_utils.plotting import * - -# try xy_plot first -vec1 = np.array([[1, 2], [3, 4], [5, 6]]) -vec2 = np.array([[1, 3], [2, 4], [3, 5]]) -vec3 = np.array([[1, 4], [2, 5], [3, 6]]) -vec4 = np.array([[1, 5], [2, 6], [3, 7]]) -xy_plot('xy_plot', 'x', 'y', vec1, 'vec1', vec2, 'vec2', vec3, 'vec3', vec4, 'vec4') -plt.show() - -# try xyzt_plot next -vec1 = np.array([[1, 2, 3, 4], [2, 3, 4, 5], [3, 4, 5, 6]]) -vec2 = np.array([[1, 3, 4, 5], [2, 4, 5, 6], [3, 5, 6, 7]]) -vec3 = np.array([[1, 4, 5, 6], [2, 5, 6, 7], [3, 6, 7, 8]]) -vec4 = np.array([[1, 5, 6, 7], [2, 6, 7, 8], [3, 7, 8, 9]]) -xyzt_plot('xyzt_plot', vec1, 'vec1', vec2, 'vec2', vec3, 'vec3', vec4, 'vec4') -plt.show() - diff --git a/low_cost_ws/src/arg_utils/scripts/plot_poses.py b/low_cost_ws/src/arg_utils/scripts/plot_poses.py deleted file mode 100644 index 8d8e053..0000000 --- a/low_cost_ws/src/arg_utils/scripts/plot_poses.py +++ /dev/null @@ -1,60 +0,0 @@ -# Examples in pytransform3d https://dfki-ric.github.io/pytransform3d/_auto_examples/index.html - -import numpy as np -import matplotlib.pyplot as plt - -from pytransform3d.transformations import plot_transform -from pytransform3d.plot_utils import make_3d_axis -import pytransform3d.camera as pc -import pytransform3d.transformations as pt -from pytransform3d import rotations as pr -from pytransform3d.plot_utils import remove_frame - -ax = make_3d_axis(ax_s=1, unit="m", n_ticks=6) -plot_transform(ax=ax) -plt.tight_layout() -plt.show() - - -cam2world = pt.transform_from_pq([0, 0, 0, np.sqrt(0.5), -np.sqrt(0.5), 0, 0]) -# default parameters of a camera in Blender -sensor_size = np.array([0.036, 0.024]) -intrinsic_matrix = np.array([ - [0.05, 0, sensor_size[0] / 2.0], - [0, 0.05, sensor_size[1] / 2.0], - [0, 0, 1] -]) -virtual_image_distance = 1 - -ax = pt.plot_transform(A2B=cam2world, s=0.2) -pc.plot_camera( - ax, cam2world=cam2world, M=intrinsic_matrix, sensor_size=sensor_size, - virtual_image_distance=virtual_image_distance) -plt.show() - - - -alpha, beta, gamma = 0.5 * np.pi, 0.5 * np.pi, 0.5 * np.pi -p = np.array([1, 1, 1]) - -plt.figure(figsize=(5, 5)) - -ax = pr.plot_basis(R=np.eye(3), p=-1.5 * p, ax_s=2) -pr.plot_axis_angle(ax, [1, 0, 0, alpha], -1.5 * p) - -pr.plot_basis( - ax, pr.active_matrix_from_extrinsic_euler_xyz([alpha, 0, 0]), -0.5 * p) -pr.plot_axis_angle(ax, [0, 1, 0, beta], p=-0.5 * p) - -pr.plot_basis( - ax, pr.active_matrix_from_extrinsic_euler_xyz([alpha, beta, 0]), 0.5 * p) -pr.plot_axis_angle(ax, [0, 0, 1, gamma], 0.5 * p) - -pr.plot_basis( - ax, - pr.active_matrix_from_extrinsic_euler_xyz([alpha, beta, gamma]), 1.5 * p, - lw=5) - -remove_frame(ax) - -plt.show() diff --git a/low_cost_ws/src/arg_utils/scripts/test_pypkg.py b/low_cost_ws/src/arg_utils/scripts/test_pypkg.py deleted file mode 100644 index 6c5a938..0000000 --- a/low_cost_ws/src/arg_utils/scripts/test_pypkg.py +++ /dev/null @@ -1,16 +0,0 @@ -#!/usr/bin/env python3 - -import add_path - -from arg_utils.import_me_if_u_can import say_it_works as say_it_works -from for_example.import_me_if_u_can import say_it_works as say_it_works_2 - -# write a test function for say_it_works -def test_say_it_works(): - assert say_it_works() == "It works!" - -def test_say_it_works_2(): - assert say_it_works_2() == "It works!" - -#say_it_works() -#say_it_works_2() diff --git a/low_cost_ws/src/arg_utils/scripts/test_transformations.py b/low_cost_ws/src/arg_utils/scripts/test_transformations.py deleted file mode 100644 index c4965e5..0000000 --- a/low_cost_ws/src/arg_utils/scripts/test_transformations.py +++ /dev/null @@ -1,8 +0,0 @@ - -import pytest - -import add_path -from arg_utils import transformations - -# test cases for transformations.py - diff --git a/low_cost_ws/src/arg_utils/setup.py b/low_cost_ws/src/arg_utils/setup.py deleted file mode 100644 index a6dffe3..0000000 --- a/low_cost_ws/src/arg_utils/setup.py +++ /dev/null @@ -1,10 +0,0 @@ -## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -# fetch values from package.xml -setup_args = generate_distutils_setup( - packages=['arg_utils'], - package_dir={'': 'include'}, -) -setup(**setup_args) \ No newline at end of file From 96b4cdc23a6c7c8511427813a98cec77a7db76e1 Mon Sep 17 00:00:00 2001 From: uwe Date: Tue, 17 Oct 2023 12:39:05 +0800 Subject: [PATCH 48/52] add arg_utils submodule --- .gitmodules | 3 +++ low_cost_ws/src/arg_utils | 1 + 2 files changed, 4 insertions(+) create mode 160000 low_cost_ws/src/arg_utils diff --git a/.gitmodules b/.gitmodules index 2b32ed6..0765cab 100644 --- a/.gitmodules +++ b/.gitmodules @@ -4,3 +4,6 @@ [submodule "low_cost_ws/src/apriltags_ros"] path = low_cost_ws/src/apriltags_ros url = git@github.com:Sensing-Intelligent-System/apriltags_ros.git +[submodule "low_cost_ws/src/arg_utils"] + path = low_cost_ws/src/arg_utils + url = git@github.com:ARG-NCTU/arg_utils.git diff --git a/low_cost_ws/src/arg_utils b/low_cost_ws/src/arg_utils new file mode 160000 index 0000000..f4fce22 --- /dev/null +++ b/low_cost_ws/src/arg_utils @@ -0,0 +1 @@ +Subproject commit f4fce22d4a8d5f9d6c268bcfc32a23e2c6f89e7d From 64d351f3afdc987b0a7ea2d3690c76fa2046b559 Mon Sep 17 00:00:00 2001 From: uwe Date: Tue, 17 Oct 2023 12:47:59 +0800 Subject: [PATCH 49/52] modify importme pytest and delete testing pypkg_from_utils --- .../rostest_example/include/for_example/__init__.py | 0 .../include/for_example/import_me_if_u_can.py | 3 +++ .../src/rostest_example/scripts/test_import_me.py | 11 +++-------- .../scripts/testing_pypkg_from_arg_utils.py | 7 ------- 4 files changed, 6 insertions(+), 15 deletions(-) create mode 100644 low_cost_ws/src/rostest_example/include/for_example/__init__.py create mode 100644 low_cost_ws/src/rostest_example/include/for_example/import_me_if_u_can.py delete mode 100644 low_cost_ws/src/rostest_example/scripts/testing_pypkg_from_arg_utils.py diff --git a/low_cost_ws/src/rostest_example/include/for_example/__init__.py b/low_cost_ws/src/rostest_example/include/for_example/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/low_cost_ws/src/rostest_example/include/for_example/import_me_if_u_can.py b/low_cost_ws/src/rostest_example/include/for_example/import_me_if_u_can.py new file mode 100644 index 0000000..d84a991 --- /dev/null +++ b/low_cost_ws/src/rostest_example/include/for_example/import_me_if_u_can.py @@ -0,0 +1,3 @@ +def say_it_works(): + print("You have successed import me!\nfrom for_example pkg :D") + return "It works!" \ No newline at end of file diff --git a/low_cost_ws/src/rostest_example/scripts/test_import_me.py b/low_cost_ws/src/rostest_example/scripts/test_import_me.py index 0f63968..37fc72d 100644 --- a/low_cost_ws/src/rostest_example/scripts/test_import_me.py +++ b/low_cost_ws/src/rostest_example/scripts/test_import_me.py @@ -1,12 +1,7 @@ import pytest import add_path -from arg_utils.import_me_if_u_can import * -from for_example.import_me_if_u_can import say_it_pytest as say_it_pytest_1 - -def test_say_it_from_arg_utils(): - assert say_it_pytest() == "You have successed import me! from arg_utils pkg" - -def test_say_it_from_for_example(): - assert say_it_pytest_1() == "You have successed import me! from for_example pkg" +from for_example.import_me_if_u_can import say_it_works as say_it_pytest +def test_say_it_from_rostest(): + assert say_it_pytest() == "It works!" diff --git a/low_cost_ws/src/rostest_example/scripts/testing_pypkg_from_arg_utils.py b/low_cost_ws/src/rostest_example/scripts/testing_pypkg_from_arg_utils.py deleted file mode 100644 index 5930a28..0000000 --- a/low_cost_ws/src/rostest_example/scripts/testing_pypkg_from_arg_utils.py +++ /dev/null @@ -1,7 +0,0 @@ -import add_path - -from arg_utils.import_me_if_u_can import say_it_works as say_it_works_1 -from for_example.import_me_if_u_can import say_it_works as sat_it_works_2 - -say_it_works_1() -sat_it_works_2() \ No newline at end of file From e3a8b245e0ce2573fab2492a6b192f306b99bb8f Mon Sep 17 00:00:00 2001 From: uwe Date: Wed, 18 Oct 2023 16:45:17 +0800 Subject: [PATCH 50/52] add wget and apriltag --- docker/dockerfile | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/docker/dockerfile b/docker/dockerfile index e0571b4..09ded21 100644 --- a/docker/dockerfile +++ b/docker/dockerfile @@ -56,7 +56,9 @@ RUN pip3 install --upgrade pip \ scipy \ opencv-python \ dbg \ - pytransform3d + pytransform3d \ + wget \ + apriltag RUN apt-get -o Acquire::ForceIPv4=true update && apt-get -yq dist-upgrade \ From 1af0c17602e80c77641acc5dc02deed1254784ae Mon Sep 17 00:00:00 2001 From: uwe Date: Wed, 18 Oct 2023 16:46:12 +0800 Subject: [PATCH 51/52] update arg_utils --- low_cost_ws/src/arg_utils | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/low_cost_ws/src/arg_utils b/low_cost_ws/src/arg_utils index f4fce22..5f05961 160000 --- a/low_cost_ws/src/arg_utils +++ b/low_cost_ws/src/arg_utils @@ -1 +1 @@ -Subproject commit f4fce22d4a8d5f9d6c268bcfc32a23e2c6f89e7d +Subproject commit 5f0596144a112a72fb97780ac9d3a5eece38c0d3 From 31bcad98b99b1f467e6a98867ace1155904fe4af Mon Sep 17 00:00:00 2001 From: uwe Date: Thu, 19 Oct 2023 10:17:02 +0800 Subject: [PATCH 52/52] submodule: arg_utils change dir data name --- low_cost_ws/src/arg_utils | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/low_cost_ws/src/arg_utils b/low_cost_ws/src/arg_utils index 5f05961..6a67903 160000 --- a/low_cost_ws/src/arg_utils +++ b/low_cost_ws/src/arg_utils @@ -1 +1 @@ -Subproject commit 5f0596144a112a72fb97780ac9d3a5eece38c0d3 +Subproject commit 6a6790393aab4de623da6afd6e16f7e8828e8bc4